Merge tag 'staging-4.12-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging

Pull staging/IIO updates from Greg KH:
 "Here is the big staging tree update for 4.12-rc1.

  It's a big one, adding about 350k new lines of crap^Wcode, mostly all
  in a big dump of media drivers from Intel. But there's other new
  drivers in here as well, yet-another-wifi driver, new IIO drivers, and
  a new crypto accelerator.

  We also deleted a bunch of stuff, mostly in patch cleanups, but also
  the Android ION code has shrunk a lot, and the Android low memory
  killer driver was finally deleted, much to the celebration of the -mm
  developers.

  All of these have been in linux-next with a few build issues that will
  show up when you merge to your tree"

Merge conflicts in the new rtl8723bs driver (due to the wifi changes
this merge window) handled as per linux-next, courtesy of Stephen
Rothwell.

* tag 'staging-4.12-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging: (1182 commits)
  staging: fsl-mc/dpio: add cpu <--> LE conversion for dpaa2_fd
  staging: ks7010: remove line continuations in quoted strings
  staging: vt6656: use tabs instead of spaces
  staging: android: ion: Fix unnecessary initialization of static variable
  staging: media: atomisp: fix range checking on clk_num
  staging: media: atomisp: fix misspelled word in comment
  staging: media: atomisp: kmap() can't fail
  staging: atomisp: remove #ifdef for runtime PM functions
  staging: atomisp: satm include directory is gone
  atomisp: remove some more unused files
  atomisp: remove hmm_load/store/clear indirections
  atomisp: kill off mmgr_free
  atomisp: clean up the hmm init/cleanup indirections
  atomisp: handle allocation calls before init in the hmm layer
  staging: fsl-dpaa2/eth: Add maintainer for Ethernet driver
  staging: fsl-dpaa2/eth: Add TODO file
  staging: fsl-dpaa2/eth: Add trace points
  staging: fsl-dpaa2/eth: Add driver specific stats
  staging: fsl-dpaa2/eth: Add ethtool support
  staging: fsl-dpaa2/eth: Add Freescale DPAA2 Ethernet driver
  ...

1754 files changed, 358735 insertions, 15210 deletions

diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
index b55804cac4c4..ea9726e71468 100644
--- a/drivers/base/dma-contiguous.c
+++ b/drivers/base/dma-contiguous.c
@@ -165,7 +165,8 @@ int __init dma_contiguous_reserve_area(phys_addr_t size, phys_addr_t base,
 {
 	int ret;
 
-	ret = cma_declare_contiguous(base, size, limit, 0, 0, fixed, res_cma);
+	ret = cma_declare_contiguous(base, size, limit, 0, 0, fixed,
+					"reserved", res_cma);
 	if (ret)
 		return ret;
 
@@ -258,7 +259,7 @@ static int __init rmem_cma_setup(struct reserved_mem *rmem)
 		return -EINVAL;
 	}
 
-	err = cma_init_reserved_mem(rmem->base, rmem->size, 0, &cma);
+	err = cma_init_reserved_mem(rmem->base, rmem->size, 0, rmem->name, &cma);
 	if (err) {
 		pr_err("Reserved memory: unable to setup CMA region\n");
 		return err;
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index ef8401ac1141..15de262015df 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -5,6 +5,37 @@
 
 menu "Accelerometers"
 
+config ADXL345
+	tristate
+
+config ADXL345_I2C
+	tristate "Analog Devices ADXL345 3-Axis Digital Accelerometer I2C Driver"
+	depends on INPUT_ADXL34X=n
+	depends on I2C
+	select ADXL345
+	select REGMAP_I2C
+	help
+	  Say Y here if you want to build support for the Analog Devices
+	  ADXL345 3-axis digital accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called adxl345_i2c and you will also get adxl345_core
+	  for the core module.
+
+config ADXL345_SPI
+	tristate "Analog Devices ADXL345 3-Axis Digital Accelerometer SPI Driver"
+	depends on INPUT_ADXL34X=n
+	depends on SPI
+	select ADXL345
+	select REGMAP_SPI
+	help
+	  Say Y here if you want to build support for the Analog Devices
+	  ADXL345 3-axis digital accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called adxl345_spi and you will also get adxl345_core
+	  for the core module.
+
 config BMA180
 	tristate "Bosch BMA180/BMA250 3-Axis Accelerometer Driver"
 	depends on I2C
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 69fe8edc57a2..31fba1974e95 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -3,6 +3,9 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_ADXL345) += adxl345_core.o
+obj-$(CONFIG_ADXL345_I2C) += adxl345_i2c.o
+obj-$(CONFIG_ADXL345_SPI) += adxl345_spi.o
 obj-$(CONFIG_BMA180) += bma180.o
 obj-$(CONFIG_BMA220) += bma220_spi.o
 obj-$(CONFIG_BMC150_ACCEL) += bmc150-accel-core.o
diff --git a/drivers/iio/accel/adxl345.h b/drivers/iio/accel/adxl345.h
new file mode 100644
index 000000000000..c1ddf3927c47
--- /dev/null
+++ b/drivers/iio/accel/adxl345.h
@@ -0,0 +1,18 @@
+/*
+ * ADXL345 3-Axis Digital Accelerometer
+ *
+ * Copyright (c) 2017 Eva Rachel Retuya <eraretuya@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ */
+
+#ifndef _ADXL345_H_
+#define _ADXL345_H_
+
+int adxl345_core_probe(struct device *dev, struct regmap *regmap,
+		       const char *name);
+int adxl345_core_remove(struct device *dev);
+
+#endif /* _ADXL345_H_ */
diff --git a/drivers/iio/accel/adxl345_core.c b/drivers/iio/accel/adxl345_core.c
new file mode 100644
index 000000000000..9ccb5828db98
--- /dev/null
+++ b/drivers/iio/accel/adxl345_core.c
@@ -0,0 +1,179 @@
+/*
+ * ADXL345 3-Axis Digital Accelerometer IIO core driver
+ *
+ * Copyright (c) 2017 Eva Rachel Retuya <eraretuya@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+#include <linux/iio/iio.h>
+
+#include "adxl345.h"
+
+#define ADXL345_REG_DEVID		0x00
+#define ADXL345_REG_POWER_CTL		0x2D
+#define ADXL345_REG_DATA_FORMAT		0x31
+#define ADXL345_REG_DATAX0		0x32
+#define ADXL345_REG_DATAY0		0x34
+#define ADXL345_REG_DATAZ0		0x36
+
+#define ADXL345_POWER_CTL_MEASURE	BIT(3)
+#define ADXL345_POWER_CTL_STANDBY	0x00
+
+#define ADXL345_DATA_FORMAT_FULL_RES	BIT(3) /* Up to 13-bits resolution */
+#define ADXL345_DATA_FORMAT_2G		0
+#define ADXL345_DATA_FORMAT_4G		1
+#define ADXL345_DATA_FORMAT_8G		2
+#define ADXL345_DATA_FORMAT_16G		3
+
+#define ADXL345_DEVID			0xE5
+
+/*
+ * In full-resolution mode, scale factor is maintained at ~4 mg/LSB
+ * in all g ranges.
+ *
+ * At +/- 16g with 13-bit resolution, scale is computed as:
+ * (16 + 16) * 9.81 / (2^13 - 1) = 0.0383
+ */
+static const int adxl345_uscale = 38300;
+
+struct adxl345_data {
+	struct regmap *regmap;
+	u8 data_range;
+};
+
+#define ADXL345_CHANNEL(reg, axis) {					\
+	.type = IIO_ACCEL,						\
+	.modified = 1,							\
+	.channel2 = IIO_MOD_##axis,					\
+	.address = reg,							\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),		\
+}
+
+static const struct iio_chan_spec adxl345_channels[] = {
+	ADXL345_CHANNEL(ADXL345_REG_DATAX0, X),
+	ADXL345_CHANNEL(ADXL345_REG_DATAY0, Y),
+	ADXL345_CHANNEL(ADXL345_REG_DATAZ0, Z),
+};
+
+static int adxl345_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct adxl345_data *data = iio_priv(indio_dev);
+	__le16 regval;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * Data is stored in adjacent registers:
+		 * ADXL345_REG_DATA(X0/Y0/Z0) contain the least significant byte
+		 * and ADXL345_REG_DATA(X0/Y0/Z0) + 1 the most significant byte
+		 */
+		ret = regmap_bulk_read(data->regmap, chan->address, &regval,
+				       sizeof(regval));
+		if (ret < 0)
+			return ret;
+
+		*val = sign_extend32(le16_to_cpu(regval), 12);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = adxl345_uscale;
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info adxl345_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= adxl345_read_raw,
+};
+
+int adxl345_core_probe(struct device *dev, struct regmap *regmap,
+		       const char *name)
+{
+	struct adxl345_data *data;
+	struct iio_dev *indio_dev;
+	u32 regval;
+	int ret;
+
+	ret = regmap_read(regmap, ADXL345_REG_DEVID, &regval);
+	if (ret < 0) {
+		dev_err(dev, "Error reading device ID: %d\n", ret);
+		return ret;
+	}
+
+	if (regval != ADXL345_DEVID) {
+		dev_err(dev, "Invalid device ID: %x, expected %x\n",
+			regval, ADXL345_DEVID);
+		return -ENODEV;
+	}
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	dev_set_drvdata(dev, indio_dev);
+	data->regmap = regmap;
+	/* Enable full-resolution mode */
+	data->data_range = ADXL345_DATA_FORMAT_FULL_RES;
+
+	ret = regmap_write(data->regmap, ADXL345_REG_DATA_FORMAT,
+			   data->data_range);
+	if (ret < 0) {
+		dev_err(dev, "Failed to set data range: %d\n", ret);
+		return ret;
+	}
+
+	indio_dev->dev.parent = dev;
+	indio_dev->name = name;
+	indio_dev->info = &adxl345_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = adxl345_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adxl345_channels);
+
+	/* Enable measurement mode */
+	ret = regmap_write(data->regmap, ADXL345_REG_POWER_CTL,
+			   ADXL345_POWER_CTL_MEASURE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable measurement mode: %d\n", ret);
+		return ret;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(dev, "iio_device_register failed: %d\n", ret);
+		regmap_write(data->regmap, ADXL345_REG_POWER_CTL,
+			     ADXL345_POWER_CTL_STANDBY);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(adxl345_core_probe);
+
+int adxl345_core_remove(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct adxl345_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	return regmap_write(data->regmap, ADXL345_REG_POWER_CTL,
+			    ADXL345_POWER_CTL_STANDBY);
+}
+EXPORT_SYMBOL_GPL(adxl345_core_remove);
+
+MODULE_AUTHOR("Eva Rachel Retuya <eraretuya@gmail.com>");
+MODULE_DESCRIPTION("ADXL345 3-Axis Digital Accelerometer core driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/adxl345_i2c.c b/drivers/iio/accel/adxl345_i2c.c
new file mode 100644
index 000000000000..05e1ec49700c
--- /dev/null
+++ b/drivers/iio/accel/adxl345_i2c.c
@@ -0,0 +1,73 @@
+/*
+ * ADXL345 3-Axis Digital Accelerometer I2C driver
+ *
+ * Copyright (c) 2017 Eva Rachel Retuya <eraretuya@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * 7-bit I2C slave address: 0x1D (ALT ADDRESS pin tied to VDDIO) or
+ * 0x53 (ALT ADDRESS pin grounded)
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+#include "adxl345.h"
+
+static const struct regmap_config adxl345_i2c_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static int adxl345_i2c_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct regmap *regmap;
+
+	regmap = devm_regmap_init_i2c(client, &adxl345_i2c_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "Error initializing i2c regmap: %ld\n",
+			PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	return adxl345_core_probe(&client->dev, regmap, id ? id->name : NULL);
+}
+
+static int adxl345_i2c_remove(struct i2c_client *client)
+{
+	return adxl345_core_remove(&client->dev);
+}
+
+static const struct i2c_device_id adxl345_i2c_id[] = {
+	{ "adxl345", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, adxl345_i2c_id);
+
+static const struct of_device_id adxl345_of_match[] = {
+	{ .compatible = "adi,adxl345" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, adxl345_of_match);
+
+static struct i2c_driver adxl345_i2c_driver = {
+	.driver = {
+		.name	= "adxl345_i2c",
+		.of_match_table = adxl345_of_match,
+	},
+	.probe		= adxl345_i2c_probe,
+	.remove		= adxl345_i2c_remove,
+	.id_table	= adxl345_i2c_id,
+};
+
+module_i2c_driver(adxl345_i2c_driver);
+
+MODULE_AUTHOR("Eva Rachel Retuya <eraretuya@gmail.com>");
+MODULE_DESCRIPTION("ADXL345 3-Axis Digital Accelerometer I2C driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/adxl345_spi.c b/drivers/iio/accel/adxl345_spi.c
new file mode 100644
index 000000000000..6d658196f81c
--- /dev/null
+++ b/drivers/iio/accel/adxl345_spi.c
@@ -0,0 +1,81 @@
+/*
+ * ADXL345 3-Axis Digital Accelerometer SPI driver
+ *
+ * Copyright (c) 2017 Eva Rachel Retuya <eraretuya@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License. See the file COPYING in the main
+ * directory of this archive for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/spi/spi.h>
+
+#include "adxl345.h"
+
+#define ADXL345_MAX_SPI_FREQ_HZ		5000000
+
+static const struct regmap_config adxl345_spi_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	 /* Setting bits 7 and 6 enables multiple-byte read */
+	.read_flag_mask = BIT(7) | BIT(6),
+};
+
+static int adxl345_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	struct regmap *regmap;
+
+	/* Bail out if max_speed_hz exceeds 5 MHz */
+	if (spi->max_speed_hz > ADXL345_MAX_SPI_FREQ_HZ) {
+		dev_err(&spi->dev, "SPI CLK, %d Hz exceeds 5 MHz\n",
+			spi->max_speed_hz);
+		return -EINVAL;
+	}
+
+	regmap = devm_regmap_init_spi(spi, &adxl345_spi_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&spi->dev, "Error initializing spi regmap: %ld\n",
+			PTR_ERR(regmap));
+		return PTR_ERR(regmap);
+	}
+
+	return adxl345_core_probe(&spi->dev, regmap, id->name);
+}
+
+static int adxl345_spi_remove(struct spi_device *spi)
+{
+	return adxl345_core_remove(&spi->dev);
+}
+
+static const struct spi_device_id adxl345_spi_id[] = {
+	{ "adxl345", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(spi, adxl345_spi_id);
+
+static const struct of_device_id adxl345_of_match[] = {
+	{ .compatible = "adi,adxl345" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, adxl345_of_match);
+
+static struct spi_driver adxl345_spi_driver = {
+	.driver = {
+		.name	= "adxl345_spi",
+		.of_match_table = adxl345_of_match,
+	},
+	.probe		= adxl345_spi_probe,
+	.remove		= adxl345_spi_remove,
+	.id_table	= adxl345_spi_id,
+};
+
+module_spi_driver(adxl345_spi_driver);
+
+MODULE_AUTHOR("Eva Rachel Retuya <eraretuya@gmail.com>");
+MODULE_DESCRIPTION("ADXL345 3-Axis Digital Accelerometer SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/bma180.c b/drivers/iio/accel/bma180.c
index 0890934ef66f..efc67739c28f 100644
--- a/drivers/iio/accel/bma180.c
+++ b/drivers/iio/accel/bma180.c
@@ -18,6 +18,7 @@
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
+#include <linux/of_device.h>
 #include <linux/of.h>
 #include <linux/bitops.h>
 #include <linux/slab.h>
@@ -32,7 +33,7 @@
 #define BMA180_DRV_NAME "bma180"
 #define BMA180_IRQ_NAME "bma180_event"
 
-enum {
+enum chip_ids {
 	BMA180,
 	BMA250,
 };
@@ -41,11 +42,11 @@ struct bma180_data;
 
 struct bma180_part_info {
 	const struct iio_chan_spec *channels;
-	unsigned num_channels;
+	unsigned int num_channels;
 	const int *scale_table;
-	unsigned num_scales;
+	unsigned int num_scales;
 	const int *bw_table;
-	unsigned num_bw;
+	unsigned int num_bw;
 
 	u8 int_reset_reg, int_reset_mask;
 	u8 sleep_reg, sleep_mask;
@@ -408,7 +409,7 @@ err:
 	dev_err(&data->client->dev, "failed to disable the chip\n");
 }
 
-static ssize_t bma180_show_avail(char *buf, const int *vals, unsigned n,
+static ssize_t bma180_show_avail(char *buf, const int *vals, unsigned int n,
 				 bool micros)
 {
 	size_t len = 0;
@@ -707,6 +708,7 @@ static int bma180_probe(struct i2c_client *client,
 {
 	struct bma180_data *data;
 	struct iio_dev *indio_dev;
+	enum chip_ids chip;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
@@ -716,7 +718,11 @@ static int bma180_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
-	data->part_info = &bma180_part_info[id->driver_data];
+	if (client->dev.of_node)
+		chip = (enum chip_ids)of_device_get_match_data(&client->dev);
+	else
+		chip = id->driver_data;
+	data->part_info = &bma180_part_info[chip];
 
 	ret = data->part_info->chip_config(data);
 	if (ret < 0)
@@ -844,10 +850,24 @@ static struct i2c_device_id bma180_ids[] = {
 
 MODULE_DEVICE_TABLE(i2c, bma180_ids);
 
+static const struct of_device_id bma180_of_match[] = {
+	{
+		.compatible = "bosch,bma180",
+		.data = (void *)BMA180
+	},
+	{
+		.compatible = "bosch,bma250",
+		.data = (void *)BMA250
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, bma180_of_match);
+
 static struct i2c_driver bma180_driver = {
 	.driver = {
 		.name	= "bma180",
 		.pm	= BMA180_PM_OPS,
+		.of_match_table = bma180_of_match,
 	},
 	.probe		= bma180_probe,
 	.remove		= bma180_remove,
diff --git a/drivers/iio/accel/mma7455_i2c.c b/drivers/iio/accel/mma7455_i2c.c
index 3cab5fb4a3c4..73bf81a8ab14 100644
--- a/drivers/iio/accel/mma7455_i2c.c
+++ b/drivers/iio/accel/mma7455_i2c.c
@@ -41,12 +41,20 @@ static const struct i2c_device_id mma7455_i2c_ids[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mma7455_i2c_ids);
 
+static const struct of_device_id mma7455_of_match[] = {
+	{ .compatible = "fsl,mma7455" },
+	{ .compatible = "fsl,mma7456" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mma7455_of_match);
+
 static struct i2c_driver mma7455_i2c_driver = {
 	.probe = mma7455_i2c_probe,
 	.remove = mma7455_i2c_remove,
 	.id_table = mma7455_i2c_ids,
 	.driver = {
 		.name	= "mma7455-i2c",
+		.of_match_table = mma7455_of_match,
 	},
 };
 module_i2c_driver(mma7455_i2c_driver);
diff --git a/drivers/iio/accel/mma7660.c b/drivers/iio/accel/mma7660.c
index 3a40774cca74..42fa57e41bdd 100644
--- a/drivers/iio/accel/mma7660.c
+++ b/drivers/iio/accel/mma7660.c
@@ -253,6 +253,12 @@ static const struct i2c_device_id mma7660_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mma7660_i2c_id);
 
+static const struct of_device_id mma7660_of_match[] = {
+	{ .compatible = "fsl,mma7660" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mma7660_of_match);
+
 static const struct acpi_device_id mma7660_acpi_id[] = {
 	{"MMA7660", 0},
 	{}
@@ -264,6 +270,7 @@ static struct i2c_driver mma7660_driver = {
 	.driver = {
 		.name = "mma7660",
 		.pm = MMA7660_PM_OPS,
+		.of_match_table = mma7660_of_match,
 		.acpi_match_table = ACPI_PTR(mma7660_acpi_id),
 	},
 	.probe		= mma7660_probe,
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index ff5ad3be55b4..401f47b51d83 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -130,6 +130,17 @@ config AD799X
 	  To compile this driver as a module, choose M here: the module will be
 	  called ad799x.
 
+config ASPEED_ADC
+	tristate "Aspeed ADC"
+	depends on ARCH_ASPEED || COMPILE_TEST
+	depends on COMMON_CLK
+	help
+	  If you say yes here you get support for the ADC included in Aspeed
+	  BMC SoCs.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called aspeed_adc.
+
 config AT91_ADC
 	tristate "Atmel AT91 ADC"
 	depends on ARCH_AT91
@@ -205,6 +216,17 @@ config CC10001_ADC
 	  This driver can also be built as a module. If so, the module will be
 	  called cc10001_adc.
 
+config CPCAP_ADC
+	tristate "Motorola CPCAP PMIC ADC driver"
+	depends on MFD_CPCAP
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Motorola CPCAP PMIC ADC.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called cpcap-adc.
+
 config DA9150_GPADC
 	tristate "Dialog DA9150 GPADC driver support"
 	depends on MFD_DA9150
@@ -328,6 +350,18 @@ config LPC18XX_ADC
 	  To compile this driver as a module, choose M here: the module will be
 	  called lpc18xx_adc.
 
+config LPC32XX_ADC
+	tristate "NXP LPC32XX ADC"
+	depends on ARCH_LPC32XX || COMPILE_TEST
+	depends on HAS_IOMEM
+	help
+	  Say yes here to build support for the integrated ADC inside the
+	  LPC32XX SoC. Note that this feature uses the same hardware as the
+	  touchscreen driver, so you should either select only one of the two
+	  drivers (lpc32xx_adc or lpc32xx_ts) or, in the OpenFirmware case,
+	  activate only one via device tree selection.  Provides direct access
+	  via sysfs.
+
 config LTC2485
 	tristate "Linear Technology LTC2485 ADC driver"
 	depends on I2C
@@ -337,6 +371,16 @@ config LTC2485
 	  To compile this driver as a module, choose M here: the module will be
 	  called ltc2485.
 
+config LTC2497
+	tristate "Linear Technology LTC2497 ADC driver"
+	depends on I2C
+	help
+	  Say yes here to build support for Linear Technology LTC2497
+	  16-Bit 8-/16-Channel Delta Sigma ADC.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ltc2497.
+
 config MAX1027
 	tristate "Maxim max1027 ADC driver"
 	depends on SPI
@@ -358,6 +402,18 @@ config MAX11100
 	  To compile this driver as a module, choose M here: the module will be
 	  called max11100.
 
+config MAX1118
+	tristate "Maxim max1117/max1118/max1119 ADCs driver"
+	depends on SPI
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for Maxim max1117/max1118/max1119
+	  8-bit, dual-channel ADCs.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called max1118.
+
 config MAX1363
 	tristate "Maxim max1363 ADC driver"
 	depends on I2C
@@ -377,6 +433,16 @@ config MAX1363
 	  To compile this driver as a module, choose M here: the module will be
 	  called max1363.
 
+config	MAX9611
+	tristate "Maxim max9611/max9612 ADC driver"
+	depends on I2C
+	help
+	  Say yes here to build support for Maxim max9611/max9612 current sense
+	  amplifier with 12-bits ADC interface.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called max9611.
+
 config MCP320X
 	tristate "Microchip Technology MCP3x01/02/04/08"
 	depends on SPI
@@ -451,6 +517,20 @@ config PALMAS_GPADC
 	  is used in smartphones and tablets and supports a 16 channel
 	  general purpose ADC.
 
+config QCOM_VADC_COMMON
+	tristate
+
+config QCOM_PM8XXX_XOADC
+	tristate "Qualcomm SSBI PM8xxx PMIC XOADCs"
+	depends on MFD_PM8XXX
+	select QCOM_VADC_COMMON
+	help
+	  ADC driver for the XOADC portions of the Qualcomm PM8xxx PMICs
+	  using SSBI transport: PM8018, PM8038, PM8058, PM8921.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called qcom-pm8xxx-xoadc.
+
 config QCOM_SPMI_IADC
 	tristate "Qualcomm SPMI PMIC current ADC"
 	depends on SPMI
@@ -469,6 +549,7 @@ config QCOM_SPMI_VADC
 	tristate "Qualcomm SPMI PMIC voltage ADC"
 	depends on SPMI
 	select REGMAP_SPMI
+	select QCOM_VADC_COMMON
 	help
 	  This is the IIO Voltage ADC driver for Qualcomm QPNP VADC Chip.
 
@@ -503,6 +584,17 @@ config ROCKCHIP_SARADC
 	  To compile this driver as a module, choose M here: the
 	  module will be called rockchip_saradc.
 
+config SPEAR_ADC
+	tristate "ST SPEAr ADC"
+	depends on PLAT_SPEAR || COMPILE_TEST
+	depends on HAS_IOMEM
+	help
+	  Say yes here to build support for the integrated ADC inside the
+	  ST SPEAr SoC. Provides direct access via sysfs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called spear_adc.
+
 config STM32_ADC_CORE
 	tristate "STMicroelectronics STM32 adc core"
 	depends on ARCH_STM32 || COMPILE_TEST
@@ -532,7 +624,7 @@ config STM32_ADC
 
 config STX104
 	tristate "Apex Embedded Systems STX104 driver"
-	depends on X86 && ISA_BUS_API
+	depends on PC104 && X86 && ISA_BUS_API
 	select GPIOLIB
 	help
 	  Say yes here to build support for the Apex Embedded Systems STX104
@@ -545,6 +637,24 @@ config STX104
 	  The base port addresses for the devices may be configured via the base
 	  array module parameter.
 
+config SUN4I_GPADC
+	tristate "Support for the Allwinner SoCs GPADC"
+	depends on IIO
+	depends on MFD_SUN4I_GPADC || MACH_SUN8I
+	depends on THERMAL || !THERMAL_OF
+	help
+	  Say yes here to build support for Allwinner (A10, A13 and A31) SoCs
+	  GPADC. This ADC provides 4 channels which can be used as an ADC or as
+	  a touchscreen input and one channel for thermal sensor.
+
+	  The thermal sensor slows down ADC readings and can be disabled by
+	  disabling CONFIG_THERMAL_OF. However, the thermal sensor should be
+	  enabled by default since the SoC temperature is usually more critical
+	  than ADC readings.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called sun4i-gpadc-iio.
+
 config TI_ADC081C
 	tristate "Texas Instruments ADC081C/ADC101C/ADC121C family"
 	depends on I2C
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index a01de757f42c..9339bec4babe 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_AD7791) += ad7791.o
 obj-$(CONFIG_AD7793) += ad7793.o
 obj-$(CONFIG_AD7887) += ad7887.o
 obj-$(CONFIG_AD799X) += ad799x.o
+obj-$(CONFIG_ASPEED_ADC) += aspeed_adc.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_AT91_SAMA5D2_ADC) += at91-sama5d2_adc.o
 obj-$(CONFIG_AXP20X_ADC) += axp20x_adc.o
@@ -21,6 +22,7 @@ obj-$(CONFIG_AXP288_ADC) += axp288_adc.o
 obj-$(CONFIG_BCM_IPROC_ADC) += bcm_iproc_adc.o
 obj-$(CONFIG_BERLIN2_ADC) += berlin2-adc.o
 obj-$(CONFIG_CC10001_ADC) += cc10001_adc.o
+obj-$(CONFIG_CPCAP_ADC) += cpcap-adc.o
 obj-$(CONFIG_DA9150_GPADC) += da9150-gpadc.o
 obj-$(CONFIG_ENVELOPE_DETECTOR) += envelope-detector.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
@@ -31,10 +33,14 @@ obj-$(CONFIG_IMX7D_ADC) += imx7d_adc.o
 obj-$(CONFIG_INA2XX_ADC) += ina2xx-adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
 obj-$(CONFIG_LPC18XX_ADC) += lpc18xx_adc.o
+obj-$(CONFIG_LPC32XX_ADC) += lpc32xx_adc.o
 obj-$(CONFIG_LTC2485) += ltc2485.o
+obj-$(CONFIG_LTC2497) += ltc2497.o
 obj-$(CONFIG_MAX1027) += max1027.o
 obj-$(CONFIG_MAX11100) += max11100.o
+obj-$(CONFIG_MAX1118) += max1118.o
 obj-$(CONFIG_MAX1363) += max1363.o
+obj-$(CONFIG_MAX9611) += max9611.o
 obj-$(CONFIG_MCP320X) += mcp320x.o
 obj-$(CONFIG_MCP3422) += mcp3422.o
 obj-$(CONFIG_MEDIATEK_MT6577_AUXADC) += mt6577_auxadc.o
@@ -44,10 +50,14 @@ obj-$(CONFIG_MXS_LRADC_ADC) += mxs-lradc-adc.o
 obj-$(CONFIG_NAU7802) += nau7802.o
 obj-$(CONFIG_PALMAS_GPADC) += palmas_gpadc.o
 obj-$(CONFIG_QCOM_SPMI_IADC) += qcom-spmi-iadc.o
+obj-$(CONFIG_QCOM_VADC_COMMON) += qcom-vadc-common.o
 obj-$(CONFIG_QCOM_SPMI_VADC) += qcom-spmi-vadc.o
+obj-$(CONFIG_QCOM_PM8XXX_XOADC) += qcom-pm8xxx-xoadc.o
 obj-$(CONFIG_RCAR_GYRO_ADC) += rcar-gyroadc.o
 obj-$(CONFIG_ROCKCHIP_SARADC) += rockchip_saradc.o
+obj-$(CONFIG_SPEAR_ADC) += spear_adc.o
 obj-$(CONFIG_STX104) += stx104.o
+obj-$(CONFIG_SUN4I_GPADC) += sun4i-gpadc-iio.o
 obj-$(CONFIG_STM32_ADC_CORE) += stm32-adc-core.o
 obj-$(CONFIG_STM32_ADC) += stm32-adc.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
diff --git a/drivers/iio/adc/ad799x.c b/drivers/iio/adc/ad799x.c
index 9704090b7908..22426ae4af97 100644
--- a/drivers/iio/adc/ad799x.c
+++ b/drivers/iio/adc/ad799x.c
@@ -520,7 +520,7 @@ static struct attribute *ad799x_event_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group ad799x_event_attrs_group = {
+static const struct attribute_group ad799x_event_attrs_group = {
 	.attrs = ad799x_event_attributes,
 };
 
diff --git a/drivers/iio/adc/aspeed_adc.c b/drivers/iio/adc/aspeed_adc.c
new file mode 100644
index 000000000000..62670cbfa2bb
--- /dev/null
+++ b/drivers/iio/adc/aspeed_adc.c
@@ -0,0 +1,295 @@
+/*
+ * Aspeed AST2400/2500 ADC
+ *
+ * Copyright (C) 2017 Google, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/driver.h>
+
+#define ASPEED_RESOLUTION_BITS		10
+#define ASPEED_CLOCKS_PER_SAMPLE	12
+
+#define ASPEED_REG_ENGINE_CONTROL	0x00
+#define ASPEED_REG_INTERRUPT_CONTROL	0x04
+#define ASPEED_REG_VGA_DETECT_CONTROL	0x08
+#define ASPEED_REG_CLOCK_CONTROL	0x0C
+#define ASPEED_REG_MAX			0xC0
+
+#define ASPEED_OPERATION_MODE_POWER_DOWN	(0x0 << 1)
+#define ASPEED_OPERATION_MODE_STANDBY		(0x1 << 1)
+#define ASPEED_OPERATION_MODE_NORMAL		(0x7 << 1)
+
+#define ASPEED_ENGINE_ENABLE		BIT(0)
+
+struct aspeed_adc_model_data {
+	const char *model_name;
+	unsigned int min_sampling_rate;	// Hz
+	unsigned int max_sampling_rate;	// Hz
+	unsigned int vref_voltage;	// mV
+};
+
+struct aspeed_adc_data {
+	struct device	*dev;
+	void __iomem	*base;
+	spinlock_t	clk_lock;
+	struct clk_hw	*clk_prescaler;
+	struct clk_hw	*clk_scaler;
+};
+
+#define ASPEED_CHAN(_idx, _data_reg_addr) {			\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = (_idx),					\
+	.address = (_data_reg_addr),				\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+}
+
+static const struct iio_chan_spec aspeed_adc_iio_channels[] = {
+	ASPEED_CHAN(0, 0x10),
+	ASPEED_CHAN(1, 0x12),
+	ASPEED_CHAN(2, 0x14),
+	ASPEED_CHAN(3, 0x16),
+	ASPEED_CHAN(4, 0x18),
+	ASPEED_CHAN(5, 0x1A),
+	ASPEED_CHAN(6, 0x1C),
+	ASPEED_CHAN(7, 0x1E),
+	ASPEED_CHAN(8, 0x20),
+	ASPEED_CHAN(9, 0x22),
+	ASPEED_CHAN(10, 0x24),
+	ASPEED_CHAN(11, 0x26),
+	ASPEED_CHAN(12, 0x28),
+	ASPEED_CHAN(13, 0x2A),
+	ASPEED_CHAN(14, 0x2C),
+	ASPEED_CHAN(15, 0x2E),
+};
+
+static int aspeed_adc_read_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int *val, int *val2, long mask)
+{
+	struct aspeed_adc_data *data = iio_priv(indio_dev);
+	const struct aspeed_adc_model_data *model_data =
+			of_device_get_match_data(data->dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		*val = readw(data->base + chan->address);
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = model_data->vref_voltage;
+		*val2 = ASPEED_RESOLUTION_BITS;
+		return IIO_VAL_FRACTIONAL_LOG2;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = clk_get_rate(data->clk_scaler->clk) /
+				ASPEED_CLOCKS_PER_SAMPLE;
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int aspeed_adc_write_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int val, int val2, long mask)
+{
+	struct aspeed_adc_data *data = iio_priv(indio_dev);
+	const struct aspeed_adc_model_data *model_data =
+			of_device_get_match_data(data->dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		if (val < model_data->min_sampling_rate ||
+			val > model_data->max_sampling_rate)
+			return -EINVAL;
+
+		clk_set_rate(data->clk_scaler->clk,
+				val * ASPEED_CLOCKS_PER_SAMPLE);
+		return 0;
+
+	case IIO_CHAN_INFO_SCALE:
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * Technically, these could be written but the only reasons
+		 * for doing so seem better handled in userspace.  EPERM is
+		 * returned to signal this is a policy choice rather than a
+		 * hardware limitation.
+		 */
+		return -EPERM;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int aspeed_adc_reg_access(struct iio_dev *indio_dev,
+				 unsigned int reg, unsigned int writeval,
+				 unsigned int *readval)
+{
+	struct aspeed_adc_data *data = iio_priv(indio_dev);
+
+	if (!readval || reg % 4 || reg > ASPEED_REG_MAX)
+		return -EINVAL;
+
+	*readval = readl(data->base + reg);
+
+	return 0;
+}
+
+static const struct iio_info aspeed_adc_iio_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = aspeed_adc_read_raw,
+	.write_raw = aspeed_adc_write_raw,
+	.debugfs_reg_access = aspeed_adc_reg_access,
+};
+
+static int aspeed_adc_probe(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev;
+	struct aspeed_adc_data *data;
+	const struct aspeed_adc_model_data *model_data;
+	struct resource *res;
+	const char *clk_parent_name;
+	int ret;
+	u32 adc_engine_control_reg_val;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->base))
+		return PTR_ERR(data->base);
+
+	/* Register ADC clock prescaler with source specified by device tree. */
+	spin_lock_init(&data->clk_lock);
+	clk_parent_name = of_clk_get_parent_name(pdev->dev.of_node, 0);
+
+	data->clk_prescaler = clk_hw_register_divider(
+				&pdev->dev, "prescaler", clk_parent_name, 0,
+				data->base + ASPEED_REG_CLOCK_CONTROL,
+				17, 15, 0, &data->clk_lock);
+	if (IS_ERR(data->clk_prescaler))
+		return PTR_ERR(data->clk_prescaler);
+
+	/*
+	 * Register ADC clock scaler downstream from the prescaler. Allow rate
+	 * setting to adjust the prescaler as well.
+	 */
+	data->clk_scaler = clk_hw_register_divider(
+				&pdev->dev, "scaler", "prescaler",
+				CLK_SET_RATE_PARENT,
+				data->base + ASPEED_REG_CLOCK_CONTROL,
+				0, 10, 0, &data->clk_lock);
+	if (IS_ERR(data->clk_scaler)) {
+		ret = PTR_ERR(data->clk_scaler);
+		goto scaler_error;
+	}
+
+	/* Start all channels in normal mode. */
+	clk_prepare_enable(data->clk_scaler->clk);
+	adc_engine_control_reg_val = GENMASK(31, 16) |
+		ASPEED_OPERATION_MODE_NORMAL | ASPEED_ENGINE_ENABLE;
+	writel(adc_engine_control_reg_val,
+		data->base + ASPEED_REG_ENGINE_CONTROL);
+
+	model_data = of_device_get_match_data(&pdev->dev);
+	indio_dev->name = model_data->model_name;
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &aspeed_adc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = aspeed_adc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(aspeed_adc_iio_channels);
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto iio_register_error;
+
+	return 0;
+
+iio_register_error:
+	writel(ASPEED_OPERATION_MODE_POWER_DOWN,
+		data->base + ASPEED_REG_ENGINE_CONTROL);
+	clk_disable_unprepare(data->clk_scaler->clk);
+	clk_hw_unregister_divider(data->clk_scaler);
+
+scaler_error:
+	clk_hw_unregister_divider(data->clk_prescaler);
+	return ret;
+}
+
+static int aspeed_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct aspeed_adc_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	writel(ASPEED_OPERATION_MODE_POWER_DOWN,
+		data->base + ASPEED_REG_ENGINE_CONTROL);
+	clk_disable_unprepare(data->clk_scaler->clk);
+	clk_hw_unregister_divider(data->clk_scaler);
+	clk_hw_unregister_divider(data->clk_prescaler);
+
+	return 0;
+}
+
+static const struct aspeed_adc_model_data ast2400_model_data = {
+	.model_name = "ast2400-adc",
+	.vref_voltage = 2500, // mV
+	.min_sampling_rate = 10000,
+	.max_sampling_rate = 500000,
+};
+
+static const struct aspeed_adc_model_data ast2500_model_data = {
+	.model_name = "ast2500-adc",
+	.vref_voltage = 1800, // mV
+	.min_sampling_rate = 1,
+	.max_sampling_rate = 1000000,
+};
+
+static const struct of_device_id aspeed_adc_matches[] = {
+	{ .compatible = "aspeed,ast2400-adc", .data = &ast2400_model_data },
+	{ .compatible = "aspeed,ast2500-adc", .data = &ast2500_model_data },
+	{},
+};
+MODULE_DEVICE_TABLE(of, aspeed_adc_matches);
+
+static struct platform_driver aspeed_adc_driver = {
+	.probe = aspeed_adc_probe,
+	.remove = aspeed_adc_remove,
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.of_match_table = aspeed_adc_matches,
+	}
+};
+
+module_platform_driver(aspeed_adc_driver);
+
+MODULE_AUTHOR("Rick Altherr <raltherr@google.com>");
+MODULE_DESCRIPTION("Aspeed AST2400/2500 ADC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/adc/cpcap-adc.c b/drivers/iio/adc/cpcap-adc.c
new file mode 100644
index 000000000000..62d37f8725b8
--- /dev/null
+++ b/drivers/iio/adc/cpcap-adc.c
@@ -0,0 +1,1007 @@
+/*
+ * Copyright (C) 2017 Tony Lindgren <tony@atomide.com>
+ *
+ * Rewritten for Linux IIO framework with some code based on
+ * earlier driver found in the Motorola Linux kernel:
+ *
+ * Copyright (C) 2009-2010 Motorola, Inc.
+ *
+ * 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/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+
+#include <linux/iio/buffer.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/mfd/motorola-cpcap.h>
+
+/* Register CPCAP_REG_ADCC1 bits */
+#define CPCAP_BIT_ADEN_AUTO_CLR		BIT(15)	/* Currently unused */
+#define CPCAP_BIT_CAL_MODE		BIT(14) /* Set with BIT_RAND0 */
+#define CPCAP_BIT_ADC_CLK_SEL1		BIT(13)	/* Currently unused */
+#define CPCAP_BIT_ADC_CLK_SEL0		BIT(12)	/* Currently unused */
+#define CPCAP_BIT_ATOX			BIT(11)
+#define CPCAP_BIT_ATO3			BIT(10)
+#define CPCAP_BIT_ATO2			BIT(9)
+#define CPCAP_BIT_ATO1			BIT(8)
+#define CPCAP_BIT_ATO0			BIT(7)
+#define CPCAP_BIT_ADA2			BIT(6)
+#define CPCAP_BIT_ADA1			BIT(5)
+#define CPCAP_BIT_ADA0			BIT(4)
+#define CPCAP_BIT_AD_SEL1		BIT(3)	/* Set for bank1 */
+#define CPCAP_BIT_RAND1			BIT(2)	/* Set for channel 16 & 17 */
+#define CPCAP_BIT_RAND0			BIT(1)	/* Set with CAL_MODE */
+#define CPCAP_BIT_ADEN			BIT(0)	/* Currently unused */
+
+/* Register CPCAP_REG_ADCC2 bits */
+#define CPCAP_BIT_CAL_FACTOR_ENABLE	BIT(15)	/* Currently unused */
+#define CPCAP_BIT_BATDETB_EN		BIT(14)	/* Currently unused */
+#define CPCAP_BIT_ADTRIG_ONESHOT	BIT(13)	/* Set for !TIMING_IMM */
+#define CPCAP_BIT_ASC			BIT(12)	/* Set for TIMING_IMM */
+#define CPCAP_BIT_ATOX_PS_FACTOR	BIT(11)
+#define CPCAP_BIT_ADC_PS_FACTOR1	BIT(10)
+#define CPCAP_BIT_ADC_PS_FACTOR0	BIT(9)
+#define CPCAP_BIT_AD4_SELECT		BIT(8)	/* Currently unused */
+#define CPCAP_BIT_ADC_BUSY		BIT(7)	/* Currently unused */
+#define CPCAP_BIT_THERMBIAS_EN		BIT(6)	/* Currently unused */
+#define CPCAP_BIT_ADTRIG_DIS		BIT(5)	/* Disable interrupt */
+#define CPCAP_BIT_LIADC			BIT(4)	/* Currently unused */
+#define CPCAP_BIT_TS_REFEN		BIT(3)	/* Currently unused */
+#define CPCAP_BIT_TS_M2			BIT(2)	/* Currently unused */
+#define CPCAP_BIT_TS_M1			BIT(1)	/* Currently unused */
+#define CPCAP_BIT_TS_M0			BIT(0)	/* Currently unused */
+
+#define CPCAP_MAX_TEMP_LVL		27
+#define CPCAP_FOUR_POINT_TWO_ADC	801
+#define ST_ADC_CAL_CHRGI_HIGH_THRESHOLD	530
+#define ST_ADC_CAL_CHRGI_LOW_THRESHOLD	494
+#define ST_ADC_CAL_BATTI_HIGH_THRESHOLD	530
+#define ST_ADC_CAL_BATTI_LOW_THRESHOLD	494
+#define ST_ADC_CALIBRATE_DIFF_THRESHOLD	3
+
+#define CPCAP_ADC_MAX_RETRIES		5	/* Calibration and quirk */
+
+/**
+ * struct cpcap_adc_ato - timing settings for cpcap adc
+ *
+ * Unfortunately no cpcap documentation available, please document when
+ * using these.
+ */
+struct cpcap_adc_ato {
+	unsigned short ato_in;
+	unsigned short atox_in;
+	unsigned short adc_ps_factor_in;
+	unsigned short atox_ps_factor_in;
+	unsigned short ato_out;
+	unsigned short atox_out;
+	unsigned short adc_ps_factor_out;
+	unsigned short atox_ps_factor_out;
+};
+
+/**
+ * struct cpcap-adc - cpcap adc device driver data
+ * @reg: cpcap regmap
+ * @dev: struct device
+ * @vendor: cpcap vendor
+ * @irq: interrupt
+ * @lock: mutex
+ * @ato: request timings
+ * @wq_data_avail: work queue
+ * @done: work done
+ */
+struct cpcap_adc {
+	struct regmap *reg;
+	struct device *dev;
+	u16 vendor;
+	int irq;
+	struct mutex lock;	/* ADC register access lock */
+	const struct cpcap_adc_ato *ato;
+	wait_queue_head_t wq_data_avail;
+	bool done;
+};
+
+/**
+ * enum cpcap_adc_channel - cpcap adc channels
+ */
+enum cpcap_adc_channel {
+	/* Bank0 channels */
+	CPCAP_ADC_AD0_BATTDETB,	/* Battery detection */
+	CPCAP_ADC_BATTP,	/* Battery voltage */
+	CPCAP_ADC_VBUS,		/* USB VBUS voltage */
+	CPCAP_ADC_AD3,		/* Battery temperature when charging */
+	CPCAP_ADC_BPLUS_AD4,	/* Another battery or system voltage */
+	CPCAP_ADC_CHG_ISENSE,	/* Calibrated charge current */
+	CPCAP_ADC_BATTI,	/* Calibrated system current */
+	CPCAP_ADC_USB_ID,	/* USB OTG ID, unused on droid 4? */
+
+	/* Bank1 channels */
+	CPCAP_ADC_AD8,		/* Seems unused */
+	CPCAP_ADC_AD9,		/* Seems unused */
+	CPCAP_ADC_LICELL,	/* Maybe system voltage? Always 3V */
+	CPCAP_ADC_HV_BATTP,	/* Another battery detection? */
+	CPCAP_ADC_TSX1_AD12,	/* Seems unused, for touchscreen? */
+	CPCAP_ADC_TSX2_AD13,	/* Seems unused, for touchscreen? */
+	CPCAP_ADC_TSY1_AD14,	/* Seems unused, for touchscreen? */
+	CPCAP_ADC_TSY2_AD15,	/* Seems unused, for touchscreen? */
+
+	/* Remuxed channels using bank0 entries */
+	CPCAP_ADC_BATTP_PI16,	/* Alternative mux mode for BATTP */
+	CPCAP_ADC_BATTI_PI17,	/* Alternative mux mode for BATTI */
+
+	CPCAP_ADC_CHANNEL_NUM,
+};
+
+/**
+ * enum cpcap_adc_timing - cpcap adc timing options
+ *
+ * CPCAP_ADC_TIMING_IMM seems to be immediate with no timings.
+ * Please document when using.
+ */
+enum cpcap_adc_timing {
+	CPCAP_ADC_TIMING_IMM,
+	CPCAP_ADC_TIMING_IN,
+	CPCAP_ADC_TIMING_OUT,
+};
+
+/**
+ * struct cpcap_adc_phasing_tbl - cpcap phasing table
+ * @offset: offset in the phasing table
+ * @multiplier: multiplier in the phasing table
+ * @divider: divider in the phasing table
+ * @min: minimum value
+ * @max: maximum value
+ */
+struct cpcap_adc_phasing_tbl {
+	short offset;
+	unsigned short multiplier;
+	unsigned short divider;
+	short min;
+	short max;
+};
+
+/**
+ * struct cpcap_adc_conversion_tbl - cpcap conversion table
+ * @conv_type: conversion type
+ * @align_offset: align offset
+ * @conv_offset: conversion offset
+ * @cal_offset: calibration offset
+ * @multiplier: conversion multiplier
+ * @divider: conversion divider
+ */
+struct cpcap_adc_conversion_tbl {
+	enum iio_chan_info_enum conv_type;
+	int align_offset;
+	int conv_offset;
+	int cal_offset;
+	int multiplier;
+	int divider;
+};
+
+/**
+ * struct cpcap_adc_request - cpcap adc request
+ * @channel: request channel
+ * @phase_tbl: channel phasing table
+ * @conv_tbl: channel conversion table
+ * @bank_index: channel index within the bank
+ * @timing: timing settings
+ * @result: result
+ */
+struct cpcap_adc_request {
+	int channel;
+	const struct cpcap_adc_phasing_tbl *phase_tbl;
+	const struct cpcap_adc_conversion_tbl *conv_tbl;
+	int bank_index;
+	enum cpcap_adc_timing timing;
+	int result;
+};
+
+/* Phasing table for channels. Note that channels 16 & 17 use BATTP and BATTI */
+static const struct cpcap_adc_phasing_tbl bank_phasing[] = {
+	/* Bank0 */
+	[CPCAP_ADC_AD0_BATTDETB] = {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_BATTP] =        {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_VBUS] =         {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_AD3] =          {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_BPLUS_AD4] =    {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_CHG_ISENSE] =   {0, 0x80, 0x80, -512,  511},
+	[CPCAP_ADC_BATTI] =        {0, 0x80, 0x80, -512,  511},
+	[CPCAP_ADC_USB_ID] =       {0, 0x80, 0x80,    0, 1023},
+
+	/* Bank1 */
+	[CPCAP_ADC_AD8] =          {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_AD9] =          {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_LICELL] =       {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_HV_BATTP] =     {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_TSX1_AD12] =    {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_TSX2_AD13] =    {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_TSY1_AD14] =    {0, 0x80, 0x80,    0, 1023},
+	[CPCAP_ADC_TSY2_AD15] =    {0, 0x80, 0x80,    0, 1023},
+};
+
+/*
+ * Conversion table for channels. Updated during init based on calibration.
+ * Here too channels 16 & 17 use BATTP and BATTI.
+ */
+static struct cpcap_adc_conversion_tbl bank_conversion[] = {
+	/* Bank0 */
+	[CPCAP_ADC_AD0_BATTDETB] = {
+		IIO_CHAN_INFO_PROCESSED,    0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_BATTP] = {
+		IIO_CHAN_INFO_PROCESSED,    0, 2400, 0,  2300, 1023,
+	},
+	[CPCAP_ADC_VBUS] = {
+		IIO_CHAN_INFO_PROCESSED,    0,    0, 0, 10000, 1023,
+	},
+	[CPCAP_ADC_AD3] = {
+		IIO_CHAN_INFO_PROCESSED,    0,    0, 0,     1,    1,
+		},
+	[CPCAP_ADC_BPLUS_AD4] = {
+		IIO_CHAN_INFO_PROCESSED,    0, 2400, 0,  2300, 1023,
+	},
+	[CPCAP_ADC_CHG_ISENSE] = {
+		IIO_CHAN_INFO_PROCESSED, -512,    2, 0,  5000, 1023,
+	},
+	[CPCAP_ADC_BATTI] = {
+		IIO_CHAN_INFO_PROCESSED, -512,    2, 0,  5000, 1023,
+	},
+	[CPCAP_ADC_USB_ID] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+
+	/* Bank1 */
+	[CPCAP_ADC_AD8] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_AD9] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_LICELL] = {
+		IIO_CHAN_INFO_PROCESSED,    0,    0, 0,  3400, 1023,
+	},
+	[CPCAP_ADC_HV_BATTP] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_TSX1_AD12] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_TSX2_AD13] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_TSY1_AD14] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+	[CPCAP_ADC_TSY2_AD15] = {
+		IIO_CHAN_INFO_RAW,          0,    0, 0,     1,    1,
+	},
+};
+
+/*
+ * Temperature lookup table of register values to milliCelcius.
+ * REVISIT: Check the duplicate 0x3ff entry in a freezer
+ */
+static const int temp_map[CPCAP_MAX_TEMP_LVL][2] = {
+	{ 0x03ff, -40000 },
+	{ 0x03ff, -35000 },
+	{ 0x03ef, -30000 },
+	{ 0x03b2, -25000 },
+	{ 0x036c, -20000 },
+	{ 0x0320, -15000 },
+	{ 0x02d0, -10000 },
+	{ 0x027f, -5000 },
+	{ 0x022f, 0 },
+	{ 0x01e4, 5000 },
+	{ 0x019f, 10000 },
+	{ 0x0161, 15000 },
+	{ 0x012b, 20000 },
+	{ 0x00fc, 25000 },
+	{ 0x00d4, 30000 },
+	{ 0x00b2, 35000 },
+	{ 0x0095, 40000 },
+	{ 0x007d, 45000 },
+	{ 0x0069, 50000 },
+	{ 0x0059, 55000 },
+	{ 0x004b, 60000 },
+	{ 0x003f, 65000 },
+	{ 0x0036, 70000 },
+	{ 0x002e, 75000 },
+	{ 0x0027, 80000 },
+	{ 0x0022, 85000 },
+	{ 0x001d, 90000 },
+};
+
+#define CPCAP_CHAN(_type, _index, _address, _datasheet_name) {	\
+	.type = (_type), \
+	.address = (_address), \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_PROCESSED), \
+	.scan_index = (_index), \
+	.scan_type = { \
+		.sign = 'u', \
+		.realbits = 10, \
+		.storagebits = 16, \
+		.endianness = IIO_CPU, \
+	}, \
+	.datasheet_name = (_datasheet_name), \
+}
+
+/*
+ * The datasheet names are from Motorola mapphone Linux kernel except
+ * for the last two which might be uncalibrated charge voltage and
+ * current.
+ */
+static const struct iio_chan_spec cpcap_adc_channels[] = {
+	/* Bank0 */
+	CPCAP_CHAN(IIO_TEMP,    0, CPCAP_REG_ADCD0,  "battdetb"),
+	CPCAP_CHAN(IIO_VOLTAGE, 1, CPCAP_REG_ADCD1,  "battp"),
+	CPCAP_CHAN(IIO_VOLTAGE, 2, CPCAP_REG_ADCD2,  "vbus"),
+	CPCAP_CHAN(IIO_TEMP,    3, CPCAP_REG_ADCD3,  "ad3"),
+	CPCAP_CHAN(IIO_VOLTAGE, 4, CPCAP_REG_ADCD4,  "ad4"),
+	CPCAP_CHAN(IIO_CURRENT, 5, CPCAP_REG_ADCD5,  "chg_isense"),
+	CPCAP_CHAN(IIO_CURRENT, 6, CPCAP_REG_ADCD6,  "batti"),
+	CPCAP_CHAN(IIO_VOLTAGE, 7, CPCAP_REG_ADCD7,  "usb_id"),
+
+	/* Bank1 */
+	CPCAP_CHAN(IIO_CURRENT, 8, CPCAP_REG_ADCD0,  "ad8"),
+	CPCAP_CHAN(IIO_VOLTAGE, 9, CPCAP_REG_ADCD1,  "ad9"),
+	CPCAP_CHAN(IIO_VOLTAGE, 10, CPCAP_REG_ADCD2, "licell"),
+	CPCAP_CHAN(IIO_VOLTAGE, 11, CPCAP_REG_ADCD3, "hv_battp"),
+	CPCAP_CHAN(IIO_VOLTAGE, 12, CPCAP_REG_ADCD4, "tsx1_ad12"),
+	CPCAP_CHAN(IIO_VOLTAGE, 13, CPCAP_REG_ADCD5, "tsx2_ad13"),
+	CPCAP_CHAN(IIO_VOLTAGE, 14, CPCAP_REG_ADCD6, "tsy1_ad14"),
+	CPCAP_CHAN(IIO_VOLTAGE, 15, CPCAP_REG_ADCD7, "tsy2_ad15"),
+
+	/* There are two registers with multiplexed functionality */
+	CPCAP_CHAN(IIO_VOLTAGE, 16, CPCAP_REG_ADCD0, "chg_vsense"),
+	CPCAP_CHAN(IIO_CURRENT, 17, CPCAP_REG_ADCD1, "batti2"),
+};
+
+static irqreturn_t cpcap_adc_irq_thread(int irq, void *data)
+{
+	struct iio_dev *indio_dev = data;
+	struct cpcap_adc *ddata = iio_priv(indio_dev);
+	int error;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ADTRIG_DIS,
+				   CPCAP_BIT_ADTRIG_DIS);
+	if (error)
+		return IRQ_NONE;
+
+	ddata->done = true;
+	wake_up_interruptible(&ddata->wq_data_avail);
+
+	return IRQ_HANDLED;
+}
+
+/* ADC calibration functions */
+static void cpcap_adc_setup_calibrate(struct cpcap_adc *ddata,
+				      enum cpcap_adc_channel chan)
+{
+	unsigned int value = 0;
+	unsigned long timeout = jiffies + msecs_to_jiffies(3000);
+	int error;
+
+	if ((chan != CPCAP_ADC_CHG_ISENSE) &&
+	    (chan != CPCAP_ADC_BATTI))
+		return;
+
+	value |= CPCAP_BIT_CAL_MODE | CPCAP_BIT_RAND0;
+	value |= ((chan << 4) &
+		  (CPCAP_BIT_ADA2 | CPCAP_BIT_ADA1 | CPCAP_BIT_ADA0));
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC1,
+				   CPCAP_BIT_CAL_MODE | CPCAP_BIT_ATOX |
+				   CPCAP_BIT_ATO3 | CPCAP_BIT_ATO2 |
+				   CPCAP_BIT_ATO1 | CPCAP_BIT_ATO0 |
+				   CPCAP_BIT_ADA2 | CPCAP_BIT_ADA1 |
+				   CPCAP_BIT_ADA0 | CPCAP_BIT_AD_SEL1 |
+				   CPCAP_BIT_RAND1 | CPCAP_BIT_RAND0,
+				   value);
+	if (error)
+		return;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ATOX_PS_FACTOR |
+				   CPCAP_BIT_ADC_PS_FACTOR1 |
+				   CPCAP_BIT_ADC_PS_FACTOR0,
+				   0);
+	if (error)
+		return;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ADTRIG_DIS,
+				   CPCAP_BIT_ADTRIG_DIS);
+	if (error)
+		return;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ASC,
+				   CPCAP_BIT_ASC);
+	if (error)
+		return;
+
+	do {
+		schedule_timeout_uninterruptible(1);
+		error = regmap_read(ddata->reg, CPCAP_REG_ADCC2, &value);
+		if (error)
+			return;
+	} while ((value & CPCAP_BIT_ASC) && time_before(jiffies, timeout));
+
+	if (value & CPCAP_BIT_ASC)
+		dev_err(ddata->dev,
+			"Timeout waiting for calibration to complete\n");
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC1,
+				   CPCAP_BIT_CAL_MODE, 0);
+	if (error)
+		return;
+}
+
+static int cpcap_adc_calibrate_one(struct cpcap_adc *ddata,
+				   int channel,
+				   u16 calibration_register,
+				   int lower_threshold,
+				   int upper_threshold)
+{
+	unsigned int calibration_data[2];
+	unsigned short cal_data_diff;
+	int i, error;
+
+	for (i = 0; i < CPCAP_ADC_MAX_RETRIES; i++) {
+		calibration_data[0]  = 0;
+		calibration_data[1]  = 0;
+		cal_data_diff = 0;
+		cpcap_adc_setup_calibrate(ddata, channel);
+		error = regmap_read(ddata->reg, calibration_register,
+				    &calibration_data[0]);
+		if (error)
+			return error;
+		cpcap_adc_setup_calibrate(ddata, channel);
+		error = regmap_read(ddata->reg, calibration_register,
+				    &calibration_data[1]);
+		if (error)
+			return error;
+
+		if (calibration_data[0] > calibration_data[1])
+			cal_data_diff =
+				calibration_data[0] - calibration_data[1];
+		else
+			cal_data_diff =
+				calibration_data[1] - calibration_data[0];
+
+		if (((calibration_data[1] >= lower_threshold) &&
+		     (calibration_data[1] <= upper_threshold) &&
+		     (cal_data_diff <= ST_ADC_CALIBRATE_DIFF_THRESHOLD)) ||
+		    (ddata->vendor == CPCAP_VENDOR_TI)) {
+			bank_conversion[channel].cal_offset =
+				((short)calibration_data[1] * -1) + 512;
+			dev_dbg(ddata->dev, "ch%i calibration complete: %i\n",
+				channel, bank_conversion[channel].cal_offset);
+			break;
+		}
+		usleep_range(5000, 10000);
+	}
+
+	return 0;
+}
+
+static int cpcap_adc_calibrate(struct cpcap_adc *ddata)
+{
+	int error;
+
+	error = cpcap_adc_calibrate_one(ddata, CPCAP_ADC_CHG_ISENSE,
+					CPCAP_REG_ADCAL1,
+					ST_ADC_CAL_CHRGI_LOW_THRESHOLD,
+					ST_ADC_CAL_CHRGI_HIGH_THRESHOLD);
+	if (error)
+		return error;
+
+	error = cpcap_adc_calibrate_one(ddata, CPCAP_ADC_BATTI,
+					CPCAP_REG_ADCAL2,
+					ST_ADC_CAL_BATTI_LOW_THRESHOLD,
+					ST_ADC_CAL_BATTI_HIGH_THRESHOLD);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+/* ADC setup, read and scale functions */
+static void cpcap_adc_setup_bank(struct cpcap_adc *ddata,
+				 struct cpcap_adc_request *req)
+{
+	const struct cpcap_adc_ato *ato = ddata->ato;
+	unsigned short value1 = 0;
+	unsigned short value2 = 0;
+	int error;
+
+	if (!ato)
+		return;
+
+	switch (req->channel) {
+	case CPCAP_ADC_AD8 ... CPCAP_ADC_TSY2_AD15:
+		value1 |= CPCAP_BIT_AD_SEL1;
+		break;
+	case CPCAP_ADC_BATTP_PI16 ... CPCAP_ADC_BATTI_PI17:
+		value1 |= CPCAP_BIT_RAND1;
+	default:
+		break;
+	}
+
+	switch (req->timing) {
+	case CPCAP_ADC_TIMING_IN:
+		value1 |= ato->ato_in;
+		value1 |= ato->atox_in;
+		value2 |= ato->adc_ps_factor_in;
+		value2 |= ato->atox_ps_factor_in;
+		break;
+	case CPCAP_ADC_TIMING_OUT:
+		value1 |= ato->ato_out;
+		value1 |= ato->atox_out;
+		value2 |= ato->adc_ps_factor_out;
+		value2 |= ato->atox_ps_factor_out;
+		break;
+
+	case CPCAP_ADC_TIMING_IMM:
+	default:
+		break;
+	}
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC1,
+				   CPCAP_BIT_CAL_MODE | CPCAP_BIT_ATOX |
+				   CPCAP_BIT_ATO3 | CPCAP_BIT_ATO2 |
+				   CPCAP_BIT_ATO1 | CPCAP_BIT_ATO0 |
+				   CPCAP_BIT_ADA2 | CPCAP_BIT_ADA1 |
+				   CPCAP_BIT_ADA0 | CPCAP_BIT_AD_SEL1 |
+				   CPCAP_BIT_RAND1 | CPCAP_BIT_RAND0,
+				   value1);
+	if (error)
+		return;
+
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ATOX_PS_FACTOR |
+				   CPCAP_BIT_ADC_PS_FACTOR1 |
+				   CPCAP_BIT_ADC_PS_FACTOR0,
+				   value2);
+	if (error)
+		return;
+
+	if (req->timing == CPCAP_ADC_TIMING_IMM) {
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+					   CPCAP_BIT_ADTRIG_DIS,
+					   CPCAP_BIT_ADTRIG_DIS);
+		if (error)
+			return;
+
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+					   CPCAP_BIT_ASC,
+					   CPCAP_BIT_ASC);
+		if (error)
+			return;
+	} else {
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+					   CPCAP_BIT_ADTRIG_ONESHOT,
+					   CPCAP_BIT_ADTRIG_ONESHOT);
+		if (error)
+			return;
+
+		error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+					   CPCAP_BIT_ADTRIG_DIS, 0);
+		if (error)
+			return;
+	}
+}
+
+/*
+ * Occasionally the ADC does not seem to start and there will be no
+ * interrupt. Let's re-init interrupt to prevent the ADC from hanging
+ * for the next request. It is unclear why this happens, but the next
+ * request will usually work after doing this.
+ */
+static void cpcap_adc_quirk_reset_lost_irq(struct cpcap_adc *ddata)
+{
+	int error;
+
+	dev_info(ddata->dev, "lost ADC irq, attempting to reinit\n");
+	disable_irq(ddata->irq);
+	error = regmap_update_bits(ddata->reg, CPCAP_REG_ADCC2,
+				   CPCAP_BIT_ADTRIG_DIS,
+				   CPCAP_BIT_ADTRIG_DIS);
+	if (error)
+		dev_warn(ddata->dev, "%s reset failed: %i\n",
+			 __func__, error);
+	enable_irq(ddata->irq);
+}
+
+static int cpcap_adc_start_bank(struct cpcap_adc *ddata,
+				struct cpcap_adc_request *req)
+{
+	int i, error;
+
+	req->timing = CPCAP_ADC_TIMING_IMM;
+	ddata->done = false;
+
+	for (i = 0; i < CPCAP_ADC_MAX_RETRIES; i++) {
+		cpcap_adc_setup_bank(ddata, req);
+		error = wait_event_interruptible_timeout(ddata->wq_data_avail,
+							 ddata->done,
+							 msecs_to_jiffies(50));
+		if (error > 0)
+			return 0;
+
+		if (error == 0) {
+			cpcap_adc_quirk_reset_lost_irq(ddata);
+			error = -ETIMEDOUT;
+			continue;
+		}
+
+		if (error < 0)
+			return error;
+	}
+
+	return error;
+}
+
+static void cpcap_adc_phase(struct cpcap_adc_request *req)
+{
+	const struct cpcap_adc_conversion_tbl *conv_tbl = req->conv_tbl;
+	const struct cpcap_adc_phasing_tbl *phase_tbl = req->phase_tbl;
+	int index = req->channel;
+
+	/* Remuxed channels 16 and 17 use BATTP and BATTI entries */
+	switch (req->channel) {
+	case CPCAP_ADC_BATTP:
+	case CPCAP_ADC_BATTP_PI16:
+		index = req->bank_index;
+		req->result -= phase_tbl[index].offset;
+		req->result -= CPCAP_FOUR_POINT_TWO_ADC;
+		req->result *= phase_tbl[index].multiplier;
+		if (phase_tbl[index].divider == 0)
+			return;
+		req->result /= phase_tbl[index].divider;
+		req->result += CPCAP_FOUR_POINT_TWO_ADC;
+		break;
+	case CPCAP_ADC_BATTI_PI17:
+		index = req->bank_index;
+		/* fallthrough */
+	default:
+		req->result += conv_tbl[index].cal_offset;
+		req->result += conv_tbl[index].align_offset;
+		req->result *= phase_tbl[index].multiplier;
+		if (phase_tbl[index].divider == 0)
+			return;
+		req->result /= phase_tbl[index].divider;
+		req->result += phase_tbl[index].offset;
+		break;
+	}
+
+	if (req->result < phase_tbl[index].min)
+		req->result = phase_tbl[index].min;
+	else if (req->result > phase_tbl[index].max)
+		req->result = phase_tbl[index].max;
+}
+
+/* Looks up temperatures in a table and calculates averages if needed */
+static int cpcap_adc_table_to_millicelcius(unsigned short value)
+{
+	int i, result = 0, alpha;
+
+	if (value <= temp_map[CPCAP_MAX_TEMP_LVL - 1][0])
+		return temp_map[CPCAP_MAX_TEMP_LVL - 1][1];
+
+	if (value >= temp_map[0][0])
+		return temp_map[0][1];
+
+	for (i = 0; i < CPCAP_MAX_TEMP_LVL - 1; i++) {
+		if ((value <= temp_map[i][0]) &&
+		    (value >= temp_map[i + 1][0])) {
+			if (value == temp_map[i][0]) {
+				result = temp_map[i][1];
+			} else if (value == temp_map[i + 1][0]) {
+				result = temp_map[i + 1][1];
+			} else {
+				alpha = ((value - temp_map[i][0]) * 1000) /
+					(temp_map[i + 1][0] - temp_map[i][0]);
+
+				result = temp_map[i][1] +
+					((alpha * (temp_map[i + 1][1] -
+						 temp_map[i][1])) / 1000);
+			}
+			break;
+		}
+	}
+
+	return result;
+}
+
+static void cpcap_adc_convert(struct cpcap_adc_request *req)
+{
+	const struct cpcap_adc_conversion_tbl *conv_tbl = req->conv_tbl;
+	int index = req->channel;
+
+	/* Remuxed channels 16 and 17 use BATTP and BATTI entries */
+	switch (req->channel) {
+	case CPCAP_ADC_BATTP_PI16:
+		index = CPCAP_ADC_BATTP;
+		break;
+	case CPCAP_ADC_BATTI_PI17:
+		index = CPCAP_ADC_BATTI;
+		break;
+	default:
+		break;
+	}
+
+	/* No conversion for raw channels */
+	if (conv_tbl[index].conv_type == IIO_CHAN_INFO_RAW)
+		return;
+
+	/* Temperatures use a lookup table instead of conversion table */
+	if ((req->channel == CPCAP_ADC_AD0_BATTDETB) ||
+	    (req->channel == CPCAP_ADC_AD3)) {
+		req->result =
+			cpcap_adc_table_to_millicelcius(req->result);
+
+		return;
+	}
+
+	/* All processed channels use a conversion table */
+	req->result *= conv_tbl[index].multiplier;
+	if (conv_tbl[index].divider == 0)
+		return;
+	req->result /= conv_tbl[index].divider;
+	req->result += conv_tbl[index].conv_offset;
+}
+
+/*
+ * REVISIT: Check if timed sampling can use multiple channels at the
+ * same time. If not, replace channel_mask with just channel.
+ */
+static int cpcap_adc_read_bank_scaled(struct cpcap_adc *ddata,
+				      struct cpcap_adc_request *req)
+{
+	int calibration_data, error, addr;
+
+	if (ddata->vendor == CPCAP_VENDOR_TI) {
+		error = regmap_read(ddata->reg, CPCAP_REG_ADCAL1,
+				    &calibration_data);
+		if (error)
+			return error;
+		bank_conversion[CPCAP_ADC_CHG_ISENSE].cal_offset =
+			((short)calibration_data * -1) + 512;
+
+		error = regmap_read(ddata->reg, CPCAP_REG_ADCAL2,
+				    &calibration_data);
+		if (error)
+			return error;
+		bank_conversion[CPCAP_ADC_BATTI].cal_offset =
+			((short)calibration_data * -1) + 512;
+	}
+
+	addr = CPCAP_REG_ADCD0 + req->bank_index * 4;
+
+	error = regmap_read(ddata->reg, addr, &req->result);
+	if (error)
+		return error;
+
+	req->result &= 0x3ff;
+	cpcap_adc_phase(req);
+	cpcap_adc_convert(req);
+
+	return 0;
+}
+
+static int cpcap_adc_init_request(struct cpcap_adc_request *req,
+				  int channel)
+{
+	req->channel = channel;
+	req->phase_tbl = bank_phasing;
+	req->conv_tbl = bank_conversion;
+
+	switch (channel) {
+	case CPCAP_ADC_AD0_BATTDETB ... CPCAP_ADC_USB_ID:
+		req->bank_index = channel;
+		break;
+	case CPCAP_ADC_AD8 ... CPCAP_ADC_TSY2_AD15:
+		req->bank_index = channel - 8;
+		break;
+	case CPCAP_ADC_BATTP_PI16:
+		req->bank_index = CPCAP_ADC_BATTP;
+		break;
+	case CPCAP_ADC_BATTI_PI17:
+		req->bank_index = CPCAP_ADC_BATTI;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cpcap_adc_read(struct iio_dev *indio_dev,
+			  struct iio_chan_spec const *chan,
+			  int *val, int *val2, long mask)
+{
+	struct cpcap_adc *ddata = iio_priv(indio_dev);
+	struct cpcap_adc_request req;
+	int error;
+
+	error = cpcap_adc_init_request(&req, chan->channel);
+	if (error)
+		return error;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&ddata->lock);
+		error = cpcap_adc_start_bank(ddata, &req);
+		if (error)
+			goto err_unlock;
+		error = regmap_read(ddata->reg, chan->address, val);
+		if (error)
+			goto err_unlock;
+		mutex_unlock(&ddata->lock);
+		break;
+	case IIO_CHAN_INFO_PROCESSED:
+		mutex_lock(&ddata->lock);
+		error = cpcap_adc_start_bank(ddata, &req);
+		if (error)
+			goto err_unlock;
+		error = cpcap_adc_read_bank_scaled(ddata, &req);
+		if (error)
+			goto err_unlock;
+		mutex_unlock(&ddata->lock);
+		*val = req.result;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+
+err_unlock:
+	mutex_unlock(&ddata->lock);
+	dev_err(ddata->dev, "error reading ADC: %i\n", error);
+
+	return error;
+}
+
+static const struct iio_info cpcap_adc_info = {
+	.read_raw = &cpcap_adc_read,
+	.driver_module = THIS_MODULE,
+};
+
+/*
+ * Configuration for Motorola mapphone series such as droid 4.
+ * Copied from the Motorola mapphone kernel tree.
+ */
+static const struct cpcap_adc_ato mapphone_adc = {
+	.ato_in = 0x0480,
+	.atox_in = 0,
+	.adc_ps_factor_in = 0x0200,
+	.atox_ps_factor_in = 0,
+	.ato_out = 0,
+	.atox_out = 0,
+	.adc_ps_factor_out = 0,
+	.atox_ps_factor_out = 0,
+};
+
+static const struct of_device_id cpcap_adc_id_table[] = {
+	{
+		.compatible = "motorola,cpcap-adc",
+	},
+	{
+		.compatible = "motorola,mapphone-cpcap-adc",
+		.data = &mapphone_adc,
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, cpcap_adc_id_table);
+
+static int cpcap_adc_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct cpcap_adc *ddata;
+	struct iio_dev *indio_dev;
+	int error;
+
+	match = of_match_device(of_match_ptr(cpcap_adc_id_table),
+				&pdev->dev);
+	if (!match)
+		return -EINVAL;
+
+	if (!match->data) {
+		dev_err(&pdev->dev, "no configuration data found\n");
+
+		return -ENODEV;
+	}
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*ddata));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "failed to allocate iio device\n");
+
+		return -ENOMEM;
+	}
+	ddata = iio_priv(indio_dev);
+	ddata->ato = match->data;
+	ddata->dev = &pdev->dev;
+
+	mutex_init(&ddata->lock);
+	init_waitqueue_head(&ddata->wq_data_avail);
+
+	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->channels = cpcap_adc_channels;
+	indio_dev->num_channels = ARRAY_SIZE(cpcap_adc_channels);
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->info = &cpcap_adc_info;
+
+	ddata->reg = dev_get_regmap(pdev->dev.parent, NULL);
+	if (!ddata->reg)
+		return -ENODEV;
+
+	error = cpcap_get_vendor(ddata->dev, ddata->reg, &ddata->vendor);
+	if (error)
+		return error;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	ddata->irq = platform_get_irq_byname(pdev, "adcdone");
+	if (!ddata->irq)
+		return -ENODEV;
+
+	error = devm_request_threaded_irq(&pdev->dev, ddata->irq, NULL,
+					  cpcap_adc_irq_thread,
+					  IRQF_TRIGGER_NONE,
+					  "cpcap-adc", indio_dev);
+	if (error) {
+		dev_err(&pdev->dev, "could not get irq: %i\n",
+			error);
+
+		return error;
+	}
+
+	error = cpcap_adc_calibrate(ddata);
+	if (error)
+		return error;
+
+	dev_info(&pdev->dev, "CPCAP ADC device probed\n");
+
+	return devm_iio_device_register(&pdev->dev, indio_dev);
+}
+
+static struct platform_driver cpcap_adc_driver = {
+	.driver = {
+		.name = "cpcap_adc",
+		.of_match_table = of_match_ptr(cpcap_adc_id_table),
+	},
+	.probe = cpcap_adc_probe,
+};
+
+module_platform_driver(cpcap_adc_driver);
+
+MODULE_ALIAS("platform:cpcap_adc");
+MODULE_DESCRIPTION("CPCAP ADC driver");
+MODULE_AUTHOR("Tony Lindgren <tony@atomide.com");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index ad1775b5f83c..6c5a7be9f8c1 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -579,7 +579,7 @@ static int exynos_read_s3c64xx_ts(struct iio_dev *indio_dev, int *x, int *y)
 
 static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
 {
-	struct exynos_adc *info = (struct exynos_adc *)dev_id;
+	struct exynos_adc *info = dev_id;
 	u32 mask = info->data->mask;
 
 	/* Read value */
diff --git a/drivers/iio/adc/hx711.c b/drivers/iio/adc/hx711.c
index 139639f73769..27005d84ed73 100644
--- a/drivers/iio/adc/hx711.c
+++ b/drivers/iio/adc/hx711.c
@@ -369,7 +369,7 @@ static struct attribute *hx711_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group hx711_attribute_group = {
+static const struct attribute_group hx711_attribute_group = {
 	.attrs = hx711_attributes,
 };
 
diff --git a/drivers/iio/adc/imx7d_adc.c b/drivers/iio/adc/imx7d_adc.c
index e2241ee94783..254b29a68b9d 100644
--- a/drivers/iio/adc/imx7d_adc.c
+++ b/drivers/iio/adc/imx7d_adc.c
@@ -365,7 +365,7 @@ static int imx7d_adc_read_data(struct imx7d_adc *info)
 
 static irqreturn_t imx7d_adc_isr(int irq, void *dev_id)
 {
-	struct imx7d_adc *info = (struct imx7d_adc *)dev_id;
+	struct imx7d_adc *info = dev_id;
 	int status;
 
 	status = readl(info->regs + IMX7D_REG_ADC_INT_STATUS);
diff --git a/drivers/iio/adc/ina2xx-adc.c b/drivers/iio/adc/ina2xx-adc.c
index 3263231276ca..db9838230257 100644
--- a/drivers/iio/adc/ina2xx-adc.c
+++ b/drivers/iio/adc/ina2xx-adc.c
@@ -28,6 +28,7 @@
 #include <linux/iio/sysfs.h>
 #include <linux/kthread.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/regmap.h>
 #include <linux/util_macros.h>
 
@@ -635,6 +636,7 @@ static int ina2xx_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	struct iio_buffer *buffer;
 	unsigned int val;
+	enum ina2xx_ids type;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*chip));
@@ -652,7 +654,11 @@ static int ina2xx_probe(struct i2c_client *client,
 		return PTR_ERR(chip->regmap);
 	}
 
-	chip->config = &ina2xx_config[id->driver_data];
+	if (client->dev.of_node)
+		type = (enum ina2xx_ids)of_device_get_match_data(&client->dev);
+	else
+		type = id->driver_data;
+	chip->config = &ina2xx_config[type];
 
 	mutex_init(&chip->state_lock);
 
@@ -726,9 +732,35 @@ static const struct i2c_device_id ina2xx_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ina2xx_id);
 
+static const struct of_device_id ina2xx_of_match[] = {
+	{
+		.compatible = "ti,ina219",
+		.data = (void *)ina219
+	},
+	{
+		.compatible = "ti,ina220",
+		.data = (void *)ina219
+	},
+	{
+		.compatible = "ti,ina226",
+		.data = (void *)ina226
+	},
+	{
+		.compatible = "ti,ina230",
+		.data = (void *)ina226
+	},
+	{
+		.compatible = "ti,ina231",
+		.data = (void *)ina226
+	},
+	{},
+};
+MODULE_DEVICE_TABLE(of, ina2xx_of_match);
+
 static struct i2c_driver ina2xx_driver = {
 	.driver = {
 		   .name = KBUILD_MODNAME,
+		   .of_match_table = ina2xx_of_match,
 	},
 	.probe = ina2xx_probe,
 	.remove = ina2xx_remove,
diff --git a/drivers/staging/iio/adc/lpc32xx_adc.c b/drivers/iio/adc/lpc32xx_adc.c
index b51f237cd817..0de709b4288b 100644
--- a/drivers/staging/iio/adc/lpc32xx_adc.c
+++ b/drivers/iio/adc/lpc32xx_adc.c
@@ -38,27 +38,30 @@
 /*
  * LPC32XX registers definitions
  */
-#define LPC32XX_ADC_SELECT(x)	((x) + 0x04)
-#define LPC32XX_ADC_CTRL(x)	((x) + 0x08)
-#define LPC32XX_ADC_VALUE(x)	((x) + 0x48)
-
-/* Bit definitions for LPC32XX_ADC_SELECT: */
-#define AD_REFm         0x00000200 /* constant, always write this value! */
-#define AD_REFp		0x00000080 /* constant, always write this value! */
-#define AD_IN		0x00000010 /* multiple of this is the */
-				   /* channel number: 0, 1, 2 */
-#define AD_INTERNAL	0x00000004 /* constant, always write this value! */
-
-/* Bit definitions for LPC32XX_ADC_CTRL: */
-#define AD_STROBE	0x00000002
-#define AD_PDN_CTRL	0x00000004
-
-/* Bit definitions for LPC32XX_ADC_VALUE: */
-#define ADC_VALUE_MASK	0x000003FF
-
-#define MOD_NAME "lpc32xx-adc"
-
-struct lpc32xx_adc_info {
+#define LPC32XXAD_SELECT(x)	((x) + 0x04)
+#define LPC32XXAD_CTRL(x)	((x) + 0x08)
+#define LPC32XXAD_VALUE(x)	((x) + 0x48)
+
+/* Bit definitions for LPC32XXAD_SELECT: */
+/* constant, always write this value! */
+#define LPC32XXAD_REFm         0x00000200
+/* constant, always write this value! */
+#define LPC32XXAD_REFp		0x00000080
+ /* multiple of this is the channel number: 0, 1, 2 */
+#define LPC32XXAD_IN		0x00000010
+/* constant, always write this value! */
+#define LPC32XXAD_INTERNAL	0x00000004
+
+/* Bit definitions for LPC32XXAD_CTRL: */
+#define LPC32XXAD_STROBE	0x00000002
+#define LPC32XXAD_PDN_CTRL	0x00000004
+
+/* Bit definitions for LPC32XXAD_VALUE: */
+#define LPC32XXAD_VALUE_MASK	0x000003FF
+
+#define LPC32XXAD_NAME "lpc32xx-adc"
+
+struct lpc32xx_adc_state {
 	void __iomem *adc_base;
 	struct clk *clk;
 	struct completion completion;
@@ -72,20 +75,21 @@ static int lpc32xx_read_raw(struct iio_dev *indio_dev,
 			    int *val2,
 			    long mask)
 {
-	struct lpc32xx_adc_info *info = iio_priv(indio_dev);
+	struct lpc32xx_adc_state *st = iio_priv(indio_dev);
 
 	if (mask == IIO_CHAN_INFO_RAW) {
 		mutex_lock(&indio_dev->mlock);
-		clk_prepare_enable(info->clk);
+		clk_prepare_enable(st->clk);
 		/* Measurement setup */
-		__raw_writel(AD_INTERNAL | (chan->address) | AD_REFp | AD_REFm,
-			     LPC32XX_ADC_SELECT(info->adc_base));
+		__raw_writel(LPC32XXAD_INTERNAL | (chan->address) |
+			     LPC32XXAD_REFp | LPC32XXAD_REFm,
+			     LPC32XXAD_SELECT(st->adc_base));
 		/* Trigger conversion */
-		__raw_writel(AD_PDN_CTRL | AD_STROBE,
-			     LPC32XX_ADC_CTRL(info->adc_base));
-		wait_for_completion(&info->completion); /* set by ISR */
-		clk_disable_unprepare(info->clk);
-		*val = info->value;
+		__raw_writel(LPC32XXAD_PDN_CTRL | LPC32XXAD_STROBE,
+			     LPC32XXAD_CTRL(st->adc_base));
+		wait_for_completion(&st->completion); /* set by ISR */
+		clk_disable_unprepare(st->clk);
+		*val = st->value;
 		mutex_unlock(&indio_dev->mlock);
 
 		return IIO_VAL_INT;
@@ -104,7 +108,7 @@ static const struct iio_info lpc32xx_adc_iio_info = {
 	.indexed = 1,					\
 	.channel = _index,				\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
-	.address = AD_IN * _index,			\
+	.address = LPC32XXAD_IN * _index,		\
 	.scan_index = _index,				\
 }
 
@@ -116,19 +120,19 @@ static const struct iio_chan_spec lpc32xx_adc_iio_channels[] = {
 
 static irqreturn_t lpc32xx_adc_isr(int irq, void *dev_id)
 {
-	struct lpc32xx_adc_info *info = dev_id;
+	struct lpc32xx_adc_state *st = dev_id;
 
 	/* Read value and clear irq */
-	info->value = __raw_readl(LPC32XX_ADC_VALUE(info->adc_base)) &
-				ADC_VALUE_MASK;
-	complete(&info->completion);
+	st->value = __raw_readl(LPC32XXAD_VALUE(st->adc_base)) &
+		LPC32XXAD_VALUE_MASK;
+	complete(&st->completion);
 
 	return IRQ_HANDLED;
 }
 
 static int lpc32xx_adc_probe(struct platform_device *pdev)
 {
-	struct lpc32xx_adc_info *info = NULL;
+	struct lpc32xx_adc_state *st = NULL;
 	struct resource *res;
 	int retval = -ENODEV;
 	struct iio_dev *iodev = NULL;
@@ -140,23 +144,23 @@ static int lpc32xx_adc_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
-	iodev = devm_iio_device_alloc(&pdev->dev, sizeof(*info));
+	iodev = devm_iio_device_alloc(&pdev->dev, sizeof(*st));
 	if (!iodev)
 		return -ENOMEM;
 
-	info = iio_priv(iodev);
+	st = iio_priv(iodev);
 
-	info->adc_base = devm_ioremap(&pdev->dev, res->start,
-						resource_size(res));
-	if (!info->adc_base) {
+	st->adc_base = devm_ioremap(&pdev->dev, res->start,
+				    resource_size(res));
+	if (!st->adc_base) {
 		dev_err(&pdev->dev, "failed mapping memory\n");
 		return -EBUSY;
 	}
 
-	info->clk = devm_clk_get(&pdev->dev, NULL);
-	if (IS_ERR(info->clk)) {
+	st->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(st->clk)) {
 		dev_err(&pdev->dev, "failed getting clock\n");
-		return PTR_ERR(info->clk);
+		return PTR_ERR(st->clk);
 	}
 
 	irq = platform_get_irq(pdev, 0);
@@ -166,7 +170,7 @@ static int lpc32xx_adc_probe(struct platform_device *pdev)
 	}
 
 	retval = devm_request_irq(&pdev->dev, irq, lpc32xx_adc_isr, 0,
-				  MOD_NAME, info);
+				  LPC32XXAD_NAME, st);
 	if (retval < 0) {
 		dev_err(&pdev->dev, "failed requesting interrupt\n");
 		return retval;
@@ -174,9 +178,9 @@ static int lpc32xx_adc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, iodev);
 
-	init_completion(&info->completion);
+	init_completion(&st->completion);
 
-	iodev->name = MOD_NAME;
+	iodev->name = LPC32XXAD_NAME;
 	iodev->dev.parent = &pdev->dev;
 	iodev->info = &lpc32xx_adc_iio_info;
 	iodev->modes = INDIO_DIRECT_MODE;
@@ -203,7 +207,7 @@ MODULE_DEVICE_TABLE(of, lpc32xx_adc_match);
 static struct platform_driver lpc32xx_adc_driver = {
 	.probe		= lpc32xx_adc_probe,
 	.driver		= {
-		.name	= MOD_NAME,
+		.name	= LPC32XXAD_NAME,
 		.of_match_table = of_match_ptr(lpc32xx_adc_match),
 	},
 };
diff --git a/drivers/iio/adc/ltc2497.c b/drivers/iio/adc/ltc2497.c
new file mode 100644
index 000000000000..2691b10023f5
--- /dev/null
+++ b/drivers/iio/adc/ltc2497.c
@@ -0,0 +1,279 @@
+/*
+ * ltc2497.c - Driver for Analog Devices/Linear Technology LTC2497 ADC
+ *
+ * Copyright (C) 2017 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2.
+ *
+ * Datasheet: http://cds.linear.com/docs/en/datasheet/2497fd.pdf
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+
+#define LTC2497_ENABLE			0xA0
+#define LTC2497_SGL			BIT(4)
+#define LTC2497_DIFF			0
+#define LTC2497_SIGN			BIT(3)
+#define LTC2497_CONFIG_DEFAULT		LTC2497_ENABLE
+#define LTC2497_CONVERSION_TIME_MS	150ULL
+
+struct ltc2497_st {
+	struct i2c_client *client;
+	struct regulator *ref;
+	ktime_t	time_prev;
+	u8 addr_prev;
+	/*
+	 * DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+	__be32 buf ____cacheline_aligned;
+};
+
+static int ltc2497_wait_conv(struct ltc2497_st *st)
+{
+	s64 time_elapsed;
+
+	time_elapsed = ktime_ms_delta(ktime_get(), st->time_prev);
+
+	if (time_elapsed < LTC2497_CONVERSION_TIME_MS) {
+		/* delay if conversion time not passed
+		 * since last read or write
+		 */
+		if (msleep_interruptible(
+		    LTC2497_CONVERSION_TIME_MS - time_elapsed))
+			return -ERESTARTSYS;
+
+		return 0;
+	}
+
+	if (time_elapsed - LTC2497_CONVERSION_TIME_MS <= 0) {
+		/* We're in automatic mode -
+		 * so the last reading is stil not outdated
+		 */
+		return 0;
+	}
+
+	return 1;
+}
+
+static int ltc2497_read(struct ltc2497_st *st, u8 address, int *val)
+{
+	struct i2c_client *client = st->client;
+	int ret;
+
+	ret = ltc2497_wait_conv(st);
+	if (ret < 0)
+		return ret;
+
+	if (ret || st->addr_prev != address) {
+		ret = i2c_smbus_write_byte(st->client,
+					   LTC2497_ENABLE | address);
+		if (ret < 0)
+			return ret;
+		st->addr_prev = address;
+		if (msleep_interruptible(LTC2497_CONVERSION_TIME_MS))
+			return -ERESTARTSYS;
+	}
+	ret = i2c_master_recv(client, (char *)&st->buf, 3);
+	if (ret < 0)  {
+		dev_err(&client->dev, "i2c_master_recv failed\n");
+		return ret;
+	}
+	st->time_prev = ktime_get();
+
+	/* convert and shift the result,
+	 * and finally convert from offset binary to signed integer
+	 */
+	*val = (be32_to_cpu(st->buf) >> 14) - (1 << 17);
+
+	return ret;
+}
+
+static int ltc2497_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct ltc2497_st *st = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+		ret = ltc2497_read(st, chan->address, val);
+		mutex_unlock(&indio_dev->mlock);
+		if (ret < 0)
+			return ret;
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		ret = regulator_get_voltage(st->ref);
+		if (ret < 0)
+			return ret;
+
+		*val = ret / 1000;
+		*val2 = 17;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+#define LTC2497_CHAN(_chan, _addr) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.channel = (_chan), \
+	.address = (_addr | (_chan / 2) | ((_chan & 1) ? LTC2497_SIGN : 0)), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+}
+
+#define LTC2497_CHAN_DIFF(_chan, _addr) { \
+	.type = IIO_VOLTAGE, \
+	.indexed = 1, \
+	.channel = (_chan) * 2 + ((_addr) & LTC2497_SIGN ? 1 : 0), \
+	.channel2 = (_chan) * 2 + ((_addr) & LTC2497_SIGN ? 0 : 1),\
+	.address = (_addr | _chan), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+	.differential = 1, \
+}
+
+static const struct iio_chan_spec ltc2497_channel[] = {
+	LTC2497_CHAN(0, LTC2497_SGL),
+	LTC2497_CHAN(1, LTC2497_SGL),
+	LTC2497_CHAN(2, LTC2497_SGL),
+	LTC2497_CHAN(3, LTC2497_SGL),
+	LTC2497_CHAN(4, LTC2497_SGL),
+	LTC2497_CHAN(5, LTC2497_SGL),
+	LTC2497_CHAN(6, LTC2497_SGL),
+	LTC2497_CHAN(7, LTC2497_SGL),
+	LTC2497_CHAN(8, LTC2497_SGL),
+	LTC2497_CHAN(9, LTC2497_SGL),
+	LTC2497_CHAN(10, LTC2497_SGL),
+	LTC2497_CHAN(11, LTC2497_SGL),
+	LTC2497_CHAN(12, LTC2497_SGL),
+	LTC2497_CHAN(13, LTC2497_SGL),
+	LTC2497_CHAN(14, LTC2497_SGL),
+	LTC2497_CHAN(15, LTC2497_SGL),
+	LTC2497_CHAN_DIFF(0, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(1, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(2, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(3, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(4, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(5, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(6, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(7, LTC2497_DIFF),
+	LTC2497_CHAN_DIFF(0, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(1, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(2, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(3, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(4, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(5, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(6, LTC2497_DIFF | LTC2497_SIGN),
+	LTC2497_CHAN_DIFF(7, LTC2497_DIFF | LTC2497_SIGN),
+};
+
+static const struct iio_info ltc2497_info = {
+	.read_raw = ltc2497_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int ltc2497_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct ltc2497_st *st;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C |
+				     I2C_FUNC_SMBUS_WRITE_BYTE))
+		return -EOPNOTSUPP;
+
+	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;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
+	indio_dev->info = &ltc2497_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = ltc2497_channel;
+	indio_dev->num_channels = ARRAY_SIZE(ltc2497_channel);
+
+	st->ref = devm_regulator_get(&client->dev, "vref");
+	if (IS_ERR(st->ref))
+		return PTR_ERR(st->ref);
+
+	ret = regulator_enable(st->ref);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte(st->client, LTC2497_CONFIG_DEFAULT);
+	if (ret < 0)
+		goto err_regulator_disable;
+
+	st->addr_prev = LTC2497_CONFIG_DEFAULT;
+	st->time_prev = ktime_get();
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto err_regulator_disable;
+
+	return 0;
+
+err_regulator_disable:
+	regulator_disable(st->ref);
+
+	return ret;
+}
+
+static int ltc2497_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct ltc2497_st *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	regulator_disable(st->ref);
+
+	return 0;
+}
+
+static const struct i2c_device_id ltc2497_id[] = {
+	{ "ltc2497", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc2497_id);
+
+static const struct of_device_id ltc2497_of_match[] = {
+	{ .compatible = "lltc,ltc2497", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ltc2497_of_match);
+
+static struct i2c_driver ltc2497_driver = {
+	.driver = {
+		.name = "ltc2497",
+		.of_match_table = of_match_ptr(ltc2497_of_match),
+	},
+	.probe = ltc2497_probe,
+	.remove = ltc2497_remove,
+	.id_table = ltc2497_id,
+};
+module_i2c_driver(ltc2497_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Linear Technology LTC2497 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/max1027.c b/drivers/iio/adc/max1027.c
index 3b7c4f78f37a..ebc715927e63 100644
--- a/drivers/iio/adc/max1027.c
+++ b/drivers/iio/adc/max1027.c
@@ -364,7 +364,7 @@ static int max1027_set_trigger_state(struct iio_trigger *trig, bool state)
 
 static irqreturn_t max1027_trigger_handler(int irq, void *private)
 {
-	struct iio_poll_func *pf = (struct iio_poll_func *)private;
+	struct iio_poll_func *pf = private;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct max1027_state *st = iio_priv(indio_dev);
 
diff --git a/drivers/iio/adc/max11100.c b/drivers/iio/adc/max11100.c
index a088cf99bfe1..1180bcc22ff1 100644
--- a/drivers/iio/adc/max11100.c
+++ b/drivers/iio/adc/max11100.c
@@ -124,8 +124,8 @@ static int max11100_probe(struct spi_device *spi)
 	indio_dev->name = "max11100";
 	indio_dev->info = &max11100_info;
 	indio_dev->modes = INDIO_DIRECT_MODE;
-	indio_dev->channels = max11100_channels,
-	indio_dev->num_channels = ARRAY_SIZE(max11100_channels),
+	indio_dev->channels = max11100_channels;
+	indio_dev->num_channels = ARRAY_SIZE(max11100_channels);
 
 	state->vref_reg = devm_regulator_get(&spi->dev, "vref");
 	if (IS_ERR(state->vref_reg))
@@ -167,7 +167,6 @@ MODULE_DEVICE_TABLE(of, max11100_ids);
 static struct spi_driver max11100_driver = {
 	.driver = {
 		.name	= "max11100",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(max11100_ids),
 	},
 	.probe		= max11100_probe,
diff --git a/drivers/iio/adc/max1118.c b/drivers/iio/adc/max1118.c
new file mode 100644
index 000000000000..2e9648a078c4
--- /dev/null
+++ b/drivers/iio/adc/max1118.c
@@ -0,0 +1,307 @@
+/*
+ * MAX1117/MAX1118/MAX1119 8-bit, dual-channel ADCs driver
+ *
+ * Copyright (c) 2017 Akinobu Mita <akinobu.mita@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * Datasheet: https://datasheets.maximintegrated.com/en/ds/MAX1117-MAX1119.pdf
+ *
+ * SPI interface connections
+ *
+ * SPI                MAXIM
+ * Master  Direction  MAX1117/8/9
+ * ------  ---------  -----------
+ * nCS        -->     CNVST
+ * SCK        -->     SCLK
+ * MISO       <--     DOUT
+ * ------  ---------  -----------
+ */
+
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/regulator/consumer.h>
+
+enum max1118_id {
+	max1117,
+	max1118,
+	max1119,
+};
+
+struct max1118 {
+	struct spi_device *spi;
+	struct mutex lock;
+	struct regulator *reg;
+
+	u8 data ____cacheline_aligned;
+};
+
+#define MAX1118_CHANNEL(ch)						\
+	{								\
+		.type = IIO_VOLTAGE,					\
+		.indexed = 1,						\
+		.channel = (ch),					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+		.scan_index = ch,					\
+		.scan_type = {						\
+			.sign = 'u',					\
+			.realbits = 8,					\
+			.storagebits = 8,				\
+		},							\
+	}
+
+static const struct iio_chan_spec max1118_channels[] = {
+	MAX1118_CHANNEL(0),
+	MAX1118_CHANNEL(1),
+	IIO_CHAN_SOFT_TIMESTAMP(2),
+};
+
+static int max1118_read(struct spi_device *spi, int channel)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct max1118 *adc = iio_priv(indio_dev);
+	struct spi_transfer xfers[] = {
+		/*
+		 * To select CH1 for conversion, CNVST pin must be brought high
+		 * and low for a second time.
+		 */
+		{
+			.len = 0,
+			.delay_usecs = 1,	/* > CNVST Low Time 100 ns */
+			.cs_change = 1,
+		},
+		/*
+		 * The acquisition interval begins with the falling edge of
+		 * CNVST.  The total acquisition and conversion process takes
+		 * <7.5us.
+		 */
+		{
+			.len = 0,
+			.delay_usecs = 8,
+		},
+		{
+			.rx_buf = &adc->data,
+			.len = 1,
+		},
+	};
+	int ret;
+
+	if (channel == 0)
+		ret = spi_sync_transfer(spi, xfers + 1, 2);
+	else
+		ret = spi_sync_transfer(spi, xfers, 3);
+
+	if (ret)
+		return ret;
+
+	return adc->data;
+}
+
+static int max1118_get_vref_mV(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct max1118 *adc = iio_priv(indio_dev);
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	int vref_uV;
+
+	switch (id->driver_data) {
+	case max1117:
+		return 2048;
+	case max1119:
+		return 4096;
+	case max1118:
+		vref_uV = regulator_get_voltage(adc->reg);
+		if (vref_uV < 0)
+			return vref_uV;
+		return vref_uV / 1000;
+	}
+
+	return -ENODEV;
+}
+
+static int max1118_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val, int *val2, long mask)
+{
+	struct max1118 *adc = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&adc->lock);
+		*val = max1118_read(adc->spi, chan->channel);
+		mutex_unlock(&adc->lock);
+		if (*val < 0)
+			return *val;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = max1118_get_vref_mV(adc->spi);
+		if (*val < 0)
+			return *val;
+		*val2 = 8;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info max1118_info = {
+	.read_raw = max1118_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static irqreturn_t max1118_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct max1118 *adc = iio_priv(indio_dev);
+	u8 data[16] = { }; /* 2x 8-bit ADC data + padding + 8 bytes timestamp */
+	int scan_index;
+	int i = 0;
+
+	mutex_lock(&adc->lock);
+
+	for_each_set_bit(scan_index, indio_dev->active_scan_mask,
+			indio_dev->masklength) {
+		const struct iio_chan_spec *scan_chan =
+				&indio_dev->channels[scan_index];
+		int ret = max1118_read(adc->spi, scan_chan->channel);
+
+		if (ret < 0) {
+			dev_warn(&adc->spi->dev,
+				"failed to get conversion data\n");
+			goto out;
+		}
+
+		data[i] = ret;
+		i++;
+	}
+	iio_push_to_buffers_with_timestamp(indio_dev, data,
+					   iio_get_time_ns(indio_dev));
+out:
+	mutex_unlock(&adc->lock);
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int max1118_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct max1118 *adc;
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc = iio_priv(indio_dev);
+	adc->spi = spi;
+	mutex_init(&adc->lock);
+
+	if (id->driver_data == max1118) {
+		adc->reg = devm_regulator_get(&spi->dev, "vref");
+		if (IS_ERR(adc->reg)) {
+			dev_err(&spi->dev, "failed to get vref regulator\n");
+			return PTR_ERR(adc->reg);
+		}
+		ret = regulator_enable(adc->reg);
+		if (ret)
+			return ret;
+	}
+
+	spi_set_drvdata(spi, indio_dev);
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->info = &max1118_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = max1118_channels;
+	indio_dev->num_channels = ARRAY_SIZE(max1118_channels);
+
+	/*
+	 * To reinitiate a conversion on CH0, it is necessary to allow for a
+	 * conversion to be complete and all of the data to be read out.  Once
+	 * a conversion has been completed, the MAX1117/MAX1118/MAX1119 will go
+	 * into AutoShutdown mode until the next conversion is initiated.
+	 */
+	max1118_read(spi, 0);
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					max1118_trigger_handler, NULL);
+	if (ret)
+		goto err_reg_disable;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_buffer_cleanup;
+
+	return 0;
+
+err_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+err_reg_disable:
+	if (id->driver_data == max1118)
+		regulator_disable(adc->reg);
+
+	return ret;
+}
+
+static int max1118_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct max1118 *adc = iio_priv(indio_dev);
+	const struct spi_device_id *id = spi_get_device_id(spi);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	if (id->driver_data == max1118)
+		return regulator_disable(adc->reg);
+
+	return 0;
+}
+
+static const struct spi_device_id max1118_id[] = {
+	{ "max1117", max1117 },
+	{ "max1118", max1118 },
+	{ "max1119", max1119 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, max1118_id);
+
+#ifdef CONFIG_OF
+
+static const struct of_device_id max1118_dt_ids[] = {
+	{ .compatible = "maxim,max1117" },
+	{ .compatible = "maxim,max1118" },
+	{ .compatible = "maxim,max1119" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, max1118_dt_ids);
+
+#endif
+
+static struct spi_driver max1118_spi_driver = {
+	.driver = {
+		.name = "max1118",
+		.of_match_table = of_match_ptr(max1118_dt_ids),
+	},
+	.probe = max1118_probe,
+	.remove = max1118_remove,
+	.id_table = max1118_id,
+};
+module_spi_driver(max1118_spi_driver);
+
+MODULE_AUTHOR("Akinobu Mita <akinobu.mita@gmail.com>");
+MODULE_DESCRIPTION("MAXIM MAX1117/MAX1118/MAX1119 ADCs driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/max1363.c b/drivers/iio/adc/max1363.c
index c6c12feb4a08..80eada4886b3 100644
--- a/drivers/iio/adc/max1363.c
+++ b/drivers/iio/adc/max1363.c
@@ -1007,7 +1007,7 @@ static struct attribute *max1363_event_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group max1363_event_attribute_group = {
+static const struct attribute_group max1363_event_attribute_group = {
 	.attrs = max1363_event_attributes,
 };
 
diff --git a/drivers/iio/adc/max9611.c b/drivers/iio/adc/max9611.c
new file mode 100644
index 000000000000..ec82106480e1
--- /dev/null
+++ b/drivers/iio/adc/max9611.c
@@ -0,0 +1,585 @@
+/*
+ * iio/adc/max9611.c
+ *
+ * Maxim max9611/max9612 high side current sense amplifier with
+ * 12-bit ADC interface.
+ *
+ * Copyright (C) 2017 Jacopo Mondi
+ *
+ * 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 driver supports input common-mode voltage, current-sense
+ * amplifier with programmable gains and die temperature reading from
+ * Maxim max9611/max9612.
+ *
+ * Op-amp, analog comparator, and watchdog functionalities are not
+ * supported by this driver.
+ */
+
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+
+#define DRIVER_NAME			"max9611"
+
+/* max9611 register addresses */
+#define MAX9611_REG_CSA_DATA		0x00
+#define MAX9611_REG_RS_DATA		0x02
+#define MAX9611_REG_TEMP_DATA		0x08
+#define MAX9611_REG_CTRL1		0x0a
+#define MAX9611_REG_CTRL2		0x0b
+
+/* max9611 REG1 mux configuration options */
+#define MAX9611_MUX_MASK		GENMASK(3, 0)
+#define MAX9611_MUX_SENSE_1x		0x00
+#define MAX9611_MUX_SENSE_4x		0x01
+#define MAX9611_MUX_SENSE_8x		0x02
+#define MAX9611_INPUT_VOLT		0x03
+#define MAX9611_MUX_TEMP		0x06
+
+/* max9611 voltage (both csa and input) helper macros */
+#define MAX9611_VOLTAGE_SHIFT		0x04
+#define MAX9611_VOLTAGE_RAW(_r)		((_r) >> MAX9611_VOLTAGE_SHIFT)
+
+/*
+ * max9611 current sense amplifier voltage output:
+ * LSB and offset values depends on selected gain (1x, 4x, 8x)
+ *
+ * GAIN		LSB (nV)	OFFSET (LSB steps)
+ * 1x		107500		1
+ * 4x		26880		1
+ * 8x		13440		3
+ *
+ * The complete formula to calculate current sense voltage is:
+ *     (((adc_read >> 4) - offset) / ((1 / LSB) * 10^-3)
+ */
+#define MAX9611_CSA_1X_LSB_nV		107500
+#define MAX9611_CSA_4X_LSB_nV		26880
+#define MAX9611_CSA_8X_LSB_nV		13440
+
+#define MAX9611_CSA_1X_OFFS_RAW		1
+#define MAX9611_CSA_4X_OFFS_RAW		1
+#define MAX9611_CSA_8X_OFFS_RAW		3
+
+/*
+ * max9611 common input mode (CIM): LSB is 14mV, with 14mV offset at 25 C
+ *
+ * The complete formula to calculate input common voltage is:
+ *     (((adc_read >> 4) * 1000) - offset) / (1 / 14 * 1000)
+ */
+#define MAX9611_CIM_LSB_mV		14
+#define MAX9611_CIM_OFFSET_RAW		1
+
+/*
+ * max9611 temperature reading: LSB is 480 milli degrees Celsius
+ *
+ * The complete formula to calculate temperature is:
+ *     ((adc_read >> 7) * 1000) / (1 / 480 * 1000)
+ */
+#define MAX9611_TEMP_MAX_POS		0x7f80
+#define MAX9611_TEMP_MAX_NEG		0xff80
+#define MAX9611_TEMP_MIN_NEG		0xd980
+#define MAX9611_TEMP_MASK		GENMASK(7, 15)
+#define MAX9611_TEMP_SHIFT		0x07
+#define MAX9611_TEMP_RAW(_r)		((_r) >> MAX9611_TEMP_SHIFT)
+#define MAX9611_TEMP_SCALE_NUM		1000000
+#define MAX9611_TEMP_SCALE_DIV		2083
+
+struct max9611_dev {
+	struct device *dev;
+	struct i2c_client *i2c_client;
+	struct mutex lock;
+	unsigned int shunt_resistor_uohm;
+};
+
+enum max9611_conf_ids {
+	CONF_SENSE_1x,
+	CONF_SENSE_4x,
+	CONF_SENSE_8x,
+	CONF_IN_VOLT,
+	CONF_TEMP,
+};
+
+/**
+ * max9611_mux_conf - associate ADC mux configuration with register address
+ *		      where data shall be read from
+ */
+static const unsigned int max9611_mux_conf[][2] = {
+	/* CONF_SENSE_1x */
+	{ MAX9611_MUX_SENSE_1x, MAX9611_REG_CSA_DATA },
+	/* CONF_SENSE_4x */
+	{ MAX9611_MUX_SENSE_4x, MAX9611_REG_CSA_DATA },
+	/* CONF_SENSE_8x */
+	{ MAX9611_MUX_SENSE_8x, MAX9611_REG_CSA_DATA },
+	/* CONF_IN_VOLT */
+	{ MAX9611_INPUT_VOLT, MAX9611_REG_RS_DATA },
+	/* CONF_TEMP */
+	{ MAX9611_MUX_TEMP, MAX9611_REG_TEMP_DATA },
+};
+
+enum max9611_csa_gain {
+	CSA_GAIN_1x,
+	CSA_GAIN_4x,
+	CSA_GAIN_8x,
+};
+
+enum max9611_csa_gain_params {
+	CSA_GAIN_LSB_nV,
+	CSA_GAIN_OFFS_RAW,
+};
+
+/**
+ * max9611_csa_gain_conf - associate gain multiplier with LSB and
+ *			   offset values.
+ *
+ * Group together parameters associated with configurable gain
+ * on current sense amplifier path to ADC interface.
+ * Current sense read routine adjusts gain until it gets a meaningful
+ * value; use this structure to retrieve the correct LSB and offset values.
+ */
+static const unsigned int max9611_gain_conf[][2] = {
+	{ /* [0] CSA_GAIN_1x */
+		MAX9611_CSA_1X_LSB_nV,
+		MAX9611_CSA_1X_OFFS_RAW,
+	},
+	{ /* [1] CSA_GAIN_4x */
+		MAX9611_CSA_4X_LSB_nV,
+		MAX9611_CSA_4X_OFFS_RAW,
+	},
+	{ /* [2] CSA_GAIN_8x */
+		MAX9611_CSA_8X_LSB_nV,
+		MAX9611_CSA_8X_OFFS_RAW,
+	},
+};
+
+enum max9611_chan_addrs {
+	MAX9611_CHAN_VOLTAGE_INPUT,
+	MAX9611_CHAN_VOLTAGE_SENSE,
+	MAX9611_CHAN_TEMPERATURE,
+	MAX9611_CHAN_CURRENT_LOAD,
+	MAX9611_CHAN_POWER_LOAD,
+};
+
+static const struct iio_chan_spec max9611_channels[] = {
+	{
+	  .type			= IIO_TEMP,
+	  .info_mask_separate	= BIT(IIO_CHAN_INFO_RAW) |
+				  BIT(IIO_CHAN_INFO_SCALE),
+	  .address		= MAX9611_CHAN_TEMPERATURE,
+	},
+	{
+	  .type			= IIO_VOLTAGE,
+	  .info_mask_separate	= BIT(IIO_CHAN_INFO_PROCESSED),
+	  .address		= MAX9611_CHAN_VOLTAGE_SENSE,
+	  .indexed		= 1,
+	  .channel		= 0,
+	},
+	{
+	  .type			= IIO_VOLTAGE,
+	  .info_mask_separate	= BIT(IIO_CHAN_INFO_RAW)   |
+				  BIT(IIO_CHAN_INFO_SCALE) |
+				  BIT(IIO_CHAN_INFO_OFFSET),
+	  .address		= MAX9611_CHAN_VOLTAGE_INPUT,
+	  .indexed		= 1,
+	  .channel		= 1,
+	},
+	{
+	  .type			= IIO_CURRENT,
+	  .info_mask_separate	= BIT(IIO_CHAN_INFO_PROCESSED),
+	  .address		= MAX9611_CHAN_CURRENT_LOAD,
+	},
+	{
+	  .type			= IIO_POWER,
+	  .info_mask_separate	= BIT(IIO_CHAN_INFO_PROCESSED),
+	  .address		= MAX9611_CHAN_POWER_LOAD
+	},
+};
+
+/**
+ * max9611_read_single() - read a single value from ADC interface
+ *
+ * Data registers are 16 bit long, spread between two 8 bit registers
+ * with consecutive addresses.
+ * Configure ADC mux first, then read register at address "reg_addr".
+ * The smbus_read_word routine asks for 16 bits and the ADC is kind enough
+ * to return values from "reg_addr" and "reg_addr + 1" consecutively.
+ * Data are transmitted with big-endian ordering: MSB arrives first.
+ *
+ * @max9611: max9611 device
+ * @selector: index for mux and register configuration
+ * @raw_val: the value returned from ADC
+ */
+static int max9611_read_single(struct max9611_dev *max9611,
+			       enum max9611_conf_ids selector,
+			       u16 *raw_val)
+{
+	int ret;
+
+	u8 mux_conf = max9611_mux_conf[selector][0] & MAX9611_MUX_MASK;
+	u8 reg_addr = max9611_mux_conf[selector][1];
+
+	/*
+	 * Keep mutex lock held during read-write to avoid mux register
+	 * (CTRL1) re-configuration.
+	 */
+	mutex_lock(&max9611->lock);
+	ret = i2c_smbus_write_byte_data(max9611->i2c_client,
+					MAX9611_REG_CTRL1, mux_conf);
+	if (ret) {
+		dev_err(max9611->dev, "i2c write byte failed: 0x%2x - 0x%2x\n",
+			MAX9611_REG_CTRL1, mux_conf);
+		mutex_unlock(&max9611->lock);
+		return ret;
+	}
+
+	/*
+	 * need a delay here to make register configuration
+	 * stabilize. 1 msec at least, from empirical testing.
+	 */
+	usleep_range(1000, 2000);
+
+	ret = i2c_smbus_read_word_swapped(max9611->i2c_client, reg_addr);
+	if (ret < 0) {
+		dev_err(max9611->dev, "i2c read word from 0x%2x failed\n",
+			reg_addr);
+		mutex_unlock(&max9611->lock);
+		return ret;
+	}
+
+	*raw_val = ret;
+	mutex_unlock(&max9611->lock);
+
+	return 0;
+}
+
+/**
+ * max9611_read_csa_voltage() - read current sense amplifier output voltage
+ *
+ * Current sense amplifier output voltage is read through a configurable
+ * 1x, 4x or 8x gain.
+ * Start with plain 1x gain, and adjust gain control properly until a
+ * meaningful value is read from ADC output.
+ *
+ * @max9611: max9611 device
+ * @adc_raw: raw value read from ADC output
+ * @csa_gain: gain configuration option selector
+ */
+static int max9611_read_csa_voltage(struct max9611_dev *max9611,
+				    u16 *adc_raw,
+				    enum max9611_csa_gain *csa_gain)
+{
+	enum max9611_conf_ids gain_selectors[] = {
+		CONF_SENSE_1x,
+		CONF_SENSE_4x,
+		CONF_SENSE_8x
+	};
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(gain_selectors); ++i) {
+		ret = max9611_read_single(max9611, gain_selectors[i], adc_raw);
+		if (ret)
+			return ret;
+
+		if (*adc_raw > 0) {
+			*csa_gain = gain_selectors[i];
+			return 0;
+		}
+	}
+
+	return -EIO;
+}
+
+static int max9611_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct max9611_dev *dev = iio_priv(indio_dev);
+	enum max9611_csa_gain gain_selector;
+	const unsigned int *csa_gain;
+	u16 adc_data;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+
+		switch (chan->address) {
+		case MAX9611_CHAN_TEMPERATURE:
+			ret = max9611_read_single(dev, CONF_TEMP,
+						  &adc_data);
+			if (ret)
+				return -EINVAL;
+
+			*val = MAX9611_TEMP_RAW(adc_data);
+			return IIO_VAL_INT;
+
+		case MAX9611_CHAN_VOLTAGE_INPUT:
+			ret = max9611_read_single(dev, CONF_IN_VOLT,
+						  &adc_data);
+			if (ret)
+				return -EINVAL;
+
+			*val = MAX9611_VOLTAGE_RAW(adc_data);
+			return IIO_VAL_INT;
+		}
+
+		break;
+
+	case IIO_CHAN_INFO_OFFSET:
+		/* MAX9611_CHAN_VOLTAGE_INPUT */
+		*val = MAX9611_CIM_OFFSET_RAW;
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+
+		switch (chan->address) {
+		case MAX9611_CHAN_TEMPERATURE:
+			*val = MAX9611_TEMP_SCALE_NUM;
+			*val2 = MAX9611_TEMP_SCALE_DIV;
+
+			return IIO_VAL_FRACTIONAL;
+
+		case MAX9611_CHAN_VOLTAGE_INPUT:
+			*val = MAX9611_CIM_LSB_mV;
+
+			return IIO_VAL_INT;
+		}
+
+		break;
+
+	case IIO_CHAN_INFO_PROCESSED:
+
+		switch (chan->address) {
+		case MAX9611_CHAN_VOLTAGE_SENSE:
+			/*
+			 * processed (mV): (raw - offset) * LSB (nV) / 10^6
+			 *
+			 * Even if max9611 can output raw csa voltage readings,
+			 * use a produced value as scale depends on gain.
+			 */
+			ret = max9611_read_csa_voltage(dev, &adc_data,
+						       &gain_selector);
+			if (ret)
+				return -EINVAL;
+
+			csa_gain = max9611_gain_conf[gain_selector];
+
+			adc_data -= csa_gain[CSA_GAIN_OFFS_RAW];
+			*val = MAX9611_VOLTAGE_RAW(adc_data) *
+			       csa_gain[CSA_GAIN_LSB_nV];
+			*val2 = 1000000;
+
+			return IIO_VAL_FRACTIONAL;
+
+		case MAX9611_CHAN_CURRENT_LOAD:
+			/* processed (mA): Vcsa (nV) / Rshunt (uOhm)  */
+			ret = max9611_read_csa_voltage(dev, &adc_data,
+						       &gain_selector);
+			if (ret)
+				return -EINVAL;
+
+			csa_gain = max9611_gain_conf[gain_selector];
+
+			adc_data -= csa_gain[CSA_GAIN_OFFS_RAW];
+			*val = MAX9611_VOLTAGE_RAW(adc_data) *
+			       csa_gain[CSA_GAIN_LSB_nV];
+			*val2 = dev->shunt_resistor_uohm;
+
+			return IIO_VAL_FRACTIONAL;
+
+		case MAX9611_CHAN_POWER_LOAD:
+			/*
+			 * processed (mW): Vin (mV) * Vcsa (uV) /
+			 *		   Rshunt (uOhm)
+			 */
+			ret = max9611_read_single(dev, CONF_IN_VOLT,
+						  &adc_data);
+			if (ret)
+				return -EINVAL;
+
+			adc_data -= MAX9611_CIM_OFFSET_RAW;
+			*val = MAX9611_VOLTAGE_RAW(adc_data) *
+			       MAX9611_CIM_LSB_mV;
+
+			ret = max9611_read_csa_voltage(dev, &adc_data,
+						       &gain_selector);
+			if (ret)
+				return -EINVAL;
+
+			csa_gain = max9611_gain_conf[gain_selector];
+
+			/* divide by 10^3 here to avoid 32bit overflow */
+			adc_data -= csa_gain[CSA_GAIN_OFFS_RAW];
+			*val *= MAX9611_VOLTAGE_RAW(adc_data) *
+				csa_gain[CSA_GAIN_LSB_nV] / 1000;
+			*val2 = dev->shunt_resistor_uohm;
+
+			return IIO_VAL_FRACTIONAL;
+		}
+
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static ssize_t max9611_shunt_resistor_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct max9611_dev *max9611 = iio_priv(dev_to_iio_dev(dev));
+	unsigned int i, r;
+
+	i = max9611->shunt_resistor_uohm / 1000;
+	r = max9611->shunt_resistor_uohm % 1000;
+
+	return sprintf(buf, "%u.%03u\n", i, r);
+}
+
+static IIO_DEVICE_ATTR(in_power_shunt_resistor, 0444,
+		       max9611_shunt_resistor_show, NULL, 0);
+static IIO_DEVICE_ATTR(in_current_shunt_resistor, 0444,
+		       max9611_shunt_resistor_show, NULL, 0);
+
+static struct attribute *max9611_attributes[] = {
+	&iio_dev_attr_in_power_shunt_resistor.dev_attr.attr,
+	&iio_dev_attr_in_current_shunt_resistor.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group max9611_attribute_group = {
+	.attrs = max9611_attributes,
+};
+
+static const struct iio_info indio_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= max9611_read_raw,
+	.attrs		= &max9611_attribute_group,
+};
+
+static int max9611_init(struct max9611_dev *max9611)
+{
+	struct i2c_client *client = max9611->i2c_client;
+	u16 regval;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_WRITE_BYTE	|
+				     I2C_FUNC_SMBUS_READ_WORD_DATA)) {
+		dev_err(max9611->dev,
+			"I2c adapter does not support smbus write_byte or read_word functionalities: aborting probe.\n");
+		return -EINVAL;
+	}
+
+	/* Make sure die temperature is in range to test communications. */
+	ret = max9611_read_single(max9611, CONF_TEMP, &regval);
+	if (ret)
+		return ret;
+
+	regval = ret & MAX9611_TEMP_MASK;
+
+	if ((regval > MAX9611_TEMP_MAX_POS &&
+	     regval < MAX9611_TEMP_MIN_NEG) ||
+	     regval > MAX9611_TEMP_MAX_NEG) {
+		dev_err(max9611->dev,
+			"Invalid value received from ADC 0x%4x: aborting\n",
+			regval);
+		return -EIO;
+	}
+
+	/* Mux shall be zeroed back before applying other configurations */
+	ret = i2c_smbus_write_byte_data(max9611->i2c_client,
+					MAX9611_REG_CTRL1, 0);
+	if (ret) {
+		dev_err(max9611->dev, "i2c write byte failed: 0x%2x - 0x%2x\n",
+			MAX9611_REG_CTRL1, 0);
+		return ret;
+	}
+
+	ret = i2c_smbus_write_byte_data(max9611->i2c_client,
+					MAX9611_REG_CTRL2, 0);
+	if (ret) {
+		dev_err(max9611->dev, "i2c write byte failed: 0x%2x - 0x%2x\n",
+			MAX9611_REG_CTRL2, 0);
+		return ret;
+	}
+	usleep_range(1000, 2000);
+
+	return 0;
+}
+
+static const struct of_device_id max9611_of_table[] = {
+	{.compatible = "maxim,max9611", .data = "max9611"},
+	{.compatible = "maxim,max9612", .data = "max9612"},
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, max9611_of_table);
+static int max9611_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	const char * const shunt_res_prop = "shunt-resistor-micro-ohms";
+	const struct device_node *of_node = client->dev.of_node;
+	const struct of_device_id *of_id =
+		of_match_device(max9611_of_table, &client->dev);
+	struct max9611_dev *max9611;
+	struct iio_dev *indio_dev;
+	unsigned int of_shunt;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*max9611));
+	if (IS_ERR(indio_dev))
+		return PTR_ERR(indio_dev);
+
+	i2c_set_clientdata(client, indio_dev);
+
+	max9611			= iio_priv(indio_dev);
+	max9611->dev		= &client->dev;
+	max9611->i2c_client	= client;
+	mutex_init(&max9611->lock);
+
+	ret = of_property_read_u32(of_node, shunt_res_prop, &of_shunt);
+	if (ret) {
+		dev_err(&client->dev,
+			"Missing %s property for %s node\n",
+			shunt_res_prop, of_node->full_name);
+		return ret;
+	}
+	max9611->shunt_resistor_uohm = of_shunt;
+
+	ret = max9611_init(max9611);
+	if (ret)
+		return ret;
+
+	indio_dev->dev.parent	= &client->dev;
+	indio_dev->dev.of_node	= client->dev.of_node;
+	indio_dev->name		= of_id->data;
+	indio_dev->modes	= INDIO_DIRECT_MODE;
+	indio_dev->info		= &indio_info;
+	indio_dev->channels	= max9611_channels;
+	indio_dev->num_channels	= ARRAY_SIZE(max9611_channels);
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static struct i2c_driver max9611_driver = {
+	.driver = {
+		   .name = DRIVER_NAME,
+		   .owner = THIS_MODULE,
+		   .of_match_table = max9611_of_table,
+	},
+	.probe = max9611_probe,
+};
+module_i2c_driver(max9611_driver);
+
+MODULE_AUTHOR("Jacopo Mondi <jacopo+renesas@jmondi.org>");
+MODULE_DESCRIPTION("Maxim max9611/12 current sense amplifier with 12bit ADC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/meson_saradc.c b/drivers/iio/adc/meson_saradc.c
index 89def6034f40..dd4190b50df6 100644
--- a/drivers/iio/adc/meson_saradc.c
+++ b/drivers/iio/adc/meson_saradc.c
@@ -18,7 +18,9 @@
 #include <linux/io.h>
 #include <linux/iio/iio.h>
 #include <linux/module.h>
+#include <linux/interrupt.h>
 #include <linux/of.h>
+#include <linux/of_irq.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
@@ -163,6 +165,9 @@
 	#define MESON_SAR_ADC_REG13_12BIT_CALIBRATION_MASK	GENMASK(13, 8)
 
 #define MESON_SAR_ADC_MAX_FIFO_SIZE				32
+#define MESON_SAR_ADC_TIMEOUT					100 /* ms */
+/* for use with IIO_VAL_INT_PLUS_MICRO */
+#define MILLION							1000000
 
 #define MESON_SAR_ADC_CHAN(_chan) {					\
 	.type = IIO_VOLTAGE,						\
@@ -170,7 +175,9 @@
 	.channel = _chan,						\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
 				BIT(IIO_CHAN_INFO_AVERAGE_RAW),		\
-	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\
+				BIT(IIO_CHAN_INFO_CALIBBIAS) |		\
+				BIT(IIO_CHAN_INFO_CALIBSCALE),		\
 	.datasheet_name = "SAR_ADC_CH"#_chan,				\
 }
 
@@ -229,6 +236,9 @@ struct meson_sar_adc_priv {
 	struct clk_gate				clk_gate;
 	struct clk				*adc_div_clk;
 	struct clk_divider			clk_div;
+	struct completion			done;
+	int					calibbias;
+	int					calibscale;
 };
 
 static const struct regmap_config meson_sar_adc_regmap_config = {
@@ -248,6 +258,17 @@ static unsigned int meson_sar_adc_get_fifo_count(struct iio_dev *indio_dev)
 	return FIELD_GET(MESON_SAR_ADC_REG0_FIFO_COUNT_MASK, regval);
 }
 
+static int meson_sar_adc_calib_val(struct iio_dev *indio_dev, int val)
+{
+	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
+	int tmp;
+
+	/* use val_calib = scale * val_raw + offset calibration function */
+	tmp = div_s64((s64)val * priv->calibscale, MILLION) + priv->calibbias;
+
+	return clamp(tmp, 0, (1 << priv->data->resolution) - 1);
+}
+
 static int meson_sar_adc_wait_busy_clear(struct iio_dev *indio_dev)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
@@ -274,33 +295,31 @@ static int meson_sar_adc_read_raw_sample(struct iio_dev *indio_dev,
 					 int *val)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
-	int ret, regval, fifo_chan, fifo_val, sum = 0, count = 0;
+	int regval, fifo_chan, fifo_val, count;
 
-	ret = meson_sar_adc_wait_busy_clear(indio_dev);
-	if (ret)
-		return ret;
-
-	while (meson_sar_adc_get_fifo_count(indio_dev) > 0 &&
-	       count < MESON_SAR_ADC_MAX_FIFO_SIZE) {
-		regmap_read(priv->regmap, MESON_SAR_ADC_FIFO_RD, &regval);
-
-		fifo_chan = FIELD_GET(MESON_SAR_ADC_FIFO_RD_CHAN_ID_MASK,
-				      regval);
-		if (fifo_chan != chan->channel)
-			continue;
-
-		fifo_val = FIELD_GET(MESON_SAR_ADC_FIFO_RD_SAMPLE_VALUE_MASK,
-				     regval);
-		fifo_val &= (BIT(priv->data->resolution) - 1);
+	if(!wait_for_completion_timeout(&priv->done,
+				msecs_to_jiffies(MESON_SAR_ADC_TIMEOUT)))
+		return -ETIMEDOUT;
 
-		sum += fifo_val;
-		count++;
+	count = meson_sar_adc_get_fifo_count(indio_dev);
+	if (count != 1) {
+		dev_err(&indio_dev->dev,
+			"ADC FIFO has %d element(s) instead of one\n", count);
+		return -EINVAL;
 	}
 
-	if (!count)
-		return -ENOENT;
+	regmap_read(priv->regmap, MESON_SAR_ADC_FIFO_RD, &regval);
+	fifo_chan = FIELD_GET(MESON_SAR_ADC_FIFO_RD_CHAN_ID_MASK, regval);
+	if (fifo_chan != chan->channel) {
+		dev_err(&indio_dev->dev,
+			"ADC FIFO entry belongs to channel %d instead of %d\n",
+			fifo_chan, chan->channel);
+		return -EINVAL;
+	}
 
-	*val = sum / count;
+	fifo_val = FIELD_GET(MESON_SAR_ADC_FIFO_RD_SAMPLE_VALUE_MASK, regval);
+	fifo_val &= GENMASK(priv->data->resolution - 1, 0);
+	*val = meson_sar_adc_calib_val(indio_dev, fifo_val);
 
 	return 0;
 }
@@ -378,6 +397,12 @@ static void meson_sar_adc_start_sample_engine(struct iio_dev *indio_dev)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
 
+	reinit_completion(&priv->done);
+
+	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG0,
+			   MESON_SAR_ADC_REG0_FIFO_IRQ_EN,
+			   MESON_SAR_ADC_REG0_FIFO_IRQ_EN);
+
 	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG0,
 			   MESON_SAR_ADC_REG0_SAMPLE_ENGINE_ENABLE,
 			   MESON_SAR_ADC_REG0_SAMPLE_ENGINE_ENABLE);
@@ -392,6 +417,9 @@ static void meson_sar_adc_stop_sample_engine(struct iio_dev *indio_dev)
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
 
 	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG0,
+			   MESON_SAR_ADC_REG0_FIFO_IRQ_EN, 0);
+
+	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG0,
 			   MESON_SAR_ADC_REG0_SAMPLING_STOP,
 			   MESON_SAR_ADC_REG0_SAMPLING_STOP);
 
@@ -516,6 +544,15 @@ static int meson_sar_adc_iio_info_read_raw(struct iio_dev *indio_dev,
 		*val2 = priv->data->resolution;
 		return IIO_VAL_FRACTIONAL_LOG2;
 
+	case IIO_CHAN_INFO_CALIBBIAS:
+		*val = priv->calibbias;
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_CALIBSCALE:
+		*val = priv->calibscale / MILLION;
+		*val2 = priv->calibscale % MILLION;
+		return IIO_VAL_INT_PLUS_MICRO;
+
 	default:
 		return -EINVAL;
 	}
@@ -643,6 +680,7 @@ static int meson_sar_adc_hw_enable(struct iio_dev *indio_dev)
 {
 	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
 	int ret;
+	u32 regval;
 
 	ret = meson_sar_adc_lock(indio_dev);
 	if (ret)
@@ -667,6 +705,9 @@ static int meson_sar_adc_hw_enable(struct iio_dev *indio_dev)
 		goto err_sana_clk;
 	}
 
+	regval = FIELD_PREP(MESON_SAR_ADC_REG0_FIFO_CNT_IRQ_MASK, 1);
+	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG0,
+			   MESON_SAR_ADC_REG0_FIFO_CNT_IRQ_MASK, regval);
 	regmap_update_bits(priv->regmap, MESON_SAR_ADC_REG11,
 			   MESON_SAR_ADC_REG11_BANDGAP_EN,
 			   MESON_SAR_ADC_REG11_BANDGAP_EN);
@@ -728,6 +769,66 @@ static int meson_sar_adc_hw_disable(struct iio_dev *indio_dev)
 	return 0;
 }
 
+static irqreturn_t meson_sar_adc_irq(int irq, void *data)
+{
+	struct iio_dev *indio_dev = data;
+	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
+	unsigned int cnt, threshold;
+	u32 regval;
+
+	regmap_read(priv->regmap, MESON_SAR_ADC_REG0, &regval);
+	cnt = FIELD_GET(MESON_SAR_ADC_REG0_FIFO_COUNT_MASK, regval);
+	threshold = FIELD_GET(MESON_SAR_ADC_REG0_FIFO_CNT_IRQ_MASK, regval);
+
+	if (cnt < threshold)
+		return IRQ_NONE;
+
+	complete(&priv->done);
+
+	return IRQ_HANDLED;
+}
+
+static int meson_sar_adc_calib(struct iio_dev *indio_dev)
+{
+	struct meson_sar_adc_priv *priv = iio_priv(indio_dev);
+	int ret, nominal0, nominal1, value0, value1;
+
+	/* use points 25% and 75% for calibration */
+	nominal0 = (1 << priv->data->resolution) / 4;
+	nominal1 = (1 << priv->data->resolution) * 3 / 4;
+
+	meson_sar_adc_set_chan7_mux(indio_dev, CHAN7_MUX_VDD_DIV4);
+	usleep_range(10, 20);
+	ret = meson_sar_adc_get_sample(indio_dev,
+				       &meson_sar_adc_iio_channels[7],
+				       MEAN_AVERAGING, EIGHT_SAMPLES, &value0);
+	if (ret < 0)
+		goto out;
+
+	meson_sar_adc_set_chan7_mux(indio_dev, CHAN7_MUX_VDD_MUL3_DIV4);
+	usleep_range(10, 20);
+	ret = meson_sar_adc_get_sample(indio_dev,
+				       &meson_sar_adc_iio_channels[7],
+				       MEAN_AVERAGING, EIGHT_SAMPLES, &value1);
+	if (ret < 0)
+		goto out;
+
+	if (value1 <= value0) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	priv->calibscale = div_s64((nominal1 - nominal0) * (s64)MILLION,
+				   value1 - value0);
+	priv->calibbias = nominal0 - div_s64((s64)value0 * priv->calibscale,
+					     MILLION);
+	ret = 0;
+out:
+	meson_sar_adc_set_chan7_mux(indio_dev, CHAN7_MUX_CH7_INPUT);
+
+	return ret;
+}
+
 static const struct iio_info meson_sar_adc_iio_info = {
 	.read_raw = meson_sar_adc_iio_info_read_raw,
 	.driver_module = THIS_MODULE,
@@ -770,7 +871,7 @@ static int meson_sar_adc_probe(struct platform_device *pdev)
 	struct resource *res;
 	void __iomem *base;
 	const struct of_device_id *match;
-	int ret;
+	int irq, ret;
 
 	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*priv));
 	if (!indio_dev) {
@@ -779,6 +880,7 @@ static int meson_sar_adc_probe(struct platform_device *pdev)
 	}
 
 	priv = iio_priv(indio_dev);
+	init_completion(&priv->done);
 
 	match = of_match_device(meson_sar_adc_of_match, &pdev->dev);
 	priv->data = match->data;
@@ -797,6 +899,15 @@ static int meson_sar_adc_probe(struct platform_device *pdev)
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
+	irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	if (!irq)
+		return -EINVAL;
+
+	ret = devm_request_irq(&pdev->dev, irq, meson_sar_adc_irq, IRQF_SHARED,
+			       dev_name(&pdev->dev), indio_dev);
+	if (ret)
+		return ret;
+
 	priv->regmap = devm_regmap_init_mmio(&pdev->dev, base,
 					     &meson_sar_adc_regmap_config);
 	if (IS_ERR(priv->regmap))
@@ -857,6 +968,8 @@ static int meson_sar_adc_probe(struct platform_device *pdev)
 		return PTR_ERR(priv->vref);
 	}
 
+	priv->calibscale = MILLION;
+
 	ret = meson_sar_adc_init(indio_dev);
 	if (ret)
 		goto err;
@@ -865,6 +978,10 @@ static int meson_sar_adc_probe(struct platform_device *pdev)
 	if (ret)
 		goto err;
 
+	ret = meson_sar_adc_calib(indio_dev);
+	if (ret)
+		dev_warn(&pdev->dev, "calibration failed\n");
+
 	platform_set_drvdata(pdev, indio_dev);
 
 	ret = iio_device_register(indio_dev);
diff --git a/drivers/iio/adc/qcom-pm8xxx-xoadc.c b/drivers/iio/adc/qcom-pm8xxx-xoadc.c
new file mode 100644
index 000000000000..cea8f1fb444a
--- /dev/null
+++ b/drivers/iio/adc/qcom-pm8xxx-xoadc.c
@@ -0,0 +1,1036 @@
+/*
+ * Qualcomm PM8xxx PMIC XOADC driver
+ *
+ * These ADCs are known as HK/XO (house keeping / chrystal oscillator)
+ * "XO" in "XOADC" means Chrystal Oscillator. It's a bunch of
+ * specific-purpose and general purpose ADC converters and channels.
+ *
+ * Copyright (C) 2017 Linaro Ltd.
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ */
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+
+#include "qcom-vadc-common.h"
+
+/*
+ * Definitions for the "user processor" registers lifted from the v3.4
+ * Qualcomm tree. Their kernel has two out-of-tree drivers for the ADC:
+ * drivers/misc/pmic8058-xoadc.c
+ * drivers/hwmon/pm8xxx-adc.c
+ * None of them contain any complete register specification, so this is
+ * a best effort of combining the information.
+ */
+
+/* These appear to be "battery monitor" registers */
+#define ADC_ARB_BTM_CNTRL1			0x17e
+#define ADC_ARB_BTM_CNTRL1_EN_BTM		BIT(0)
+#define ADC_ARB_BTM_CNTRL1_SEL_OP_MODE		BIT(1)
+#define ADC_ARB_BTM_CNTRL1_MEAS_INTERVAL1	BIT(2)
+#define ADC_ARB_BTM_CNTRL1_MEAS_INTERVAL2	BIT(3)
+#define ADC_ARB_BTM_CNTRL1_MEAS_INTERVAL3	BIT(4)
+#define ADC_ARB_BTM_CNTRL1_MEAS_INTERVAL4	BIT(5)
+#define ADC_ARB_BTM_CNTRL1_EOC			BIT(6)
+#define ADC_ARB_BTM_CNTRL1_REQ			BIT(7)
+
+#define ADC_ARB_BTM_AMUX_CNTRL			0x17f
+#define ADC_ARB_BTM_ANA_PARAM			0x180
+#define ADC_ARB_BTM_DIG_PARAM			0x181
+#define ADC_ARB_BTM_RSV				0x182
+#define ADC_ARB_BTM_DATA1			0x183
+#define ADC_ARB_BTM_DATA0			0x184
+#define ADC_ARB_BTM_BAT_COOL_THR1		0x185
+#define ADC_ARB_BTM_BAT_COOL_THR0		0x186
+#define ADC_ARB_BTM_BAT_WARM_THR1		0x187
+#define ADC_ARB_BTM_BAT_WARM_THR0		0x188
+#define ADC_ARB_BTM_CNTRL2			0x18c
+
+/* Proper ADC registers */
+
+#define ADC_ARB_USRP_CNTRL			0x197
+#define ADC_ARB_USRP_CNTRL_EN_ARB		BIT(0)
+#define ADC_ARB_USRP_CNTRL_RSV1			BIT(1)
+#define ADC_ARB_USRP_CNTRL_RSV2			BIT(2)
+#define ADC_ARB_USRP_CNTRL_RSV3			BIT(3)
+#define ADC_ARB_USRP_CNTRL_RSV4			BIT(4)
+#define ADC_ARB_USRP_CNTRL_RSV5			BIT(5)
+#define ADC_ARB_USRP_CNTRL_EOC			BIT(6)
+#define ADC_ARB_USRP_CNTRL_REQ			BIT(7)
+
+#define ADC_ARB_USRP_AMUX_CNTRL			0x198
+/*
+ * The channel mask includes the bits selecting channel mux and prescaler
+ * on PM8058, or channel mux and premux on PM8921.
+ */
+#define ADC_ARB_USRP_AMUX_CNTRL_CHAN_MASK	0xfc
+#define ADC_ARB_USRP_AMUX_CNTRL_RSV0		BIT(0)
+#define ADC_ARB_USRP_AMUX_CNTRL_RSV1		BIT(1)
+/* On PM8058 this is prescaling, on PM8921 this is premux */
+#define ADC_ARB_USRP_AMUX_CNTRL_PRESCALEMUX0	BIT(2)
+#define ADC_ARB_USRP_AMUX_CNTRL_PRESCALEMUX1	BIT(3)
+#define ADC_ARB_USRP_AMUX_CNTRL_SEL0		BIT(4)
+#define ADC_ARB_USRP_AMUX_CNTRL_SEL1		BIT(5)
+#define ADC_ARB_USRP_AMUX_CNTRL_SEL2		BIT(6)
+#define ADC_ARB_USRP_AMUX_CNTRL_SEL3		BIT(7)
+#define ADC_AMUX_PREMUX_SHIFT			2
+#define ADC_AMUX_SEL_SHIFT			4
+
+/* We know very little about the bits in this register */
+#define ADC_ARB_USRP_ANA_PARAM			0x199
+#define ADC_ARB_USRP_ANA_PARAM_DIS		0xFE
+#define ADC_ARB_USRP_ANA_PARAM_EN		0xFF
+
+#define ADC_ARB_USRP_DIG_PARAM			0x19A
+#define ADC_ARB_USRP_DIG_PARAM_SEL_SHIFT0	BIT(0)
+#define ADC_ARB_USRP_DIG_PARAM_SEL_SHIFT1	BIT(1)
+#define ADC_ARB_USRP_DIG_PARAM_CLK_RATE0	BIT(2)
+#define ADC_ARB_USRP_DIG_PARAM_CLK_RATE1	BIT(3)
+#define ADC_ARB_USRP_DIG_PARAM_EOC		BIT(4)
+/*
+ * On a later ADC the decimation factors are defined as
+ * 00 = 512, 01 = 1024, 10 = 2048, 11 = 4096 so assume this
+ * holds also for this older XOADC.
+ */
+#define ADC_ARB_USRP_DIG_PARAM_DEC_RATE0	BIT(5)
+#define ADC_ARB_USRP_DIG_PARAM_DEC_RATE1	BIT(6)
+#define ADC_ARB_USRP_DIG_PARAM_EN		BIT(7)
+#define ADC_DIG_PARAM_DEC_SHIFT			5
+
+#define ADC_ARB_USRP_RSV			0x19B
+#define ADC_ARB_USRP_RSV_RST			BIT(0)
+#define ADC_ARB_USRP_RSV_DTEST0			BIT(1)
+#define ADC_ARB_USRP_RSV_DTEST1			BIT(2)
+#define ADC_ARB_USRP_RSV_OP			BIT(3)
+#define ADC_ARB_USRP_RSV_IP_SEL0		BIT(4)
+#define ADC_ARB_USRP_RSV_IP_SEL1		BIT(5)
+#define ADC_ARB_USRP_RSV_IP_SEL2		BIT(6)
+#define ADC_ARB_USRP_RSV_TRM			BIT(7)
+#define ADC_RSV_IP_SEL_SHIFT			4
+
+#define ADC_ARB_USRP_DATA0			0x19D
+#define ADC_ARB_USRP_DATA1			0x19C
+
+/**
+ * Physical channels which MUST exist on all PM variants in order to provide
+ * proper reference points for calibration.
+ *
+ * @PM8XXX_CHANNEL_INTERNAL: 625mV reference channel
+ * @PM8XXX_CHANNEL_125V: 1250mV reference channel
+ * @PM8XXX_CHANNEL_INTERNAL_2: 325mV reference channel
+ * @PM8XXX_CHANNEL_MUXOFF: channel to reduce input load on mux, apparently also
+ * measures XO temperature
+ */
+#define PM8XXX_CHANNEL_INTERNAL		0x0c
+#define PM8XXX_CHANNEL_125V		0x0d
+#define PM8XXX_CHANNEL_INTERNAL_2	0x0e
+#define PM8XXX_CHANNEL_MUXOFF		0x0f
+
+/*
+ * PM8058 AMUX premux scaling, two bits. This is done of the channel before
+ * reaching the AMUX.
+ */
+#define PM8058_AMUX_PRESCALE_0 0x0 /* No scaling on the signal */
+#define PM8058_AMUX_PRESCALE_1 0x1 /* Unity scaling selected by the user */
+#define PM8058_AMUX_PRESCALE_1_DIV3 0x2 /* 1/3 prescaler on the input */
+
+/* Defines reference voltage for the XOADC */
+#define AMUX_RSV0 0x0 /* XO_IN/XOADC_GND, special selection to read XO temp */
+#define AMUX_RSV1 0x1 /* PMIC_IN/XOADC_GND */
+#define AMUX_RSV2 0x2 /* PMIC_IN/BMS_CSP */
+#define AMUX_RSV3 0x3 /* not used */
+#define AMUX_RSV4 0x4 /* XOADC_GND/XOADC_GND */
+#define AMUX_RSV5 0x5 /* XOADC_VREF/XOADC_GND */
+#define XOADC_RSV_MAX 5 /* 3 bits 0..7, 3 and 6,7 are invalid */
+
+/**
+ * struct xoadc_channel - encodes channel properties and defaults
+ * @datasheet_name: the hardwarename of this channel
+ * @pre_scale_mux: prescale (PM8058) or premux (PM8921) for selecting
+ * this channel. Both this and the amux channel is needed to uniquely
+ * identify a channel. Values 0..3.
+ * @amux_channel: value of the ADC_ARB_USRP_AMUX_CNTRL register for this
+ * channel, bits 4..7, selects the amux, values 0..f
+ * @prescale: the channels have hard-coded prescale ratios defined
+ * by the hardware, this tells us what it is
+ * @type: corresponding IIO channel type, usually IIO_VOLTAGE or
+ * IIO_TEMP
+ * @scale_fn_type: the liner interpolation etc to convert the
+ * ADC code to the value that IIO expects, in uV or millicelsius
+ * etc. This scale function can be pretty elaborate if different
+ * thermistors are connected or other hardware characteristics are
+ * deployed.
+ * @amux_ip_rsv: ratiometric scale value used by the analog muxer: this
+ * selects the reference voltage for ratiometric scaling
+ */
+struct xoadc_channel {
+	const char *datasheet_name;
+	u8 pre_scale_mux:2;
+	u8 amux_channel:4;
+	const struct vadc_prescale_ratio prescale;
+	enum iio_chan_type type;
+	enum vadc_scale_fn_type scale_fn_type;
+	u8 amux_ip_rsv:3;
+};
+
+/**
+ * struct xoadc_variant - encodes the XOADC variant characteristics
+ * @name: name of this PMIC variant
+ * @channels: the hardware channels and respective settings and defaults
+ * @broken_ratiometric: if the PMIC has broken ratiometric scaling (this
+ * is a known problem on PM8058)
+ * @prescaling: this variant uses AMUX bits 2 & 3 for prescaling (PM8058)
+ * @second_level_mux: this variant uses AMUX bits 2 & 3 for a second level
+ * mux
+ */
+struct xoadc_variant {
+	const char name[16];
+	const struct xoadc_channel *channels;
+	bool broken_ratiometric;
+	bool prescaling;
+	bool second_level_mux;
+};
+
+/*
+ * XOADC_CHAN macro parameters:
+ * _dname: the name of the channel
+ * _presmux: prescaler (PM8058) or premux (PM8921) setting for this channel
+ * _amux: the value in bits 2..7 of the ADC_ARB_USRP_AMUX_CNTRL register
+ * for this channel. On some PMICs some of the bits select a prescaler, and
+ * on some PMICs some of the bits select various complex multiplex settings.
+ * _type: IIO channel type
+ * _prenum: prescaler numerator (dividend)
+ * _preden: prescaler denominator (divisor)
+ * _scale: scaling function type, this selects how the raw valued is mangled
+ * to output the actual processed measurement
+ * _amip: analog mux input parent when using ratiometric measurements
+ */
+#define XOADC_CHAN(_dname, _presmux, _amux, _type, _prenum, _preden, _scale, _amip) \
+	{								\
+		.datasheet_name = __stringify(_dname),			\
+		.pre_scale_mux = _presmux,				\
+		.amux_channel = _amux,					\
+		.prescale = { .num = _prenum, .den = _preden },		\
+		.type = _type,						\
+		.scale_fn_type = _scale,				\
+		.amux_ip_rsv = _amip,					\
+	}
+
+/*
+ * Taken from arch/arm/mach-msm/board-9615.c in the vendor tree:
+ * TODO: incomplete, needs testing.
+ */
+static const struct xoadc_channel pm8018_xoadc_channels[] = {
+	XOADC_CHAN(VCOIN, 0x00, 0x00, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VBAT, 0x00, 0x01, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VPH_PWR, 0x00, 0x02, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DIE_TEMP, 0x00, 0x0b, IIO_TEMP, 1, 1, SCALE_PMIC_THERM, AMUX_RSV1),
+	/* Used for battery ID or battery temperature */
+	XOADC_CHAN(AMUX8, 0x00, 0x08, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV2),
+	XOADC_CHAN(INTERNAL, 0x00, 0x0c, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(125V, 0x00, 0x0d, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(MUXOFF, 0x00, 0x0f, IIO_TEMP, 1, 1, SCALE_XOTHERM, AMUX_RSV0),
+	{ }, /* Sentinel */
+};
+
+/*
+ * Taken from arch/arm/mach-msm/board-8930-pmic.c in the vendor tree:
+ * TODO: needs testing.
+ */
+static const struct xoadc_channel pm8038_xoadc_channels[] = {
+	XOADC_CHAN(VCOIN, 0x00, 0x00, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VBAT, 0x00, 0x01, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DCIN, 0x00, 0x02, IIO_VOLTAGE, 1, 6, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ICHG, 0x00, 0x03, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VPH_PWR, 0x00, 0x04, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX5, 0x00, 0x05, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX6, 0x00, 0x06, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX7, 0x00, 0x07, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* AMUX8 used for battery temperature in most cases */
+	XOADC_CHAN(AMUX8, 0x00, 0x08, IIO_TEMP, 1, 1, SCALE_THERM_100K_PULLUP, AMUX_RSV2),
+	XOADC_CHAN(AMUX9, 0x00, 0x09, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(USB_VBUS, 0x00, 0x0a, IIO_VOLTAGE, 1, 4, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DIE_TEMP, 0x00, 0x0b, IIO_TEMP, 1, 1, SCALE_PMIC_THERM, AMUX_RSV1),
+	XOADC_CHAN(INTERNAL, 0x00, 0x0c, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(125V, 0x00, 0x0d, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(INTERNAL_2, 0x00, 0x0e, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(MUXOFF, 0x00, 0x0f, IIO_TEMP, 1, 1, SCALE_XOTHERM, AMUX_RSV0),
+	{ }, /* Sentinel */
+};
+
+/*
+ * This was created by cross-referencing the vendor tree
+ * arch/arm/mach-msm/board-msm8x60.c msm_adc_channels_data[]
+ * with the "channel types" (first field) to find the right
+ * configuration for these channels on an MSM8x60 i.e. PM8058
+ * setup.
+ */
+static const struct xoadc_channel pm8058_xoadc_channels[] = {
+	XOADC_CHAN(VCOIN, 0x00, 0x00, IIO_VOLTAGE, 1, 2, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VBAT, 0x00, 0x01, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DCIN, 0x00, 0x02, IIO_VOLTAGE, 1, 10, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ICHG, 0x00, 0x03, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VPH_PWR, 0x00, 0x04, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	/*
+	 * AMUX channels 5 thru 9 are referred to as MPP5 thru MPP9 in
+	 * some code and documentation. But they are really just 5
+	 * channels just like any other. They are connected to a switching
+	 * matrix where they can be routed to any of the MPPs, not just
+	 * 1-to-1 onto MPP5 thru 9, so naming them MPP5 thru MPP9 is
+	 * very confusing.
+	 */
+	XOADC_CHAN(AMUX5, 0x00, 0x05, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX6, 0x00, 0x06, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX7, 0x00, 0x07, IIO_VOLTAGE, 1, 2, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX8, 0x00, 0x08, IIO_VOLTAGE, 1, 2, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX9, 0x00, 0x09, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(USB_VBUS, 0x00, 0x0a, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DIE_TEMP, 0x00, 0x0b, IIO_TEMP, 1, 1, SCALE_PMIC_THERM, AMUX_RSV1),
+	XOADC_CHAN(INTERNAL, 0x00, 0x0c, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(125V, 0x00, 0x0d, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(INTERNAL_2, 0x00, 0x0e, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(MUXOFF, 0x00, 0x0f, IIO_TEMP, 1, 1, SCALE_XOTHERM, AMUX_RSV0),
+	/* There are also "unity" and divided by 3 channels (prescaler) but noone is using them */
+	{ }, /* Sentinel */
+};
+
+/*
+ * The PM8921 has some pre-muxing on its channels, this comes from the vendor tree
+ * include/linux/mfd/pm8xxx/pm8xxx-adc.h
+ * board-flo-pmic.c (Nexus 7) and board-8064-pmic.c
+ */
+static const struct xoadc_channel pm8921_xoadc_channels[] = {
+	XOADC_CHAN(VCOIN, 0x00, 0x00, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(VBAT, 0x00, 0x01, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DCIN, 0x00, 0x02, IIO_VOLTAGE, 1, 6, SCALE_DEFAULT, AMUX_RSV1),
+	/* channel "ICHG" is reserved and not used on PM8921 */
+	XOADC_CHAN(VPH_PWR, 0x00, 0x04, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(IBAT, 0x00, 0x05, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* CHAN 6 & 7 (MPP1 & MPP2) are reserved for MPP channels on PM8921 */
+	XOADC_CHAN(BATT_THERM, 0x00, 0x08, IIO_TEMP, 1, 1, SCALE_THERM_100K_PULLUP, AMUX_RSV1),
+	XOADC_CHAN(BATT_ID, 0x00, 0x09, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(USB_VBUS, 0x00, 0x0a, IIO_VOLTAGE, 1, 4, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DIE_TEMP, 0x00, 0x0b, IIO_TEMP, 1, 1, SCALE_PMIC_THERM, AMUX_RSV1),
+	XOADC_CHAN(INTERNAL, 0x00, 0x0c, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(125V, 0x00, 0x0d, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* FIXME: look into the scaling of this temperature */
+	XOADC_CHAN(CHG_TEMP, 0x00, 0x0e, IIO_TEMP, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(MUXOFF, 0x00, 0x0f, IIO_TEMP, 1, 1, SCALE_XOTHERM, AMUX_RSV0),
+	/* The following channels have premux bit 0 set to 1 (all end in 4) */
+	XOADC_CHAN(ATEST_8, 0x01, 0x00, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* Set scaling to 1/2 based on the name for these two */
+	XOADC_CHAN(USB_SNS_DIV20, 0x01, 0x01, IIO_VOLTAGE, 1, 2, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DCIN_SNS_DIV20, 0x01, 0x02, IIO_VOLTAGE, 1, 2, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX3, 0x01, 0x03, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX4, 0x01, 0x04, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX5, 0x01, 0x05, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX6, 0x01, 0x06, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX7, 0x01, 0x07, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX8, 0x01, 0x08, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* Internal test signals, I think */
+	XOADC_CHAN(ATEST_1, 0x01, 0x09, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_2, 0x01, 0x0a, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_3, 0x01, 0x0b, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_4, 0x01, 0x0c, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_5, 0x01, 0x0d, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_6, 0x01, 0x0e, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_7, 0x01, 0x0f, IIO_VOLTAGE, 1, 1, SCALE_DEFAULT, AMUX_RSV1),
+	/* The following channels have premux bit 1 set to 1 (all end in 8) */
+	/* I guess even ATEST8 will be divided by 3 here */
+	XOADC_CHAN(ATEST_8, 0x02, 0x00, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	/* I guess div 2 div 3 becomes div 6 */
+	XOADC_CHAN(USB_SNS_DIV20_DIV3, 0x02, 0x01, IIO_VOLTAGE, 1, 6, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(DCIN_SNS_DIV20_DIV3, 0x02, 0x02, IIO_VOLTAGE, 1, 6, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX3_DIV3, 0x02, 0x03, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX4_DIV3, 0x02, 0x04, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX5_DIV3, 0x02, 0x05, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX6_DIV3, 0x02, 0x06, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX7_DIV3, 0x02, 0x07, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(AMUX8_DIV3, 0x02, 0x08, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_1_DIV3, 0x02, 0x09, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_2_DIV3, 0x02, 0x0a, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_3_DIV3, 0x02, 0x0b, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_4_DIV3, 0x02, 0x0c, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_5_DIV3, 0x02, 0x0d, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_6_DIV3, 0x02, 0x0e, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	XOADC_CHAN(ATEST_7_DIV3, 0x02, 0x0f, IIO_VOLTAGE, 1, 3, SCALE_DEFAULT, AMUX_RSV1),
+	{ }, /* Sentinel */
+};
+
+/**
+ * struct pm8xxx_chan_info - ADC channel information
+ * @name: name of this channel
+ * @hwchan: pointer to hardware channel information (muxing & scaling settings)
+ * @calibration: whether to use absolute or ratiometric calibration
+ * @scale_fn_type: scaling function type
+ * @decimation: 0,1,2,3
+ * @amux_ip_rsv: ratiometric scale value if using ratiometric
+ * calibration: 0, 1, 2, 4, 5.
+ */
+struct pm8xxx_chan_info {
+	const char *name;
+	const struct xoadc_channel *hwchan;
+	enum vadc_calibration calibration;
+	u8 decimation:2;
+	u8 amux_ip_rsv:3;
+};
+
+/**
+ * struct pm8xxx_xoadc - state container for the XOADC
+ * @dev: pointer to device
+ * @map: regmap to access registers
+ * @vref: reference voltage regulator
+ * characteristics of the channels, and sensible default settings
+ * @nchans: number of channels, configured by the device tree
+ * @chans: the channel information per-channel, configured by the device tree
+ * @iio_chans: IIO channel specifiers
+ * @graph: linear calibration parameters for absolute and
+ * ratiometric measurements
+ * @complete: completion to indicate end of conversion
+ * @lock: lock to restrict access to the hardware to one client at the time
+ */
+struct pm8xxx_xoadc {
+	struct device *dev;
+	struct regmap *map;
+	const struct xoadc_variant *variant;
+	struct regulator *vref;
+	unsigned int nchans;
+	struct pm8xxx_chan_info *chans;
+	struct iio_chan_spec *iio_chans;
+	struct vadc_linear_graph graph[2];
+	struct completion complete;
+	struct mutex lock;
+};
+
+static irqreturn_t pm8xxx_eoc_irq(int irq, void *d)
+{
+	struct iio_dev *indio_dev = d;
+	struct pm8xxx_xoadc *adc = iio_priv(indio_dev);
+
+	complete(&adc->complete);
+
+	return IRQ_HANDLED;
+}
+
+static struct pm8xxx_chan_info *
+pm8xxx_get_channel(struct pm8xxx_xoadc *adc, u8 chan)
+{
+	struct pm8xxx_chan_info *ch;
+	int i;
+
+	for (i = 0; i < adc->nchans; i++) {
+		ch = &adc->chans[i];
+		if (ch->hwchan->amux_channel == chan)
+			break;
+	}
+	if (i == adc->nchans)
+		return NULL;
+
+	return ch;
+}
+
+static int pm8xxx_read_channel_rsv(struct pm8xxx_xoadc *adc,
+				   const struct pm8xxx_chan_info *ch,
+				   u8 rsv, u16 *adc_code,
+				   bool force_ratiometric)
+{
+	int ret;
+	unsigned int val;
+	u8 rsvmask, rsvval;
+	u8 lsb, msb;
+
+	dev_dbg(adc->dev, "read channel \"%s\", amux %d, prescale/mux: %d, rsv %d\n",
+		ch->name, ch->hwchan->amux_channel, ch->hwchan->pre_scale_mux, rsv);
+
+	mutex_lock(&adc->lock);
+
+	/* Mux in this channel */
+	val = ch->hwchan->amux_channel << ADC_AMUX_SEL_SHIFT;
+	val |= ch->hwchan->pre_scale_mux << ADC_AMUX_PREMUX_SHIFT;
+	ret = regmap_write(adc->map, ADC_ARB_USRP_AMUX_CNTRL, val);
+	if (ret)
+		goto unlock;
+
+	/* Set up ratiometric scale value, mask off all bits except these */
+	rsvmask = (ADC_ARB_USRP_RSV_RST | ADC_ARB_USRP_RSV_DTEST0 |
+		   ADC_ARB_USRP_RSV_DTEST1 | ADC_ARB_USRP_RSV_OP);
+	if (adc->variant->broken_ratiometric && !force_ratiometric) {
+		/*
+		 * Apparently the PM8058 has some kind of bug which is
+		 * reflected in the vendor tree drivers/misc/pmix8058-xoadc.c
+		 * which just hardcodes the RSV selector to SEL1 (0x20) for
+		 * most cases and SEL0 (0x10) for the MUXOFF channel only.
+		 * If we force ratiometric (currently only done when attempting
+		 * to do ratiometric calibration) this doesn't seem to work
+		 * very well and I suspect ratiometric conversion is simply
+		 * broken or not supported on the PM8058.
+		 *
+		 * Maybe IO_SEL2 doesn't exist on PM8058 and bits 4 & 5 select
+		 * the mode alone.
+		 *
+		 * Some PM8058 register documentation would be nice to get
+		 * this right.
+		 */
+		if (ch->hwchan->amux_channel == PM8XXX_CHANNEL_MUXOFF)
+			rsvval = ADC_ARB_USRP_RSV_IP_SEL0;
+		else
+			rsvval = ADC_ARB_USRP_RSV_IP_SEL1;
+	} else {
+		if (rsv == 0xff)
+			rsvval = (ch->amux_ip_rsv << ADC_RSV_IP_SEL_SHIFT) |
+				ADC_ARB_USRP_RSV_TRM;
+		else
+			rsvval = (rsv << ADC_RSV_IP_SEL_SHIFT) |
+				ADC_ARB_USRP_RSV_TRM;
+	}
+
+	ret = regmap_update_bits(adc->map,
+				 ADC_ARB_USRP_RSV,
+				 ~rsvmask,
+				 rsvval);
+	if (ret)
+		goto unlock;
+
+	ret = regmap_write(adc->map, ADC_ARB_USRP_ANA_PARAM,
+			   ADC_ARB_USRP_ANA_PARAM_DIS);
+	if (ret)
+		goto unlock;
+
+	/* Decimation factor */
+	ret = regmap_write(adc->map, ADC_ARB_USRP_DIG_PARAM,
+			   ADC_ARB_USRP_DIG_PARAM_SEL_SHIFT0 |
+			   ADC_ARB_USRP_DIG_PARAM_SEL_SHIFT1 |
+			   ch->decimation << ADC_DIG_PARAM_DEC_SHIFT);
+	if (ret)
+		goto unlock;
+
+	ret = regmap_write(adc->map, ADC_ARB_USRP_ANA_PARAM,
+			   ADC_ARB_USRP_ANA_PARAM_EN);
+	if (ret)
+		goto unlock;
+
+	/* Enable the arbiter, the Qualcomm code does it twice like this */
+	ret = regmap_write(adc->map, ADC_ARB_USRP_CNTRL,
+			   ADC_ARB_USRP_CNTRL_EN_ARB);
+	if (ret)
+		goto unlock;
+	ret = regmap_write(adc->map, ADC_ARB_USRP_CNTRL,
+			   ADC_ARB_USRP_CNTRL_EN_ARB);
+	if (ret)
+		goto unlock;
+
+
+	/* Fire a request! */
+	reinit_completion(&adc->complete);
+	ret = regmap_write(adc->map, ADC_ARB_USRP_CNTRL,
+			   ADC_ARB_USRP_CNTRL_EN_ARB |
+			   ADC_ARB_USRP_CNTRL_REQ);
+	if (ret)
+		goto unlock;
+
+	/* Next the interrupt occurs */
+	ret = wait_for_completion_timeout(&adc->complete,
+					  VADC_CONV_TIME_MAX_US);
+	if (!ret) {
+		dev_err(adc->dev, "conversion timed out\n");
+		ret = -ETIMEDOUT;
+		goto unlock;
+	}
+
+	ret = regmap_read(adc->map, ADC_ARB_USRP_DATA0, &val);
+	if (ret)
+		goto unlock;
+	lsb = val;
+	ret = regmap_read(adc->map, ADC_ARB_USRP_DATA1, &val);
+	if (ret)
+		goto unlock;
+	msb = val;
+	*adc_code = (msb << 8) | lsb;
+
+	/* Turn off the ADC by setting the arbiter to 0 twice */
+	ret = regmap_write(adc->map, ADC_ARB_USRP_CNTRL, 0);
+	if (ret)
+		goto unlock;
+	ret = regmap_write(adc->map, ADC_ARB_USRP_CNTRL, 0);
+	if (ret)
+		goto unlock;
+
+unlock:
+	mutex_unlock(&adc->lock);
+	return ret;
+}
+
+static int pm8xxx_read_channel(struct pm8xxx_xoadc *adc,
+			       const struct pm8xxx_chan_info *ch,
+			       u16 *adc_code)
+{
+	/*
+	 * Normally we just use the ratiometric scale value (RSV) predefined
+	 * for the channel, but during calibration we need to modify this
+	 * so this wrapper is a helper hiding the more complex version.
+	 */
+	return pm8xxx_read_channel_rsv(adc, ch, 0xff, adc_code, false);
+}
+
+static int pm8xxx_calibrate_device(struct pm8xxx_xoadc *adc)
+{
+	const struct pm8xxx_chan_info *ch;
+	u16 read_1250v;
+	u16 read_0625v;
+	u16 read_nomux_rsv5;
+	u16 read_nomux_rsv4;
+	int ret;
+
+	adc->graph[VADC_CALIB_ABSOLUTE].dx = VADC_ABSOLUTE_RANGE_UV;
+	adc->graph[VADC_CALIB_RATIOMETRIC].dx = VADC_RATIOMETRIC_RANGE;
+
+	/* Common reference channel calibration */
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_125V);
+	if (!ch)
+		return -ENODEV;
+	ret = pm8xxx_read_channel(adc, ch, &read_1250v);
+	if (ret) {
+		dev_err(adc->dev, "could not read 1.25V reference channel\n");
+		return -ENODEV;
+	}
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_INTERNAL);
+	if (!ch)
+		return -ENODEV;
+	ret = pm8xxx_read_channel(adc, ch, &read_0625v);
+	if (ret) {
+		dev_err(adc->dev, "could not read 0.625V reference channel\n");
+		return -ENODEV;
+	}
+	if (read_1250v == read_0625v) {
+		dev_err(adc->dev, "read same ADC code for 1.25V and 0.625V\n");
+		return -ENODEV;
+	}
+
+	adc->graph[VADC_CALIB_ABSOLUTE].dy = read_1250v - read_0625v;
+	adc->graph[VADC_CALIB_ABSOLUTE].gnd = read_0625v;
+
+	dev_info(adc->dev, "absolute calibration dx = %d uV, dy = %d units\n",
+		 VADC_ABSOLUTE_RANGE_UV, adc->graph[VADC_CALIB_ABSOLUTE].dy);
+
+	/* Ratiometric calibration */
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_MUXOFF);
+	if (!ch)
+		return -ENODEV;
+	ret = pm8xxx_read_channel_rsv(adc, ch, AMUX_RSV5,
+				      &read_nomux_rsv5, true);
+	if (ret) {
+		dev_err(adc->dev, "could not read MUXOFF reference channel\n");
+		return -ENODEV;
+	}
+	ret = pm8xxx_read_channel_rsv(adc, ch, AMUX_RSV4,
+				      &read_nomux_rsv4, true);
+	if (ret) {
+		dev_err(adc->dev, "could not read MUXOFF reference channel\n");
+		return -ENODEV;
+	}
+	adc->graph[VADC_CALIB_RATIOMETRIC].dy =
+		read_nomux_rsv5 - read_nomux_rsv4;
+	adc->graph[VADC_CALIB_RATIOMETRIC].gnd = read_nomux_rsv4;
+
+	dev_info(adc->dev, "ratiometric calibration dx = %d, dy = %d units\n",
+		 VADC_RATIOMETRIC_RANGE,
+		 adc->graph[VADC_CALIB_RATIOMETRIC].dy);
+
+	return 0;
+}
+
+static int pm8xxx_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct pm8xxx_xoadc *adc = iio_priv(indio_dev);
+	const struct pm8xxx_chan_info *ch;
+	u16 adc_code;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
+		ch = pm8xxx_get_channel(adc, chan->address);
+		if (!ch) {
+			dev_err(adc->dev, "no such channel %lu\n",
+				chan->address);
+			return -EINVAL;
+		}
+		ret = pm8xxx_read_channel(adc, ch, &adc_code);
+		if (ret)
+			return ret;
+
+		ret = qcom_vadc_scale(ch->hwchan->scale_fn_type,
+				      &adc->graph[ch->calibration],
+				      &ch->hwchan->prescale,
+				      (ch->calibration == VADC_CALIB_ABSOLUTE),
+				      adc_code, val);
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_RAW:
+		ch = pm8xxx_get_channel(adc, chan->address);
+		if (!ch) {
+			dev_err(adc->dev, "no such channel %lu\n",
+				chan->address);
+			return -EINVAL;
+		}
+		ret = pm8xxx_read_channel(adc, ch, &adc_code);
+		if (ret)
+			return ret;
+
+		*val = (int)adc_code;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int pm8xxx_of_xlate(struct iio_dev *indio_dev,
+			   const struct of_phandle_args *iiospec)
+{
+	struct pm8xxx_xoadc *adc = iio_priv(indio_dev);
+	u8 pre_scale_mux;
+	u8 amux_channel;
+	unsigned int i;
+
+	/*
+	 * First cell is prescaler or premux, second cell is analog
+	 * mux.
+	 */
+	if (iiospec->args_count != 2) {
+		dev_err(&indio_dev->dev, "wrong number of arguments for %s need 2 got %d\n",
+			iiospec->np->name,
+			iiospec->args_count);
+		return -EINVAL;
+	}
+	pre_scale_mux = (u8)iiospec->args[0];
+	amux_channel = (u8)iiospec->args[1];
+	dev_dbg(&indio_dev->dev, "pre scale/mux: %02x, amux: %02x\n",
+		pre_scale_mux, amux_channel);
+
+	/* We need to match exactly on the prescale/premux and channel */
+	for (i = 0; i < adc->nchans; i++)
+		if (adc->chans[i].hwchan->pre_scale_mux == pre_scale_mux &&
+		    adc->chans[i].hwchan->amux_channel == amux_channel)
+			return i;
+
+	return -EINVAL;
+}
+
+static const struct iio_info pm8xxx_xoadc_info = {
+	.driver_module = THIS_MODULE,
+	.of_xlate = pm8xxx_of_xlate,
+	.read_raw = pm8xxx_read_raw,
+};
+
+static int pm8xxx_xoadc_parse_channel(struct device *dev,
+				      struct device_node *np,
+				      const struct xoadc_channel *hw_channels,
+				      struct iio_chan_spec *iio_chan,
+				      struct pm8xxx_chan_info *ch)
+{
+	const char *name = np->name;
+	const struct xoadc_channel *hwchan;
+	u32 pre_scale_mux, amux_channel;
+	u32 rsv, dec;
+	int ret;
+	int chid;
+
+	ret = of_property_read_u32_index(np, "reg", 0, &pre_scale_mux);
+	if (ret) {
+		dev_err(dev, "invalid pre scale/mux number %s\n", name);
+		return ret;
+	}
+	ret = of_property_read_u32_index(np, "reg", 1, &amux_channel);
+	if (ret) {
+		dev_err(dev, "invalid amux channel number %s\n", name);
+		return ret;
+	}
+
+	/* Find the right channel setting */
+	chid = 0;
+	hwchan = &hw_channels[0];
+	while (hwchan && hwchan->datasheet_name) {
+		if (hwchan->pre_scale_mux == pre_scale_mux &&
+		    hwchan->amux_channel == amux_channel)
+			break;
+		hwchan++;
+		chid++;
+	}
+	/* The sentinel does not have a name assigned */
+	if (!hwchan->datasheet_name) {
+		dev_err(dev, "could not locate channel %02x/%02x\n",
+			pre_scale_mux, amux_channel);
+		return -EINVAL;
+	}
+	ch->name = name;
+	ch->hwchan = hwchan;
+	/* Everyone seems to use absolute calibration except in special cases */
+	ch->calibration = VADC_CALIB_ABSOLUTE;
+	/* Everyone seems to use default ("type 2") decimation */
+	ch->decimation = VADC_DEF_DECIMATION;
+
+	if (!of_property_read_u32(np, "qcom,ratiometric", &rsv)) {
+		ch->calibration = VADC_CALIB_RATIOMETRIC;
+		if (rsv > XOADC_RSV_MAX) {
+			dev_err(dev, "%s too large RSV value %d\n", name, rsv);
+			return -EINVAL;
+		}
+		if (rsv == AMUX_RSV3) {
+			dev_err(dev, "%s invalid RSV value %d\n", name, rsv);
+			return -EINVAL;
+		}
+	}
+
+	/* Optional decimation, if omitted we use the default */
+	ret = of_property_read_u32(np, "qcom,decimation", &dec);
+	if (!ret) {
+		ret = qcom_vadc_decimation_from_dt(dec);
+		if (ret < 0) {
+			dev_err(dev, "%s invalid decimation %d\n",
+				name, dec);
+			return ret;
+		}
+		ch->decimation = ret;
+	}
+
+	iio_chan->channel = chid;
+	iio_chan->address = hwchan->amux_channel;
+	iio_chan->datasheet_name = hwchan->datasheet_name;
+	iio_chan->type = hwchan->type;
+	/* All channels are raw or processed */
+	iio_chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+		BIT(IIO_CHAN_INFO_PROCESSED);
+	iio_chan->indexed = 1;
+
+	dev_dbg(dev, "channel [PRESCALE/MUX: %02x AMUX: %02x] \"%s\" "
+		"ref voltage: %d, decimation %d "
+		"prescale %d/%d, scale function %d\n",
+		hwchan->pre_scale_mux, hwchan->amux_channel, ch->name,
+		ch->amux_ip_rsv, ch->decimation, hwchan->prescale.num,
+		hwchan->prescale.den, hwchan->scale_fn_type);
+
+	return 0;
+}
+
+static int pm8xxx_xoadc_parse_channels(struct pm8xxx_xoadc *adc,
+				       struct device_node *np)
+{
+	struct device_node *child;
+	struct pm8xxx_chan_info *ch;
+	int ret;
+	int i;
+
+	adc->nchans = of_get_available_child_count(np);
+	if (!adc->nchans) {
+		dev_err(adc->dev, "no channel children\n");
+		return -ENODEV;
+	}
+	dev_dbg(adc->dev, "found %d ADC channels\n", adc->nchans);
+
+	adc->iio_chans = devm_kcalloc(adc->dev, adc->nchans,
+				      sizeof(*adc->iio_chans), GFP_KERNEL);
+	if (!adc->iio_chans)
+		return -ENOMEM;
+
+	adc->chans = devm_kcalloc(adc->dev, adc->nchans,
+				  sizeof(*adc->chans), GFP_KERNEL);
+	if (!adc->chans)
+		return -ENOMEM;
+
+	i = 0;
+	for_each_available_child_of_node(np, child) {
+		ch = &adc->chans[i];
+		ret = pm8xxx_xoadc_parse_channel(adc->dev, child,
+						 adc->variant->channels,
+						 &adc->iio_chans[i],
+						 ch);
+		if (ret) {
+			of_node_put(child);
+			return ret;
+		}
+		i++;
+	}
+
+	/* Check for required channels */
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_125V);
+	if (!ch) {
+		dev_err(adc->dev, "missing 1.25V reference channel\n");
+		return -ENODEV;
+	}
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_INTERNAL);
+	if (!ch) {
+		dev_err(adc->dev, "missing 0.625V reference channel\n");
+		return -ENODEV;
+	}
+	ch = pm8xxx_get_channel(adc, PM8XXX_CHANNEL_MUXOFF);
+	if (!ch) {
+		dev_err(adc->dev, "missing MUXOFF reference channel\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int pm8xxx_xoadc_probe(struct platform_device *pdev)
+{
+	const struct xoadc_variant *variant;
+	struct pm8xxx_xoadc *adc;
+	struct iio_dev *indio_dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct regmap *map;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	variant = of_device_get_match_data(dev);
+	if (!variant)
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, indio_dev);
+
+	adc = iio_priv(indio_dev);
+	adc->dev = dev;
+	adc->variant = variant;
+	init_completion(&adc->complete);
+	mutex_init(&adc->lock);
+
+	ret = pm8xxx_xoadc_parse_channels(adc, np);
+	if (ret)
+		return ret;
+
+	map = dev_get_regmap(dev->parent, NULL);
+	if (!map) {
+		dev_err(dev, "parent regmap unavailable.\n");
+		return -ENXIO;
+	}
+	adc->map = map;
+
+	/* Bring up regulator */
+	adc->vref = devm_regulator_get(dev, "xoadc-ref");
+	if (IS_ERR(adc->vref)) {
+		dev_err(dev, "failed to get XOADC VREF regulator\n");
+		return PTR_ERR(adc->vref);
+	}
+	ret = regulator_enable(adc->vref);
+	if (ret) {
+		dev_err(dev, "failed to enable XOADC VREF regulator\n");
+		return ret;
+	}
+
+	ret = devm_request_threaded_irq(dev, platform_get_irq(pdev, 0),
+			pm8xxx_eoc_irq, NULL, 0, variant->name, indio_dev);
+	if (ret) {
+		dev_err(dev, "unable to request IRQ\n");
+		goto out_disable_vref;
+	}
+
+	indio_dev->dev.parent = dev;
+	indio_dev->dev.of_node = np;
+	indio_dev->name = variant->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &pm8xxx_xoadc_info;
+	indio_dev->channels = adc->iio_chans;
+	indio_dev->num_channels = adc->nchans;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto out_disable_vref;
+
+	ret = pm8xxx_calibrate_device(adc);
+	if (ret)
+		goto out_unreg_device;
+
+	dev_info(dev, "%s XOADC driver enabled\n", variant->name);
+
+	return 0;
+
+out_unreg_device:
+	iio_device_unregister(indio_dev);
+out_disable_vref:
+	regulator_disable(adc->vref);
+
+	return ret;
+}
+
+static int pm8xxx_xoadc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct pm8xxx_xoadc *adc = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	regulator_disable(adc->vref);
+
+	return 0;
+}
+
+static const struct xoadc_variant pm8018_variant = {
+	.name = "PM8018-XOADC",
+	.channels = pm8018_xoadc_channels,
+};
+
+static const struct xoadc_variant pm8038_variant = {
+	.name = "PM8038-XOADC",
+	.channels = pm8038_xoadc_channels,
+};
+
+static const struct xoadc_variant pm8058_variant = {
+	.name = "PM8058-XOADC",
+	.channels = pm8058_xoadc_channels,
+	.broken_ratiometric = true,
+	.prescaling = true,
+};
+
+static const struct xoadc_variant pm8921_variant = {
+	.name = "PM8921-XOADC",
+	.channels = pm8921_xoadc_channels,
+	.second_level_mux = true,
+};
+
+static const struct of_device_id pm8xxx_xoadc_id_table[] = {
+	{
+		.compatible = "qcom,pm8018-adc",
+		.data = &pm8018_variant,
+	},
+	{
+		.compatible = "qcom,pm8038-adc",
+		.data = &pm8038_variant,
+	},
+	{
+		.compatible = "qcom,pm8058-adc",
+		.data = &pm8058_variant,
+	},
+	{
+		.compatible = "qcom,pm8921-adc",
+		.data = &pm8921_variant,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pm8xxx_xoadc_id_table);
+
+static struct platform_driver pm8xxx_xoadc_driver = {
+	.driver		= {
+		.name	= "pm8xxx-adc",
+		.of_match_table = pm8xxx_xoadc_id_table,
+	},
+	.probe		= pm8xxx_xoadc_probe,
+	.remove		= pm8xxx_xoadc_remove,
+};
+module_platform_driver(pm8xxx_xoadc_driver);
+
+MODULE_DESCRIPTION("PM8xxx XOADC driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:pm8xxx-xoadc");
diff --git a/drivers/iio/adc/qcom-spmi-vadc.c b/drivers/iio/adc/qcom-spmi-vadc.c
index 0a19761d656c..9e600bfd1765 100644
--- a/drivers/iio/adc/qcom-spmi-vadc.c
+++ b/drivers/iio/adc/qcom-spmi-vadc.c
@@ -28,6 +28,8 @@
 
 #include <dt-bindings/iio/qcom,spmi-vadc.h>
 
+#include "qcom-vadc-common.h"
+
 /* VADC register and bit definitions */
 #define VADC_REVISION2				0x1
 #define VADC_REVISION2_SUPPORTED_VADC		1
@@ -75,84 +77,10 @@
 
 #define VADC_DATA				0x60	/* 16 bits */
 
-#define VADC_CONV_TIME_MIN_US			2000
-#define VADC_CONV_TIME_MAX_US			2100
-
-/* Min ADC code represents 0V */
-#define VADC_MIN_ADC_CODE			0x6000
-/* Max ADC code represents full-scale range of 1.8V */
-#define VADC_MAX_ADC_CODE			0xa800
-
-#define VADC_ABSOLUTE_RANGE_UV			625000
-#define VADC_RATIOMETRIC_RANGE			1800
-
-#define VADC_DEF_PRESCALING			0 /* 1:1 */
-#define VADC_DEF_DECIMATION			0 /* 512 */
-#define VADC_DEF_HW_SETTLE_TIME			0 /* 0 us */
-#define VADC_DEF_AVG_SAMPLES			0 /* 1 sample */
-#define VADC_DEF_CALIB_TYPE			VADC_CALIB_ABSOLUTE
-
-#define VADC_DECIMATION_MIN			512
-#define VADC_DECIMATION_MAX			4096
-
-#define VADC_HW_SETTLE_DELAY_MAX		10000
-#define VADC_AVG_SAMPLES_MAX			512
-
-#define KELVINMIL_CELSIUSMIL			273150
-
-#define PMI_CHG_SCALE_1				-138890
-#define PMI_CHG_SCALE_2				391750000000LL
-
 #define VADC_CHAN_MIN			VADC_USBIN
 #define VADC_CHAN_MAX			VADC_LR_MUX3_BUF_PU1_PU2_XO_THERM
 
 /**
- * struct vadc_map_pt - Map the graph representation for ADC channel
- * @x: Represent the ADC digitized code.
- * @y: Represent the physical data which can be temperature, voltage,
- *     resistance.
- */
-struct vadc_map_pt {
-	s32 x;
-	s32 y;
-};
-
-/*
- * VADC_CALIB_ABSOLUTE: uses the 625mV and 1.25V as reference channels.
- * VADC_CALIB_RATIOMETRIC: uses the reference voltage (1.8V) and GND for
- * calibration.
- */
-enum vadc_calibration {
-	VADC_CALIB_ABSOLUTE = 0,
-	VADC_CALIB_RATIOMETRIC
-};
-
-/**
- * struct vadc_linear_graph - Represent ADC characteristics.
- * @dy: numerator slope to calculate the gain.
- * @dx: denominator slope to calculate the gain.
- * @gnd: A/D word of the ground reference used for the channel.
- *
- * Each ADC device has different offset and gain parameters which are
- * computed to calibrate the device.
- */
-struct vadc_linear_graph {
-	s32 dy;
-	s32 dx;
-	s32 gnd;
-};
-
-/**
- * struct vadc_prescale_ratio - Represent scaling ratio for ADC input.
- * @num: the inverse numerator of the gain applied to the input channel.
- * @den: the inverse denominator of the gain applied to the input channel.
- */
-struct vadc_prescale_ratio {
-	u32 num;
-	u32 den;
-};
-
-/**
  * struct vadc_channel_prop - VADC channel property.
  * @channel: channel number, refer to the channel list.
  * @calibration: calibration type.
@@ -162,9 +90,8 @@ struct vadc_prescale_ratio {
  *	start of conversion.
  * @avg_samples: ability to provide single result from the ADC
  *	that is an average of multiple measurements.
- * @scale_fn: Represents the scaling function to convert voltage
+ * @scale_fn_type: Represents the scaling function to convert voltage
  *	physical units desired by the client for the channel.
- *	Referenced from enum vadc_scale_fn_type.
  */
 struct vadc_channel_prop {
 	unsigned int channel;
@@ -173,7 +100,7 @@ struct vadc_channel_prop {
 	unsigned int prescale;
 	unsigned int hw_settle_time;
 	unsigned int avg_samples;
-	unsigned int scale_fn;
+	enum vadc_scale_fn_type scale_fn_type;
 };
 
 /**
@@ -204,35 +131,6 @@ struct vadc_priv {
 	struct mutex		 lock;
 };
 
-/**
- * struct vadc_scale_fn - Scaling function prototype
- * @scale: Function pointer to one of the scaling functions
- *	which takes the adc properties, channel properties,
- *	and returns the physical result.
- */
-struct vadc_scale_fn {
-	int (*scale)(struct vadc_priv *, const struct vadc_channel_prop *,
-		     u16, int *);
-};
-
-/**
- * enum vadc_scale_fn_type - Scaling function to convert ADC code to
- *				physical scaled units for the channel.
- * SCALE_DEFAULT: Default scaling to convert raw adc code to voltage (uV).
- * SCALE_THERM_100K_PULLUP: Returns temperature in millidegC.
- *				 Uses a mapping table with 100K pullup.
- * SCALE_PMIC_THERM: Returns result in milli degree's Centigrade.
- * SCALE_XOTHERM: Returns XO thermistor voltage in millidegC.
- * SCALE_PMI_CHG_TEMP: Conversion for PMI CHG temp
- */
-enum vadc_scale_fn_type {
-	SCALE_DEFAULT = 0,
-	SCALE_THERM_100K_PULLUP,
-	SCALE_PMIC_THERM,
-	SCALE_XOTHERM,
-	SCALE_PMI_CHG_TEMP,
-};
-
 static const struct vadc_prescale_ratio vadc_prescale_ratios[] = {
 	{.num =  1, .den =  1},
 	{.num =  1, .den =  3},
@@ -244,44 +142,6 @@ static const struct vadc_prescale_ratio vadc_prescale_ratios[] = {
 	{.num =  1, .den = 10}
 };
 
-/* Voltage to temperature */
-static const struct vadc_map_pt adcmap_100k_104ef_104fb[] = {
-	{1758,	-40},
-	{1742,	-35},
-	{1719,	-30},
-	{1691,	-25},
-	{1654,	-20},
-	{1608,	-15},
-	{1551,	-10},
-	{1483,	-5},
-	{1404,	0},
-	{1315,	5},
-	{1218,	10},
-	{1114,	15},
-	{1007,	20},
-	{900,	25},
-	{795,	30},
-	{696,	35},
-	{605,	40},
-	{522,	45},
-	{448,	50},
-	{383,	55},
-	{327,	60},
-	{278,	65},
-	{237,	70},
-	{202,	75},
-	{172,	80},
-	{146,	85},
-	{125,	90},
-	{107,	95},
-	{92,	100},
-	{79,	105},
-	{68,	110},
-	{59,	115},
-	{51,	120},
-	{44,	125}
-};
-
 static int vadc_read(struct vadc_priv *vadc, u16 offset, u8 *data)
 {
 	return regmap_bulk_read(vadc->regmap, vadc->base + offset, data, 1);
@@ -553,159 +413,6 @@ err:
 	return ret;
 }
 
-static int vadc_map_voltage_temp(const struct vadc_map_pt *pts,
-				 u32 tablesize, s32 input, s64 *output)
-{
-	bool descending = 1;
-	u32 i = 0;
-
-	if (!pts)
-		return -EINVAL;
-
-	/* Check if table is descending or ascending */
-	if (tablesize > 1) {
-		if (pts[0].x < pts[1].x)
-			descending = 0;
-	}
-
-	while (i < tablesize) {
-		if ((descending) && (pts[i].x < input)) {
-			/* table entry is less than measured*/
-			 /* value and table is descending, stop */
-			break;
-		} else if ((!descending) &&
-				(pts[i].x > input)) {
-			/* table entry is greater than measured*/
-			/*value and table is ascending, stop */
-			break;
-		}
-		i++;
-	}
-
-	if (i == 0) {
-		*output = pts[0].y;
-	} else if (i == tablesize) {
-		*output = pts[tablesize - 1].y;
-	} else {
-		/* result is between search_index and search_index-1 */
-		/* interpolate linearly */
-		*output = (((s32)((pts[i].y - pts[i - 1].y) *
-			(input - pts[i - 1].x)) /
-			(pts[i].x - pts[i - 1].x)) +
-			pts[i - 1].y);
-	}
-
-	return 0;
-}
-
-static void vadc_scale_calib(struct vadc_priv *vadc, u16 adc_code,
-			     const struct vadc_channel_prop *prop,
-			     s64 *scale_voltage)
-{
-	*scale_voltage = (adc_code -
-		vadc->graph[prop->calibration].gnd);
-	*scale_voltage *= vadc->graph[prop->calibration].dx;
-	*scale_voltage = div64_s64(*scale_voltage,
-		vadc->graph[prop->calibration].dy);
-	if (prop->calibration == VADC_CALIB_ABSOLUTE)
-		*scale_voltage +=
-		vadc->graph[prop->calibration].dx;
-
-	if (*scale_voltage < 0)
-		*scale_voltage = 0;
-}
-
-static int vadc_scale_volt(struct vadc_priv *vadc,
-			   const struct vadc_channel_prop *prop, u16 adc_code,
-			   int *result_uv)
-{
-	const struct vadc_prescale_ratio *prescale;
-	s64 voltage = 0, result = 0;
-
-	vadc_scale_calib(vadc, adc_code, prop, &voltage);
-
-	prescale = &vadc_prescale_ratios[prop->prescale];
-	voltage = voltage * prescale->den;
-	result = div64_s64(voltage, prescale->num);
-	*result_uv = result;
-
-	return 0;
-}
-
-static int vadc_scale_therm(struct vadc_priv *vadc,
-			    const struct vadc_channel_prop *prop, u16 adc_code,
-			    int *result_mdec)
-{
-	s64 voltage = 0, result = 0;
-
-	vadc_scale_calib(vadc, adc_code, prop, &voltage);
-
-	if (prop->calibration == VADC_CALIB_ABSOLUTE)
-		voltage = div64_s64(voltage, 1000);
-
-	vadc_map_voltage_temp(adcmap_100k_104ef_104fb,
-			      ARRAY_SIZE(adcmap_100k_104ef_104fb),
-			      voltage, &result);
-	result *= 1000;
-	*result_mdec = result;
-
-	return 0;
-}
-
-static int vadc_scale_die_temp(struct vadc_priv *vadc,
-			       const struct vadc_channel_prop *prop,
-			       u16 adc_code, int *result_mdec)
-{
-	const struct vadc_prescale_ratio *prescale;
-	s64 voltage = 0;
-	u64 temp; /* Temporary variable for do_div */
-
-	vadc_scale_calib(vadc, adc_code, prop, &voltage);
-
-	if (voltage > 0) {
-		prescale = &vadc_prescale_ratios[prop->prescale];
-		temp = voltage * prescale->den;
-		do_div(temp, prescale->num * 2);
-		voltage = temp;
-	} else {
-		voltage = 0;
-	}
-
-	voltage -= KELVINMIL_CELSIUSMIL;
-	*result_mdec = voltage;
-
-	return 0;
-}
-
-static int vadc_scale_chg_temp(struct vadc_priv *vadc,
-			       const struct vadc_channel_prop *prop,
-			       u16 adc_code, int *result_mdec)
-{
-	const struct vadc_prescale_ratio *prescale;
-	s64 voltage = 0, result = 0;
-
-	vadc_scale_calib(vadc, adc_code, prop, &voltage);
-
-	prescale = &vadc_prescale_ratios[prop->prescale];
-	voltage = voltage * prescale->den;
-	voltage = div64_s64(voltage, prescale->num);
-	voltage = ((PMI_CHG_SCALE_1) * (voltage * 2));
-	voltage = (voltage + PMI_CHG_SCALE_2);
-	result =  div64_s64(voltage, 1000000);
-	*result_mdec = result;
-
-	return 0;
-}
-
-static int vadc_decimation_from_dt(u32 value)
-{
-	if (!is_power_of_2(value) || value < VADC_DECIMATION_MIN ||
-	    value > VADC_DECIMATION_MAX)
-		return -EINVAL;
-
-	return __ffs64(value / VADC_DECIMATION_MIN);
-}
-
 static int vadc_prescaling_from_dt(u32 num, u32 den)
 {
 	unsigned int pre;
@@ -742,14 +449,6 @@ static int vadc_avg_samples_from_dt(u32 value)
 	return __ffs64(value);
 }
 
-static struct vadc_scale_fn scale_fn[] = {
-	[SCALE_DEFAULT] = {vadc_scale_volt},
-	[SCALE_THERM_100K_PULLUP] = {vadc_scale_therm},
-	[SCALE_PMIC_THERM] = {vadc_scale_die_temp},
-	[SCALE_XOTHERM] = {vadc_scale_therm},
-	[SCALE_PMI_CHG_TEMP] = {vadc_scale_chg_temp},
-};
-
 static int vadc_read_raw(struct iio_dev *indio_dev,
 			 struct iio_chan_spec const *chan, int *val, int *val2,
 			 long mask)
@@ -766,7 +465,13 @@ static int vadc_read_raw(struct iio_dev *indio_dev,
 		if (ret)
 			break;
 
-		scale_fn[prop->scale_fn].scale(vadc, prop, adc_code, val);
+		ret = qcom_vadc_scale(prop->scale_fn_type,
+				&vadc->graph[prop->calibration],
+				&vadc_prescale_ratios[prop->prescale],
+				(prop->calibration == VADC_CALIB_ABSOLUTE),
+				adc_code, val);
+		if (ret)
+			break;
 
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_RAW:
@@ -809,7 +514,7 @@ struct vadc_channels {
 	unsigned int prescale_index;
 	enum iio_chan_type type;
 	long info_mask;
-	unsigned int scale_fn;
+	enum vadc_scale_fn_type scale_fn_type;
 };
 
 #define VADC_CHAN(_dname, _type, _mask, _pre, _scale)			\
@@ -818,7 +523,7 @@ struct vadc_channels {
 		.prescale_index = _pre,					\
 		.type = _type,						\
 		.info_mask = _mask,					\
-		.scale_fn = _scale					\
+		.scale_fn_type = _scale					\
 	},								\
 
 #define VADC_NO_CHAN(_dname, _type, _mask, _pre)			\
@@ -976,7 +681,7 @@ static int vadc_get_dt_channel_data(struct device *dev,
 
 	ret = of_property_read_u32(node, "qcom,decimation", &value);
 	if (!ret) {
-		ret = vadc_decimation_from_dt(value);
+		ret = qcom_vadc_decimation_from_dt(value);
 		if (ret < 0) {
 			dev_err(dev, "%02x invalid decimation %d\n",
 				chan, value);
@@ -1068,7 +773,7 @@ static int vadc_get_dt_data(struct vadc_priv *vadc, struct device_node *node)
 			return ret;
 		}
 
-		prop.scale_fn = vadc_chans[prop.channel].scale_fn;
+		prop.scale_fn_type = vadc_chans[prop.channel].scale_fn_type;
 		vadc->chan_props[index] = prop;
 
 		vadc_chan = &vadc_chans[prop.channel];
diff --git a/drivers/iio/adc/qcom-vadc-common.c b/drivers/iio/adc/qcom-vadc-common.c
new file mode 100644
index 000000000000..102fc51b10aa
--- /dev/null
+++ b/drivers/iio/adc/qcom-vadc-common.c
@@ -0,0 +1,230 @@
+#include <linux/bug.h>
+#include <linux/kernel.h>
+#include <linux/bitops.h>
+#include <linux/math64.h>
+#include <linux/log2.h>
+#include <linux/err.h>
+
+#include "qcom-vadc-common.h"
+
+/* Voltage to temperature */
+static const struct vadc_map_pt adcmap_100k_104ef_104fb[] = {
+	{1758,	-40},
+	{1742,	-35},
+	{1719,	-30},
+	{1691,	-25},
+	{1654,	-20},
+	{1608,	-15},
+	{1551,	-10},
+	{1483,	-5},
+	{1404,	0},
+	{1315,	5},
+	{1218,	10},
+	{1114,	15},
+	{1007,	20},
+	{900,	25},
+	{795,	30},
+	{696,	35},
+	{605,	40},
+	{522,	45},
+	{448,	50},
+	{383,	55},
+	{327,	60},
+	{278,	65},
+	{237,	70},
+	{202,	75},
+	{172,	80},
+	{146,	85},
+	{125,	90},
+	{107,	95},
+	{92,	100},
+	{79,	105},
+	{68,	110},
+	{59,	115},
+	{51,	120},
+	{44,	125}
+};
+
+static int qcom_vadc_map_voltage_temp(const struct vadc_map_pt *pts,
+				      u32 tablesize, s32 input, s64 *output)
+{
+	bool descending = 1;
+	u32 i = 0;
+
+	if (!pts)
+		return -EINVAL;
+
+	/* Check if table is descending or ascending */
+	if (tablesize > 1) {
+		if (pts[0].x < pts[1].x)
+			descending = 0;
+	}
+
+	while (i < tablesize) {
+		if ((descending) && (pts[i].x < input)) {
+			/* table entry is less than measured*/
+			 /* value and table is descending, stop */
+			break;
+		} else if ((!descending) &&
+				(pts[i].x > input)) {
+			/* table entry is greater than measured*/
+			/*value and table is ascending, stop */
+			break;
+		}
+		i++;
+	}
+
+	if (i == 0) {
+		*output = pts[0].y;
+	} else if (i == tablesize) {
+		*output = pts[tablesize - 1].y;
+	} else {
+		/* result is between search_index and search_index-1 */
+		/* interpolate linearly */
+		*output = (((s32)((pts[i].y - pts[i - 1].y) *
+			(input - pts[i - 1].x)) /
+			(pts[i].x - pts[i - 1].x)) +
+			pts[i - 1].y);
+	}
+
+	return 0;
+}
+
+static void qcom_vadc_scale_calib(const struct vadc_linear_graph *calib_graph,
+				  u16 adc_code,
+				  bool absolute,
+				  s64 *scale_voltage)
+{
+	*scale_voltage = (adc_code - calib_graph->gnd);
+	*scale_voltage *= calib_graph->dx;
+	*scale_voltage = div64_s64(*scale_voltage, calib_graph->dy);
+	if (absolute)
+		*scale_voltage += calib_graph->dx;
+
+	if (*scale_voltage < 0)
+		*scale_voltage = 0;
+}
+
+static int qcom_vadc_scale_volt(const struct vadc_linear_graph *calib_graph,
+				const struct vadc_prescale_ratio *prescale,
+				bool absolute, u16 adc_code,
+				int *result_uv)
+{
+	s64 voltage = 0, result = 0;
+
+	qcom_vadc_scale_calib(calib_graph, adc_code, absolute, &voltage);
+
+	voltage = voltage * prescale->den;
+	result = div64_s64(voltage, prescale->num);
+	*result_uv = result;
+
+	return 0;
+}
+
+static int qcom_vadc_scale_therm(const struct vadc_linear_graph *calib_graph,
+				 const struct vadc_prescale_ratio *prescale,
+				 bool absolute, u16 adc_code,
+				 int *result_mdec)
+{
+	s64 voltage = 0, result = 0;
+	int ret;
+
+	qcom_vadc_scale_calib(calib_graph, adc_code, absolute, &voltage);
+
+	if (absolute)
+		voltage = div64_s64(voltage, 1000);
+
+	ret = qcom_vadc_map_voltage_temp(adcmap_100k_104ef_104fb,
+					 ARRAY_SIZE(adcmap_100k_104ef_104fb),
+					 voltage, &result);
+	if (ret)
+		return ret;
+
+	result *= 1000;
+	*result_mdec = result;
+
+	return 0;
+}
+
+static int qcom_vadc_scale_die_temp(const struct vadc_linear_graph *calib_graph,
+				    const struct vadc_prescale_ratio *prescale,
+				    bool absolute,
+				    u16 adc_code, int *result_mdec)
+{
+	s64 voltage = 0;
+	u64 temp; /* Temporary variable for do_div */
+
+	qcom_vadc_scale_calib(calib_graph, adc_code, absolute, &voltage);
+
+	if (voltage > 0) {
+		temp = voltage * prescale->den;
+		do_div(temp, prescale->num * 2);
+		voltage = temp;
+	} else {
+		voltage = 0;
+	}
+
+	voltage -= KELVINMIL_CELSIUSMIL;
+	*result_mdec = voltage;
+
+	return 0;
+}
+
+static int qcom_vadc_scale_chg_temp(const struct vadc_linear_graph *calib_graph,
+				    const struct vadc_prescale_ratio *prescale,
+				    bool absolute,
+				    u16 adc_code, int *result_mdec)
+{
+	s64 voltage = 0, result = 0;
+
+	qcom_vadc_scale_calib(calib_graph, adc_code, absolute, &voltage);
+
+	voltage = voltage * prescale->den;
+	voltage = div64_s64(voltage, prescale->num);
+	voltage = ((PMI_CHG_SCALE_1) * (voltage * 2));
+	voltage = (voltage + PMI_CHG_SCALE_2);
+	result =  div64_s64(voltage, 1000000);
+	*result_mdec = result;
+
+	return 0;
+}
+
+int qcom_vadc_scale(enum vadc_scale_fn_type scaletype,
+		    const struct vadc_linear_graph *calib_graph,
+		    const struct vadc_prescale_ratio *prescale,
+		    bool absolute,
+		    u16 adc_code, int *result)
+{
+	switch (scaletype) {
+	case SCALE_DEFAULT:
+		return qcom_vadc_scale_volt(calib_graph, prescale,
+					    absolute, adc_code,
+					    result);
+	case SCALE_THERM_100K_PULLUP:
+	case SCALE_XOTHERM:
+		return qcom_vadc_scale_therm(calib_graph, prescale,
+					     absolute, adc_code,
+					     result);
+	case SCALE_PMIC_THERM:
+		return qcom_vadc_scale_die_temp(calib_graph, prescale,
+						absolute, adc_code,
+						result);
+	case SCALE_PMI_CHG_TEMP:
+		return qcom_vadc_scale_chg_temp(calib_graph, prescale,
+						absolute, adc_code,
+						result);
+	default:
+		return -EINVAL;
+	}
+}
+EXPORT_SYMBOL(qcom_vadc_scale);
+
+int qcom_vadc_decimation_from_dt(u32 value)
+{
+	if (!is_power_of_2(value) || value < VADC_DECIMATION_MIN ||
+	    value > VADC_DECIMATION_MAX)
+		return -EINVAL;
+
+	return __ffs64(value / VADC_DECIMATION_MIN);
+}
+EXPORT_SYMBOL(qcom_vadc_decimation_from_dt);
diff --git a/drivers/iio/adc/qcom-vadc-common.h b/drivers/iio/adc/qcom-vadc-common.h
new file mode 100644
index 000000000000..63c872a70adc
--- /dev/null
+++ b/drivers/iio/adc/qcom-vadc-common.h
@@ -0,0 +1,108 @@
+/*
+ * Code shared between the different Qualcomm PMIC voltage ADCs
+ */
+
+#ifndef QCOM_VADC_COMMON_H
+#define QCOM_VADC_COMMON_H
+
+#define VADC_CONV_TIME_MIN_US			2000
+#define VADC_CONV_TIME_MAX_US			2100
+
+/* Min ADC code represents 0V */
+#define VADC_MIN_ADC_CODE			0x6000
+/* Max ADC code represents full-scale range of 1.8V */
+#define VADC_MAX_ADC_CODE			0xa800
+
+#define VADC_ABSOLUTE_RANGE_UV			625000
+#define VADC_RATIOMETRIC_RANGE			1800
+
+#define VADC_DEF_PRESCALING			0 /* 1:1 */
+#define VADC_DEF_DECIMATION			0 /* 512 */
+#define VADC_DEF_HW_SETTLE_TIME			0 /* 0 us */
+#define VADC_DEF_AVG_SAMPLES			0 /* 1 sample */
+#define VADC_DEF_CALIB_TYPE			VADC_CALIB_ABSOLUTE
+
+#define VADC_DECIMATION_MIN			512
+#define VADC_DECIMATION_MAX			4096
+
+#define VADC_HW_SETTLE_DELAY_MAX		10000
+#define VADC_AVG_SAMPLES_MAX			512
+
+#define KELVINMIL_CELSIUSMIL			273150
+
+#define PMI_CHG_SCALE_1				-138890
+#define PMI_CHG_SCALE_2				391750000000LL
+
+/**
+ * struct vadc_map_pt - Map the graph representation for ADC channel
+ * @x: Represent the ADC digitized code.
+ * @y: Represent the physical data which can be temperature, voltage,
+ *     resistance.
+ */
+struct vadc_map_pt {
+	s32 x;
+	s32 y;
+};
+
+/*
+ * VADC_CALIB_ABSOLUTE: uses the 625mV and 1.25V as reference channels.
+ * VADC_CALIB_RATIOMETRIC: uses the reference voltage (1.8V) and GND for
+ * calibration.
+ */
+enum vadc_calibration {
+	VADC_CALIB_ABSOLUTE = 0,
+	VADC_CALIB_RATIOMETRIC
+};
+
+/**
+ * struct vadc_linear_graph - Represent ADC characteristics.
+ * @dy: numerator slope to calculate the gain.
+ * @dx: denominator slope to calculate the gain.
+ * @gnd: A/D word of the ground reference used for the channel.
+ *
+ * Each ADC device has different offset and gain parameters which are
+ * computed to calibrate the device.
+ */
+struct vadc_linear_graph {
+	s32 dy;
+	s32 dx;
+	s32 gnd;
+};
+
+/**
+ * struct vadc_prescale_ratio - Represent scaling ratio for ADC input.
+ * @num: the inverse numerator of the gain applied to the input channel.
+ * @den: the inverse denominator of the gain applied to the input channel.
+ */
+struct vadc_prescale_ratio {
+	u32 num;
+	u32 den;
+};
+
+/**
+ * enum vadc_scale_fn_type - Scaling function to convert ADC code to
+ *				physical scaled units for the channel.
+ * SCALE_DEFAULT: Default scaling to convert raw adc code to voltage (uV).
+ * SCALE_THERM_100K_PULLUP: Returns temperature in millidegC.
+ *				 Uses a mapping table with 100K pullup.
+ * SCALE_PMIC_THERM: Returns result in milli degree's Centigrade.
+ * SCALE_XOTHERM: Returns XO thermistor voltage in millidegC.
+ * SCALE_PMI_CHG_TEMP: Conversion for PMI CHG temp
+ */
+enum vadc_scale_fn_type {
+	SCALE_DEFAULT = 0,
+	SCALE_THERM_100K_PULLUP,
+	SCALE_PMIC_THERM,
+	SCALE_XOTHERM,
+	SCALE_PMI_CHG_TEMP,
+};
+
+int qcom_vadc_scale(enum vadc_scale_fn_type scaletype,
+		    const struct vadc_linear_graph *calib_graph,
+		    const struct vadc_prescale_ratio *prescale,
+		    bool absolute,
+		    u16 adc_code, int *result_mdec);
+
+int qcom_vadc_decimation_from_dt(u32 value);
+
+#endif /* QCOM_VADC_COMMON_H */
diff --git a/drivers/iio/adc/rockchip_saradc.c b/drivers/iio/adc/rockchip_saradc.c
index 85d701291654..ae6d3324f518 100644
--- a/drivers/iio/adc/rockchip_saradc.c
+++ b/drivers/iio/adc/rockchip_saradc.c
@@ -109,7 +109,7 @@ static int rockchip_saradc_read_raw(struct iio_dev *indio_dev,
 
 static irqreturn_t rockchip_saradc_isr(int irq, void *dev_id)
 {
-	struct rockchip_saradc *info = (struct rockchip_saradc *)dev_id;
+	struct rockchip_saradc *info = dev_id;
 
 	/* Read value */
 	info->last_val = readl_relaxed(info->regs + SARADC_DATA);
diff --git a/drivers/staging/iio/adc/spear_adc.c b/drivers/iio/adc/spear_adc.c
index 5dd61f6a57b9..5dd61f6a57b9 100644
--- a/drivers/staging/iio/adc/spear_adc.c
+++ b/drivers/iio/adc/spear_adc.c
diff --git a/drivers/iio/adc/stm32-adc.c b/drivers/iio/adc/stm32-adc.c
index 9b49a6addc2a..c28e7ff80e11 100644
--- a/drivers/iio/adc/stm32-adc.c
+++ b/drivers/iio/adc/stm32-adc.c
@@ -60,6 +60,8 @@
 #define STM32F4_EOC			BIT(1)
 
 /* STM32F4_ADC_CR1 - bit fields */
+#define STM32F4_RES_SHIFT		24
+#define STM32F4_RES_MASK		GENMASK(25, 24)
 #define STM32F4_SCAN			BIT(8)
 #define STM32F4_EOCIE			BIT(5)
 
@@ -141,6 +143,7 @@ struct stm32_adc_regs {
  * @lock:		spinlock
  * @bufi:		data buffer index
  * @num_conv:		expected number of scan conversions
+ * @res:		data resolution (e.g. RES bitfield value)
  * @trigger_polarity:	external trigger polarity (e.g. exten)
  * @dma_chan:		dma channel
  * @rx_buf:		dma rx buffer cpu address
@@ -157,6 +160,7 @@ struct stm32_adc {
 	spinlock_t		lock;		/* interrupt lock */
 	unsigned int		bufi;
 	unsigned int		num_conv;
+	u32			res;
 	u32			trigger_polarity;
 	struct dma_chan		*dma_chan;
 	u8			*rx_buf;
@@ -196,6 +200,11 @@ static const struct stm32_adc_chan_spec stm32f4_adc123_channels[] = {
 	{ IIO_VOLTAGE, 15, "in15" },
 };
 
+static const unsigned int stm32f4_adc_resolutions[] = {
+	/* sorted values so the index matches RES[1:0] in STM32F4_ADC_CR1 */
+	12, 10, 8, 6,
+};
+
 /**
  * stm32f4_sq - describe regular sequence registers
  * - L: sequence len (register & bit field)
@@ -302,6 +311,14 @@ static void stm32_adc_conv_irq_disable(struct stm32_adc *adc)
 	stm32_adc_clr_bits(adc, STM32F4_ADC_CR1, STM32F4_EOCIE);
 }
 
+static void stm32_adc_set_res(struct stm32_adc *adc)
+{
+	u32 val = stm32_adc_readl(adc, STM32F4_ADC_CR1);
+
+	val = (val & ~STM32F4_RES_MASK) | (adc->res << STM32F4_RES_SHIFT);
+	stm32_adc_writel(adc, STM32F4_ADC_CR1, val);
+}
+
 /**
  * stm32_adc_start_conv() - Start conversions for regular channels.
  * @adc: stm32 adc instance
@@ -870,11 +887,37 @@ static const struct iio_chan_spec_ext_info stm32_adc_ext_info[] = {
 	{},
 };
 
+static int stm32_adc_of_get_resolution(struct iio_dev *indio_dev)
+{
+	struct device_node *node = indio_dev->dev.of_node;
+	struct stm32_adc *adc = iio_priv(indio_dev);
+	unsigned int i;
+	u32 res;
+
+	if (of_property_read_u32(node, "assigned-resolution-bits", &res))
+		res = stm32f4_adc_resolutions[0];
+
+	for (i = 0; i < ARRAY_SIZE(stm32f4_adc_resolutions); i++)
+		if (res == stm32f4_adc_resolutions[i])
+			break;
+	if (i >= ARRAY_SIZE(stm32f4_adc_resolutions)) {
+		dev_err(&indio_dev->dev, "Bad resolution: %u bits\n", res);
+		return -EINVAL;
+	}
+
+	dev_dbg(&indio_dev->dev, "Using %u bits resolution\n", res);
+	adc->res = i;
+
+	return 0;
+}
+
 static void stm32_adc_chan_init_one(struct iio_dev *indio_dev,
 				    struct iio_chan_spec *chan,
 				    const struct stm32_adc_chan_spec *channel,
 				    int scan_index)
 {
+	struct stm32_adc *adc = iio_priv(indio_dev);
+
 	chan->type = channel->type;
 	chan->channel = channel->channel;
 	chan->datasheet_name = channel->name;
@@ -883,7 +926,7 @@ static void stm32_adc_chan_init_one(struct iio_dev *indio_dev,
 	chan->info_mask_separate = BIT(IIO_CHAN_INFO_RAW);
 	chan->info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE);
 	chan->scan_type.sign = 'u';
-	chan->scan_type.realbits = 12;
+	chan->scan_type.realbits = stm32f4_adc_resolutions[adc->res];
 	chan->scan_type.storagebits = 16;
 	chan->ext_info = stm32_adc_ext_info;
 }
@@ -1022,6 +1065,11 @@ static int stm32_adc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	ret = stm32_adc_of_get_resolution(indio_dev);
+	if (ret < 0)
+		goto err_clk_disable;
+	stm32_adc_set_res(adc);
+
 	ret = stm32_adc_chan_of_init(indio_dev);
 	if (ret < 0)
 		goto err_clk_disable;
diff --git a/drivers/iio/adc/stx104.c b/drivers/iio/adc/stx104.c
index be2de48844bc..2df84fa5e3fc 100644
--- a/drivers/iio/adc/stx104.c
+++ b/drivers/iio/adc/stx104.c
@@ -318,6 +318,7 @@ static int stx104_probe(struct device *dev, unsigned int id)
 	}
 
 	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
 
 	priv = iio_priv(indio_dev);
 	priv->base = base[id];
diff --git a/drivers/iio/adc/sun4i-gpadc-iio.c b/drivers/iio/adc/sun4i-gpadc-iio.c
new file mode 100644
index 000000000000..b23527309088
--- /dev/null
+++ b/drivers/iio/adc/sun4i-gpadc-iio.c
@@ -0,0 +1,719 @@
+/* ADC driver for sunxi platforms' (A10, A13 and A31) GPADC
+ *
+ * Copyright (c) 2016 Quentin Schulz <quentin.schulz@free-electrons.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.
+ *
+ * The Allwinner SoCs all have an ADC that can also act as a touchscreen
+ * controller and a thermal sensor.
+ * The thermal sensor works only when the ADC acts as a touchscreen controller
+ * and is configured to throw an interrupt every fixed periods of time (let say
+ * every X seconds).
+ * One would be tempted to disable the IP on the hardware side rather than
+ * disabling interrupts to save some power but that resets the internal clock of
+ * the IP, resulting in having to wait X seconds every time we want to read the
+ * value of the thermal sensor.
+ * This is also the reason of using autosuspend in pm_runtime. If there was no
+ * autosuspend, the thermal sensor would need X seconds after every
+ * pm_runtime_get_sync to get a value from the ADC. The autosuspend allows the
+ * thermal sensor to be requested again in a certain time span before it gets
+ * shutdown for not being used.
+ */
+
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.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/thermal.h>
+#include <linux/delay.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/machine.h>
+#include <linux/mfd/sun4i-gpadc.h>
+
+static unsigned int sun4i_gpadc_chan_select(unsigned int chan)
+{
+	return SUN4I_GPADC_CTRL1_ADC_CHAN_SELECT(chan);
+}
+
+static unsigned int sun6i_gpadc_chan_select(unsigned int chan)
+{
+	return SUN6I_GPADC_CTRL1_ADC_CHAN_SELECT(chan);
+}
+
+struct gpadc_data {
+	int		temp_offset;
+	int		temp_scale;
+	unsigned int	tp_mode_en;
+	unsigned int	tp_adc_select;
+	unsigned int	(*adc_chan_select)(unsigned int chan);
+	unsigned int	adc_chan_mask;
+};
+
+static const struct gpadc_data sun4i_gpadc_data = {
+	.temp_offset = -1932,
+	.temp_scale = 133,
+	.tp_mode_en = SUN4I_GPADC_CTRL1_TP_MODE_EN,
+	.tp_adc_select = SUN4I_GPADC_CTRL1_TP_ADC_SELECT,
+	.adc_chan_select = &sun4i_gpadc_chan_select,
+	.adc_chan_mask = SUN4I_GPADC_CTRL1_ADC_CHAN_MASK,
+};
+
+static const struct gpadc_data sun5i_gpadc_data = {
+	.temp_offset = -1447,
+	.temp_scale = 100,
+	.tp_mode_en = SUN4I_GPADC_CTRL1_TP_MODE_EN,
+	.tp_adc_select = SUN4I_GPADC_CTRL1_TP_ADC_SELECT,
+	.adc_chan_select = &sun4i_gpadc_chan_select,
+	.adc_chan_mask = SUN4I_GPADC_CTRL1_ADC_CHAN_MASK,
+};
+
+static const struct gpadc_data sun6i_gpadc_data = {
+	.temp_offset = -1623,
+	.temp_scale = 167,
+	.tp_mode_en = SUN6I_GPADC_CTRL1_TP_MODE_EN,
+	.tp_adc_select = SUN6I_GPADC_CTRL1_TP_ADC_SELECT,
+	.adc_chan_select = &sun6i_gpadc_chan_select,
+	.adc_chan_mask = SUN6I_GPADC_CTRL1_ADC_CHAN_MASK,
+};
+
+static const struct gpadc_data sun8i_a33_gpadc_data = {
+	.temp_offset = -1662,
+	.temp_scale = 162,
+	.tp_mode_en = SUN8I_GPADC_CTRL1_CHOP_TEMP_EN,
+};
+
+struct sun4i_gpadc_iio {
+	struct iio_dev			*indio_dev;
+	struct completion		completion;
+	int				temp_data;
+	u32				adc_data;
+	struct regmap			*regmap;
+	unsigned int			fifo_data_irq;
+	atomic_t			ignore_fifo_data_irq;
+	unsigned int			temp_data_irq;
+	atomic_t			ignore_temp_data_irq;
+	const struct gpadc_data		*data;
+	bool				no_irq;
+	/* prevents concurrent reads of temperature and ADC */
+	struct mutex			mutex;
+};
+
+#define SUN4I_GPADC_ADC_CHANNEL(_channel, _name) {		\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = _channel,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.datasheet_name = _name,				\
+}
+
+static struct iio_map sun4i_gpadc_hwmon_maps[] = {
+	{
+		.adc_channel_label = "temp_adc",
+		.consumer_dev_name = "iio_hwmon.0",
+	},
+	{ /* sentinel */ },
+};
+
+static const struct iio_chan_spec sun4i_gpadc_channels[] = {
+	SUN4I_GPADC_ADC_CHANNEL(0, "adc_chan0"),
+	SUN4I_GPADC_ADC_CHANNEL(1, "adc_chan1"),
+	SUN4I_GPADC_ADC_CHANNEL(2, "adc_chan2"),
+	SUN4I_GPADC_ADC_CHANNEL(3, "adc_chan3"),
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE) |
+				      BIT(IIO_CHAN_INFO_OFFSET),
+		.datasheet_name = "temp_adc",
+	},
+};
+
+static const struct iio_chan_spec sun4i_gpadc_channels_no_temp[] = {
+	SUN4I_GPADC_ADC_CHANNEL(0, "adc_chan0"),
+	SUN4I_GPADC_ADC_CHANNEL(1, "adc_chan1"),
+	SUN4I_GPADC_ADC_CHANNEL(2, "adc_chan2"),
+	SUN4I_GPADC_ADC_CHANNEL(3, "adc_chan3"),
+};
+
+static const struct iio_chan_spec sun8i_a33_gpadc_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE) |
+				      BIT(IIO_CHAN_INFO_OFFSET),
+		.datasheet_name = "temp_adc",
+	},
+};
+
+static const struct regmap_config sun4i_gpadc_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+	.fast_io = true,
+};
+
+static int sun4i_prepare_for_irq(struct iio_dev *indio_dev, int channel,
+				 unsigned int irq)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+	int ret;
+	u32 reg;
+
+	pm_runtime_get_sync(indio_dev->dev.parent);
+
+	reinit_completion(&info->completion);
+
+	ret = regmap_write(info->regmap, SUN4I_GPADC_INT_FIFOC,
+			   SUN4I_GPADC_INT_FIFOC_TP_FIFO_TRIG_LEVEL(1) |
+			   SUN4I_GPADC_INT_FIFOC_TP_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	ret = regmap_read(info->regmap, SUN4I_GPADC_CTRL1, &reg);
+	if (ret)
+		return ret;
+
+	if (irq == info->fifo_data_irq) {
+		ret = regmap_write(info->regmap, SUN4I_GPADC_CTRL1,
+				   info->data->tp_mode_en |
+				   info->data->tp_adc_select |
+				   info->data->adc_chan_select(channel));
+		/*
+		 * When the IP changes channel, it needs a bit of time to get
+		 * correct values.
+		 */
+		if ((reg & info->data->adc_chan_mask) !=
+			 info->data->adc_chan_select(channel))
+			mdelay(10);
+
+	} else {
+		/*
+		 * The temperature sensor returns valid data only when the ADC
+		 * operates in touchscreen mode.
+		 */
+		ret = regmap_write(info->regmap, SUN4I_GPADC_CTRL1,
+				   info->data->tp_mode_en);
+	}
+
+	if (ret)
+		return ret;
+
+	/*
+	 * When the IP changes mode between ADC or touchscreen, it
+	 * needs a bit of time to get correct values.
+	 */
+	if ((reg & info->data->tp_adc_select) != info->data->tp_adc_select)
+		mdelay(100);
+
+	return 0;
+}
+
+static int sun4i_gpadc_read(struct iio_dev *indio_dev, int channel, int *val,
+			    unsigned int irq)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&info->mutex);
+
+	ret = sun4i_prepare_for_irq(indio_dev, channel, irq);
+	if (ret)
+		goto err;
+
+	enable_irq(irq);
+
+	/*
+	 * The temperature sensor throws an interruption periodically (currently
+	 * set at periods of ~0.6s in sun4i_gpadc_runtime_resume). A 1s delay
+	 * makes sure an interruption occurs in normal conditions. If it doesn't
+	 * occur, then there is a timeout.
+	 */
+	if (!wait_for_completion_timeout(&info->completion,
+					 msecs_to_jiffies(1000))) {
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	if (irq == info->fifo_data_irq)
+		*val = info->adc_data;
+	else
+		*val = info->temp_data;
+
+	ret = 0;
+	pm_runtime_mark_last_busy(indio_dev->dev.parent);
+
+err:
+	pm_runtime_put_autosuspend(indio_dev->dev.parent);
+	mutex_unlock(&info->mutex);
+
+	return ret;
+}
+
+static int sun4i_gpadc_adc_read(struct iio_dev *indio_dev, int channel,
+				int *val)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+
+	return sun4i_gpadc_read(indio_dev, channel, val, info->fifo_data_irq);
+}
+
+static int sun4i_gpadc_temp_read(struct iio_dev *indio_dev, int *val)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+
+	if (info->no_irq) {
+		pm_runtime_get_sync(indio_dev->dev.parent);
+
+		regmap_read(info->regmap, SUN4I_GPADC_TEMP_DATA, val);
+
+		pm_runtime_mark_last_busy(indio_dev->dev.parent);
+		pm_runtime_put_autosuspend(indio_dev->dev.parent);
+
+		return 0;
+	}
+
+	return sun4i_gpadc_read(indio_dev, 0, val, info->temp_data_irq);
+}
+
+static int sun4i_gpadc_temp_offset(struct iio_dev *indio_dev, int *val)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+
+	*val = info->data->temp_offset;
+
+	return 0;
+}
+
+static int sun4i_gpadc_temp_scale(struct iio_dev *indio_dev, int *val)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+
+	*val = info->data->temp_scale;
+
+	return 0;
+}
+
+static int sun4i_gpadc_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan, int *val,
+				int *val2, long mask)
+{
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_OFFSET:
+		ret = sun4i_gpadc_temp_offset(indio_dev, val);
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type == IIO_VOLTAGE)
+			ret = sun4i_gpadc_adc_read(indio_dev, chan->channel,
+						   val);
+		else
+			ret = sun4i_gpadc_temp_read(indio_dev, val);
+
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		if (chan->type == IIO_VOLTAGE) {
+			/* 3000mV / 4096 * raw */
+			*val = 0;
+			*val2 = 732421875;
+			return IIO_VAL_INT_PLUS_NANO;
+		}
+
+		ret = sun4i_gpadc_temp_scale(indio_dev, val);
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info sun4i_gpadc_iio_info = {
+	.read_raw = sun4i_gpadc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static irqreturn_t sun4i_gpadc_temp_data_irq_handler(int irq, void *dev_id)
+{
+	struct sun4i_gpadc_iio *info = dev_id;
+
+	if (atomic_read(&info->ignore_temp_data_irq))
+		goto out;
+
+	if (!regmap_read(info->regmap, SUN4I_GPADC_TEMP_DATA, &info->temp_data))
+		complete(&info->completion);
+
+out:
+	disable_irq_nosync(info->temp_data_irq);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t sun4i_gpadc_fifo_data_irq_handler(int irq, void *dev_id)
+{
+	struct sun4i_gpadc_iio *info = dev_id;
+
+	if (atomic_read(&info->ignore_fifo_data_irq))
+		goto out;
+
+	if (!regmap_read(info->regmap, SUN4I_GPADC_DATA, &info->adc_data))
+		complete(&info->completion);
+
+out:
+	disable_irq_nosync(info->fifo_data_irq);
+	return IRQ_HANDLED;
+}
+
+static int sun4i_gpadc_runtime_suspend(struct device *dev)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(dev_get_drvdata(dev));
+
+	/* Disable the ADC on IP */
+	regmap_write(info->regmap, SUN4I_GPADC_CTRL1, 0);
+	/* Disable temperature sensor on IP */
+	regmap_write(info->regmap, SUN4I_GPADC_TPR, 0);
+
+	return 0;
+}
+
+static int sun4i_gpadc_runtime_resume(struct device *dev)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(dev_get_drvdata(dev));
+
+	/* clkin = 6MHz */
+	regmap_write(info->regmap, SUN4I_GPADC_CTRL0,
+		     SUN4I_GPADC_CTRL0_ADC_CLK_DIVIDER(2) |
+		     SUN4I_GPADC_CTRL0_FS_DIV(7) |
+		     SUN4I_GPADC_CTRL0_T_ACQ(63));
+	regmap_write(info->regmap, SUN4I_GPADC_CTRL1, info->data->tp_mode_en);
+	regmap_write(info->regmap, SUN4I_GPADC_CTRL3,
+		     SUN4I_GPADC_CTRL3_FILTER_EN |
+		     SUN4I_GPADC_CTRL3_FILTER_TYPE(1));
+	/* period = SUN4I_GPADC_TPR_TEMP_PERIOD * 256 * 16 / clkin; ~0.6s */
+	regmap_write(info->regmap, SUN4I_GPADC_TPR,
+		     SUN4I_GPADC_TPR_TEMP_ENABLE |
+		     SUN4I_GPADC_TPR_TEMP_PERIOD(800));
+
+	return 0;
+}
+
+static int sun4i_gpadc_get_temp(void *data, int *temp)
+{
+	struct sun4i_gpadc_iio *info = data;
+	int val, scale, offset;
+
+	if (sun4i_gpadc_temp_read(info->indio_dev, &val))
+		return -ETIMEDOUT;
+
+	sun4i_gpadc_temp_scale(info->indio_dev, &scale);
+	sun4i_gpadc_temp_offset(info->indio_dev, &offset);
+
+	*temp = (val + offset) * scale;
+
+	return 0;
+}
+
+static const struct thermal_zone_of_device_ops sun4i_ts_tz_ops = {
+	.get_temp = &sun4i_gpadc_get_temp,
+};
+
+static const struct dev_pm_ops sun4i_gpadc_pm_ops = {
+	.runtime_suspend = &sun4i_gpadc_runtime_suspend,
+	.runtime_resume = &sun4i_gpadc_runtime_resume,
+};
+
+static int sun4i_irq_init(struct platform_device *pdev, const char *name,
+			  irq_handler_t handler, const char *devname,
+			  unsigned int *irq, atomic_t *atomic)
+{
+	int ret;
+	struct sun4i_gpadc_dev *mfd_dev = dev_get_drvdata(pdev->dev.parent);
+	struct sun4i_gpadc_iio *info = iio_priv(dev_get_drvdata(&pdev->dev));
+
+	/*
+	 * Once the interrupt is activated, the IP continuously performs
+	 * conversions thus throws interrupts. The interrupt is activated right
+	 * after being requested but we want to control when these interrupts
+	 * occur thus we disable it right after being requested. However, an
+	 * interrupt might occur between these two instructions and we have to
+	 * make sure that does not happen, by using atomic flags. We set the
+	 * flag before requesting the interrupt and unset it right after
+	 * disabling the interrupt. When an interrupt occurs between these two
+	 * instructions, reading the atomic flag will tell us to ignore the
+	 * interrupt.
+	 */
+	atomic_set(atomic, 1);
+
+	ret = platform_get_irq_byname(pdev, name);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "no %s interrupt registered\n", name);
+		return ret;
+	}
+
+	ret = regmap_irq_get_virq(mfd_dev->regmap_irqc, ret);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to get virq for irq %s\n", name);
+		return ret;
+	}
+
+	*irq = ret;
+	ret = devm_request_any_context_irq(&pdev->dev, *irq, handler, 0,
+					   devname, info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "could not request %s interrupt: %d\n",
+			name, ret);
+		return ret;
+	}
+
+	disable_irq(*irq);
+	atomic_set(atomic, 0);
+
+	return 0;
+}
+
+static const struct of_device_id sun4i_gpadc_of_id[] = {
+	{
+		.compatible = "allwinner,sun8i-a33-ths",
+		.data = &sun8i_a33_gpadc_data,
+	},
+	{ /* sentinel */ }
+};
+
+static int sun4i_gpadc_probe_dt(struct platform_device *pdev,
+				struct iio_dev *indio_dev)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+	const struct of_device_id *of_dev;
+	struct thermal_zone_device *tzd;
+	struct resource *mem;
+	void __iomem *base;
+	int ret;
+
+	of_dev = of_match_device(sun4i_gpadc_of_id, &pdev->dev);
+	if (!of_dev)
+		return -ENODEV;
+
+	info->no_irq = true;
+	info->data = (struct gpadc_data *)of_dev->data;
+	indio_dev->num_channels = ARRAY_SIZE(sun8i_a33_gpadc_channels);
+	indio_dev->channels = sun8i_a33_gpadc_channels;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	info->regmap = devm_regmap_init_mmio(&pdev->dev, base,
+					     &sun4i_gpadc_regmap_config);
+	if (IS_ERR(info->regmap)) {
+		ret = PTR_ERR(info->regmap);
+		dev_err(&pdev->dev, "failed to init regmap: %d\n", ret);
+		return ret;
+	}
+
+	if (!IS_ENABLED(CONFIG_THERMAL_OF))
+		return 0;
+
+	tzd = devm_thermal_zone_of_sensor_register(&pdev->dev, 0, info,
+						   &sun4i_ts_tz_ops);
+	if (IS_ERR(tzd))
+		dev_err(&pdev->dev, "could not register thermal sensor: %ld\n",
+			PTR_ERR(tzd));
+
+	return PTR_ERR_OR_ZERO(tzd);
+}
+
+static int sun4i_gpadc_probe_mfd(struct platform_device *pdev,
+				 struct iio_dev *indio_dev)
+{
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+	struct sun4i_gpadc_dev *sun4i_gpadc_dev =
+		dev_get_drvdata(pdev->dev.parent);
+	int ret;
+
+	info->no_irq = false;
+	info->regmap = sun4i_gpadc_dev->regmap;
+
+	indio_dev->num_channels = ARRAY_SIZE(sun4i_gpadc_channels);
+	indio_dev->channels = sun4i_gpadc_channels;
+
+	info->data = (struct gpadc_data *)platform_get_device_id(pdev)->driver_data;
+
+	/*
+	 * Since the controller needs to be in touchscreen mode for its thermal
+	 * sensor to operate properly, and that switching between the two modes
+	 * needs a delay, always registering in the thermal framework will
+	 * significantly slow down the conversion rate of the ADCs.
+	 *
+	 * Therefore, instead of depending on THERMAL_OF in Kconfig, we only
+	 * register the sensor if that option is enabled, eventually leaving
+	 * that choice to the user.
+	 */
+
+	if (IS_ENABLED(CONFIG_THERMAL_OF)) {
+		/*
+		 * This driver is a child of an MFD which has a node in the DT
+		 * but not its children, because of DT backward compatibility
+		 * for A10, A13 and A31 SoCs. Therefore, the resulting devices
+		 * of this driver do not have an of_node variable.
+		 * However, its parent (the MFD driver) has an of_node variable
+		 * and since devm_thermal_zone_of_sensor_register uses its first
+		 * argument to match the phandle defined in the node of the
+		 * thermal driver with the of_node of the device passed as first
+		 * argument and the third argument to call ops from
+		 * thermal_zone_of_device_ops, the solution is to use the parent
+		 * device as first argument to match the phandle with its
+		 * of_node, and the device from this driver as third argument to
+		 * return the temperature.
+		 */
+		struct thermal_zone_device *tzd;
+		tzd = devm_thermal_zone_of_sensor_register(pdev->dev.parent, 0,
+							   info,
+							   &sun4i_ts_tz_ops);
+		if (IS_ERR(tzd)) {
+			dev_err(&pdev->dev,
+				"could not register thermal sensor: %ld\n",
+				PTR_ERR(tzd));
+			return PTR_ERR(tzd);
+		}
+	} else {
+		indio_dev->num_channels =
+			ARRAY_SIZE(sun4i_gpadc_channels_no_temp);
+		indio_dev->channels = sun4i_gpadc_channels_no_temp;
+	}
+
+	if (IS_ENABLED(CONFIG_THERMAL_OF)) {
+		ret = sun4i_irq_init(pdev, "TEMP_DATA_PENDING",
+				     sun4i_gpadc_temp_data_irq_handler,
+				     "temp_data", &info->temp_data_irq,
+				     &info->ignore_temp_data_irq);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = sun4i_irq_init(pdev, "FIFO_DATA_PENDING",
+			     sun4i_gpadc_fifo_data_irq_handler, "fifo_data",
+			     &info->fifo_data_irq, &info->ignore_fifo_data_irq);
+	if (ret < 0)
+		return ret;
+
+	if (IS_ENABLED(CONFIG_THERMAL_OF)) {
+		ret = iio_map_array_register(indio_dev, sun4i_gpadc_hwmon_maps);
+		if (ret < 0) {
+			dev_err(&pdev->dev,
+				"failed to register iio map array\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int sun4i_gpadc_probe(struct platform_device *pdev)
+{
+	struct sun4i_gpadc_iio *info;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*info));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	info = iio_priv(indio_dev);
+	platform_set_drvdata(pdev, indio_dev);
+
+	mutex_init(&info->mutex);
+	info->indio_dev = indio_dev;
+	init_completion(&info->completion);
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->info = &sun4i_gpadc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	if (pdev->dev.of_node)
+		ret = sun4i_gpadc_probe_dt(pdev, indio_dev);
+	else
+		ret = sun4i_gpadc_probe_mfd(pdev, indio_dev);
+
+	if (ret)
+		return ret;
+
+	pm_runtime_set_autosuspend_delay(&pdev->dev,
+					 SUN4I_GPADC_AUTOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = devm_iio_device_register(&pdev->dev, indio_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "could not register the device\n");
+		goto err_map;
+	}
+
+	return 0;
+
+err_map:
+	if (!info->no_irq && IS_ENABLED(CONFIG_THERMAL_OF))
+		iio_map_array_unregister(indio_dev);
+
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return ret;
+}
+
+static int sun4i_gpadc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
+
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	if (!info->no_irq && IS_ENABLED(CONFIG_THERMAL_OF))
+		iio_map_array_unregister(indio_dev);
+
+	return 0;
+}
+
+static const struct platform_device_id sun4i_gpadc_id[] = {
+	{ "sun4i-a10-gpadc-iio", (kernel_ulong_t)&sun4i_gpadc_data },
+	{ "sun5i-a13-gpadc-iio", (kernel_ulong_t)&sun5i_gpadc_data },
+	{ "sun6i-a31-gpadc-iio", (kernel_ulong_t)&sun6i_gpadc_data },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver sun4i_gpadc_driver = {
+	.driver = {
+		.name = "sun4i-gpadc-iio",
+		.of_match_table = sun4i_gpadc_of_id,
+		.pm = &sun4i_gpadc_pm_ops,
+	},
+	.id_table = sun4i_gpadc_id,
+	.probe = sun4i_gpadc_probe,
+	.remove = sun4i_gpadc_remove,
+};
+
+module_platform_driver(sun4i_gpadc_driver);
+
+MODULE_DESCRIPTION("ADC driver for sunxi platforms");
+MODULE_AUTHOR("Quentin Schulz <quentin.schulz@free-electrons.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-ads1015.c b/drivers/iio/adc/ti-ads1015.c
index 422b314f5a3f..f76d979fb7e8 100644
--- a/drivers/iio/adc/ti-ads1015.c
+++ b/drivers/iio/adc/ti-ads1015.c
@@ -15,6 +15,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/regmap.h>
@@ -55,7 +56,7 @@
 #define ADS1015_DEFAULT_DATA_RATE	4
 #define ADS1015_DEFAULT_CHAN		0
 
-enum {
+enum chip_ids {
 	ADS1015,
 	ADS1115,
 };
@@ -578,6 +579,7 @@ static int ads1015_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	struct ads1015_data *data;
 	int ret;
+	enum chip_ids chip;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
@@ -593,7 +595,11 @@ static int ads1015_probe(struct i2c_client *client,
 	indio_dev->name = ADS1015_DRV_NAME;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	switch (id->driver_data) {
+	if (client->dev.of_node)
+		chip = (enum chip_ids)of_device_get_match_data(&client->dev);
+	else
+		chip = id->driver_data;
+	switch (chip) {
 	case ADS1015:
 		indio_dev->channels = ads1015_channels;
 		indio_dev->num_channels = ARRAY_SIZE(ads1015_channels);
@@ -698,9 +704,23 @@ static const struct i2c_device_id ads1015_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ads1015_id);
 
+static const struct of_device_id ads1015_of_match[] = {
+	{
+		.compatible = "ti,ads1015",
+		.data = (void *)ADS1015
+	},
+	{
+		.compatible = "ti,ads1115",
+		.data = (void *)ADS1115
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, ads1015_of_match);
+
 static struct i2c_driver ads1015_driver = {
 	.driver = {
 		.name = ADS1015_DRV_NAME,
+		.of_match_table = ads1015_of_match,
 		.pm = &ads1015_pm_ops,
 	},
 	.probe		= ads1015_probe,
diff --git a/drivers/iio/adc/vf610_adc.c b/drivers/iio/adc/vf610_adc.c
index 228a003adeed..01fc76f7d660 100644
--- a/drivers/iio/adc/vf610_adc.c
+++ b/drivers/iio/adc/vf610_adc.c
@@ -584,7 +584,7 @@ static int vf610_adc_read_data(struct vf610_adc *info)
 
 static irqreturn_t vf610_adc_isr(int irq, void *dev_id)
 {
-	struct iio_dev *indio_dev = (struct iio_dev *)dev_id;
+	struct iio_dev *indio_dev = dev_id;
 	struct vf610_adc *info = iio_priv(indio_dev);
 	int coco;
 
diff --git a/drivers/iio/chemical/ams-iaq-core.c b/drivers/iio/chemical/ams-iaq-core.c
index 41a8e6f2e31d..c948ad2ee9ad 100644
--- a/drivers/iio/chemical/ams-iaq-core.c
+++ b/drivers/iio/chemical/ams-iaq-core.c
@@ -163,7 +163,7 @@ static int ams_iaqcore_probe(struct i2c_client *client,
 	mutex_init(&data->lock);
 
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->info = &ams_iaqcore_info,
+	indio_dev->info = &ams_iaqcore_info;
 	indio_dev->name = dev_name(&client->dev);
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
diff --git a/drivers/iio/chemical/vz89x.c b/drivers/iio/chemical/vz89x.c
index 8e0e4415c161..f75eea6822f2 100644
--- a/drivers/iio/chemical/vz89x.c
+++ b/drivers/iio/chemical/vz89x.c
@@ -393,7 +393,7 @@ static int vz89x_probe(struct i2c_client *client,
 	mutex_init(&data->lock);
 
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->info = &vz89x_info,
+	indio_dev->info = &vz89x_info;
 	indio_dev->name = dev_name(&client->dev);
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c
index c17596f7ed2c..38e8783e4b05 100644
--- a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c
+++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors.c
@@ -267,31 +267,12 @@ static int cros_ec_sensors_probe(struct platform_device *pdev)
 	else
 		state->core.read_ec_sensors_data = cros_ec_sensors_read_cmd;
 
-	ret = iio_triggered_buffer_setup(indio_dev, NULL,
-					 cros_ec_sensors_capture, NULL);
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+			cros_ec_sensors_capture, NULL);
 	if (ret)
 		return ret;
 
-	ret = iio_device_register(indio_dev);
-	if (ret)
-		goto error_uninit_buffer;
-
-	return 0;
-
-error_uninit_buffer:
-	iio_triggered_buffer_cleanup(indio_dev);
-
-	return ret;
-}
-
-static int cros_ec_sensors_remove(struct platform_device *pdev)
-{
-	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
-
-	iio_device_unregister(indio_dev);
-	iio_triggered_buffer_cleanup(indio_dev);
-
-	return 0;
+	return devm_iio_device_register(dev, indio_dev);
 }
 
 static const struct platform_device_id cros_ec_sensors_ids[] = {
@@ -313,7 +294,6 @@ static struct platform_driver cros_ec_sensors_platform_driver = {
 		.name	= "cros-ec-sensors",
 	},
 	.probe		= cros_ec_sensors_probe,
-	.remove		= cros_ec_sensors_remove,
 	.id_table	= cros_ec_sensors_ids,
 };
 module_platform_driver(cros_ec_sensors_platform_driver);
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
index 01e02b9926d4..1c0874cdf665 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-attributes.c
@@ -38,6 +38,12 @@ static struct {
 	{HID_USAGE_SENSOR_ACCEL_3D,
 		HID_USAGE_SENSOR_UNITS_G, 9, 806650000},
 
+	{HID_USAGE_SENSOR_GRAVITY_VECTOR, 0, 9, 806650000},
+	{HID_USAGE_SENSOR_GRAVITY_VECTOR,
+		HID_USAGE_SENSOR_UNITS_METERS_PER_SEC_SQRD, 1, 0},
+	{HID_USAGE_SENSOR_GRAVITY_VECTOR,
+		HID_USAGE_SENSOR_UNITS_G, 9, 806650000},
+
 	{HID_USAGE_SENSOR_GYRO_3D, 0, 0, 17453293},
 	{HID_USAGE_SENSOR_GYRO_3D,
 		HID_USAGE_SENSOR_UNITS_RADIANS_PER_SECOND, 1, 0},
@@ -62,6 +68,11 @@ static struct {
 	{HID_USAGE_SENSOR_TIME_TIMESTAMP, 0, 1000000000, 0},
 	{HID_USAGE_SENSOR_TIME_TIMESTAMP, HID_USAGE_SENSOR_UNITS_MILLISECOND,
 		1000000, 0},
+
+	{HID_USAGE_SENSOR_TEMPERATURE, 0, 1000, 0},
+	{HID_USAGE_SENSOR_TEMPERATURE, HID_USAGE_SENSOR_UNITS_DEGREES, 1000, 0},
+
+	{HID_USAGE_SENSOR_HUMIDITY, 0, 1000, 0},
 };
 
 static int pow_10(unsigned power)
@@ -221,7 +232,15 @@ int hid_sensor_write_samp_freq_value(struct hid_sensor_common *st,
 	if (ret < 0 || value < 0)
 		ret = -EINVAL;
 
-	return ret;
+	ret = sensor_hub_get_feature(st->hsdev,
+				     st->poll.report_id,
+				     st->poll.index, sizeof(value), &value);
+	if (ret < 0 || value < 0)
+		return -EINVAL;
+
+	st->poll_interval = value;
+
+	return 0;
 }
 EXPORT_SYMBOL(hid_sensor_write_samp_freq_value);
 
@@ -266,7 +285,16 @@ int hid_sensor_write_raw_hyst_value(struct hid_sensor_common *st,
 	if (ret < 0 || value < 0)
 		ret = -EINVAL;
 
-	return ret;
+	ret = sensor_hub_get_feature(st->hsdev,
+				     st->sensitivity.report_id,
+				     st->sensitivity.index, sizeof(value),
+				     &value);
+	if (ret < 0 || value < 0)
+		return -EINVAL;
+
+	st->raw_hystersis = value;
+
+	return 0;
 }
 EXPORT_SYMBOL(hid_sensor_write_raw_hyst_value);
 
@@ -369,6 +397,9 @@ int hid_sensor_get_reporting_interval(struct hid_sensor_hub_device *hsdev,
 	/* Default unit of measure is milliseconds */
 	if (st->poll.units == 0)
 		st->poll.units = HID_USAGE_SENSOR_UNITS_MILLISECOND;
+
+	st->poll_interval = -1;
+
 	return 0;
 
 }
@@ -399,6 +430,8 @@ int hid_sensor_parse_common_attributes(struct hid_sensor_hub_device *hsdev,
 			HID_USAGE_SENSOR_PROP_SENSITIVITY_ABS,
 			 &st->sensitivity);
 
+	st->raw_hystersis = -1;
+
 	sensor_hub_input_get_attribute_info(hsdev,
 					    HID_INPUT_REPORT, usage_id,
 					    HID_USAGE_SENSOR_TIME_TIMESTAMP,
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index ecf592d69043..0b5dea050239 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -51,6 +51,8 @@ static int _hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 			st->report_state.report_id,
 			st->report_state.index,
 			HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM);
+
+		poll_value = hid_sensor_read_poll_value(st);
 	} else {
 		int val;
 
@@ -87,9 +89,7 @@ static int _hid_sensor_power_state(struct hid_sensor_common *st, bool state)
 	sensor_hub_get_feature(st->hsdev, st->power_state.report_id,
 			       st->power_state.index,
 			       sizeof(state_val), &state_val);
-	if (state)
-		poll_value = hid_sensor_read_poll_value(st);
-	if (poll_value > 0)
+	if (state && poll_value)
 		msleep_interruptible(poll_value * 2);
 
 	return 0;
@@ -127,6 +127,20 @@ static void hid_sensor_set_power_work(struct work_struct *work)
 	struct hid_sensor_common *attrb = container_of(work,
 						       struct hid_sensor_common,
 						       work);
+
+	if (attrb->poll_interval >= 0)
+		sensor_hub_set_feature(attrb->hsdev, attrb->poll.report_id,
+				       attrb->poll.index,
+				       sizeof(attrb->poll_interval),
+				       &attrb->poll_interval);
+
+	if (attrb->raw_hystersis >= 0)
+		sensor_hub_set_feature(attrb->hsdev,
+				       attrb->sensitivity.report_id,
+				       attrb->sensitivity.index,
+				       sizeof(attrb->raw_hystersis),
+				       &attrb->raw_hystersis);
+
 	_hid_sensor_power_state(attrb, true);
 }
 
@@ -138,6 +152,10 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 
 void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 {
+	pm_runtime_disable(&attrb->pdev->dev);
+	pm_runtime_set_suspended(&attrb->pdev->dev);
+	pm_runtime_put_noidle(&attrb->pdev->dev);
+
 	cancel_work_sync(&attrb->work);
 	iio_trigger_unregister(attrb->trigger);
 	iio_trigger_free(attrb->trigger);
diff --git a/drivers/iio/common/ms_sensors/ms_sensors_i2c.c b/drivers/iio/common/ms_sensors/ms_sensors_i2c.c
index ecf7721ecaf4..125b5ff61d19 100644
--- a/drivers/iio/common/ms_sensors/ms_sensors_i2c.c
+++ b/drivers/iio/common/ms_sensors/ms_sensors_i2c.c
@@ -74,7 +74,7 @@ EXPORT_SYMBOL(ms_sensors_reset);
 int ms_sensors_read_prom_word(void *cli, int cmd, u16 *word)
 {
 	int ret;
-	struct i2c_client *client = (struct i2c_client *)cli;
+	struct i2c_client *client = cli;
 
 	ret = i2c_smbus_read_word_swapped(client, cmd);
 	if (ret < 0) {
@@ -107,7 +107,7 @@ int ms_sensors_convert_and_read(void *cli, u8 conv, u8 rd,
 {
 	int ret;
 	__be32 buf = 0;
-	struct i2c_client *client = (struct i2c_client *)cli;
+	struct i2c_client *client = cli;
 
 	/* Trigger conversion */
 	ret = i2c_smbus_write_byte(client, conv);
diff --git a/drivers/iio/counter/104-quad-8.c b/drivers/iio/counter/104-quad-8.c
index f9b8fc9ae13f..ba3d9030cd51 100644
--- a/drivers/iio/counter/104-quad-8.c
+++ b/drivers/iio/counter/104-quad-8.c
@@ -551,6 +551,7 @@ static int quad8_probe(struct device *dev, unsigned int id)
 	indio_dev->num_channels = ARRAY_SIZE(quad8_channels);
 	indio_dev->channels = quad8_channels;
 	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
 
 	priv = iio_priv(indio_dev);
 	priv->base = base[id];
diff --git a/drivers/iio/counter/Kconfig b/drivers/iio/counter/Kconfig
index 44627f6e4861..b37e5fc03149 100644
--- a/drivers/iio/counter/Kconfig
+++ b/drivers/iio/counter/Kconfig
@@ -7,7 +7,7 @@ menu "Counters"
 
 config 104_QUAD_8
 	tristate "ACCES 104-QUAD-8 driver"
-	depends on X86 && ISA_BUS_API
+	depends on PC104 && X86 && ISA_BUS_API
 	help
 	  Say yes here to build support for the ACCES 104-QUAD-8 quadrature
 	  encoder counter/interface device family (104-QUAD-8, 104-QUAD-4).
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index d3084028905b..df5abc46cd3f 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -118,6 +118,16 @@ config AD5624R_SPI
 	  Say yes here to build support for Analog Devices AD5624R, AD5644R and
 	  AD5664R converters (DAC). This driver uses the common SPI interface.
 
+config LTC2632
+	tristate "Linear Technology LTC2632-12/10/8 DAC spi driver"
+	depends on SPI
+	help
+	  Say yes here to build support for Linear Technology
+	  LTC2632-12, LTC2632-10, LTC2632-8 converters (DAC).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ltc2632.
+
 config AD5686
 	tristate "Analog Devices AD5686R/AD5685R/AD5684R DAC SPI driver"
 	depends on SPI
@@ -274,6 +284,21 @@ config MCP4922
 	  To compile this driver as a module, choose M here: the module
 	  will be called mcp4922.
 
+config STM32_DAC
+	tristate "STMicroelectronics STM32 DAC"
+	depends on (ARCH_STM32 && OF) || COMPILE_TEST
+	depends on REGULATOR
+	select STM32_DAC_CORE
+	help
+	  Say yes here to build support for STMicroelectronics STM32 Digital
+	  to Analog Converter (DAC).
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called stm32-dac.
+
+config STM32_DAC_CORE
+	tristate
+
 config VF610_DAC
 	tristate "Vybrid vf610 DAC driver"
 	depends on OF
diff --git a/drivers/iio/dac/Makefile b/drivers/iio/dac/Makefile
index f01bf4a99867..603587cc2f07 100644
--- a/drivers/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -24,9 +24,12 @@ obj-$(CONFIG_AD8801) += ad8801.o
 obj-$(CONFIG_CIO_DAC) += cio-dac.o
 obj-$(CONFIG_DPOT_DAC) += dpot-dac.o
 obj-$(CONFIG_LPC18XX_DAC) += lpc18xx_dac.o
+obj-$(CONFIG_LTC2632) += ltc2632.o
 obj-$(CONFIG_M62332) += m62332.o
 obj-$(CONFIG_MAX517) += max517.o
 obj-$(CONFIG_MAX5821) += max5821.o
 obj-$(CONFIG_MCP4725) += mcp4725.o
 obj-$(CONFIG_MCP4922) += mcp4922.o
+obj-$(CONFIG_STM32_DAC_CORE) += stm32-dac-core.o
+obj-$(CONFIG_STM32_DAC) += stm32-dac.o
 obj-$(CONFIG_VF610_DAC) += vf610_dac.o
diff --git a/drivers/iio/dac/ad5504.c b/drivers/iio/dac/ad5504.c
index 788b3d6fd1cc..712d86b4be09 100644
--- a/drivers/iio/dac/ad5504.c
+++ b/drivers/iio/dac/ad5504.c
@@ -212,7 +212,7 @@ static struct attribute *ad5504_ev_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group ad5504_ev_attribute_group = {
+static const struct attribute_group ad5504_ev_attribute_group = {
 	.attrs = ad5504_ev_attributes,
 };
 
@@ -223,7 +223,7 @@ static irqreturn_t ad5504_event_handler(int irq, void *private)
 					    0,
 					    IIO_EV_TYPE_THRESH,
 					    IIO_EV_DIR_RISING),
-		       iio_get_time_ns((struct iio_dev *)private));
+		       iio_get_time_ns(private));
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/iio/dac/ad7303.c b/drivers/iio/dac/ad7303.c
index e690dd11e99f..4b0f942b8914 100644
--- a/drivers/iio/dac/ad7303.c
+++ b/drivers/iio/dac/ad7303.c
@@ -184,9 +184,9 @@ static const struct iio_chan_spec_ext_info ad7303_ext_info[] = {
 	.address = (chan),					\
 	.scan_type = {						\
 		.sign = 'u',					\
-		.realbits = '8',				\
-		.storagebits = '8',				\
-		.shift = '0',					\
+		.realbits = 8,					\
+		.storagebits = 8,				\
+		.shift = 0,					\
 	},							\
 	.ext_info = ad7303_ext_info,				\
 }
diff --git a/drivers/iio/dac/cio-dac.c b/drivers/iio/dac/cio-dac.c
index 5a743e2a779d..a0464227a3a0 100644
--- a/drivers/iio/dac/cio-dac.c
+++ b/drivers/iio/dac/cio-dac.c
@@ -119,6 +119,7 @@ static int cio_dac_probe(struct device *dev, unsigned int id)
 	indio_dev->channels = cio_dac_channels;
 	indio_dev->num_channels = CIO_DAC_NUM_CHAN;
 	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
 
 	priv = iio_priv(indio_dev);
 	priv->base = base[id];
diff --git a/drivers/iio/dac/ltc2632.c b/drivers/iio/dac/ltc2632.c
new file mode 100644
index 000000000000..ac5e05f6eb8b
--- /dev/null
+++ b/drivers/iio/dac/ltc2632.c
@@ -0,0 +1,314 @@
+/*
+ * LTC2632 Digital to analog convertors spi driver
+ *
+ * Copyright 2017 Maxime Roussin-Bélanger
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+
+#define LTC2632_DAC_CHANNELS                    2
+
+#define LTC2632_ADDR_DAC0                       0x0
+#define LTC2632_ADDR_DAC1                       0x1
+
+#define LTC2632_CMD_WRITE_INPUT_N               0x0
+#define LTC2632_CMD_UPDATE_DAC_N                0x1
+#define LTC2632_CMD_WRITE_INPUT_N_UPDATE_ALL    0x2
+#define LTC2632_CMD_WRITE_INPUT_N_UPDATE_N      0x3
+#define LTC2632_CMD_POWERDOWN_DAC_N             0x4
+#define LTC2632_CMD_POWERDOWN_CHIP              0x5
+#define LTC2632_CMD_INTERNAL_REFER              0x6
+#define LTC2632_CMD_EXTERNAL_REFER              0x7
+
+/**
+ * struct ltc2632_chip_info - chip specific information
+ * @channels:		channel spec for the DAC
+ * @vref_mv:		reference voltage
+ */
+struct ltc2632_chip_info {
+	const struct iio_chan_spec *channels;
+	const int vref_mv;
+};
+
+/**
+ * struct ltc2632_state - driver instance specific data
+ * @spi_dev:			pointer to the spi_device struct
+ * @powerdown_cache_mask	used to show current channel powerdown state
+ */
+struct ltc2632_state {
+	struct spi_device *spi_dev;
+	unsigned int powerdown_cache_mask;
+};
+
+enum ltc2632_supported_device_ids {
+	ID_LTC2632L12,
+	ID_LTC2632L10,
+	ID_LTC2632L8,
+	ID_LTC2632H12,
+	ID_LTC2632H10,
+	ID_LTC2632H8,
+};
+
+static int ltc2632_spi_write(struct spi_device *spi,
+			     u8 cmd, u8 addr, u16 val, u8 shift)
+{
+	u32 data;
+	u8 msg[3];
+
+	/*
+	 * The input shift register is 24 bits wide.
+	 * The next four are the command bits, C3 to C0,
+	 * followed by the 4-bit DAC address, A3 to A0, and then the
+	 * 12-, 10-, 8-bit data-word. The data-word comprises the 12-,
+	 * 10-, 8-bit input code followed by 4, 6, or 8 don't care bits.
+	 */
+	data = (cmd << 20) | (addr << 16) | (val << shift);
+	msg[0] = data >> 16;
+	msg[1] = data >> 8;
+	msg[2] = data;
+
+	return spi_write(spi, msg, sizeof(msg));
+}
+
+static int ltc2632_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val,
+			    int *val2,
+			    long m)
+{
+	struct ltc2632_chip_info *chip_info;
+
+	const struct ltc2632_state *st = iio_priv(indio_dev);
+	const struct spi_device_id *spi_dev_id = spi_get_device_id(st->spi_dev);
+
+	chip_info = (struct ltc2632_chip_info *)spi_dev_id->driver_data;
+
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		*val = chip_info->vref_mv;
+		*val2 = chan->scan_type.realbits;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
+	return -EINVAL;
+}
+
+static int ltc2632_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val,
+			     int val2,
+			     long mask)
+{
+	struct ltc2632_state *st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (val >= (1 << chan->scan_type.realbits) || val < 0)
+			return -EINVAL;
+
+		return ltc2632_spi_write(st->spi_dev,
+					 LTC2632_CMD_WRITE_INPUT_N_UPDATE_N,
+					 chan->address, val,
+					 chan->scan_type.shift);
+	default:
+		return -EINVAL;
+	}
+}
+
+static ssize_t ltc2632_read_dac_powerdown(struct iio_dev *indio_dev,
+					  uintptr_t private,
+					  const struct iio_chan_spec *chan,
+					  char *buf)
+{
+	struct ltc2632_state *st = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n",
+		       !!(st->powerdown_cache_mask & (1 << chan->channel)));
+}
+
+static ssize_t ltc2632_write_dac_powerdown(struct iio_dev *indio_dev,
+					   uintptr_t private,
+					   const struct iio_chan_spec *chan,
+					   const char *buf,
+					   size_t len)
+{
+	bool pwr_down;
+	int ret;
+	struct ltc2632_state *st = iio_priv(indio_dev);
+
+	ret = strtobool(buf, &pwr_down);
+	if (ret)
+		return ret;
+
+	if (pwr_down)
+		st->powerdown_cache_mask |= (1 << chan->channel);
+	else
+		st->powerdown_cache_mask &= ~(1 << chan->channel);
+
+	ret = ltc2632_spi_write(st->spi_dev,
+				LTC2632_CMD_POWERDOWN_DAC_N,
+				chan->channel, 0, 0);
+
+	return ret ? ret : len;
+}
+
+static const struct iio_info ltc2632_info = {
+	.write_raw	= ltc2632_write_raw,
+	.read_raw	= ltc2632_read_raw,
+	.driver_module	= THIS_MODULE,
+};
+
+static const struct iio_chan_spec_ext_info ltc2632_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = ltc2632_read_dac_powerdown,
+		.write = ltc2632_write_dac_powerdown,
+		.shared = IIO_SEPARATE,
+	},
+	{ },
+};
+
+#define LTC2632_CHANNEL(_chan, _bits) { \
+		.type = IIO_VOLTAGE, \
+		.indexed = 1, \
+		.output = 1, \
+		.channel = (_chan), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+		.address = (_chan), \
+		.scan_type = { \
+			.realbits	= (_bits), \
+			.shift		= 16 - (_bits), \
+		}, \
+		.ext_info = ltc2632_ext_info, \
+}
+
+#define DECLARE_LTC2632_CHANNELS(_name, _bits) \
+	const struct iio_chan_spec _name ## _channels[] = { \
+		LTC2632_CHANNEL(0, _bits), \
+		LTC2632_CHANNEL(1, _bits), \
+	}
+
+static DECLARE_LTC2632_CHANNELS(ltc2632l12, 12);
+static DECLARE_LTC2632_CHANNELS(ltc2632l10, 10);
+static DECLARE_LTC2632_CHANNELS(ltc2632l8, 8);
+
+static DECLARE_LTC2632_CHANNELS(ltc2632h12, 12);
+static DECLARE_LTC2632_CHANNELS(ltc2632h10, 10);
+static DECLARE_LTC2632_CHANNELS(ltc2632h8, 8);
+
+static const struct ltc2632_chip_info ltc2632_chip_info_tbl[] = {
+	[ID_LTC2632L12] = {
+		.channels	= ltc2632l12_channels,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2632L10] = {
+		.channels	= ltc2632l10_channels,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2632L8] =  {
+		.channels	= ltc2632l8_channels,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2632H12] = {
+		.channels	= ltc2632h12_channels,
+		.vref_mv	= 4096,
+	},
+	[ID_LTC2632H10] = {
+		.channels	= ltc2632h10_channels,
+		.vref_mv	= 4096,
+	},
+	[ID_LTC2632H8] =  {
+		.channels	= ltc2632h8_channels,
+		.vref_mv	= 4096,
+	},
+};
+
+static int ltc2632_probe(struct spi_device *spi)
+{
+	struct ltc2632_state *st;
+	struct iio_dev *indio_dev;
+	struct ltc2632_chip_info *chip_info;
+	int ret;
+
+	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->spi_dev = spi;
+
+	chip_info = (struct ltc2632_chip_info *)
+			spi_get_device_id(spi)->driver_data;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = dev_of_node(&spi->dev) ? dev_of_node(&spi->dev)->name
+						 : spi_get_device_id(spi)->name;
+	indio_dev->info = &ltc2632_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = chip_info->channels;
+	indio_dev->num_channels = LTC2632_DAC_CHANNELS;
+
+	ret = ltc2632_spi_write(spi, LTC2632_CMD_INTERNAL_REFER, 0, 0, 0);
+	if (ret) {
+		dev_err(&spi->dev,
+			"Set internal reference command failed, %d\n", ret);
+		return ret;
+	}
+
+	return devm_iio_device_register(&spi->dev, indio_dev);
+}
+
+static const struct spi_device_id ltc2632_id[] = {
+	{ "ltc2632-l12", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632L12] },
+	{ "ltc2632-l10", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632L10] },
+	{ "ltc2632-l8", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632L8] },
+	{ "ltc2632-h12", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632H12] },
+	{ "ltc2632-h10", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632H10] },
+	{ "ltc2632-h8", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2632H8] },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ltc2632_id);
+
+static struct spi_driver ltc2632_driver = {
+	.driver		= {
+		.name	= "ltc2632",
+	},
+	.probe		= ltc2632_probe,
+	.id_table	= ltc2632_id,
+};
+module_spi_driver(ltc2632_driver);
+
+static const struct of_device_id ltc2632_of_match[] = {
+	{
+		.compatible = "lltc,ltc2632-l12",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632L12]
+	}, {
+		.compatible = "lltc,ltc2632-l10",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632L10]
+	}, {
+		.compatible = "lltc,ltc2632-l8",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632L8]
+	}, {
+		.compatible = "lltc,ltc2632-h12",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632H12]
+	}, {
+		.compatible = "lltc,ltc2632-h10",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632H10]
+	}, {
+		.compatible = "lltc,ltc2632-h8",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2632H8]
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, ltc2632_of_match);
+
+MODULE_AUTHOR("Maxime Roussin-Belanger <maxime.roussinbelanger@gmail.com>");
+MODULE_DESCRIPTION("LTC2632 DAC SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/dac/max5821.c b/drivers/iio/dac/max5821.c
index 86e9e112f554..193fac3059a3 100644
--- a/drivers/iio/dac/max5821.c
+++ b/drivers/iio/dac/max5821.c
@@ -392,6 +392,7 @@ MODULE_DEVICE_TABLE(of, max5821_of_match);
 static struct i2c_driver max5821_driver = {
 	.driver = {
 		.name	= "max5821",
+		.of_match_table = max5821_of_match,
 		.pm     = MAX5821_PM_OPS,
 	},
 	.probe		= max5821_probe,
diff --git a/drivers/iio/dac/mcp4725.c b/drivers/iio/dac/mcp4725.c
index db109f0cdd8c..6ab1f23e5a79 100644
--- a/drivers/iio/dac/mcp4725.c
+++ b/drivers/iio/dac/mcp4725.c
@@ -19,6 +19,7 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/regulator/consumer.h>
+#include <linux/of_device.h>
 #include <linux/of.h>
 
 #include <linux/iio/iio.h>
@@ -199,7 +200,7 @@ static ssize_t mcp4725_write_powerdown(struct iio_dev *indio_dev,
 	return len;
 }
 
-enum {
+enum chip_id {
 	MCP4725,
 	MCP4726,
 };
@@ -406,7 +407,10 @@ static int mcp4725_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
-	data->id = id->driver_data;
+	if (client->dev.of_node)
+		data->id = (enum chip_id)of_device_get_match_data(&client->dev);
+	else
+		data->id = id->driver_data;
 	pdata = dev_get_platdata(&client->dev);
 
 	if (!pdata) {
@@ -525,9 +529,25 @@ static const struct i2c_device_id mcp4725_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mcp4725_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id mcp4725_of_match[] = {
+	{
+		.compatible = "microchip,mcp4725",
+		.data = (void *)MCP4725
+	},
+	{
+		.compatible = "microchip,mcp4726",
+		.data = (void *)MCP4726
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mcp4725_of_match);
+#endif
+
 static struct i2c_driver mcp4725_driver = {
 	.driver = {
 		.name	= MCP4725_DRV_NAME,
+		.of_match_table = of_match_ptr(mcp4725_of_match),
 		.pm	= MCP4725_PM_OPS,
 	},
 	.probe		= mcp4725_probe,
diff --git a/drivers/iio/dac/stm32-dac-core.c b/drivers/iio/dac/stm32-dac-core.c
new file mode 100644
index 000000000000..75e48788c7ea
--- /dev/null
+++ b/drivers/iio/dac/stm32-dac-core.c
@@ -0,0 +1,180 @@
+/*
+ * This file is part of STM32 DAC driver
+ *
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ * Author: Fabrice Gasnier <fabrice.gasnier@st.com>.
+ *
+ * License type: GPLv2
+ *
+ * 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. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+
+#include "stm32-dac-core.h"
+
+/**
+ * struct stm32_dac_priv - stm32 DAC core private data
+ * @pclk:		peripheral clock common for all DACs
+ * @rst:		peripheral reset control
+ * @vref:		regulator reference
+ * @common:		Common data for all DAC instances
+ */
+struct stm32_dac_priv {
+	struct clk *pclk;
+	struct reset_control *rst;
+	struct regulator *vref;
+	struct stm32_dac_common common;
+};
+
+static struct stm32_dac_priv *to_stm32_dac_priv(struct stm32_dac_common *com)
+{
+	return container_of(com, struct stm32_dac_priv, common);
+}
+
+static const struct regmap_config stm32_dac_regmap_cfg = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = sizeof(u32),
+	.max_register = 0x3fc,
+};
+
+static int stm32_dac_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct stm32_dac_priv *priv;
+	struct regmap *regmap;
+	struct resource *res;
+	void __iomem *mmio;
+	int ret;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mmio = devm_ioremap_resource(dev, res);
+	if (IS_ERR(mmio))
+		return PTR_ERR(mmio);
+
+	regmap = devm_regmap_init_mmio(dev, mmio, &stm32_dac_regmap_cfg);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+	priv->common.regmap = regmap;
+
+	priv->vref = devm_regulator_get(dev, "vref");
+	if (IS_ERR(priv->vref)) {
+		ret = PTR_ERR(priv->vref);
+		dev_err(dev, "vref get failed, %d\n", ret);
+		return ret;
+	}
+
+	ret = regulator_enable(priv->vref);
+	if (ret < 0) {
+		dev_err(dev, "vref enable failed\n");
+		return ret;
+	}
+
+	ret = regulator_get_voltage(priv->vref);
+	if (ret < 0) {
+		dev_err(dev, "vref get voltage failed, %d\n", ret);
+		goto err_vref;
+	}
+	priv->common.vref_mv = ret / 1000;
+	dev_dbg(dev, "vref+=%dmV\n", priv->common.vref_mv);
+
+	priv->pclk = devm_clk_get(dev, "pclk");
+	if (IS_ERR(priv->pclk)) {
+		ret = PTR_ERR(priv->pclk);
+		dev_err(dev, "pclk get failed\n");
+		goto err_vref;
+	}
+
+	ret = clk_prepare_enable(priv->pclk);
+	if (ret < 0) {
+		dev_err(dev, "pclk enable failed\n");
+		goto err_vref;
+	}
+
+	priv->rst = devm_reset_control_get(dev, NULL);
+	if (!IS_ERR(priv->rst)) {
+		reset_control_assert(priv->rst);
+		udelay(2);
+		reset_control_deassert(priv->rst);
+	}
+
+	/* When clock speed is higher than 80MHz, set HFSEL */
+	priv->common.hfsel = (clk_get_rate(priv->pclk) > 80000000UL);
+	ret = regmap_update_bits(regmap, STM32_DAC_CR, STM32H7_DAC_CR_HFSEL,
+				 priv->common.hfsel ? STM32H7_DAC_CR_HFSEL : 0);
+	if (ret)
+		goto err_pclk;
+
+	platform_set_drvdata(pdev, &priv->common);
+
+	ret = of_platform_populate(pdev->dev.of_node, NULL, NULL, dev);
+	if (ret < 0) {
+		dev_err(dev, "failed to populate DT children\n");
+		goto err_pclk;
+	}
+
+	return 0;
+
+err_pclk:
+	clk_disable_unprepare(priv->pclk);
+err_vref:
+	regulator_disable(priv->vref);
+
+	return ret;
+}
+
+static int stm32_dac_remove(struct platform_device *pdev)
+{
+	struct stm32_dac_common *common = platform_get_drvdata(pdev);
+	struct stm32_dac_priv *priv = to_stm32_dac_priv(common);
+
+	of_platform_depopulate(&pdev->dev);
+	clk_disable_unprepare(priv->pclk);
+	regulator_disable(priv->vref);
+
+	return 0;
+}
+
+static const struct of_device_id stm32_dac_of_match[] = {
+	{ .compatible = "st,stm32h7-dac-core", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, stm32_dac_of_match);
+
+static struct platform_driver stm32_dac_driver = {
+	.probe = stm32_dac_probe,
+	.remove = stm32_dac_remove,
+	.driver = {
+		.name = "stm32-dac-core",
+		.of_match_table = stm32_dac_of_match,
+	},
+};
+module_platform_driver(stm32_dac_driver);
+
+MODULE_AUTHOR("Fabrice Gasnier <fabrice.gasnier@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STM32 DAC core driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:stm32-dac-core");
diff --git a/drivers/iio/dac/stm32-dac-core.h b/drivers/iio/dac/stm32-dac-core.h
new file mode 100644
index 000000000000..daf09931857c
--- /dev/null
+++ b/drivers/iio/dac/stm32-dac-core.h
@@ -0,0 +1,51 @@
+/*
+ * This file is part of STM32 DAC driver
+ *
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ * Author: Fabrice Gasnier <fabrice.gasnier@st.com>.
+ *
+ * License type: GPLv2
+ *
+ * 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. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __STM32_DAC_CORE_H
+#define __STM32_DAC_CORE_H
+
+#include <linux/regmap.h>
+
+/* STM32 DAC registers */
+#define STM32_DAC_CR		0x00
+#define STM32_DAC_DHR12R1	0x08
+#define STM32_DAC_DHR12R2	0x14
+#define STM32_DAC_DOR1		0x2C
+#define STM32_DAC_DOR2		0x30
+
+/* STM32_DAC_CR bit fields */
+#define STM32_DAC_CR_EN1		BIT(0)
+#define STM32H7_DAC_CR_HFSEL		BIT(15)
+#define STM32_DAC_CR_EN2		BIT(16)
+
+/**
+ * struct stm32_dac_common - stm32 DAC driver common data (for all instances)
+ * @regmap: DAC registers shared via regmap
+ * @vref_mv: reference voltage (mv)
+ * @hfsel: high speed bus clock selected
+ */
+struct stm32_dac_common {
+	struct regmap			*regmap;
+	int				vref_mv;
+	bool				hfsel;
+};
+
+#endif
diff --git a/drivers/iio/dac/stm32-dac.c b/drivers/iio/dac/stm32-dac.c
new file mode 100644
index 000000000000..50f8ec091058
--- /dev/null
+++ b/drivers/iio/dac/stm32-dac.c
@@ -0,0 +1,334 @@
+/*
+ * This file is part of STM32 DAC driver
+ *
+ * Copyright (C) 2017, STMicroelectronics - All Rights Reserved
+ * Authors: Amelie Delaunay <amelie.delaunay@st.com>
+ *	    Fabrice Gasnier <fabrice.gasnier@st.com>
+ *
+ * License type: GPLv2
+ *
+ * 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. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "stm32-dac-core.h"
+
+#define STM32_DAC_CHANNEL_1		1
+#define STM32_DAC_CHANNEL_2		2
+#define STM32_DAC_IS_CHAN_1(ch)		((ch) & STM32_DAC_CHANNEL_1)
+
+/**
+ * struct stm32_dac - private data of DAC driver
+ * @common:		reference to DAC common data
+ */
+struct stm32_dac {
+	struct stm32_dac_common *common;
+};
+
+static int stm32_dac_is_enabled(struct iio_dev *indio_dev, int channel)
+{
+	struct stm32_dac *dac = iio_priv(indio_dev);
+	u32 en, val;
+	int ret;
+
+	ret = regmap_read(dac->common->regmap, STM32_DAC_CR, &val);
+	if (ret < 0)
+		return ret;
+	if (STM32_DAC_IS_CHAN_1(channel))
+		en = FIELD_GET(STM32_DAC_CR_EN1, val);
+	else
+		en = FIELD_GET(STM32_DAC_CR_EN2, val);
+
+	return !!en;
+}
+
+static int stm32_dac_set_enable_state(struct iio_dev *indio_dev, int ch,
+				      bool enable)
+{
+	struct stm32_dac *dac = iio_priv(indio_dev);
+	u32 msk = STM32_DAC_IS_CHAN_1(ch) ? STM32_DAC_CR_EN1 : STM32_DAC_CR_EN2;
+	u32 en = enable ? msk : 0;
+	int ret;
+
+	ret = regmap_update_bits(dac->common->regmap, STM32_DAC_CR, msk, en);
+	if (ret < 0) {
+		dev_err(&indio_dev->dev, "%s failed\n", en ?
+			"Enable" : "Disable");
+		return ret;
+	}
+
+	/*
+	 * When HFSEL is set, it is not allowed to write the DHRx register
+	 * during 8 clock cycles after the ENx bit is set. It is not allowed
+	 * to make software/hardware trigger during this period either.
+	 */
+	if (en && dac->common->hfsel)
+		udelay(1);
+
+	return 0;
+}
+
+static int stm32_dac_get_value(struct stm32_dac *dac, int channel, int *val)
+{
+	int ret;
+
+	if (STM32_DAC_IS_CHAN_1(channel))
+		ret = regmap_read(dac->common->regmap, STM32_DAC_DOR1, val);
+	else
+		ret = regmap_read(dac->common->regmap, STM32_DAC_DOR2, val);
+
+	return ret ? ret : IIO_VAL_INT;
+}
+
+static int stm32_dac_set_value(struct stm32_dac *dac, int channel, int val)
+{
+	int ret;
+
+	if (STM32_DAC_IS_CHAN_1(channel))
+		ret = regmap_write(dac->common->regmap, STM32_DAC_DHR12R1, val);
+	else
+		ret = regmap_write(dac->common->regmap, STM32_DAC_DHR12R2, val);
+
+	return ret;
+}
+
+static int stm32_dac_read_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int *val, int *val2, long mask)
+{
+	struct stm32_dac *dac = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return stm32_dac_get_value(dac, chan->channel, val);
+	case IIO_CHAN_INFO_SCALE:
+		*val = dac->common->vref_mv;
+		*val2 = chan->scan_type.realbits;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int stm32_dac_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val, int val2, long mask)
+{
+	struct stm32_dac *dac = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return stm32_dac_set_value(dac, chan->channel, val);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int stm32_dac_debugfs_reg_access(struct iio_dev *indio_dev,
+					unsigned reg, unsigned writeval,
+					unsigned *readval)
+{
+	struct stm32_dac *dac = iio_priv(indio_dev);
+
+	if (!readval)
+		return regmap_write(dac->common->regmap, reg, writeval);
+	else
+		return regmap_read(dac->common->regmap, reg, readval);
+}
+
+static const struct iio_info stm32_dac_iio_info = {
+	.read_raw = stm32_dac_read_raw,
+	.write_raw = stm32_dac_write_raw,
+	.debugfs_reg_access = stm32_dac_debugfs_reg_access,
+	.driver_module = THIS_MODULE,
+};
+
+static const char * const stm32_dac_powerdown_modes[] = {
+	"three_state",
+};
+
+static int stm32_dac_get_powerdown_mode(struct iio_dev *indio_dev,
+					const struct iio_chan_spec *chan)
+{
+	return 0;
+}
+
+static int stm32_dac_set_powerdown_mode(struct iio_dev *indio_dev,
+					const struct iio_chan_spec *chan,
+					unsigned int type)
+{
+	return 0;
+}
+
+static ssize_t stm32_dac_read_powerdown(struct iio_dev *indio_dev,
+					uintptr_t private,
+					const struct iio_chan_spec *chan,
+					char *buf)
+{
+	int ret = stm32_dac_is_enabled(indio_dev, chan->channel);
+
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", ret ? 0 : 1);
+}
+
+static ssize_t stm32_dac_write_powerdown(struct iio_dev *indio_dev,
+					 uintptr_t private,
+					 const struct iio_chan_spec *chan,
+					 const char *buf, size_t len)
+{
+	bool powerdown;
+	int ret;
+
+	ret = strtobool(buf, &powerdown);
+	if (ret)
+		return ret;
+
+	ret = stm32_dac_set_enable_state(indio_dev, chan->channel, !powerdown);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+static const struct iio_enum stm32_dac_powerdown_mode_en = {
+	.items = stm32_dac_powerdown_modes,
+	.num_items = ARRAY_SIZE(stm32_dac_powerdown_modes),
+	.get = stm32_dac_get_powerdown_mode,
+	.set = stm32_dac_set_powerdown_mode,
+};
+
+static const struct iio_chan_spec_ext_info stm32_dac_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = stm32_dac_read_powerdown,
+		.write = stm32_dac_write_powerdown,
+		.shared = IIO_SEPARATE,
+	},
+	IIO_ENUM("powerdown_mode", IIO_SEPARATE, &stm32_dac_powerdown_mode_en),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &stm32_dac_powerdown_mode_en),
+	{},
+};
+
+#define STM32_DAC_CHANNEL(chan, name) {			\
+	.type = IIO_VOLTAGE,				\
+	.indexed = 1,					\
+	.output = 1,					\
+	.channel = chan,				\
+	.info_mask_separate =				\
+		BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_SCALE),		\
+	/* scan_index is always 0 as num_channels is 1 */ \
+	.scan_type = {					\
+		.sign = 'u',				\
+		.realbits = 12,				\
+		.storagebits = 16,			\
+	},						\
+	.datasheet_name = name,				\
+	.ext_info = stm32_dac_ext_info			\
+}
+
+static const struct iio_chan_spec stm32_dac_channels[] = {
+	STM32_DAC_CHANNEL(STM32_DAC_CHANNEL_1, "out1"),
+	STM32_DAC_CHANNEL(STM32_DAC_CHANNEL_2, "out2"),
+};
+
+static int stm32_dac_chan_of_init(struct iio_dev *indio_dev)
+{
+	struct device_node *np = indio_dev->dev.of_node;
+	unsigned int i;
+	u32 channel;
+	int ret;
+
+	ret = of_property_read_u32(np, "reg", &channel);
+	if (ret) {
+		dev_err(&indio_dev->dev, "Failed to read reg property\n");
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(stm32_dac_channels); i++) {
+		if (stm32_dac_channels[i].channel == channel)
+			break;
+	}
+	if (i >= ARRAY_SIZE(stm32_dac_channels)) {
+		dev_err(&indio_dev->dev, "Invalid st,dac-channel\n");
+		return -EINVAL;
+	}
+
+	indio_dev->channels = &stm32_dac_channels[i];
+	/*
+	 * Expose only one channel here, as they can be used independently,
+	 * with separate trigger. Then separate IIO devices are instantiated
+	 * to manage this.
+	 */
+	indio_dev->num_channels = 1;
+
+	return 0;
+};
+
+static int stm32_dac_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct iio_dev *indio_dev;
+	struct stm32_dac *dac;
+	int ret;
+
+	if (!np)
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*dac));
+	if (!indio_dev)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, indio_dev);
+
+	dac = iio_priv(indio_dev);
+	dac->common = dev_get_drvdata(pdev->dev.parent);
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->info = &stm32_dac_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = stm32_dac_chan_of_init(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	return devm_iio_device_register(&pdev->dev, indio_dev);
+}
+
+static const struct of_device_id stm32_dac_of_match[] = {
+	{ .compatible = "st,stm32-dac", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, stm32_dac_of_match);
+
+static struct platform_driver stm32_dac_driver = {
+	.probe = stm32_dac_probe,
+	.driver = {
+		.name = "stm32-dac",
+		.of_match_table = stm32_dac_of_match,
+	},
+};
+module_platform_driver(stm32_dac_driver);
+
+MODULE_ALIAS("platform:stm32-dac");
+MODULE_AUTHOR("Amelie Delaunay <amelie.delaunay@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STM32 DAC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/gyro/itg3200_core.c b/drivers/iio/gyro/itg3200_core.c
index c102a6325bb0..cfa2db04a8ab 100644
--- a/drivers/iio/gyro/itg3200_core.c
+++ b/drivers/iio/gyro/itg3200_core.c
@@ -377,9 +377,16 @@ static const struct i2c_device_id itg3200_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, itg3200_id);
 
+static const struct of_device_id itg3200_of_match[] = {
+	{ .compatible = "invensense,itg3200" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, itg3200_of_match);
+
 static struct i2c_driver itg3200_driver = {
 	.driver = {
 		.name	= "itg3200",
+		.of_match_table = itg3200_of_match,
 		.pm	= &itg3200_pm_ops,
 	},
 	.id_table	= itg3200_id,
diff --git a/drivers/iio/gyro/mpu3050-i2c.c b/drivers/iio/gyro/mpu3050-i2c.c
index 06007200bf49..93f08b304a63 100644
--- a/drivers/iio/gyro/mpu3050-i2c.c
+++ b/drivers/iio/gyro/mpu3050-i2c.c
@@ -70,9 +70,8 @@ static int mpu3050_i2c_probe(struct i2c_client *client,
 		dev_err(&client->dev, "failed to allocate I2C mux\n");
 	else {
 		mpu3050->i2cmux->priv = mpu3050;
-		ret = i2c_mux_add_adapter(mpu3050->i2cmux, 0, 0, 0);
-		if (ret)
-			dev_err(&client->dev, "failed to add I2C mux\n");
+		/* Ignore failure, not critical */
+		i2c_mux_add_adapter(mpu3050->i2cmux, 0, 0, 0);
 	}
 
 	return 0;
diff --git a/drivers/iio/health/Kconfig b/drivers/iio/health/Kconfig
index c5f004a8e447..a2ecb4c94c2a 100644
--- a/drivers/iio/health/Kconfig
+++ b/drivers/iio/health/Kconfig
@@ -46,6 +46,19 @@ config MAX30100
 	  To compile this driver as a module, choose M here: the
 	  module will be called max30100.
 
+config MAX30102
+	tristate "MAX30102 heart rate and pulse oximeter sensor"
+	depends on I2C
+	select REGMAP_I2C
+	select IIO_BUFFER
+	select IIO_KFIFO_BUF
+	help
+	  Say Y here to build I2C interface support for the Maxim
+	  MAX30102 heart rate, and pulse oximeter sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max30102.
+
 endmenu
 
 endmenu
diff --git a/drivers/iio/health/Makefile b/drivers/iio/health/Makefile
index 9955a2ae8df1..3558f9d64954 100644
--- a/drivers/iio/health/Makefile
+++ b/drivers/iio/health/Makefile
@@ -7,3 +7,4 @@
 obj-$(CONFIG_AFE4403)		+= afe4403.o
 obj-$(CONFIG_AFE4404)		+= afe4404.o
 obj-$(CONFIG_MAX30100)		+= max30100.o
+obj-$(CONFIG_MAX30102)		+= max30102.o
diff --git a/drivers/iio/health/max30100.c b/drivers/iio/health/max30100.c
index f6e283c4d686..849d71747f9f 100644
--- a/drivers/iio/health/max30100.c
+++ b/drivers/iio/health/max30100.c
@@ -449,6 +449,7 @@ static int max30100_probe(struct i2c_client *client,
 	indio_dev->available_scan_masks = max30100_scan_masks;
 	indio_dev->modes = (INDIO_BUFFER_SOFTWARE | INDIO_DIRECT_MODE);
 	indio_dev->setup_ops = &max30100_buffer_setup_ops;
+	indio_dev->dev.parent = &client->dev;
 
 	data = iio_priv(indio_dev);
 	data->indio_dev = indio_dev;
diff --git a/drivers/iio/health/max30102.c b/drivers/iio/health/max30102.c
new file mode 100644
index 000000000000..839b875c29b9
--- /dev/null
+++ b/drivers/iio/health/max30102.c
@@ -0,0 +1,486 @@
+/*
+ * max30102.c - Support for MAX30102 heart rate and pulse oximeter sensor
+ *
+ * Copyright (C) 2017 Matt Ranostay <matt@ranostay.consulting>
+ *
+ * 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.
+ *
+ * TODO: proximity power saving feature
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+
+#define MAX30102_REGMAP_NAME	"max30102_regmap"
+#define MAX30102_DRV_NAME	"max30102"
+
+#define MAX30102_REG_INT_STATUS			0x00
+#define MAX30102_REG_INT_STATUS_PWR_RDY		BIT(0)
+#define MAX30102_REG_INT_STATUS_PROX_INT	BIT(4)
+#define MAX30102_REG_INT_STATUS_ALC_OVF		BIT(5)
+#define MAX30102_REG_INT_STATUS_PPG_RDY		BIT(6)
+#define MAX30102_REG_INT_STATUS_FIFO_RDY	BIT(7)
+
+#define MAX30102_REG_INT_ENABLE			0x02
+#define MAX30102_REG_INT_ENABLE_PROX_INT_EN	BIT(4)
+#define MAX30102_REG_INT_ENABLE_ALC_OVF_EN	BIT(5)
+#define MAX30102_REG_INT_ENABLE_PPG_EN		BIT(6)
+#define MAX30102_REG_INT_ENABLE_FIFO_EN		BIT(7)
+#define MAX30102_REG_INT_ENABLE_MASK		0xf0
+#define MAX30102_REG_INT_ENABLE_MASK_SHIFT	4
+
+#define MAX30102_REG_FIFO_WR_PTR		0x04
+#define MAX30102_REG_FIFO_OVR_CTR		0x05
+#define MAX30102_REG_FIFO_RD_PTR		0x06
+#define MAX30102_REG_FIFO_DATA			0x07
+#define MAX30102_REG_FIFO_DATA_ENTRY_LEN	6
+
+#define MAX30102_REG_FIFO_CONFIG		0x08
+#define MAX30102_REG_FIFO_CONFIG_AVG_4SAMPLES	BIT(1)
+#define MAX30102_REG_FIFO_CONFIG_AVG_SHIFT	5
+#define MAX30102_REG_FIFO_CONFIG_AFULL		BIT(0)
+
+#define MAX30102_REG_MODE_CONFIG		0x09
+#define MAX30102_REG_MODE_CONFIG_MODE_SPO2_EN	BIT(0)
+#define MAX30102_REG_MODE_CONFIG_MODE_HR_EN	BIT(1)
+#define MAX30102_REG_MODE_CONFIG_MODE_MASK	0x03
+#define MAX30102_REG_MODE_CONFIG_PWR		BIT(7)
+
+#define MAX30102_REG_SPO2_CONFIG		0x0a
+#define MAX30102_REG_SPO2_CONFIG_PULSE_411_US	0x03
+#define MAX30102_REG_SPO2_CONFIG_SR_400HZ	0x03
+#define MAX30102_REG_SPO2_CONFIG_SR_MASK	0x07
+#define MAX30102_REG_SPO2_CONFIG_SR_MASK_SHIFT	2
+#define MAX30102_REG_SPO2_CONFIG_ADC_4096_STEPS	BIT(0)
+#define MAX30102_REG_SPO2_CONFIG_ADC_MASK_SHIFT	5
+
+#define MAX30102_REG_RED_LED_CONFIG		0x0c
+#define MAX30102_REG_IR_LED_CONFIG		0x0d
+
+#define MAX30102_REG_TEMP_CONFIG		0x21
+#define MAX30102_REG_TEMP_CONFIG_TEMP_EN	BIT(0)
+
+#define MAX30102_REG_TEMP_INTEGER		0x1f
+#define MAX30102_REG_TEMP_FRACTION		0x20
+
+struct max30102_data {
+	struct i2c_client *client;
+	struct iio_dev *indio_dev;
+	struct mutex lock;
+	struct regmap *regmap;
+
+	u8 buffer[8];
+	__be32 processed_buffer[2]; /* 2 x 18-bit (padded to 32-bits) */
+};
+
+static const struct regmap_config max30102_regmap_config = {
+	.name = MAX30102_REGMAP_NAME,
+
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+static const unsigned long max30102_scan_masks[] = {0x3, 0};
+
+static const struct iio_chan_spec max30102_channels[] = {
+	{
+		.type = IIO_INTENSITY,
+		.channel2 = IIO_MOD_LIGHT_RED,
+		.modified = 1,
+
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 'u',
+			.shift = 8,
+			.realbits = 18,
+			.storagebits = 32,
+			.endianness = IIO_BE,
+		},
+	},
+	{
+		.type = IIO_INTENSITY,
+		.channel2 = IIO_MOD_LIGHT_IR,
+		.modified = 1,
+
+		.scan_index = 1,
+		.scan_type = {
+			.sign = 'u',
+			.shift = 8,
+			.realbits = 18,
+			.storagebits = 32,
+			.endianness = IIO_BE,
+		},
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = -1,
+	},
+};
+
+static int max30102_set_powermode(struct max30102_data *data, bool state)
+{
+	return regmap_update_bits(data->regmap, MAX30102_REG_MODE_CONFIG,
+				  MAX30102_REG_MODE_CONFIG_PWR,
+				  state ? 0 : MAX30102_REG_MODE_CONFIG_PWR);
+}
+
+static int max30102_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct max30102_data *data = iio_priv(indio_dev);
+
+	return max30102_set_powermode(data, true);
+}
+
+static int max30102_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct max30102_data *data = iio_priv(indio_dev);
+
+	return max30102_set_powermode(data, false);
+}
+
+static const struct iio_buffer_setup_ops max30102_buffer_setup_ops = {
+	.postenable = max30102_buffer_postenable,
+	.predisable = max30102_buffer_predisable,
+};
+
+static inline int max30102_fifo_count(struct max30102_data *data)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(data->regmap, MAX30102_REG_INT_STATUS, &val);
+	if (ret)
+		return ret;
+
+	/* FIFO has one sample slot left */
+	if (val & MAX30102_REG_INT_STATUS_FIFO_RDY)
+		return 1;
+
+	return 0;
+}
+
+static int max30102_read_measurement(struct max30102_data *data)
+{
+	int ret;
+	u8 *buffer = (u8 *) &data->buffer;
+
+	ret = i2c_smbus_read_i2c_block_data(data->client,
+					    MAX30102_REG_FIFO_DATA,
+					    MAX30102_REG_FIFO_DATA_ENTRY_LEN,
+					    buffer);
+
+	memcpy(&data->processed_buffer[0], &buffer[0], 3);
+	memcpy(&data->processed_buffer[1], &buffer[3], 3);
+
+	return (ret == MAX30102_REG_FIFO_DATA_ENTRY_LEN) ? 0 : -EINVAL;
+}
+
+static irqreturn_t max30102_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct max30102_data *data = iio_priv(indio_dev);
+	int ret, cnt = 0;
+
+	mutex_lock(&data->lock);
+
+	while (cnt || (cnt = max30102_fifo_count(data)) > 0) {
+		ret = max30102_read_measurement(data);
+		if (ret)
+			break;
+
+		iio_push_to_buffers(data->indio_dev, data->processed_buffer);
+		cnt--;
+	}
+
+	mutex_unlock(&data->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int max30102_get_current_idx(unsigned int val, int *reg)
+{
+	/* each step is 0.200 mA */
+	*reg = val / 200;
+
+	return *reg > 0xff ? -EINVAL : 0;
+}
+
+static int max30102_led_init(struct max30102_data *data)
+{
+	struct device *dev = &data->client->dev;
+	struct device_node *np = dev->of_node;
+	unsigned int val;
+	int reg, ret;
+
+	ret = of_property_read_u32(np, "maxim,red-led-current-microamp", &val);
+	if (ret) {
+		dev_info(dev, "no red-led-current-microamp set\n");
+
+		/* Default to 7 mA RED LED */
+		val = 7000;
+	}
+
+	ret = max30102_get_current_idx(val, &reg);
+	if (ret) {
+		dev_err(dev, "invalid RED LED current setting %d\n", val);
+		return ret;
+	}
+
+	ret = regmap_write(data->regmap, MAX30102_REG_RED_LED_CONFIG, reg);
+	if (ret)
+		return ret;
+
+	ret = of_property_read_u32(np, "maxim,ir-led-current-microamp", &val);
+	if (ret) {
+		dev_info(dev, "no ir-led-current-microamp set\n");
+
+		/* Default to 7 mA IR LED */
+		val = 7000;
+	}
+
+	ret = max30102_get_current_idx(val, &reg);
+	if (ret) {
+		dev_err(dev, "invalid IR LED current setting %d", val);
+		return ret;
+	}
+
+	return regmap_write(data->regmap, MAX30102_REG_IR_LED_CONFIG, reg);
+}
+
+static int max30102_chip_init(struct max30102_data *data)
+{
+	int ret;
+
+	/* setup LED current settings */
+	ret = max30102_led_init(data);
+	if (ret)
+		return ret;
+
+	/* enable 18-bit HR + SPO2 readings at 400Hz */
+	ret = regmap_write(data->regmap, MAX30102_REG_SPO2_CONFIG,
+				(MAX30102_REG_SPO2_CONFIG_ADC_4096_STEPS
+				 << MAX30102_REG_SPO2_CONFIG_ADC_MASK_SHIFT) |
+				(MAX30102_REG_SPO2_CONFIG_SR_400HZ
+				 << MAX30102_REG_SPO2_CONFIG_SR_MASK_SHIFT) |
+				 MAX30102_REG_SPO2_CONFIG_PULSE_411_US);
+	if (ret)
+		return ret;
+
+	/* enable SPO2 mode */
+	ret = regmap_update_bits(data->regmap, MAX30102_REG_MODE_CONFIG,
+				 MAX30102_REG_MODE_CONFIG_MODE_MASK,
+				 MAX30102_REG_MODE_CONFIG_MODE_HR_EN |
+				 MAX30102_REG_MODE_CONFIG_MODE_SPO2_EN);
+	if (ret)
+		return ret;
+
+	/* average 4 samples + generate FIFO interrupt */
+	ret = regmap_write(data->regmap, MAX30102_REG_FIFO_CONFIG,
+				(MAX30102_REG_FIFO_CONFIG_AVG_4SAMPLES
+				 << MAX30102_REG_FIFO_CONFIG_AVG_SHIFT) |
+				 MAX30102_REG_FIFO_CONFIG_AFULL);
+	if (ret)
+		return ret;
+
+	/* enable FIFO interrupt */
+	return regmap_update_bits(data->regmap, MAX30102_REG_INT_ENABLE,
+				 MAX30102_REG_INT_ENABLE_MASK,
+				 MAX30102_REG_INT_ENABLE_FIFO_EN);
+}
+
+static int max30102_read_temp(struct max30102_data *data, int *val)
+{
+	int ret;
+	unsigned int reg;
+
+	ret = regmap_read(data->regmap, MAX30102_REG_TEMP_INTEGER, &reg);
+	if (ret < 0)
+		return ret;
+	*val = reg << 4;
+
+	ret = regmap_read(data->regmap, MAX30102_REG_TEMP_FRACTION, &reg);
+	if (ret < 0)
+		return ret;
+
+	*val |= reg & 0xf;
+	*val = sign_extend32(*val, 11);
+
+	return 0;
+}
+
+static int max30102_get_temp(struct max30102_data *data, int *val)
+{
+	int ret;
+
+	/* start acquisition */
+	ret = regmap_update_bits(data->regmap, MAX30102_REG_TEMP_CONFIG,
+				 MAX30102_REG_TEMP_CONFIG_TEMP_EN,
+				 MAX30102_REG_TEMP_CONFIG_TEMP_EN);
+	if (ret)
+		return ret;
+
+	msleep(35);
+
+	return max30102_read_temp(data, val);
+}
+
+static int max30102_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int *val, int *val2, long mask)
+{
+	struct max30102_data *data = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * Temperature reading can only be acquired while engine
+		 * is running
+		 */
+		mutex_lock(&indio_dev->mlock);
+
+		if (!iio_buffer_enabled(indio_dev))
+			ret = -EBUSY;
+		else {
+			ret = max30102_get_temp(data, val);
+			if (!ret)
+				ret = IIO_VAL_INT;
+		}
+
+		mutex_unlock(&indio_dev->mlock);
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 1;  /* 0.0625 */
+		*val2 = 16;
+		ret = IIO_VAL_FRACTIONAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct iio_info max30102_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = max30102_read_raw,
+};
+
+static int max30102_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct max30102_data *data;
+	struct iio_buffer *buffer;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	buffer = devm_iio_kfifo_allocate(&client->dev);
+	if (!buffer)
+		return -ENOMEM;
+
+	iio_device_attach_buffer(indio_dev, buffer);
+
+	indio_dev->name = MAX30102_DRV_NAME;
+	indio_dev->channels = max30102_channels;
+	indio_dev->info = &max30102_info;
+	indio_dev->num_channels = ARRAY_SIZE(max30102_channels);
+	indio_dev->available_scan_masks = max30102_scan_masks;
+	indio_dev->modes = (INDIO_BUFFER_SOFTWARE | INDIO_DIRECT_MODE);
+	indio_dev->setup_ops = &max30102_buffer_setup_ops;
+	indio_dev->dev.parent = &client->dev;
+
+	data = iio_priv(indio_dev);
+	data->indio_dev = indio_dev;
+	data->client = client;
+
+	mutex_init(&data->lock);
+	i2c_set_clientdata(client, indio_dev);
+
+	data->regmap = devm_regmap_init_i2c(client, &max30102_regmap_config);
+	if (IS_ERR(data->regmap)) {
+		dev_err(&client->dev, "regmap initialization failed.\n");
+		return PTR_ERR(data->regmap);
+	}
+	max30102_set_powermode(data, false);
+
+	ret = max30102_chip_init(data);
+	if (ret)
+		return ret;
+
+	if (client->irq <= 0) {
+		dev_err(&client->dev, "no valid irq defined\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_threaded_irq(&client->dev, client->irq,
+					NULL, max30102_interrupt_handler,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					"max30102_irq", indio_dev);
+	if (ret) {
+		dev_err(&client->dev, "request irq (%d) failed\n", client->irq);
+		return ret;
+	}
+
+	return iio_device_register(indio_dev);
+}
+
+static int max30102_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct max30102_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	max30102_set_powermode(data, false);
+
+	return 0;
+}
+
+static const struct i2c_device_id max30102_id[] = {
+	{ "max30102", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, max30102_id);
+
+static const struct of_device_id max30102_dt_ids[] = {
+	{ .compatible = "maxim,max30102" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max30102_dt_ids);
+
+static struct i2c_driver max30102_driver = {
+	.driver = {
+		.name	= MAX30102_DRV_NAME,
+		.of_match_table	= of_match_ptr(max30102_dt_ids),
+	},
+	.probe		= max30102_probe,
+	.remove		= max30102_remove,
+	.id_table	= max30102_id,
+};
+module_i2c_driver(max30102_driver);
+
+MODULE_AUTHOR("Matt Ranostay <matt@ranostay.consulting>");
+MODULE_DESCRIPTION("MAX30102 heart rate and pulse oximeter sensor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/humidity/Kconfig b/drivers/iio/humidity/Kconfig
index 912477d54be2..14b9ce453d9d 100644
--- a/drivers/iio/humidity/Kconfig
+++ b/drivers/iio/humidity/Kconfig
@@ -36,6 +36,20 @@ config HDC100X
 	  To compile this driver as a module, choose M here: the module
 	  will be called hdc100x.
 
+config HID_SENSOR_HUMIDITY
+	tristate "HID Environmental humidity sensor"
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	select HID_SENSOR_IIO_TRIGGER
+	help
+	  Say yes here to build support for the HID SENSOR
+	  humidity driver
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called hid-sensor-humidity.
+
 config HTS221
 	tristate "STMicroelectronics HTS221 sensor Driver"
 	depends on (I2C || SPI)
diff --git a/drivers/iio/humidity/Makefile b/drivers/iio/humidity/Makefile
index a6850e47c100..be0dedeb8f3c 100644
--- a/drivers/iio/humidity/Makefile
+++ b/drivers/iio/humidity/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_AM2315) += am2315.o
 obj-$(CONFIG_DHT11) += dht11.o
 obj-$(CONFIG_HDC100X) += hdc100x.o
+obj-$(CONFIG_HID_SENSOR_HUMIDITY) += hid-sensor-humidity.o
 
 hts221-y := hts221_core.o \
 	    hts221_buffer.o
@@ -15,3 +16,5 @@ obj-$(CONFIG_HTS221_SPI) += hts221_spi.o
 obj-$(CONFIG_HTU21) += htu21.o
 obj-$(CONFIG_SI7005) += si7005.o
 obj-$(CONFIG_SI7020) += si7020.o
+
+ccflags-y += -I$(srctree)/drivers/iio/common/hid-sensors
diff --git a/drivers/iio/humidity/hdc100x.c b/drivers/iio/humidity/hdc100x.c
index 265c34da52d1..aa17115f54c9 100644
--- a/drivers/iio/humidity/hdc100x.c
+++ b/drivers/iio/humidity/hdc100x.c
@@ -79,7 +79,7 @@ static struct attribute *hdc100x_attributes[] = {
 	NULL
 };
 
-static struct attribute_group hdc100x_attribute_group = {
+static const struct attribute_group hdc100x_attribute_group = {
 	.attrs = hdc100x_attributes,
 };
 
diff --git a/drivers/iio/humidity/hid-sensor-humidity.c b/drivers/iio/humidity/hid-sensor-humidity.c
new file mode 100644
index 000000000000..6e09c1acfe51
--- /dev/null
+++ b/drivers/iio/humidity/hid-sensor-humidity.c
@@ -0,0 +1,315 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2017, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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.
+ */
+#include <linux/device.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "hid-sensor-trigger.h"
+
+struct hid_humidity_state {
+	struct hid_sensor_common common_attributes;
+	struct hid_sensor_hub_attribute_info humidity_attr;
+	s32 humidity_data;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec humidity_channels[] = {
+	{
+		.type = IIO_HUMIDITYRELATIVE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+			BIT(IIO_CHAN_INFO_HYSTERESIS),
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1)
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void humidity_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+					int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is s32 */
+	channels[channel].scan_type.storagebits = sizeof(s32) * 8;
+}
+
+static int humidity_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct hid_humidity_state *humid_st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type != IIO_HUMIDITYRELATIVE)
+			return -EINVAL;
+		hid_sensor_power_state(&humid_st->common_attributes, true);
+		*val = sensor_hub_input_attr_get_raw_value(
+				humid_st->common_attributes.hsdev,
+				HID_USAGE_SENSOR_HUMIDITY,
+				HID_USAGE_SENSOR_ATMOSPHERIC_HUMIDITY,
+				humid_st->humidity_attr.report_id,
+				SENSOR_HUB_SYNC);
+		hid_sensor_power_state(&humid_st->common_attributes, false);
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = humid_st->scale_pre_decml;
+		*val2 = humid_st->scale_post_decml;
+
+		return humid_st->scale_precision;
+
+	case IIO_CHAN_INFO_OFFSET:
+		*val = humid_st->value_offset;
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return hid_sensor_read_samp_freq_value(
+				&humid_st->common_attributes, val, val2);
+
+	case IIO_CHAN_INFO_HYSTERESIS:
+		return hid_sensor_read_raw_hyst_value(
+				&humid_st->common_attributes, val, val2);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int humidity_write_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int val, int val2, long mask)
+{
+	struct hid_humidity_state *humid_st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return hid_sensor_write_samp_freq_value(
+				&humid_st->common_attributes, val, val2);
+
+	case IIO_CHAN_INFO_HYSTERESIS:
+		return hid_sensor_write_raw_hyst_value(
+				&humid_st->common_attributes, val, val2);
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info humidity_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &humidity_read_raw,
+	.write_raw = &humidity_write_raw,
+};
+
+/* Callback handler to send event after all samples are received and captured */
+static int humidity_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned int usage_id, void *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct hid_humidity_state *humid_st = iio_priv(indio_dev);
+
+	if (atomic_read(&humid_st->common_attributes.data_ready))
+		iio_push_to_buffers_with_timestamp(indio_dev,
+					&humid_st->humidity_data,
+					iio_get_time_ns(indio_dev));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int humidity_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned int usage_id, size_t raw_len,
+				char *raw_data, void *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct hid_humidity_state *humid_st = iio_priv(indio_dev);
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_ATMOSPHERIC_HUMIDITY:
+		humid_st->humidity_data = *(s32 *)raw_data;
+
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Parse report which is specific to an usage id */
+static int humidity_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned int usage_id,
+				struct hid_humidity_state *st)
+{
+	int ret;
+
+	ret = sensor_hub_input_get_attribute_info(hsdev, HID_INPUT_REPORT,
+					usage_id,
+					HID_USAGE_SENSOR_ATMOSPHERIC_HUMIDITY,
+					&st->humidity_attr);
+	if (ret < 0)
+		return ret;
+
+	humidity_adjust_channel_bit_mask(channels, 0, st->humidity_attr.size);
+
+	st->scale_precision = hid_sensor_format_scale(
+						HID_USAGE_SENSOR_HUMIDITY,
+						&st->humidity_attr,
+						&st->scale_pre_decml,
+						&st->scale_post_decml);
+
+	/* Set Sensitivity field ids, when there is no individual modifier */
+	if (st->common_attributes.sensitivity.index < 0)
+		sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
+			HID_USAGE_SENSOR_ATMOSPHERIC_HUMIDITY,
+			&st->common_attributes.sensitivity);
+
+	return ret;
+}
+
+static struct hid_sensor_hub_callbacks humidity_callbacks = {
+	.send_event = &humidity_proc_event,
+	.capture_sample = &humidity_capture_sample,
+};
+
+/* Function to initialize the processing for usage id */
+static int hid_humidity_probe(struct platform_device *pdev)
+{
+	static const char *name = "humidity";
+	struct iio_dev *indio_dev;
+	struct hid_humidity_state *humid_st;
+	struct iio_chan_spec *humid_chans;
+	struct hid_sensor_hub_device *hsdev = dev_get_platdata(&pdev->dev);
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*humid_st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	humid_st = iio_priv(indio_dev);
+	humid_st->common_attributes.hsdev = hsdev;
+	humid_st->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+					HID_USAGE_SENSOR_HUMIDITY,
+					&humid_st->common_attributes);
+	if (ret)
+		return ret;
+
+	humid_chans = devm_kmemdup(&indio_dev->dev, humidity_channels,
+					sizeof(humidity_channels), GFP_KERNEL);
+	if (!humid_chans)
+		return -ENOMEM;
+
+	ret = humidity_parse_report(pdev, hsdev, humid_chans,
+				HID_USAGE_SENSOR_HUMIDITY, humid_st);
+	if (ret)
+		return ret;
+
+	indio_dev->channels = humid_chans;
+	indio_dev->num_channels = ARRAY_SIZE(humidity_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &humidity_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = devm_iio_triggered_buffer_setup(&pdev->dev, indio_dev,
+					&iio_pollfunc_store_time, NULL, NULL);
+	if (ret)
+		return ret;
+
+	atomic_set(&humid_st->common_attributes.data_ready, 0);
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+				&humid_st->common_attributes);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	humidity_callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_HUMIDITY,
+					&humidity_callbacks);
+	if (ret)
+		goto error_remove_trigger;
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_remove_callback;
+
+	return ret;
+
+error_remove_callback:
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_HUMIDITY);
+error_remove_trigger:
+	hid_sensor_remove_trigger(&humid_st->common_attributes);
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int hid_humidity_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = dev_get_platdata(&pdev->dev);
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct hid_humidity_state *humid_st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_HUMIDITY);
+	hid_sensor_remove_trigger(&humid_st->common_attributes);
+
+	return 0;
+}
+
+static const struct platform_device_id hid_humidity_ids[] = {
+	{
+		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
+		.name = "HID-SENSOR-200032",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, hid_humidity_ids);
+
+static struct platform_driver hid_humidity_platform_driver = {
+	.id_table = hid_humidity_ids,
+	.driver = {
+		.name	= KBUILD_MODNAME,
+		.pm	= &hid_sensor_pm_ops,
+	},
+	.probe		= hid_humidity_probe,
+	.remove		= hid_humidity_remove,
+};
+module_platform_driver(hid_humidity_platform_driver);
+
+MODULE_DESCRIPTION("HID Environmental humidity sensor");
+MODULE_AUTHOR("Song Hongyan <hongyan.song@intel.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/humidity/hts221_buffer.c b/drivers/iio/humidity/hts221_buffer.c
index 72ddcdac21a2..7d19a3da7ab7 100644
--- a/drivers/iio/humidity/hts221_buffer.c
+++ b/drivers/iio/humidity/hts221_buffer.c
@@ -41,7 +41,7 @@ static const struct iio_trigger_ops hts221_trigger_ops = {
 
 static irqreturn_t hts221_trigger_handler_thread(int irq, void *private)
 {
-	struct hts221_hw *hw = (struct hts221_hw *)private;
+	struct hts221_hw *hw = private;
 	u8 status;
 	int err;
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index b9fcbf18aa99..96dabbd2f004 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -114,6 +114,12 @@ static const struct inv_mpu6050_hw hw_info[] = {
 		.config = &chip_config_6050,
 	},
 	{
+		.whoami = INV_MPU9250_WHOAMI_VALUE,
+		.name = "MPU9250",
+		.reg = &reg_set_6500,
+		.config = &chip_config_6050,
+	},
+	{
 		.whoami = INV_ICM20608_WHOAMI_VALUE,
 		.name = "ICM20608",
 		.reg = &reg_set_6500,
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
index 2c3f8964a3ea..64b5f5b92200 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
@@ -17,6 +17,7 @@
 #include <linux/i2c.h>
 #include <linux/iio/iio.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include "inv_mpu_iio.h"
 
 static const struct regmap_config inv_mpu_regmap_config = {
@@ -69,7 +70,8 @@ static int inv_mpu6050_deselect_bypass(struct i2c_mux_core *muxc, u32 chan_id)
 	return 0;
 }
 
-static const char *inv_mpu_match_acpi_device(struct device *dev, int *chip_id)
+static const char *inv_mpu_match_acpi_device(struct device *dev,
+					     enum inv_devices *chip_id)
 {
 	const struct acpi_device_id *id;
 
@@ -93,7 +95,8 @@ static int inv_mpu_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct inv_mpu6050_state *st;
-	int result, chip_type;
+	int result;
+	enum inv_devices chip_type;
 	struct regmap *regmap;
 	const char *name;
 
@@ -101,8 +104,13 @@ static int inv_mpu_probe(struct i2c_client *client,
 				     I2C_FUNC_SMBUS_I2C_BLOCK))
 		return -EOPNOTSUPP;
 
-	if (id) {
-		chip_type = (int)id->driver_data;
+	if (client->dev.of_node) {
+		chip_type = (enum inv_devices)
+			of_device_get_match_data(&client->dev);
+		name = client->name;
+	} else if (id) {
+		chip_type = (enum inv_devices)
+			id->driver_data;
 		name = id->name;
 	} else if (ACPI_HANDLE(&client->dev)) {
 		name = inv_mpu_match_acpi_device(&client->dev, &chip_type);
@@ -170,12 +178,38 @@ static const struct i2c_device_id inv_mpu_id[] = {
 	{"mpu6050", INV_MPU6050},
 	{"mpu6500", INV_MPU6500},
 	{"mpu9150", INV_MPU9150},
+	{"mpu9250", INV_MPU9250},
 	{"icm20608", INV_ICM20608},
 	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
 
+static const struct of_device_id inv_of_match[] = {
+	{
+		.compatible = "invensense,mpu6050",
+		.data = (void *)INV_MPU6050
+	},
+	{
+		.compatible = "invensense,mpu6500",
+		.data = (void *)INV_MPU6500
+	},
+	{
+		.compatible = "invensense,mpu9150",
+		.data = (void *)INV_MPU9150
+	},
+	{
+		.compatible = "invensense,mpu9250",
+		.data = (void *)INV_MPU9250
+	},
+	{
+		.compatible = "invensense,icm20608",
+		.data = (void *)INV_ICM20608
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, inv_of_match);
+
 static const struct acpi_device_id inv_acpi_match[] = {
 	{"INVN6500", INV_MPU6500},
 	{ },
@@ -188,6 +222,7 @@ static struct i2c_driver inv_mpu_driver = {
 	.remove		=	inv_mpu_remove,
 	.id_table	=	inv_mpu_id,
 	.driver = {
+		.of_match_table = inv_of_match,
 		.acpi_match_table = ACPI_PTR(inv_acpi_match),
 		.name	=	"inv-mpu6050-i2c",
 		.pm     =       &inv_mpu_pmops,
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index f0e8c5dd9fae..ef13de7a2c20 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -70,6 +70,7 @@ enum inv_devices {
 	INV_MPU6500,
 	INV_MPU6000,
 	INV_MPU9150,
+	INV_MPU9250,
 	INV_ICM20608,
 	INV_NUM_PARTS
 };
@@ -226,6 +227,7 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_WHOAMI_VALUE		0x68
 #define INV_MPU6500_WHOAMI_VALUE		0x70
 #define INV_MPU9150_WHOAMI_VALUE		0x68
+#define INV_MPU9250_WHOAMI_VALUE		0x71
 #define INV_ICM20608_WHOAMI_VALUE		0xAF
 
 /* scan element definition */
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
index 6e6476dfa188..74506e5ac0db 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
@@ -82,6 +82,7 @@ static const struct spi_device_id inv_mpu_id[] = {
 	{"mpu6000", INV_MPU6000},
 	{"mpu6500", INV_MPU6500},
 	{"mpu9150", INV_MPU9150},
+	{"mpu9250", INV_MPU9250},
 	{"icm20608", INV_ICM20608},
 	{}
 };
diff --git a/drivers/iio/imu/st_lsm6dsx/Kconfig b/drivers/iio/imu/st_lsm6dsx/Kconfig
index 935d4cd071a3..e57337159b57 100644
--- a/drivers/iio/imu/st_lsm6dsx/Kconfig
+++ b/drivers/iio/imu/st_lsm6dsx/Kconfig
@@ -8,7 +8,7 @@ config IIO_ST_LSM6DSX
 	select IIO_ST_LSM6DSX_SPI if (SPI_MASTER)
 	help
 	  Say yes here to build support for STMicroelectronics LSM6DSx imu
-	  sensor. Supported devices: lsm6ds3, lsm6dsm
+	  sensor. Supported devices: lsm6ds3, lsm6ds3h, lsm6dsl, lsm6dsm
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called st_lsm6dsx.
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 69deafe1c10d..4839db7b9690 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -15,11 +15,16 @@
 #include <linux/device.h>
 
 #define ST_LSM6DS3_DEV_NAME	"lsm6ds3"
+#define ST_LSM6DS3H_DEV_NAME	"lsm6ds3h"
+#define ST_LSM6DSL_DEV_NAME	"lsm6dsl"
 #define ST_LSM6DSM_DEV_NAME	"lsm6dsm"
 
 enum st_lsm6dsx_hw_id {
 	ST_LSM6DS3_ID,
+	ST_LSM6DS3H_ID,
+	ST_LSM6DSL_ID,
 	ST_LSM6DSM_ID,
+	ST_LSM6DSX_MAX_ID,
 };
 
 #define ST_LSM6DSX_CHAN_SIZE		2
@@ -50,7 +55,7 @@ struct st_lsm6dsx_reg {
 struct st_lsm6dsx_settings {
 	u8 wai;
 	u16 max_fifo_size;
-	enum st_lsm6dsx_hw_id id;
+	enum st_lsm6dsx_hw_id id[ST_LSM6DSX_MAX_ID];
 };
 
 enum st_lsm6dsx_sensor_id {
@@ -66,6 +71,7 @@ enum st_lsm6dsx_fifo_mode {
 
 /**
  * struct st_lsm6dsx_sensor - ST IMU sensor instance
+ * @name: Sensor name.
  * @id: Sensor identifier.
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
  * @gain: Configured sensor sensitivity.
@@ -78,6 +84,7 @@ enum st_lsm6dsx_fifo_mode {
  * @ts: Latest timestamp from the interrupt handler.
  */
 struct st_lsm6dsx_sensor {
+	char name[32];
 	enum st_lsm6dsx_sensor_id id;
 	struct st_lsm6dsx_hw *hw;
 
@@ -128,7 +135,7 @@ struct st_lsm6dsx_hw {
 #endif /* CONFIG_SPI_MASTER */
 };
 
-int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
+int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		     const struct st_lsm6dsx_transfer_function *tf_ops);
 int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
 int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
index 81b572d7699a..c8e5cfd0ef0b 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -1,9 +1,10 @@
 /*
  * STMicroelectronics st_lsm6dsx FIFO buffer library driver
  *
- * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
- * from gyroscope and accelerometer. Samples are queued without any tag
- * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
+ * LSM6DS3/LSM6DS3H/LSM6DSL/LSM6DSM: The FIFO buffer can be configured
+ * to store data from gyroscope and accelerometer. Samples are queued
+ * without any tag according to a specific pattern based on 'FIFO data sets'
+ * (6 bytes each):
  *  - 1st data set is reserved for gyroscope data
  *  - 2nd data set is reserved for accelerometer data
  * The FIFO pattern changes depending on the ODRs and decimation factors
@@ -206,7 +207,7 @@ out:
 }
 
 /**
- * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
+ * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DS3H-LSM6DSL-LSM6DSM read FIFO routine
  * @hw: Pointer to instance of struct st_lsm6dsx_hw.
  *
  * Read samples from the hw FIFO and push them to IIO buffers.
@@ -363,7 +364,7 @@ static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
 
 static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 {
-	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
+	struct st_lsm6dsx_hw *hw = private;
 	struct st_lsm6dsx_sensor *sensor;
 	int i;
 
@@ -387,7 +388,7 @@ static irqreturn_t st_lsm6dsx_handler_irq(int irq, void *private)
 
 static irqreturn_t st_lsm6dsx_handler_thread(int irq, void *private)
 {
-	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
+	struct st_lsm6dsx_hw *hw = private;
 	int count;
 
 	mutex_lock(&hw->fifo_lock);
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index c92ddcc190e2..462a27b70453 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -17,7 +17,7 @@
  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
  *   - FIFO size: 8KB
  *
- * - LSM6DSM:
+ * - LSM6DS3H/LSM6DSL/LSM6DSM:
  *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
  *   - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
@@ -74,12 +74,6 @@
 #define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR	0x24
 #define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR	0x26
 
-#define ST_LSM6DS3_WHOAMI			0x69
-#define ST_LSM6DSM_WHOAMI			0x6a
-
-#define ST_LSM6DS3_MAX_FIFO_SIZE		8192
-#define ST_LSM6DSM_MAX_FIFO_SIZE		4096
-
 #define ST_LSM6DSX_ACC_FS_2G_GAIN		IIO_G_TO_M_S_2(61)
 #define ST_LSM6DSX_ACC_FS_4G_GAIN		IIO_G_TO_M_S_2(122)
 #define ST_LSM6DSX_ACC_FS_8G_GAIN		IIO_G_TO_M_S_2(244)
@@ -164,14 +158,26 @@ static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
 
 static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 	{
-		.wai = ST_LSM6DS3_WHOAMI,
-		.max_fifo_size = ST_LSM6DS3_MAX_FIFO_SIZE,
-		.id = ST_LSM6DS3_ID,
+		.wai = 0x69,
+		.max_fifo_size = 8192,
+		.id = {
+			[0] = ST_LSM6DS3_ID,
+		},
 	},
 	{
-		.wai = ST_LSM6DSM_WHOAMI,
-		.max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
-		.id = ST_LSM6DSM_ID,
+		.wai = 0x69,
+		.max_fifo_size = 4096,
+		.id = {
+			[0] = ST_LSM6DS3H_ID,
+		},
+	},
+	{
+		.wai = 0x6a,
+		.max_fifo_size = 4096,
+		.id = {
+			[0] = ST_LSM6DSL_ID,
+			[1] = ST_LSM6DSM_ID,
+		},
 	},
 };
 
@@ -241,11 +247,15 @@ out:
 
 static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw, int id)
 {
-	int err, i;
+	int err, i, j;
 	u8 data;
 
 	for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
-		if (id == st_lsm6dsx_sensor_settings[i].id)
+		for (j = 0; j < ST_LSM6DSX_MAX_ID; j++) {
+			if (id == st_lsm6dsx_sensor_settings[i].id[j])
+				break;
+		}
+		if (j < ST_LSM6DSX_MAX_ID)
 			break;
 	}
 
@@ -298,32 +308,40 @@ static int st_lsm6dsx_set_full_scale(struct st_lsm6dsx_sensor *sensor,
 	return 0;
 }
 
-static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
+static int st_lsm6dsx_check_odr(struct st_lsm6dsx_sensor *sensor, u16 odr,
+				u8 *val)
 {
-	enum st_lsm6dsx_sensor_id id = sensor->id;
-	int i, err;
-	u8 val;
+	int i;
 
 	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
-		if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr)
+		if (st_lsm6dsx_odr_table[sensor->id].odr_avl[i].hz == odr)
 			break;
 
 	if (i == ST_LSM6DSX_ODR_LIST_SIZE)
 		return -EINVAL;
 
-	val = st_lsm6dsx_odr_table[id].odr_avl[i].val;
-	err = st_lsm6dsx_write_with_mask(sensor->hw,
-					 st_lsm6dsx_odr_table[id].reg.addr,
-					 st_lsm6dsx_odr_table[id].reg.mask,
-					 val);
-	if (err < 0)
-		return err;
-
+	*val = st_lsm6dsx_odr_table[sensor->id].odr_avl[i].val;
 	sensor->odr = odr;
 
 	return 0;
 }
 
+static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
+{
+	enum st_lsm6dsx_sensor_id id = sensor->id;
+	int err;
+	u8 val;
+
+	err = st_lsm6dsx_check_odr(sensor, odr, &val);
+	if (err < 0)
+		return err;
+
+	return st_lsm6dsx_write_with_mask(sensor->hw,
+					  st_lsm6dsx_odr_table[id].reg.addr,
+					  st_lsm6dsx_odr_table[id].reg.mask,
+					  val);
+}
+
 int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
 {
 	int err;
@@ -426,9 +444,12 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		err = st_lsm6dsx_set_full_scale(sensor, val2);
 		break;
-	case IIO_CHAN_INFO_SAMP_FREQ:
-		err = st_lsm6dsx_set_odr(sensor, val);
+	case IIO_CHAN_INFO_SAMP_FREQ: {
+		u8 data;
+
+		err = st_lsm6dsx_check_odr(sensor, val, &data);
 		break;
+	}
 	default:
 		err = -EINVAL;
 		break;
@@ -538,19 +559,11 @@ static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
 static int st_lsm6dsx_of_get_drdy_pin(struct st_lsm6dsx_hw *hw, int *drdy_pin)
 {
 	struct device_node *np = hw->dev->of_node;
-	int err;
 
 	if (!np)
 		return -EINVAL;
 
-	err = of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
-	if (err == -ENODATA) {
-		/* if the property has not been specified use default value */
-		*drdy_pin = 1;
-		err = 0;
-	}
-
-	return err;
+	return of_property_read_u32(np, "st,drdy-int-pin", drdy_pin);
 }
 
 static int st_lsm6dsx_get_drdy_reg(struct st_lsm6dsx_hw *hw, u8 *drdy_reg)
@@ -621,7 +634,8 @@ static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
 }
 
 static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
-					       enum st_lsm6dsx_sensor_id id)
+					       enum st_lsm6dsx_sensor_id id,
+					       const char *name)
 {
 	struct st_lsm6dsx_sensor *sensor;
 	struct iio_dev *iio_dev;
@@ -645,27 +659,30 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
 	case ST_LSM6DSX_ID_ACC:
 		iio_dev->channels = st_lsm6dsx_acc_channels;
 		iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
-		iio_dev->name = "lsm6dsx_accel";
 		iio_dev->info = &st_lsm6dsx_acc_info;
 
 		sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
+		scnprintf(sensor->name, sizeof(sensor->name), "%s_accel",
+			  name);
 		break;
 	case ST_LSM6DSX_ID_GYRO:
 		iio_dev->channels = st_lsm6dsx_gyro_channels;
 		iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
-		iio_dev->name = "lsm6dsx_gyro";
 		iio_dev->info = &st_lsm6dsx_gyro_info;
 
 		sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
+		scnprintf(sensor->name, sizeof(sensor->name), "%s_gyro",
+			  name);
 		break;
 	default:
 		return NULL;
 	}
+	iio_dev->name = sensor->name;
 
 	return iio_dev;
 }
 
-int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
+int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id, const char *name,
 		     const struct st_lsm6dsx_transfer_function *tf_ops)
 {
 	struct st_lsm6dsx_hw *hw;
@@ -689,7 +706,7 @@ int st_lsm6dsx_probe(struct device *dev, int irq, int hw_id,
 		return err;
 
 	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
-		hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i);
+		hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i, name);
 		if (!hw->iio_devs[i])
 			return -ENOMEM;
 	}
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
index ea3041186e1e..09a51cfb9b5e 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
@@ -61,7 +61,7 @@ static int st_lsm6dsx_i2c_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
 	return st_lsm6dsx_probe(&client->dev, client->irq,
-				(int)id->driver_data,
+				(int)id->driver_data, id->name,
 				&st_lsm6dsx_transfer_fn);
 }
 
@@ -71,6 +71,14 @@ static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
 		.data = (void *)ST_LSM6DS3_ID,
 	},
 	{
+		.compatible = "st,lsm6ds3h",
+		.data = (void *)ST_LSM6DS3H_ID,
+	},
+	{
+		.compatible = "st,lsm6dsl",
+		.data = (void *)ST_LSM6DSL_ID,
+	},
+	{
 		.compatible = "st,lsm6dsm",
 		.data = (void *)ST_LSM6DSM_ID,
 	},
@@ -80,6 +88,8 @@ MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match);
 
 static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = {
 	{ ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID },
+	{ ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID },
+	{ ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID },
 	{ ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
 	{},
 };
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
index fbe72470ed1e..f765a5058488 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
@@ -78,7 +78,7 @@ static int st_lsm6dsx_spi_probe(struct spi_device *spi)
 	const struct spi_device_id *id = spi_get_device_id(spi);
 
 	return st_lsm6dsx_probe(&spi->dev, spi->irq,
-				(int)id->driver_data,
+				(int)id->driver_data, id->name,
 				&st_lsm6dsx_transfer_fn);
 }
 
@@ -88,6 +88,14 @@ static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
 		.data = (void *)ST_LSM6DS3_ID,
 	},
 	{
+		.compatible = "st,lsm6ds3h",
+		.data = (void *)ST_LSM6DS3H_ID,
+	},
+	{
+		.compatible = "st,lsm6dsl",
+		.data = (void *)ST_LSM6DSL_ID,
+	},
+	{
 		.compatible = "st,lsm6dsm",
 		.data = (void *)ST_LSM6DSM_ID,
 	},
@@ -97,6 +105,8 @@ MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match);
 
 static const struct spi_device_id st_lsm6dsx_spi_id_table[] = {
 	{ ST_LSM6DS3_DEV_NAME, ST_LSM6DS3_ID },
+	{ ST_LSM6DS3H_DEV_NAME, ST_LSM6DS3H_ID },
+	{ ST_LSM6DSL_DEV_NAME, ST_LSM6DSL_ID },
 	{ ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
 	{},
 };
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index 5f731ead9d46..33e755d8d825 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -136,6 +136,16 @@ config CM36651
 	 To compile this driver as a module, choose M here:
 	 the module will be called cm36651.
 
+config IIO_CROS_EC_LIGHT_PROX
+	tristate "ChromeOS EC Light and Proximity Sensors"
+	depends on IIO_CROS_EC_SENSORS_CORE
+	help
+	  Say Y here if you use the light and proximity sensors
+	  presented by the ChromeOS EC Sensor hub.
+
+	  To compile this driver as a module, choose M here:
+	  the module will be called cros_ec_light_prox.
+
 config GP2AP020A00F
 	tristate "Sharp GP2AP020A00F Proximity/ALS sensor"
 	depends on I2C
@@ -395,4 +405,14 @@ config VEML6070
 	 To compile this driver as a module, choose M here: the
 	 module will be called veml6070.
 
+config VL6180
+	tristate "VL6180 ALS, range and proximity sensor"
+	depends on I2C
+	help
+	 Say Y here if you want to build a driver for the STMicroelectronics
+	 VL6180 combined ambient light, range and proximity sensor.
+
+	 To compile this driver as a module, choose M here: the
+	 module will be called vl6180.
+
 endmenu
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index c13a2399e6df..681363c2b298 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_CM3232)		+= cm3232.o
 obj-$(CONFIG_CM3323)		+= cm3323.o
 obj-$(CONFIG_CM3605)		+= cm3605.o
 obj-$(CONFIG_CM36651)		+= cm36651.o
+obj-$(CONFIG_IIO_CROS_EC_LIGHT_PROX) += cros_ec_light_prox.o
 obj-$(CONFIG_GP2AP020A00F)	+= gp2ap020a00f.o
 obj-$(CONFIG_HID_SENSOR_ALS)	+= hid-sensor-als.o
 obj-$(CONFIG_HID_SENSOR_PROX)	+= hid-sensor-prox.o
@@ -37,3 +38,4 @@ obj-$(CONFIG_TSL4531)		+= tsl4531.o
 obj-$(CONFIG_US5182D)		+= us5182d.o
 obj-$(CONFIG_VCNL4000)		+= vcnl4000.o
 obj-$(CONFIG_VEML6070)		+= veml6070.o
+obj-$(CONFIG_VL6180)		+= vl6180.o
diff --git a/drivers/iio/light/apds9960.c b/drivers/iio/light/apds9960.c
index a4304edc3e0f..518a47e9377b 100644
--- a/drivers/iio/light/apds9960.c
+++ b/drivers/iio/light/apds9960.c
@@ -343,7 +343,7 @@ static struct attribute *apds9960_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group apds9960_attribute_group = {
+static const struct attribute_group apds9960_attribute_group = {
 	.attrs = apds9960_attributes,
 };
 
@@ -1112,6 +1112,8 @@ static int apds9960_runtime_resume(struct device *dev)
 #endif
 
 static const struct dev_pm_ops apds9960_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
 	SET_RUNTIME_PM_OPS(apds9960_runtime_suspend,
 			   apds9960_runtime_resume, NULL)
 };
@@ -1122,9 +1124,16 @@ static const struct i2c_device_id apds9960_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, apds9960_id);
 
+static const struct of_device_id apds9960_of_match[] = {
+	{ .compatible = "avago,apds9960" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, apds9960_of_match);
+
 static struct i2c_driver apds9960_driver = {
 	.driver = {
 		.name	= APDS9960_DRV_NAME,
+		.of_match_table = apds9960_of_match,
 		.pm	= &apds9960_pm_ops,
 	},
 	.probe		= apds9960_probe,
diff --git a/drivers/iio/light/bh1750.c b/drivers/iio/light/bh1750.c
index b05946604f80..6c61187e630f 100644
--- a/drivers/iio/light/bh1750.c
+++ b/drivers/iio/light/bh1750.c
@@ -212,7 +212,7 @@ static struct attribute *bh1750_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group bh1750_attribute_group = {
+static const struct attribute_group bh1750_attribute_group = {
 	.attrs = bh1750_attributes,
 };
 
diff --git a/drivers/iio/light/cros_ec_light_prox.c b/drivers/iio/light/cros_ec_light_prox.c
new file mode 100644
index 000000000000..721722376fd0
--- /dev/null
+++ b/drivers/iio/light/cros_ec_light_prox.c
@@ -0,0 +1,289 @@
+/*
+ * cros_ec_light_prox - Driver for light and prox sensors behing CrosEC.
+ *
+ * Copyright (C) 2017 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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/device.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/kernel.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+
+#include "../common/cros_ec_sensors/cros_ec_sensors_core.h"
+
+/*
+ * We only represent one entry for light or proximity. EC is merging different
+ * light sensors to return the what the eye would see. For proximity, we
+ * currently support only one light source.
+ */
+#define CROS_EC_LIGHT_PROX_MAX_CHANNELS (1 + 1)
+
+/* State data for ec_sensors iio driver. */
+struct cros_ec_light_prox_state {
+	/* Shared by all sensors */
+	struct cros_ec_sensors_core_state core;
+
+	struct iio_chan_spec channels[CROS_EC_LIGHT_PROX_MAX_CHANNELS];
+};
+
+static int cros_ec_light_prox_read(struct iio_dev *indio_dev,
+				   struct iio_chan_spec const *chan,
+				   int *val, int *val2, long mask)
+{
+	struct cros_ec_light_prox_state *st = iio_priv(indio_dev);
+	u16 data = 0;
+	s64 val64;
+	int ret = IIO_VAL_INT;
+	int idx = chan->scan_index;
+
+	mutex_lock(&st->core.cmd_lock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type == IIO_PROXIMITY) {
+			if (cros_ec_sensors_read_cmd(indio_dev, 1 << idx,
+						     (s16 *)&data) < 0) {
+				ret = -EIO;
+				break;
+			}
+			*val = data;
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+	case IIO_CHAN_INFO_PROCESSED:
+		if (chan->type == IIO_LIGHT) {
+			if (cros_ec_sensors_read_cmd(indio_dev, 1 << idx,
+						     (s16 *)&data) < 0) {
+				ret = -EIO;
+				break;
+			}
+			/*
+			 * The data coming from the light sensor is
+			 * pre-processed and represents the ambient light
+			 * illuminance reading expressed in lux.
+			 */
+			*val = data;
+			ret = IIO_VAL_INT;
+		} else {
+			ret = -EINVAL;
+		}
+		break;
+	case IIO_CHAN_INFO_CALIBBIAS:
+		st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_OFFSET;
+		st->core.param.sensor_offset.flags = 0;
+
+		if (cros_ec_motion_send_host_cmd(&st->core, 0)) {
+			ret = -EIO;
+			break;
+		}
+
+		/* Save values */
+		st->core.calib[0] = st->core.resp->sensor_offset.offset[0];
+
+		*val = st->core.calib[idx];
+		break;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		/*
+		 * RANGE is used for calibration
+		 * scale is a number x.y, where x is coded on 16 bits,
+		 * y coded on 16 bits, between 0 and 9999.
+		 */
+		st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_RANGE;
+		st->core.param.sensor_range.data = EC_MOTION_SENSE_NO_VALUE;
+
+		if (cros_ec_motion_send_host_cmd(&st->core, 0)) {
+			ret = -EIO;
+			break;
+		}
+
+		val64 = st->core.resp->sensor_range.ret;
+		*val = val64 >> 16;
+		*val2 = (val64 & 0xffff) * 100;
+		ret = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret = cros_ec_sensors_core_read(&st->core, chan, val, val2,
+						mask);
+		break;
+	}
+
+	mutex_unlock(&st->core.cmd_lock);
+
+	return ret;
+}
+
+static int cros_ec_light_prox_write(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val, int val2, long mask)
+{
+	struct cros_ec_light_prox_state *st = iio_priv(indio_dev);
+	int ret = 0;
+	int idx = chan->scan_index;
+
+	mutex_lock(&st->core.cmd_lock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_CALIBBIAS:
+		st->core.calib[idx] = val;
+		/* Send to EC for each axis, even if not complete */
+		st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_OFFSET;
+		st->core.param.sensor_offset.flags = MOTION_SENSE_SET_OFFSET;
+		st->core.param.sensor_offset.offset[0] = st->core.calib[0];
+		st->core.param.sensor_offset.temp =
+					EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+		if (cros_ec_motion_send_host_cmd(&st->core, 0))
+			ret = -EIO;
+		break;
+	case IIO_CHAN_INFO_CALIBSCALE:
+		st->core.param.cmd = MOTIONSENSE_CMD_SENSOR_RANGE;
+		st->core.param.sensor_range.data = (val << 16) | (val2 / 100);
+		if (cros_ec_motion_send_host_cmd(&st->core, 0))
+			ret = -EIO;
+		break;
+	default:
+		ret = cros_ec_sensors_core_write(&st->core, chan, val, val2,
+						 mask);
+		break;
+	}
+
+	mutex_unlock(&st->core.cmd_lock);
+
+	return ret;
+}
+
+static const struct iio_info cros_ec_light_prox_info = {
+	.read_raw = &cros_ec_light_prox_read,
+	.write_raw = &cros_ec_light_prox_write,
+	.driver_module = THIS_MODULE,
+};
+
+static int cros_ec_light_prox_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct cros_ec_dev *ec_dev = dev_get_drvdata(dev->parent);
+	struct cros_ec_device *ec_device;
+	struct iio_dev *indio_dev;
+	struct cros_ec_light_prox_state *state;
+	struct iio_chan_spec *channel;
+	int ret;
+
+	if (!ec_dev || !ec_dev->ec_dev) {
+		dev_warn(dev, "No CROS EC device found.\n");
+		return -EINVAL;
+	}
+	ec_device = ec_dev->ec_dev;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*state));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	ret = cros_ec_sensors_core_init(pdev, indio_dev, true);
+	if (ret)
+		return ret;
+
+	indio_dev->info = &cros_ec_light_prox_info;
+	state = iio_priv(indio_dev);
+	state->core.type = state->core.resp->info.type;
+	state->core.loc = state->core.resp->info.location;
+	channel = state->channels;
+
+	/* Common part */
+	channel->info_mask_shared_by_all =
+		BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+		BIT(IIO_CHAN_INFO_FREQUENCY);
+	channel->scan_type.realbits = CROS_EC_SENSOR_BITS;
+	channel->scan_type.storagebits = CROS_EC_SENSOR_BITS;
+	channel->scan_type.shift = 0;
+	channel->scan_index = 0;
+	channel->ext_info = cros_ec_sensors_ext_info;
+	channel->scan_type.sign = 'u';
+
+	state->core.calib[0] = 0;
+
+	/* Sensor specific */
+	switch (state->core.type) {
+	case MOTIONSENSE_TYPE_LIGHT:
+		channel->type = IIO_LIGHT;
+		channel->info_mask_separate =
+			BIT(IIO_CHAN_INFO_PROCESSED) |
+			BIT(IIO_CHAN_INFO_CALIBBIAS) |
+			BIT(IIO_CHAN_INFO_CALIBSCALE);
+		break;
+	case MOTIONSENSE_TYPE_PROX:
+		channel->type = IIO_PROXIMITY;
+		channel->info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_CALIBBIAS) |
+			BIT(IIO_CHAN_INFO_CALIBSCALE);
+		break;
+	default:
+		dev_warn(dev, "Unknown motion sensor\n");
+		return -EINVAL;
+	}
+
+	/* Timestamp */
+	channel++;
+	channel->type = IIO_TIMESTAMP;
+	channel->channel = -1;
+	channel->scan_index = 1;
+	channel->scan_type.sign = 's';
+	channel->scan_type.realbits = 64;
+	channel->scan_type.storagebits = 64;
+
+	indio_dev->channels = state->channels;
+
+	indio_dev->num_channels = CROS_EC_LIGHT_PROX_MAX_CHANNELS;
+
+	state->core.read_ec_sensors_data = cros_ec_sensors_read_cmd;
+
+	ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+					      cros_ec_sensors_capture, NULL);
+	if (ret)
+		return ret;
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+
+static const struct platform_device_id cros_ec_light_prox_ids[] = {
+	{
+		.name = "cros-ec-prox",
+	},
+	{
+		.name = "cros-ec-light",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, cros_ec_light_prox_ids);
+
+static struct platform_driver cros_ec_light_prox_platform_driver = {
+	.driver = {
+		.name	= "cros-ec-light-prox",
+	},
+	.probe		= cros_ec_light_prox_probe,
+	.id_table	= cros_ec_light_prox_ids,
+};
+module_platform_driver(cros_ec_light_prox_platform_driver);
+
+MODULE_DESCRIPTION("ChromeOS EC light/proximity sensors driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 45ca056f019e..73fced8a63b7 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -240,6 +240,13 @@ static int prox_parse_report(struct platform_device *pdev,
 			st->common_attributes.sensitivity.index,
 			st->common_attributes.sensitivity.report_id);
 	}
+	if (st->common_attributes.sensitivity.index < 0)
+		sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
+			HID_USAGE_SENSOR_HUMAN_PRESENCE,
+			&st->common_attributes.sensitivity);
+
 	return ret;
 }
 
diff --git a/drivers/iio/light/lm3533-als.c b/drivers/iio/light/lm3533-als.c
index f409c2047c05..0443fd2e8757 100644
--- a/drivers/iio/light/lm3533-als.c
+++ b/drivers/iio/light/lm3533-als.c
@@ -690,7 +690,7 @@ static struct attribute *lm3533_als_event_attributes[] = {
 	NULL
 };
 
-static struct attribute_group lm3533_als_event_attribute_group = {
+static const struct attribute_group lm3533_als_event_attribute_group = {
 	.attrs = lm3533_als_event_attributes
 };
 
@@ -714,7 +714,7 @@ static struct attribute *lm3533_als_attributes[] = {
 	NULL
 };
 
-static struct attribute_group lm3533_als_attribute_group = {
+static const struct attribute_group lm3533_als_attribute_group = {
 	.attrs = lm3533_als_attributes
 };
 
diff --git a/drivers/iio/light/tsl2563.c b/drivers/iio/light/tsl2563.c
index 04598ae993d4..e7d4ea75e007 100644
--- a/drivers/iio/light/tsl2563.c
+++ b/drivers/iio/light/tsl2563.c
@@ -884,9 +884,19 @@ static const struct i2c_device_id tsl2563_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, tsl2563_id);
 
+static const struct of_device_id tsl2563_of_match[] = {
+	{ .compatible = "amstaos,tsl2560" },
+	{ .compatible = "amstaos,tsl2561" },
+	{ .compatible = "amstaos,tsl2562" },
+	{ .compatible = "amstaos,tsl2563" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, tsl2563_of_match);
+
 static struct i2c_driver tsl2563_i2c_driver = {
 	.driver = {
 		.name	 = "tsl2563",
+		.of_match_table = tsl2563_of_match,
 		.pm	= TSL2563_PM_OPS,
 	},
 	.probe		= tsl2563_probe,
diff --git a/drivers/iio/light/us5182d.c b/drivers/iio/light/us5182d.c
index 18cf2e29e4d5..d571ad7291ed 100644
--- a/drivers/iio/light/us5182d.c
+++ b/drivers/iio/light/us5182d.c
@@ -972,10 +972,17 @@ static const struct i2c_device_id us5182d_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, us5182d_id);
 
+static const struct of_device_id us5182d_of_match[] = {
+	{ .compatible = "upisemi,usd5182" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, us5182d_of_match);
+
 static struct i2c_driver us5182d_driver = {
 	.driver = {
 		.name = US5182D_DRV_NAME,
 		.pm = &us5182d_pm_ops,
+		.of_match_table = us5182d_of_match,
 		.acpi_match_table = ACPI_PTR(us5182d_acpi_match),
 	},
 	.probe = us5182d_probe,
diff --git a/drivers/iio/light/vl6180.c b/drivers/iio/light/vl6180.c
new file mode 100644
index 000000000000..6e25b724d941
--- /dev/null
+++ b/drivers/iio/light/vl6180.c
@@ -0,0 +1,543 @@
+/*
+ * vl6180.c - Support for STMicroelectronics VL6180 ALS, range and proximity
+ * sensor
+ *
+ * Copyright 2017 Peter Meerwald-Stadler <pmeerw@pmeerw.net>
+ * Copyright 2017 Manivannan Sadhasivam <manivannanece23@gmail.com>
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for VL6180 (7-bit I2C slave address 0x29)
+ *
+ * Range: 0 to 100mm
+ * ALS: < 1 Lux up to 100 kLux
+ * IR: 850nm
+ *
+ * TODO: irq, threshold events, continuous mode, hardware buffer
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define VL6180_DRV_NAME "vl6180"
+
+/* Device identification register and value */
+#define VL6180_MODEL_ID	0x000
+#define VL6180_MODEL_ID_VAL 0xb4
+
+/* Configuration registers */
+#define VL6180_INTR_CONFIG 0x014
+#define VL6180_INTR_CLEAR 0x015
+#define VL6180_OUT_OF_RESET 0x016
+#define VL6180_HOLD 0x017
+#define VL6180_RANGE_START 0x018
+#define VL6180_ALS_START 0x038
+#define VL6180_ALS_GAIN 0x03f
+#define VL6180_ALS_IT 0x040
+
+/* Status registers */
+#define VL6180_RANGE_STATUS 0x04d
+#define VL6180_ALS_STATUS 0x04e
+#define VL6180_INTR_STATUS 0x04f
+
+/* Result value registers */
+#define VL6180_ALS_VALUE 0x050
+#define VL6180_RANGE_VALUE 0x062
+#define VL6180_RANGE_RATE 0x066
+
+/* bits of the RANGE_START and ALS_START register */
+#define VL6180_MODE_CONT BIT(1) /* continuous mode */
+#define VL6180_STARTSTOP BIT(0) /* start measurement, auto-reset */
+
+/* bits of the INTR_STATUS and INTR_CONFIG register */
+#define VL6180_ALS_READY BIT(5)
+#define VL6180_RANGE_READY BIT(2)
+
+/* bits of the INTR_CLEAR register */
+#define VL6180_CLEAR_ERROR BIT(2)
+#define VL6180_CLEAR_ALS BIT(1)
+#define VL6180_CLEAR_RANGE BIT(0)
+
+/* bits of the HOLD register */
+#define VL6180_HOLD_ON BIT(0)
+
+/* default value for the ALS_IT register */
+#define VL6180_ALS_IT_100 0x63 /* 100 ms */
+
+/* values for the ALS_GAIN register */
+#define VL6180_ALS_GAIN_1 0x46
+#define VL6180_ALS_GAIN_1_25 0x45
+#define VL6180_ALS_GAIN_1_67 0x44
+#define VL6180_ALS_GAIN_2_5 0x43
+#define VL6180_ALS_GAIN_5 0x42
+#define VL6180_ALS_GAIN_10 0x41
+#define VL6180_ALS_GAIN_20 0x40
+#define VL6180_ALS_GAIN_40 0x47
+
+struct vl6180_data {
+	struct i2c_client *client;
+	struct mutex lock;
+};
+
+enum { VL6180_ALS, VL6180_RANGE, VL6180_PROX };
+
+/**
+ * struct vl6180_chan_regs - Registers for accessing channels
+ * @drdy_mask:			Data ready bit in status register
+ * @start_reg:			Conversion start register
+ * @value_reg:			Result value register
+ * @word:			Register word length
+ */
+struct vl6180_chan_regs {
+	u8 drdy_mask;
+	u16 start_reg, value_reg;
+	bool word;
+};
+
+static const struct vl6180_chan_regs vl6180_chan_regs_table[] = {
+	[VL6180_ALS] = {
+		.drdy_mask = VL6180_ALS_READY,
+		.start_reg = VL6180_ALS_START,
+		.value_reg = VL6180_ALS_VALUE,
+		.word = true,
+	},
+	[VL6180_RANGE] = {
+		.drdy_mask = VL6180_RANGE_READY,
+		.start_reg = VL6180_RANGE_START,
+		.value_reg = VL6180_RANGE_VALUE,
+		.word = false,
+	},
+	[VL6180_PROX] = {
+		.drdy_mask = VL6180_RANGE_READY,
+		.start_reg = VL6180_RANGE_START,
+		.value_reg = VL6180_RANGE_RATE,
+		.word = true,
+	},
+};
+
+static int vl6180_read(struct i2c_client *client, u16 cmd, void *databuf,
+		       u8 len)
+{
+	__be16 cmdbuf = cpu_to_be16(cmd);
+	struct i2c_msg msgs[2] = {
+		{ .addr = client->addr, .len = sizeof(cmdbuf), .buf = (u8 *) &cmdbuf },
+		{ .addr = client->addr, .len = len, .buf = databuf,
+		  .flags = I2C_M_RD } };
+	int ret;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		dev_err(&client->dev, "failed reading register 0x%04x\n", cmd);
+
+	return ret;
+}
+
+static int vl6180_read_byte(struct i2c_client *client, u16 cmd)
+{
+	u8 data;
+	int ret;
+
+	ret = vl6180_read(client, cmd, &data, sizeof(data));
+	if (ret < 0)
+		return ret;
+
+	return data;
+}
+
+static int vl6180_read_word(struct i2c_client *client, u16 cmd)
+{
+	__be16 data;
+	int ret;
+
+	ret = vl6180_read(client, cmd, &data, sizeof(data));
+	if (ret < 0)
+		return ret;
+
+	return be16_to_cpu(data);
+}
+
+static int vl6180_write_byte(struct i2c_client *client, u16 cmd, u8 val)
+{
+	u8 buf[3];
+	struct i2c_msg msgs[1] = {
+		{ .addr = client->addr, .len = sizeof(buf), .buf = (u8 *) &buf } };
+	int ret;
+
+	buf[0] = cmd >> 8;
+	buf[1] = cmd & 0xff;
+	buf[2] = val;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writing register 0x%04x\n", cmd);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vl6180_write_word(struct i2c_client *client, u16 cmd, u16 val)
+{
+	__be16 buf[2];
+	struct i2c_msg msgs[1] = {
+		{ .addr = client->addr, .len = sizeof(buf), .buf = (u8 *) &buf } };
+	int ret;
+
+	buf[0] = cpu_to_be16(cmd);
+	buf[1] = cpu_to_be16(val);
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0) {
+		dev_err(&client->dev, "failed writing register 0x%04x\n", cmd);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int vl6180_measure(struct vl6180_data *data, int addr)
+{
+	struct i2c_client *client = data->client;
+	int tries = 20, ret;
+	u16 value;
+
+	mutex_lock(&data->lock);
+	/* Start single shot measurement */
+	ret = vl6180_write_byte(client,
+		vl6180_chan_regs_table[addr].start_reg, VL6180_STARTSTOP);
+	if (ret < 0)
+		goto fail;
+
+	while (tries--) {
+		ret = vl6180_read_byte(client, VL6180_INTR_STATUS);
+		if (ret < 0)
+			goto fail;
+
+		if (ret & vl6180_chan_regs_table[addr].drdy_mask)
+			break;
+		msleep(20);
+	}
+
+	if (tries < 0) {
+		ret = -EIO;
+		goto fail;
+	}
+
+	/* Read result value from appropriate registers */
+	ret = vl6180_chan_regs_table[addr].word ?
+		vl6180_read_word(client, vl6180_chan_regs_table[addr].value_reg) :
+		vl6180_read_byte(client, vl6180_chan_regs_table[addr].value_reg);
+	if (ret < 0)
+		goto fail;
+	value = ret;
+
+	/* Clear the interrupt flag after data read */
+	ret = vl6180_write_byte(client, VL6180_INTR_CLEAR,
+		VL6180_CLEAR_ERROR | VL6180_CLEAR_ALS | VL6180_CLEAR_RANGE);
+	if (ret < 0)
+		goto fail;
+
+	ret = value;
+
+fail:
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static const struct iio_chan_spec vl6180_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.address = VL6180_ALS,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_INT_TIME) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_HARDWAREGAIN),
+	}, {
+		.type = IIO_DISTANCE,
+		.address = VL6180_RANGE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
+	}, {
+		.type = IIO_PROXIMITY,
+		.address = VL6180_PROX,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+	}
+};
+
+/*
+ * Columns 3 & 4 represent the same value in decimal and hex notations.
+ * Kept in order to avoid the datatype conversion while reading the
+ * hardware_gain.
+ */
+static const int vl6180_als_gain[8][4] = {
+	{ 1,	0,	70,	VL6180_ALS_GAIN_1 },
+	{ 1,    250000, 69,	VL6180_ALS_GAIN_1_25 },
+	{ 1,    670000, 68,	VL6180_ALS_GAIN_1_67 },
+	{ 2,    500000, 67,	VL6180_ALS_GAIN_2_5 },
+	{ 5,    0,      66,	VL6180_ALS_GAIN_5 },
+	{ 10,   0,      65,	VL6180_ALS_GAIN_10 },
+	{ 20,   0,      64,	VL6180_ALS_GAIN_20 },
+	{ 40,   0,      71,	VL6180_ALS_GAIN_40 }
+};
+
+static int vl6180_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct vl6180_data *data = iio_priv(indio_dev);
+	int ret, i;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = vl6180_measure(data, chan->address);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_INT_TIME:
+		ret = vl6180_read_word(data->client, VL6180_ALS_IT);
+		if (ret < 0)
+			return ret;
+		*val = 0; /* 1 count = 1ms (0 = 1ms) */
+		*val2 = (ret + 1) * 1000; /* convert to seconds */
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_LIGHT:
+			*val = 0; /* one ALS count is 0.32 Lux */
+			*val2 = 320000;
+			break;
+		case IIO_DISTANCE:
+			*val = 0; /* sensor reports mm, scale to meter */
+			*val2 = 1000;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		ret = vl6180_read_byte(data->client, VL6180_ALS_GAIN);
+		if (ret < 0)
+			return -EINVAL;
+		for (i = 0; i < ARRAY_SIZE(vl6180_als_gain); i++) {
+			if (ret == vl6180_als_gain[i][2]) {
+				*val = vl6180_als_gain[i][0];
+				*val2 = vl6180_als_gain[i][1];
+			}
+		}
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static IIO_CONST_ATTR(als_gain_available, "1 1.25 1.67 2.5 5 10 20 40");
+
+static struct attribute *vl6180_attributes[] = {
+	&iio_const_attr_als_gain_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group vl6180_attribute_group = {
+	.attrs = vl6180_attributes,
+};
+
+/* HOLD is needed before updating any config registers */
+static int vl6180_hold(struct vl6180_data *data, bool hold)
+{
+	return vl6180_write_byte(data->client, VL6180_HOLD,
+		hold ? VL6180_HOLD_ON : 0);
+}
+
+static int vl6180_set_als_gain(struct vl6180_data *data, int val, int val2)
+{
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(vl6180_als_gain); i++) {
+		if (val == vl6180_als_gain[i][0] &&
+			val2 == vl6180_als_gain[i][1]) {
+			mutex_lock(&data->lock);
+			ret = vl6180_hold(data, true);
+			if (ret < 0)
+				goto fail;
+			ret = vl6180_write_byte(data->client, VL6180_ALS_GAIN,
+				vl6180_als_gain[i][3]);
+fail:
+			vl6180_hold(data, false);
+			mutex_unlock(&data->lock);
+			return ret;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int vl6180_set_it(struct vl6180_data *data, int val2)
+{
+	int ret;
+
+	mutex_lock(&data->lock);
+	ret = vl6180_hold(data, true);
+	if (ret < 0)
+		goto fail;
+	ret = vl6180_write_word(data->client, VL6180_ALS_IT,
+		(val2 - 500) / 1000); /* write value in ms */
+fail:
+	vl6180_hold(data, false);
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static int vl6180_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct vl6180_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_INT_TIME:
+		if (val != 0 || val2 < 500 || val2 >= 512500)
+			return -EINVAL;
+
+		return vl6180_set_it(data, val2);
+	case IIO_CHAN_INFO_HARDWAREGAIN:
+		if (chan->type != IIO_LIGHT)
+			return -EINVAL;
+
+		return vl6180_set_als_gain(data, val, val2);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info vl6180_info = {
+	.read_raw = vl6180_read_raw,
+	.write_raw = vl6180_write_raw,
+	.attrs = &vl6180_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+static int vl6180_init(struct vl6180_data *data)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+
+	ret = vl6180_read_byte(client, VL6180_MODEL_ID);
+	if (ret < 0)
+		return ret;
+
+	if (ret != VL6180_MODEL_ID_VAL) {
+		dev_err(&client->dev, "invalid model ID %02x\n", ret);
+		return -ENODEV;
+	}
+
+	ret = vl6180_hold(data, true);
+	if (ret < 0)
+		return ret;
+
+	ret = vl6180_read_byte(client, VL6180_OUT_OF_RESET);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * Detect false reset condition here. This bit is always set when the
+	 * system comes out of reset.
+	 */
+	if (ret != 0x01)
+		dev_info(&client->dev, "device is not fresh out of reset\n");
+
+	/* Enable ALS and Range ready interrupts */
+	ret = vl6180_write_byte(client, VL6180_INTR_CONFIG,
+				VL6180_ALS_READY | VL6180_RANGE_READY);
+	if (ret < 0)
+		return ret;
+
+	/* ALS integration time: 100ms */
+	ret = vl6180_write_word(client, VL6180_ALS_IT, VL6180_ALS_IT_100);
+	if (ret < 0)
+		return ret;
+
+	/* ALS gain: 1 */
+	ret = vl6180_write_byte(client, VL6180_ALS_GAIN, VL6180_ALS_GAIN_1);
+	if (ret < 0)
+		return ret;
+
+	ret = vl6180_write_byte(client, VL6180_OUT_OF_RESET, 0x00);
+	if (ret < 0)
+		return ret;
+
+	return vl6180_hold(data, false);
+}
+
+static int vl6180_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct vl6180_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &vl6180_info;
+	indio_dev->channels = vl6180_channels;
+	indio_dev->num_channels = ARRAY_SIZE(vl6180_channels);
+	indio_dev->name = VL6180_DRV_NAME;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = vl6180_init(data);
+	if (ret < 0)
+		return ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct of_device_id vl6180_of_match[] = {
+	{ .compatible = "st,vl6180", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, vl6180_of_match);
+
+static const struct i2c_device_id vl6180_id[] = {
+	{ "vl6180", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, vl6180_id);
+
+static struct i2c_driver vl6180_driver = {
+	.driver = {
+		.name   = VL6180_DRV_NAME,
+		.of_match_table = of_match_ptr(vl6180_of_match),
+	},
+	.probe  = vl6180_probe,
+	.id_table = vl6180_id,
+};
+
+module_i2c_driver(vl6180_driver);
+
+MODULE_AUTHOR("Peter Meerwald-Stadler <pmeerw@pmeerw.net>");
+MODULE_AUTHOR("Manivannan Sadhasivam <manivannanece23@gmail.com>");
+MODULE_DESCRIPTION("STMicro VL6180 ALS, range and proximity sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/magnetometer/bmc150_magn_i2c.c b/drivers/iio/magnetometer/bmc150_magn_i2c.c
index ee05722587aa..57e40dd1222e 100644
--- a/drivers/iio/magnetometer/bmc150_magn_i2c.c
+++ b/drivers/iio/magnetometer/bmc150_magn_i2c.c
@@ -63,9 +63,18 @@ static const struct i2c_device_id bmc150_magn_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, bmc150_magn_i2c_id);
 
+static const struct of_device_id bmc150_magn_of_match[] = {
+	{ .compatible = "bosch,bmc150_magn" },
+	{ .compatible = "bosch,bmc156_magn" },
+	{ .compatible = "bosch,bmm150_magn" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, bmc150_magn_of_match);
+
 static struct i2c_driver bmc150_magn_driver = {
 	.driver = {
 		.name	= "bmc150_magn_i2c",
+		.of_match_table = bmc150_magn_of_match,
 		.acpi_match_table = ACPI_PTR(bmc150_magn_acpi_match),
 		.pm	= &bmc150_magn_pm_ops,
 	},
diff --git a/drivers/iio/magnetometer/mag3110.c b/drivers/iio/magnetometer/mag3110.c
index b4f643fb3b1e..dad8d57f7402 100644
--- a/drivers/iio/magnetometer/mag3110.c
+++ b/drivers/iio/magnetometer/mag3110.c
@@ -441,9 +441,16 @@ static const struct i2c_device_id mag3110_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mag3110_id);
 
+static const struct of_device_id mag3110_of_match[] = {
+	{ .compatible = "fsl,mag3110" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mag3110_of_match);
+
 static struct i2c_driver mag3110_driver = {
 	.driver = {
 		.name	= "mag3110",
+		.of_match_table = mag3110_of_match,
 		.pm	= MAG3110_PM_OPS,
 	},
 	.probe = mag3110_probe,
diff --git a/drivers/iio/potentiostat/lmp91000.c b/drivers/iio/potentiostat/lmp91000.c
index e22714365022..afa8de3418d0 100644
--- a/drivers/iio/potentiostat/lmp91000.c
+++ b/drivers/iio/potentiostat/lmp91000.c
@@ -325,6 +325,7 @@ static int lmp91000_probe(struct i2c_client *client,
 	indio_dev->channels = lmp91000_channels;
 	indio_dev->num_channels = ARRAY_SIZE(lmp91000_channels);
 	indio_dev->name = LMP91000_DRV_NAME;
+	indio_dev->dev.parent = &client->dev;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	i2c_set_clientdata(client, indio_dev);
 
diff --git a/drivers/iio/pressure/bmp280-core.c b/drivers/iio/pressure/bmp280-core.c
index 4d18826ac63c..d82b788374b6 100644
--- a/drivers/iio/pressure/bmp280-core.c
+++ b/drivers/iio/pressure/bmp280-core.c
@@ -175,11 +175,12 @@ static u32 bmp280_compensate_humidity(struct bmp280_data *data,
 	}
 	H6 = sign_extend32(tmp, 7);
 
-	var = ((s32)data->t_fine) - 76800;
-	var = ((((adc_humidity << 14) - (H4 << 20) - (H5 * var)) + 16384) >> 15)
-		* (((((((var * H6) >> 10) * (((var * H3) >> 11) + 32768)) >> 10)
-		+ 2097152) * H2 + 8192) >> 14);
-	var -= ((((var >> 15) * (var >> 15)) >> 7) * H1) >> 4;
+	var = ((s32)data->t_fine) - (s32)76800;
+	var = ((((adc_humidity << 14) - (H4 << 20) - (H5 * var))
+		+ (s32)16384) >> 15) * (((((((var * H6) >> 10)
+		* (((var * (s32)H3) >> 11) + (s32)32768)) >> 10)
+		+ (s32)2097152) * H2 + 8192) >> 14);
+	var -= ((((var >> 15) * (var >> 15)) >> 7) * (s32)H1) >> 4;
 
 	return var >> 12;
 };
diff --git a/drivers/iio/pressure/hp03.c b/drivers/iio/pressure/hp03.c
index ac76515d5d49..8c7b3ec3d84a 100644
--- a/drivers/iio/pressure/hp03.c
+++ b/drivers/iio/pressure/hp03.c
@@ -297,9 +297,16 @@ static const struct i2c_device_id hp03_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, hp03_id);
 
+static const struct of_device_id hp03_of_match[] = {
+	{ .compatible = "hoperf,hp03" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, hp03_of_match);
+
 static struct i2c_driver hp03_driver = {
 	.driver = {
 		.name	= "hp03",
+		.of_match_table = hp03_of_match,
 	},
 	.probe		= hp03_probe,
 	.remove		= hp03_remove,
diff --git a/drivers/iio/pressure/mpl3115.c b/drivers/iio/pressure/mpl3115.c
index 525644a7442d..619b963714c7 100644
--- a/drivers/iio/pressure/mpl3115.c
+++ b/drivers/iio/pressure/mpl3115.c
@@ -321,9 +321,16 @@ static const struct i2c_device_id mpl3115_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mpl3115_id);
 
+static const struct of_device_id mpl3115_of_match[] = {
+	{ .compatible = "fsl,mpl3115" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mpl3115_of_match);
+
 static struct i2c_driver mpl3115_driver = {
 	.driver = {
 		.name	= "mpl3115",
+		.of_match_table = mpl3115_of_match,
 		.pm	= MPL3115_PM_OPS,
 	},
 	.probe = mpl3115_probe,
diff --git a/drivers/iio/pressure/zpa2326.c b/drivers/iio/pressure/zpa2326.c
index c720c3ac0b9b..e58a0ad07477 100644
--- a/drivers/iio/pressure/zpa2326.c
+++ b/drivers/iio/pressure/zpa2326.c
@@ -751,7 +751,7 @@ static void zpa2326_suspend(struct iio_dev *indio_dev)
  */
 static irqreturn_t zpa2326_handle_irq(int irq, void *data)
 {
-	struct iio_dev *indio_dev = (struct iio_dev *)data;
+	struct iio_dev *indio_dev = data;
 
 	if (iio_buffer_enabled(indio_dev)) {
 		/* Timestamping needed for buffered sampling only. */
@@ -790,7 +790,7 @@ static irqreturn_t zpa2326_handle_irq(int irq, void *data)
  */
 static irqreturn_t zpa2326_handle_threaded_irq(int irq, void *data)
 {
-	struct iio_dev         *indio_dev = (struct iio_dev *)data;
+	struct iio_dev         *indio_dev = data;
 	struct zpa2326_private *priv = iio_priv(indio_dev);
 	unsigned int            val;
 	bool                    cont;
diff --git a/drivers/iio/proximity/Kconfig b/drivers/iio/proximity/Kconfig
index ab96cb7a0054..5b81a8c9d438 100644
--- a/drivers/iio/proximity/Kconfig
+++ b/drivers/iio/proximity/Kconfig
@@ -32,6 +32,17 @@ config LIDAR_LITE_V2
 	  To compile this driver as a module, choose M here: the
 	  module will be called pulsedlight-lite-v2
 
+config SRF04
+	tristate "Devantech SRF04 ultrasonic ranger sensor"
+	depends on GPIOLIB
+	help
+	  Say Y here to build a driver for Devantech SRF04 ultrasonic
+	  ranger sensor. This driver can be used to measure the distance
+	  of objects. It is using two GPIOs.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called srf04.
+
 config SX9500
 	tristate "SX9500 Semtech proximity sensor"
 	select IIO_BUFFER
diff --git a/drivers/iio/proximity/Makefile b/drivers/iio/proximity/Makefile
index e914c2a5dd49..ed1b6f4cc209 100644
--- a/drivers/iio/proximity/Makefile
+++ b/drivers/iio/proximity/Makefile
@@ -5,5 +5,6 @@
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_AS3935)		+= as3935.o
 obj-$(CONFIG_LIDAR_LITE_V2)	+= pulsedlight-lidar-lite-v2.o
+obj-$(CONFIG_SRF04)		+= srf04.o
 obj-$(CONFIG_SRF08)		+= srf08.o
 obj-$(CONFIG_SX9500)		+= sx9500.o
diff --git a/drivers/iio/proximity/as3935.c b/drivers/iio/proximity/as3935.c
index 5656deb17261..ddf9bee89f77 100644
--- a/drivers/iio/proximity/as3935.c
+++ b/drivers/iio/proximity/as3935.c
@@ -50,7 +50,6 @@
 #define AS3935_TUNE_CAP		0x08
 #define AS3935_CALIBRATE	0x3D
 
-#define AS3935_WRITE_DATA	BIT(15)
 #define AS3935_READ_DATA	BIT(14)
 #define AS3935_ADDRESS(x)	((x) << 8)
 
@@ -105,7 +104,7 @@ static int as3935_write(struct as3935_state *st,
 {
 	u8 *buf = st->buf;
 
-	buf[0] = (AS3935_WRITE_DATA | AS3935_ADDRESS(reg)) >> 8;
+	buf[0] = AS3935_ADDRESS(reg) >> 8;
 	buf[1] = val;
 
 	return spi_write(st->spi, buf, 2);
@@ -155,7 +154,7 @@ static struct attribute *as3935_attributes[] = {
 	NULL,
 };
 
-static struct attribute_group as3935_attribute_group = {
+static const struct attribute_group as3935_attribute_group = {
 	.attrs = as3935_attributes,
 };
 
diff --git a/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
index 20c16a08c9d9..36c1ddc251aa 100644
--- a/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
+++ b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
@@ -278,6 +278,7 @@ static int lidar_probe(struct i2c_client *client,
 	indio_dev->name = LIDAR_DRV_NAME;
 	indio_dev->channels = lidar_channels;
 	indio_dev->num_channels = ARRAY_SIZE(lidar_channels);
+	indio_dev->dev.parent = &client->dev;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
 	i2c_set_clientdata(client, indio_dev);
diff --git a/drivers/iio/proximity/srf04.c b/drivers/iio/proximity/srf04.c
new file mode 100644
index 000000000000..e37667f933b3
--- /dev/null
+++ b/drivers/iio/proximity/srf04.c
@@ -0,0 +1,304 @@
+/*
+ * SRF04: ultrasonic sensor for distance measuring by using GPIOs
+ *
+ * Copyright (c) 2017 Andreas Klinger <ak@it-klinger.de>
+ *
+ * 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.
+ *
+ * For details about the device see:
+ * http://www.robot-electronics.co.uk/htm/srf04tech.htm
+ *
+ * the measurement cycle as timing diagram looks like:
+ *
+ *          +---+
+ * GPIO     |   |
+ * trig:  --+   +------------------------------------------------------
+ *          ^   ^
+ *          |<->|
+ *         udelay(10)
+ *
+ * ultra           +-+ +-+ +-+
+ * sonic           | | | | | |
+ * burst: ---------+ +-+ +-+ +-----------------------------------------
+ *                           .
+ * ultra                     .              +-+ +-+ +-+
+ * sonic                     .              | | | | | |
+ * echo:  ----------------------------------+ +-+ +-+ +----------------
+ *                           .                        .
+ *                           +------------------------+
+ * GPIO                      |                        |
+ * echo:  -------------------+                        +---------------
+ *                           ^                        ^
+ *                           interrupt                interrupt
+ *                           (ts_rising)              (ts_falling)
+ *                           |<---------------------->|
+ *                              pulse time measured
+ *                              --> one round trip of ultra sonic waves
+ */
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+struct srf04_data {
+	struct device		*dev;
+	struct gpio_desc	*gpiod_trig;
+	struct gpio_desc	*gpiod_echo;
+	struct mutex		lock;
+	int			irqnr;
+	ktime_t			ts_rising;
+	ktime_t			ts_falling;
+	struct completion	rising;
+	struct completion	falling;
+};
+
+static irqreturn_t srf04_handle_irq(int irq, void *dev_id)
+{
+	struct iio_dev *indio_dev = dev_id;
+	struct srf04_data *data = iio_priv(indio_dev);
+	ktime_t now = ktime_get();
+
+	if (gpiod_get_value(data->gpiod_echo)) {
+		data->ts_rising = now;
+		complete(&data->rising);
+	} else {
+		data->ts_falling = now;
+		complete(&data->falling);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int srf04_read(struct srf04_data *data)
+{
+	int ret;
+	ktime_t ktime_dt;
+	u64 dt_ns;
+	u32 time_ns, distance_mm;
+
+	/*
+	 * just one read-echo-cycle can take place at a time
+	 * ==> lock against concurrent reading calls
+	 */
+	mutex_lock(&data->lock);
+
+	reinit_completion(&data->rising);
+	reinit_completion(&data->falling);
+
+	gpiod_set_value(data->gpiod_trig, 1);
+	udelay(10);
+	gpiod_set_value(data->gpiod_trig, 0);
+
+	/* it cannot take more than 20 ms */
+	ret = wait_for_completion_killable_timeout(&data->rising, HZ/50);
+	if (ret < 0) {
+		mutex_unlock(&data->lock);
+		return ret;
+	} else if (ret == 0) {
+		mutex_unlock(&data->lock);
+		return -ETIMEDOUT;
+	}
+
+	ret = wait_for_completion_killable_timeout(&data->falling, HZ/50);
+	if (ret < 0) {
+		mutex_unlock(&data->lock);
+		return ret;
+	} else if (ret == 0) {
+		mutex_unlock(&data->lock);
+		return -ETIMEDOUT;
+	}
+
+	ktime_dt = ktime_sub(data->ts_falling, data->ts_rising);
+
+	mutex_unlock(&data->lock);
+
+	dt_ns = ktime_to_ns(ktime_dt);
+	/*
+	 * measuring more than 3 meters is beyond the capabilities of
+	 * the sensor
+	 * ==> filter out invalid results for not measuring echos of
+	 *     another us sensor
+	 *
+	 * formula:
+	 *         distance       3 m
+	 * time = ---------- = --------- = 9404389 ns
+	 *          speed       319 m/s
+	 *
+	 * using a minimum speed at -20 °C of 319 m/s
+	 */
+	if (dt_ns > 9404389)
+		return -EIO;
+
+	time_ns = dt_ns;
+
+	/*
+	 * the speed as function of the temperature is approximately:
+	 *
+	 * speed = 331,5 + 0,6 * Temp
+	 *   with Temp in °C
+	 *   and speed in m/s
+	 *
+	 * use 343 m/s as ultrasonic speed at 20 °C here in absence of the
+	 * temperature
+	 *
+	 * therefore:
+	 *             time     343
+	 * distance = ------ * -----
+	 *             10^6       2
+	 *   with time in ns
+	 *   and distance in mm (one way)
+	 *
+	 * because we limit to 3 meters the multiplication with 343 just
+	 * fits into 32 bit
+	 */
+	distance_mm = time_ns * 343 / 2000000;
+
+	return distance_mm;
+}
+
+static int srf04_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *channel, int *val,
+			    int *val2, long info)
+{
+	struct srf04_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (channel->type != IIO_DISTANCE)
+		return -EINVAL;
+
+	switch (info) {
+	case IIO_CHAN_INFO_RAW:
+		ret = srf04_read(data);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/*
+		 * theoretical maximum resolution is 3 mm
+		 * 1 LSB is 1 mm
+		 */
+		*val = 0;
+		*val2 = 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info srf04_iio_info = {
+	.driver_module		= THIS_MODULE,
+	.read_raw		= srf04_read_raw,
+};
+
+static const struct iio_chan_spec srf04_chan_spec[] = {
+	{
+		.type = IIO_DISTANCE,
+		.info_mask_separate =
+				BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static int srf04_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct srf04_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(struct srf04_data));
+	if (!indio_dev) {
+		dev_err(dev, "failed to allocate IIO device\n");
+		return -ENOMEM;
+	}
+
+	data = iio_priv(indio_dev);
+	data->dev = dev;
+
+	mutex_init(&data->lock);
+	init_completion(&data->rising);
+	init_completion(&data->falling);
+
+	data->gpiod_trig = devm_gpiod_get(dev, "trig", GPIOD_OUT_LOW);
+	if (IS_ERR(data->gpiod_trig)) {
+		dev_err(dev, "failed to get trig-gpios: err=%ld\n",
+					PTR_ERR(data->gpiod_trig));
+		return PTR_ERR(data->gpiod_trig);
+	}
+
+	data->gpiod_echo = devm_gpiod_get(dev, "echo", GPIOD_IN);
+	if (IS_ERR(data->gpiod_echo)) {
+		dev_err(dev, "failed to get echo-gpios: err=%ld\n",
+					PTR_ERR(data->gpiod_echo));
+		return PTR_ERR(data->gpiod_echo);
+	}
+
+	if (gpiod_cansleep(data->gpiod_echo)) {
+		dev_err(data->dev, "cansleep-GPIOs not supported\n");
+		return -ENODEV;
+	}
+
+	data->irqnr = gpiod_to_irq(data->gpiod_echo);
+	if (data->irqnr < 0) {
+		dev_err(data->dev, "gpiod_to_irq: %d\n", data->irqnr);
+		return data->irqnr;
+	}
+
+	ret = devm_request_irq(dev, data->irqnr, srf04_handle_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			pdev->name, indio_dev);
+	if (ret < 0) {
+		dev_err(data->dev, "request_irq: %d\n", ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	indio_dev->name = "srf04";
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &srf04_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = srf04_chan_spec;
+	indio_dev->num_channels = ARRAY_SIZE(srf04_chan_spec);
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+
+static const struct of_device_id of_srf04_match[] = {
+	{ .compatible = "devantech,srf04", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, of_srf04_match);
+
+static struct platform_driver srf04_driver = {
+	.probe		= srf04_probe,
+	.driver		= {
+		.name		= "srf04-gpio",
+		.of_match_table	= of_srf04_match,
+	},
+};
+
+module_platform_driver(srf04_driver);
+
+MODULE_AUTHOR("Andreas Klinger <ak@it-klinger.de>");
+MODULE_DESCRIPTION("SRF04 ultrasonic sensor for distance measuring using GPIOs");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:srf04");
diff --git a/drivers/iio/temperature/Kconfig b/drivers/iio/temperature/Kconfig
index 3089e8d0a32d..5378976d6d27 100644
--- a/drivers/iio/temperature/Kconfig
+++ b/drivers/iio/temperature/Kconfig
@@ -19,6 +19,20 @@ config MAXIM_THERMOCOUPLE
 	  This driver can also be built as a module. If so, the module will
 	  be called maxim_thermocouple.
 
+config HID_SENSOR_TEMP
+	tristate "HID Environmental temperature sensor"
+	depends on HID_SENSOR_HUB
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select HID_SENSOR_IIO_COMMON
+	select HID_SENSOR_IIO_TRIGGER
+	help
+	  Say yes here to build support for the HID SENSOR
+	  temperature driver
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called hid-sensor-temperature.
+
 config MLX90614
 	tristate "MLX90614 contact-less infrared sensor"
 	depends on I2C
diff --git a/drivers/iio/temperature/Makefile b/drivers/iio/temperature/Makefile
index 4c4377480726..ad1d668de546 100644
--- a/drivers/iio/temperature/Makefile
+++ b/drivers/iio/temperature/Makefile
@@ -2,6 +2,7 @@
 # Makefile for industrial I/O temperature drivers
 #
 
+obj-$(CONFIG_HID_SENSOR_TEMP) += hid-sensor-temperature.o
 obj-$(CONFIG_MAXIM_THERMOCOUPLE) += maxim_thermocouple.o
 obj-$(CONFIG_MLX90614) += mlx90614.o
 obj-$(CONFIG_TMP006) += tmp006.o
diff --git a/drivers/iio/temperature/hid-sensor-temperature.c b/drivers/iio/temperature/hid-sensor-temperature.c
new file mode 100644
index 000000000000..c01efeca4002
--- /dev/null
+++ b/drivers/iio/temperature/hid-sensor-temperature.c
@@ -0,0 +1,311 @@
+/*
+ * HID Sensors Driver
+ * Copyright (c) 2017, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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.
+ */
+#include <linux/device.h>
+#include <linux/hid-sensor-hub.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "../common/hid-sensors/hid-sensor-trigger.h"
+
+struct temperature_state {
+	struct hid_sensor_common common_attributes;
+	struct hid_sensor_hub_attribute_info temperature_attr;
+	s32 temperature_data;
+	int scale_pre_decml;
+	int scale_post_decml;
+	int scale_precision;
+	int value_offset;
+};
+
+/* Channel definitions */
+static const struct iio_chan_spec temperature_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) |
+			BIT(IIO_CHAN_INFO_SCALE) |
+			BIT(IIO_CHAN_INFO_SAMP_FREQ) |
+			BIT(IIO_CHAN_INFO_HYSTERESIS),
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+/* Adjust channel real bits based on report descriptor */
+static void temperature_adjust_channel_bit_mask(struct iio_chan_spec *channels,
+					int channel, int size)
+{
+	channels[channel].scan_type.sign = 's';
+	/* Real storage bits will change based on the report desc. */
+	channels[channel].scan_type.realbits = size * 8;
+	/* Maximum size of a sample to capture is s32 */
+	channels[channel].scan_type.storagebits = sizeof(s32) * 8;
+}
+
+static int temperature_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct temperature_state *temp_st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (chan->type != IIO_TEMP)
+			return -EINVAL;
+		hid_sensor_power_state(
+			&temp_st->common_attributes, true);
+		*val = sensor_hub_input_attr_get_raw_value(
+			temp_st->common_attributes.hsdev,
+			HID_USAGE_SENSOR_TEMPERATURE,
+			HID_USAGE_SENSOR_DATA_ENVIRONMENTAL_TEMPERATURE,
+			temp_st->temperature_attr.report_id,
+			SENSOR_HUB_SYNC);
+		hid_sensor_power_state(
+				&temp_st->common_attributes,
+				false);
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = temp_st->scale_pre_decml;
+		*val2 = temp_st->scale_post_decml;
+		return temp_st->scale_precision;
+
+	case IIO_CHAN_INFO_OFFSET:
+		*val = temp_st->value_offset;
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return hid_sensor_read_samp_freq_value(
+				&temp_st->common_attributes, val, val2);
+
+	case IIO_CHAN_INFO_HYSTERESIS:
+		return hid_sensor_read_raw_hyst_value(
+				&temp_st->common_attributes, val, val2);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int temperature_write_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int val, int val2, long mask)
+{
+	struct temperature_state *temp_st = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return hid_sensor_write_samp_freq_value(
+				&temp_st->common_attributes, val, val2);
+	case IIO_CHAN_INFO_HYSTERESIS:
+		return hid_sensor_write_raw_hyst_value(
+				&temp_st->common_attributes, val, val2);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info temperature_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &temperature_read_raw,
+	.write_raw = &temperature_write_raw,
+};
+
+/* Callback handler to send event after all samples are received and captured */
+static int temperature_proc_event(struct hid_sensor_hub_device *hsdev,
+				unsigned int usage_id, void *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct temperature_state *temp_st = iio_priv(indio_dev);
+
+	if (atomic_read(&temp_st->common_attributes.data_ready))
+		iio_push_to_buffers_with_timestamp(indio_dev,
+				&temp_st->temperature_data,
+				iio_get_time_ns(indio_dev));
+
+	return 0;
+}
+
+/* Capture samples in local storage */
+static int temperature_capture_sample(struct hid_sensor_hub_device *hsdev,
+				unsigned int usage_id, size_t raw_len,
+				char *raw_data, void *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct temperature_state *temp_st = iio_priv(indio_dev);
+
+	switch (usage_id) {
+	case HID_USAGE_SENSOR_DATA_ENVIRONMENTAL_TEMPERATURE:
+		temp_st->temperature_data = *(s32 *)raw_data;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+/* Parse report which is specific to an usage id*/
+static int temperature_parse_report(struct platform_device *pdev,
+				struct hid_sensor_hub_device *hsdev,
+				struct iio_chan_spec *channels,
+				unsigned int usage_id,
+				struct temperature_state *st)
+{
+	int ret;
+
+	ret = sensor_hub_input_get_attribute_info(hsdev, HID_INPUT_REPORT,
+			usage_id,
+			HID_USAGE_SENSOR_DATA_ENVIRONMENTAL_TEMPERATURE,
+			&st->temperature_attr);
+	if (ret < 0)
+		return ret;
+
+	temperature_adjust_channel_bit_mask(channels, 0,
+					st->temperature_attr.size);
+
+	st->scale_precision = hid_sensor_format_scale(
+				HID_USAGE_SENSOR_TEMPERATURE,
+				&st->temperature_attr,
+				&st->scale_pre_decml, &st->scale_post_decml);
+
+	/* Set Sensitivity field ids, when there is no individual modifier */
+	if (st->common_attributes.sensitivity.index < 0)
+		sensor_hub_input_get_attribute_info(hsdev,
+			HID_FEATURE_REPORT, usage_id,
+			HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS |
+			HID_USAGE_SENSOR_DATA_ENVIRONMENTAL_TEMPERATURE,
+			&st->common_attributes.sensitivity);
+
+	return ret;
+}
+
+static struct hid_sensor_hub_callbacks temperature_callbacks = {
+	.send_event = &temperature_proc_event,
+	.capture_sample = &temperature_capture_sample,
+};
+
+/* Function to initialize the processing for usage id */
+static int hid_temperature_probe(struct platform_device *pdev)
+{
+	static const char *name = "temperature";
+	struct iio_dev *indio_dev;
+	struct temperature_state *temp_st;
+	struct iio_chan_spec *temp_chans;
+	struct hid_sensor_hub_device *hsdev = dev_get_platdata(&pdev->dev);
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*temp_st));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	temp_st = iio_priv(indio_dev);
+	temp_st->common_attributes.hsdev = hsdev;
+	temp_st->common_attributes.pdev = pdev;
+
+	ret = hid_sensor_parse_common_attributes(hsdev,
+					HID_USAGE_SENSOR_TEMPERATURE,
+					&temp_st->common_attributes);
+	if (ret)
+		return ret;
+
+	temp_chans = devm_kmemdup(&indio_dev->dev, temperature_channels,
+				sizeof(temperature_channels), GFP_KERNEL);
+	if (!temp_chans)
+		return -ENOMEM;
+
+	ret = temperature_parse_report(pdev, hsdev, temp_chans,
+				HID_USAGE_SENSOR_TEMPERATURE, temp_st);
+	if (ret)
+		return ret;
+
+	indio_dev->channels = temp_chans;
+	indio_dev->num_channels = ARRAY_SIZE(temperature_channels);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &temperature_info;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = devm_iio_triggered_buffer_setup(&pdev->dev, indio_dev,
+					&iio_pollfunc_store_time, NULL, NULL);
+	if (ret)
+		return ret;
+
+	atomic_set(&temp_st->common_attributes.data_ready, 0);
+	ret = hid_sensor_setup_trigger(indio_dev, name,
+				&temp_st->common_attributes);
+	if (ret)
+		return ret;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	temperature_callbacks.pdev = pdev;
+	ret = sensor_hub_register_callback(hsdev, HID_USAGE_SENSOR_TEMPERATURE,
+					&temperature_callbacks);
+	if (ret)
+		goto error_remove_trigger;
+
+	ret = devm_iio_device_register(indio_dev->dev.parent, indio_dev);
+	if (ret)
+		goto error_remove_callback;
+
+	return ret;
+
+error_remove_callback:
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_TEMPERATURE);
+error_remove_trigger:
+	hid_sensor_remove_trigger(&temp_st->common_attributes);
+	return ret;
+}
+
+/* Function to deinitialize the processing for usage id */
+static int hid_temperature_remove(struct platform_device *pdev)
+{
+	struct hid_sensor_hub_device *hsdev = dev_get_platdata(&pdev->dev);
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct temperature_state *temp_st = iio_priv(indio_dev);
+
+	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_TEMPERATURE);
+	hid_sensor_remove_trigger(&temp_st->common_attributes);
+
+	return 0;
+}
+
+static const struct platform_device_id hid_temperature_ids[] = {
+	{
+		/* Format: HID-SENSOR-usage_id_in_hex_lowercase */
+		.name = "HID-SENSOR-200033",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, hid_temperature_ids);
+
+static struct platform_driver hid_temperature_platform_driver = {
+	.id_table = hid_temperature_ids,
+	.driver = {
+		.name	= "temperature-sensor",
+		.pm	= &hid_sensor_pm_ops,
+	},
+	.probe		= hid_temperature_probe,
+	.remove		= hid_temperature_remove,
+};
+module_platform_driver(hid_temperature_platform_driver);
+
+MODULE_DESCRIPTION("HID Environmental temperature sensor");
+MODULE_AUTHOR("Song Hongyan <hongyan.song@intel.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/temperature/maxim_thermocouple.c b/drivers/iio/temperature/maxim_thermocouple.c
index f962f31a5eb2..557214202eff 100644
--- a/drivers/iio/temperature/maxim_thermocouple.c
+++ b/drivers/iio/temperature/maxim_thermocouple.c
@@ -231,6 +231,7 @@ static int maxim_thermocouple_probe(struct spi_device *spi)
 	indio_dev->available_scan_masks = chip->scan_masks;
 	indio_dev->num_channels = chip->num_channels;
 	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->dev.parent = &spi->dev;
 
 	data = iio_priv(indio_dev);
 	data->spi = spi;
diff --git a/drivers/iio/temperature/mlx90614.c b/drivers/iio/temperature/mlx90614.c
index 4b645fc672aa..2077eef4095c 100644
--- a/drivers/iio/temperature/mlx90614.c
+++ b/drivers/iio/temperature/mlx90614.c
@@ -585,6 +585,12 @@ static const struct i2c_device_id mlx90614_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, mlx90614_id);
 
+static const struct of_device_id mlx90614_of_match[] = {
+	{ .compatible = "melexis,mlx90614" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, mlx90614_of_match);
+
 #ifdef CONFIG_PM_SLEEP
 static int mlx90614_pm_suspend(struct device *dev)
 {
@@ -644,6 +650,7 @@ static const struct dev_pm_ops mlx90614_pm_ops = {
 static struct i2c_driver mlx90614_driver = {
 	.driver = {
 		.name	= "mlx90614",
+		.of_match_table = mlx90614_of_match,
 		.pm	= &mlx90614_pm_ops,
 	},
 	.probe = mlx90614_probe,
diff --git a/drivers/iio/temperature/tmp007.c b/drivers/iio/temperature/tmp007.c
index f04d0d1f6ac8..0615324d054c 100644
--- a/drivers/iio/temperature/tmp007.c
+++ b/drivers/iio/temperature/tmp007.c
@@ -11,9 +11,10 @@
  *
  * (7-bit I2C slave address (0x40 - 0x47), changeable via ADR pins)
  *
- * Note: This driver assumes that the sensor has been calibrated beforehand
- *
- * TODO: ALERT irq, limit threshold events
+ * Note:
+ * 1. This driver assumes that the sensor has been calibrated beforehand
+ * 2. Limit threshold events are enabled at the start
+ * 3. Operating mode: INT
  *
  */
 
@@ -24,25 +25,38 @@
 #include <linux/pm.h>
 #include <linux/bitops.h>
 #include <linux/of.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
 
 #define TMP007_TDIE 0x01
 #define TMP007_CONFIG 0x02
 #define TMP007_TOBJECT 0x03
 #define TMP007_STATUS 0x04
 #define TMP007_STATUS_MASK 0x05
+#define TMP007_TOBJ_HIGH_LIMIT 0x06
+#define TMP007_TOBJ_LOW_LIMIT 0x07
+#define TMP007_TDIE_HIGH_LIMIT 0x08
+#define TMP007_TDIE_LOW_LIMIT 0x09
 #define TMP007_MANUFACTURER_ID 0x1e
 #define TMP007_DEVICE_ID 0x1f
 
 #define TMP007_CONFIG_CONV_EN BIT(12)
-#define TMP007_CONFIG_COMP_EN BIT(5)
 #define TMP007_CONFIG_TC_EN BIT(6)
 #define TMP007_CONFIG_CR_MASK GENMASK(11, 9)
+#define TMP007_CONFIG_ALERT_EN BIT(8)
 #define TMP007_CONFIG_CR_SHIFT 9
 
+/* Status register flags */
+#define TMP007_STATUS_ALERT BIT(15)
 #define TMP007_STATUS_CONV_READY BIT(14)
+#define TMP007_STATUS_OHF BIT(13)
+#define TMP007_STATUS_OLF BIT(12)
+#define TMP007_STATUS_LHF BIT(11)
+#define TMP007_STATUS_LLF BIT(10)
 #define TMP007_STATUS_DATA_VALID BIT(9)
 
 #define TMP007_MANUFACTURER_MAGIC 0x5449
@@ -52,7 +66,9 @@
 
 struct tmp007_data {
 	struct i2c_client *client;
+	struct mutex lock;
 	u16 config;
+	u16 status_mask;
 };
 
 static const int tmp007_avgs[5][2] = { {4, 0}, {2, 0}, {1, 0},
@@ -156,6 +172,188 @@ static int tmp007_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
+static irqreturn_t tmp007_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct tmp007_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = i2c_smbus_read_word_swapped(data->client, TMP007_STATUS);
+	if ((ret < 0) || !(ret & (TMP007_STATUS_OHF | TMP007_STATUS_OLF |
+				TMP007_STATUS_LHF | TMP007_STATUS_LLF)))
+		return IRQ_NONE;
+
+	if (ret & TMP007_STATUS_OHF)
+		iio_push_event(indio_dev,
+				IIO_MOD_EVENT_CODE(IIO_TEMP, 0,
+					IIO_MOD_TEMP_OBJECT,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_RISING),
+				iio_get_time_ns(indio_dev));
+
+	if (ret & TMP007_STATUS_OLF)
+		iio_push_event(indio_dev,
+				IIO_MOD_EVENT_CODE(IIO_TEMP, 0,
+					IIO_MOD_TEMP_OBJECT,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_FALLING),
+				iio_get_time_ns(indio_dev));
+
+	if (ret & TMP007_STATUS_LHF)
+		iio_push_event(indio_dev,
+				IIO_MOD_EVENT_CODE(IIO_TEMP, 0,
+					IIO_MOD_TEMP_AMBIENT,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_RISING),
+				iio_get_time_ns(indio_dev));
+
+	if (ret & TMP007_STATUS_LLF)
+		iio_push_event(indio_dev,
+				IIO_MOD_EVENT_CODE(IIO_TEMP, 0,
+					IIO_MOD_TEMP_AMBIENT,
+					IIO_EV_TYPE_THRESH,
+					IIO_EV_DIR_FALLING),
+				iio_get_time_ns(indio_dev));
+
+	return IRQ_HANDLED;
+}
+
+static int tmp007_write_event_config(struct iio_dev *indio_dev,
+		const struct iio_chan_spec *chan, enum iio_event_type type,
+		enum iio_event_direction dir, int state)
+{
+	struct tmp007_data *data = iio_priv(indio_dev);
+	unsigned int status_mask;
+	int ret;
+
+	switch (chan->channel2) {
+	case IIO_MOD_TEMP_AMBIENT:
+	if (dir == IIO_EV_DIR_RISING)
+			status_mask = TMP007_STATUS_LHF;
+		else
+			status_mask = TMP007_STATUS_LLF;
+		break;
+	case IIO_MOD_TEMP_OBJECT:
+		if (dir == IIO_EV_DIR_RISING)
+			status_mask = TMP007_STATUS_OHF;
+		else
+			status_mask = TMP007_STATUS_OLF;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_lock(&data->lock);
+	ret = i2c_smbus_read_word_swapped(data->client, TMP007_STATUS_MASK);
+	mutex_unlock(&data->lock);
+	if (ret < 0)
+		return ret;
+
+	if (state)
+		ret |= status_mask;
+	else
+		ret &= ~status_mask;
+
+	return i2c_smbus_write_word_swapped(data->client, TMP007_STATUS_MASK,
+					data->status_mask = ret);
+}
+
+static int tmp007_read_event_config(struct iio_dev *indio_dev,
+		const struct iio_chan_spec *chan, enum iio_event_type type,
+		enum iio_event_direction dir)
+{
+	struct tmp007_data *data = iio_priv(indio_dev);
+	unsigned int mask;
+
+	switch (chan->channel2) {
+	case IIO_MOD_TEMP_AMBIENT:
+		if (dir == IIO_EV_DIR_RISING)
+			mask = TMP007_STATUS_LHF;
+		else
+			mask = TMP007_STATUS_LLF;
+		break;
+	case IIO_MOD_TEMP_OBJECT:
+		if (dir == IIO_EV_DIR_RISING)
+			mask = TMP007_STATUS_OHF;
+		else
+			mask = TMP007_STATUS_OLF;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return !!(data->status_mask & mask);
+}
+
+static int tmp007_read_thresh(struct iio_dev *indio_dev,
+		const struct iio_chan_spec *chan, enum iio_event_type type,
+		enum iio_event_direction dir, enum iio_event_info info,
+		int *val, int *val2)
+{
+	struct tmp007_data *data = iio_priv(indio_dev);
+	int ret;
+	u8 reg;
+
+	switch (chan->channel2) {
+	case IIO_MOD_TEMP_AMBIENT: /* LSB: 0.5 degree Celsius */
+		if (dir == IIO_EV_DIR_RISING)
+			reg = TMP007_TDIE_HIGH_LIMIT;
+		else
+			reg = TMP007_TDIE_LOW_LIMIT;
+		break;
+	case IIO_MOD_TEMP_OBJECT:
+		if (dir == IIO_EV_DIR_RISING)
+			reg = TMP007_TOBJ_HIGH_LIMIT;
+	else
+			reg = TMP007_TOBJ_LOW_LIMIT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = i2c_smbus_read_word_swapped(data->client, reg);
+	if (ret < 0)
+		return ret;
+
+	/* Shift length 7 bits = 6(15:6) + 1(0.5 LSB) */
+	*val = sign_extend32(ret, 15) >> 7;
+
+	return IIO_VAL_INT;
+}
+
+static int tmp007_write_thresh(struct iio_dev *indio_dev,
+		const struct iio_chan_spec *chan, enum iio_event_type type,
+		enum iio_event_direction dir, enum iio_event_info info,
+		int val, int val2)
+{
+	struct tmp007_data *data = iio_priv(indio_dev);
+	u8 reg;
+
+	switch (chan->channel2) {
+	case IIO_MOD_TEMP_AMBIENT:
+		if (dir == IIO_EV_DIR_RISING)
+			reg = TMP007_TDIE_HIGH_LIMIT;
+		else
+			reg = TMP007_TDIE_LOW_LIMIT;
+		break;
+	case IIO_MOD_TEMP_OBJECT:
+		if (dir == IIO_EV_DIR_RISING)
+			reg = TMP007_TOBJ_HIGH_LIMIT;
+		else
+			reg = TMP007_TOBJ_LOW_LIMIT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Full scale threshold value is +/- 256 degree Celsius */
+	if (val < -256 || val > 255)
+		return -EINVAL;
+
+	/* Shift length 7 bits = 6(15:6) + 1(0.5 LSB) */
+	return i2c_smbus_write_word_swapped(data->client, reg, (val << 7));
+}
+
 static IIO_CONST_ATTR(sampling_frequency_available, "4 2 1 0.5 0.25");
 
 static struct attribute *tmp007_attributes[] = {
@@ -167,6 +365,36 @@ static const struct attribute_group tmp007_attribute_group = {
 	.attrs = tmp007_attributes,
 };
 
+static const struct iio_event_spec tmp007_obj_event[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+			BIT(IIO_EV_INFO_ENABLE),
+	},
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+			BIT(IIO_EV_INFO_ENABLE),
+	},
+};
+
+static const struct iio_event_spec tmp007_die_event[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+			BIT(IIO_EV_INFO_ENABLE),
+	},
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+			BIT(IIO_EV_INFO_ENABLE),
+	},
+};
+
 static const struct iio_chan_spec tmp007_channels[] = {
 	{
 		.type = IIO_TEMP,
@@ -175,6 +403,8 @@ static const struct iio_chan_spec tmp007_channels[] = {
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 				BIT(IIO_CHAN_INFO_SCALE),
 		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+		.event_spec = tmp007_die_event,
+		.num_event_specs = ARRAY_SIZE(tmp007_die_event),
 	},
 	{
 		.type = IIO_TEMP,
@@ -183,12 +413,18 @@ static const struct iio_chan_spec tmp007_channels[] = {
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
 				BIT(IIO_CHAN_INFO_SCALE),
 		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+		.event_spec = tmp007_obj_event,
+		.num_event_specs = ARRAY_SIZE(tmp007_obj_event),
 	}
 };
 
 static const struct iio_info tmp007_info = {
 	.read_raw = tmp007_read_raw,
 	.write_raw = tmp007_write_raw,
+	.read_event_config = tmp007_read_event_config,
+	.write_event_config = tmp007_write_event_config,
+	.read_event_value = tmp007_read_thresh,
+	.write_event_value = tmp007_write_thresh,
 	.attrs = &tmp007_attribute_group,
 	.driver_module = THIS_MODULE,
 };
@@ -214,7 +450,6 @@ static int tmp007_probe(struct i2c_client *client,
 	struct tmp007_data *data;
 	struct iio_dev *indio_dev;
 	int ret;
-	u16  status;
 
 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA))
 		return -EOPNOTSUPP;
@@ -231,6 +466,7 @@ static int tmp007_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	mutex_init(&data->lock);
 
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->name = "tmp007";
@@ -243,7 +479,7 @@ static int tmp007_probe(struct i2c_client *client,
 	/*
 	 * Set Configuration register:
 	 * 1. Conversion ON
-	 * 2. Comparator mode
+	 * 2. ALERT enable
 	 * 3. Transient correction enable
 	 */
 
@@ -252,7 +488,7 @@ static int tmp007_probe(struct i2c_client *client,
 		return ret;
 
 	data->config = ret;
-	data->config |= (TMP007_CONFIG_CONV_EN | TMP007_CONFIG_COMP_EN | TMP007_CONFIG_TC_EN);
+	data->config |= (TMP007_CONFIG_CONV_EN | TMP007_CONFIG_ALERT_EN | TMP007_CONFIG_TC_EN);
 
 	ret = i2c_smbus_write_word_swapped(data->client, TMP007_CONFIG,
 					data->config);
@@ -260,22 +496,39 @@ static int tmp007_probe(struct i2c_client *client,
 		return ret;
 
 	/*
+	 * Only the following flags can activate ALERT pin. Data conversion/validity flags
+	 * flags can still be polled for getting temperature data
+	 *
 	 * Set Status Mask register:
-	 * 1. Conversion ready enable
-	 * 2. Data valid enable
+	 * 1. Object temperature high limit enable
+	 * 2. Object temperature low limit enable
+	 * 3. TDIE temperature high limit enable
+	 * 4. TDIE temperature low limit enable
 	 */
 
 	ret = i2c_smbus_read_word_swapped(data->client, TMP007_STATUS_MASK);
 	if (ret < 0)
 		goto error_powerdown;
 
-	status = ret;
-	status |= (TMP007_STATUS_CONV_READY | TMP007_STATUS_DATA_VALID);
+	data->status_mask = ret;
+	data->status_mask |= (TMP007_STATUS_OHF | TMP007_STATUS_OLF
+				| TMP007_STATUS_LHF | TMP007_STATUS_LLF);
 
-	ret = i2c_smbus_write_word_swapped(data->client, TMP007_STATUS_MASK, status);
+	ret = i2c_smbus_write_word_swapped(data->client, TMP007_STATUS_MASK, data->status_mask);
 	if (ret < 0)
 		goto error_powerdown;
 
+	if (client->irq) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+				NULL, tmp007_interrupt_handler,
+				IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				tmp007_id->name, indio_dev);
+		if (ret) {
+			dev_err(&client->dev, "irq request error %d\n", -ret);
+			goto error_powerdown;
+		}
+	}
+
 	return iio_device_register(indio_dev);
 
 error_powerdown:
diff --git a/drivers/iio/trigger/stm32-timer-trigger.c b/drivers/iio/trigger/stm32-timer-trigger.c
index 994b96d19750..25248d644e7c 100644
--- a/drivers/iio/trigger/stm32-timer-trigger.c
+++ b/drivers/iio/trigger/stm32-timer-trigger.c
@@ -15,6 +15,7 @@
 #include <linux/platform_device.h>
 
 #define MAX_TRIGGERS 6
+#define MAX_VALIDS 5
 
 /* List the triggers created by each timer */
 static const void *triggers_table[][MAX_TRIGGERS] = {
@@ -32,12 +33,29 @@ static const void *triggers_table[][MAX_TRIGGERS] = {
 	{ TIM12_TRGO, TIM12_CH1, TIM12_CH2,},
 };
 
+/* List the triggers accepted by each timer */
+static const void *valids_table[][MAX_VALIDS] = {
+	{ TIM5_TRGO, TIM2_TRGO, TIM3_TRGO, TIM4_TRGO,},
+	{ TIM1_TRGO, TIM8_TRGO, TIM3_TRGO, TIM4_TRGO,},
+	{ TIM1_TRGO, TIM2_TRGO, TIM5_TRGO, TIM4_TRGO,},
+	{ TIM1_TRGO, TIM2_TRGO, TIM3_TRGO, TIM8_TRGO,},
+	{ TIM2_TRGO, TIM3_TRGO, TIM4_TRGO, TIM8_TRGO,},
+	{ }, /* timer 6 */
+	{ }, /* timer 7 */
+	{ TIM1_TRGO, TIM2_TRGO, TIM4_TRGO, TIM5_TRGO,},
+	{ TIM2_TRGO, TIM3_TRGO,},
+	{ }, /* timer 10 */
+	{ }, /* timer 11 */
+	{ TIM4_TRGO, TIM5_TRGO,},
+};
+
 struct stm32_timer_trigger {
 	struct device *dev;
 	struct regmap *regmap;
 	struct clk *clk;
 	u32 max_arr;
 	const void *triggers;
+	const void *valids;
 };
 
 static int stm32_timer_start(struct stm32_timer_trigger *priv,
@@ -152,10 +170,10 @@ static ssize_t stm32_tt_read_frequency(struct device *dev,
 	regmap_read(priv->regmap, TIM_PSC, &psc);
 	regmap_read(priv->regmap, TIM_ARR, &arr);
 
-	if (psc && arr && (cr1 & TIM_CR1_CEN)) {
+	if (cr1 & TIM_CR1_CEN) {
 		freq = (unsigned long long)clk_get_rate(priv->clk);
-		do_div(freq, psc);
-		do_div(freq, arr);
+		do_div(freq, psc + 1);
+		do_div(freq, arr + 1);
 	}
 
 	return sprintf(buf, "%d\n", (unsigned int)freq);
@@ -180,8 +198,7 @@ static ssize_t stm32_tt_show_master_mode(struct device *dev,
 					 struct device_attribute *attr,
 					 char *buf)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
 	u32 cr2;
 
 	regmap_read(priv->regmap, TIM_CR2, &cr2);
@@ -194,8 +211,7 @@ static ssize_t stm32_tt_store_master_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 stm32_timer_trigger *priv = iio_priv(indio_dev);
+	struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(master_mode_table); i++) {
@@ -275,6 +291,286 @@ static int stm32_setup_iio_triggers(struct stm32_timer_trigger *priv)
 	return 0;
 }
 
+static int stm32_counter_read_raw(struct iio_dev *indio_dev,
+				  struct iio_chan_spec const *chan,
+				  int *val, int *val2, long mask)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+	{
+		u32 cnt;
+
+		regmap_read(priv->regmap, TIM_CNT, &cnt);
+		*val = cnt;
+
+		return IIO_VAL_INT;
+	}
+	case IIO_CHAN_INFO_SCALE:
+	{
+		u32 smcr;
+
+		regmap_read(priv->regmap, TIM_SMCR, &smcr);
+		smcr &= TIM_SMCR_SMS;
+
+		*val = 1;
+		*val2 = 0;
+
+		/* in quadrature case scale = 0.25 */
+		if (smcr == 3)
+			*val2 = 2;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+	}
+	}
+
+	return -EINVAL;
+}
+
+static int stm32_counter_write_raw(struct iio_dev *indio_dev,
+				   struct iio_chan_spec const *chan,
+				   int val, int val2, long mask)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		regmap_write(priv->regmap, TIM_CNT, val);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		/* fixed scale */
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info stm32_trigger_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = stm32_counter_read_raw,
+	.write_raw = stm32_counter_write_raw
+};
+
+static const char *const stm32_enable_modes[] = {
+	"always",
+	"gated",
+	"triggered",
+};
+
+static int stm32_enable_mode2sms(int mode)
+{
+	switch (mode) {
+	case 0:
+		return 0;
+	case 1:
+		return 5;
+	case 2:
+		return 6;
+	}
+
+	return -EINVAL;
+}
+
+static int stm32_set_enable_mode(struct iio_dev *indio_dev,
+				 const struct iio_chan_spec *chan,
+				 unsigned int mode)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	int sms = stm32_enable_mode2sms(mode);
+
+	if (sms < 0)
+		return sms;
+
+	regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, sms);
+
+	return 0;
+}
+
+static int stm32_sms2enable_mode(int mode)
+{
+	switch (mode) {
+	case 0:
+		return 0;
+	case 5:
+		return 1;
+	case 6:
+		return 2;
+	}
+
+	return -EINVAL;
+}
+
+static int stm32_get_enable_mode(struct iio_dev *indio_dev,
+				 const struct iio_chan_spec *chan)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	u32 smcr;
+
+	regmap_read(priv->regmap, TIM_SMCR, &smcr);
+	smcr &= TIM_SMCR_SMS;
+
+	return stm32_sms2enable_mode(smcr);
+}
+
+static const struct iio_enum stm32_enable_mode_enum = {
+	.items = stm32_enable_modes,
+	.num_items = ARRAY_SIZE(stm32_enable_modes),
+	.set = stm32_set_enable_mode,
+	.get = stm32_get_enable_mode
+};
+
+static const char *const stm32_quadrature_modes[] = {
+	"channel_A",
+	"channel_B",
+	"quadrature",
+};
+
+static int stm32_set_quadrature_mode(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     unsigned int mode)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+
+	regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, mode + 1);
+
+	return 0;
+}
+
+static int stm32_get_quadrature_mode(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	u32 smcr;
+
+	regmap_read(priv->regmap, TIM_SMCR, &smcr);
+	smcr &= TIM_SMCR_SMS;
+
+	return smcr - 1;
+}
+
+static const struct iio_enum stm32_quadrature_mode_enum = {
+	.items = stm32_quadrature_modes,
+	.num_items = ARRAY_SIZE(stm32_quadrature_modes),
+	.set = stm32_set_quadrature_mode,
+	.get = stm32_get_quadrature_mode
+};
+
+static const char *const stm32_count_direction_states[] = {
+	"up",
+	"down"
+};
+
+static int stm32_set_count_direction(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     unsigned int mode)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+
+	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_DIR, mode);
+
+	return 0;
+}
+
+static int stm32_get_count_direction(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	u32 cr1;
+
+	regmap_read(priv->regmap, TIM_CR1, &cr1);
+
+	return (cr1 & TIM_CR1_DIR);
+}
+
+static const struct iio_enum stm32_count_direction_enum = {
+	.items = stm32_count_direction_states,
+	.num_items = ARRAY_SIZE(stm32_count_direction_states),
+	.set = stm32_set_count_direction,
+	.get = stm32_get_count_direction
+};
+
+static ssize_t stm32_count_get_preset(struct iio_dev *indio_dev,
+				      uintptr_t private,
+				      const struct iio_chan_spec *chan,
+				      char *buf)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	u32 arr;
+
+	regmap_read(priv->regmap, TIM_ARR, &arr);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", arr);
+}
+
+static ssize_t stm32_count_set_preset(struct iio_dev *indio_dev,
+				      uintptr_t private,
+				      const struct iio_chan_spec *chan,
+				      const char *buf, size_t len)
+{
+	struct stm32_timer_trigger *priv = iio_priv(indio_dev);
+	unsigned int preset;
+	int ret;
+
+	ret = kstrtouint(buf, 0, &preset);
+	if (ret)
+		return ret;
+
+	regmap_write(priv->regmap, TIM_ARR, preset);
+	regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_ARPE, TIM_CR1_ARPE);
+
+	return len;
+}
+
+static const struct iio_chan_spec_ext_info stm32_trigger_count_info[] = {
+	{
+		.name = "preset",
+		.shared = IIO_SEPARATE,
+		.read = stm32_count_get_preset,
+		.write = stm32_count_set_preset
+	},
+	IIO_ENUM("count_direction", IIO_SEPARATE, &stm32_count_direction_enum),
+	IIO_ENUM_AVAILABLE("count_direction", &stm32_count_direction_enum),
+	IIO_ENUM("quadrature_mode", IIO_SEPARATE, &stm32_quadrature_mode_enum),
+	IIO_ENUM_AVAILABLE("quadrature_mode", &stm32_quadrature_mode_enum),
+	IIO_ENUM("enable_mode", IIO_SEPARATE, &stm32_enable_mode_enum),
+	IIO_ENUM_AVAILABLE("enable_mode", &stm32_enable_mode_enum),
+	{}
+};
+
+static const struct iio_chan_spec stm32_trigger_channel = {
+	.type = IIO_COUNT,
+	.channel = 0,
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+	.ext_info = stm32_trigger_count_info,
+	.indexed = 1
+};
+
+static struct stm32_timer_trigger *stm32_setup_counter_device(struct device *dev)
+{
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(dev,
+					  sizeof(struct stm32_timer_trigger));
+	if (!indio_dev)
+		return NULL;
+
+	indio_dev->name = dev_name(dev);
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &stm32_trigger_info;
+	indio_dev->num_channels = 1;
+	indio_dev->channels = &stm32_trigger_channel;
+	indio_dev->dev.of_node = dev->of_node;
+
+	ret = devm_iio_device_register(dev, indio_dev);
+	if (ret)
+		return NULL;
+
+	return iio_priv(indio_dev);
+}
+
 /**
  * is_stm32_timer_trigger
  * @trig: trigger to be checked
@@ -299,10 +595,15 @@ static int stm32_timer_trigger_probe(struct platform_device *pdev)
 	if (of_property_read_u32(dev->of_node, "reg", &index))
 		return -EINVAL;
 
-	if (index >= ARRAY_SIZE(triggers_table))
+	if (index >= ARRAY_SIZE(triggers_table) ||
+	    index >= ARRAY_SIZE(valids_table))
 		return -EINVAL;
 
-	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	/* Create an IIO device only if we have triggers to be validated */
+	if (*valids_table[index])
+		priv = stm32_setup_counter_device(dev);
+	else
+		priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 
 	if (!priv)
 		return -ENOMEM;
@@ -312,6 +613,7 @@ static int stm32_timer_trigger_probe(struct platform_device *pdev)
 	priv->clk = ddata->clk;
 	priv->max_arr = ddata->max_arr;
 	priv->triggers = triggers_table[index];
+	priv->valids = valids_table[index];
 
 	ret = stm32_setup_iio_triggers(priv);
 	if (ret)
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index b5ea9192a341..050fe34342d3 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -5285,7 +5285,6 @@ static int qam_set_symbolrate(struct drxk_state *state)
 	/* Select & calculate correct IQM rate */
 	adc_frequency = (state->m_sys_clock_freq * 1000) / 3;
 	ratesel = 0;
-	/* printk(KERN_DEBUG "drxk: SR %d\n", state->props.symbol_rate); */
 	if (state->props.symbol_rate <= 1188750)
 		ratesel = 3;
 	else if (state->props.symbol_rate <= 2377500)
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index b1fc626125aa..268d4e6ef48a 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -34,6 +34,8 @@ source "drivers/staging/rtl8192u/Kconfig"
 
 source "drivers/staging/rtl8192e/Kconfig"
 
+source "drivers/staging/rtl8723bs/Kconfig"
+
 source "drivers/staging/rtl8712/Kconfig"
 
 source "drivers/staging/rtl8188eu/Kconfig"
@@ -92,6 +94,8 @@ source "drivers/staging/fbtft/Kconfig"
 
 source "drivers/staging/fsl-mc/Kconfig"
 
+source "drivers/staging/fsl-dpaa2/Kconfig"
+
 source "drivers/staging/wilc1000/Kconfig"
 
 source "drivers/staging/most/Kconfig"
@@ -102,7 +106,7 @@ source "drivers/staging/greybus/Kconfig"
 
 source "drivers/staging/vc04_services/Kconfig"
 
-source "drivers/staging/bcm2835-audio/Kconfig"
+source "drivers/staging/ccree/Kconfig"
 
 source "drivers/staging/typec/Kconfig"
 
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 682127c20da5..b93e6f5f0f6e 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_COMEDI)		+= comedi/
 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)		+= rtl8188eu/
 obj-$(CONFIG_RTS5208)		+= rts5208/
@@ -36,9 +37,10 @@ obj-$(CONFIG_UNISYSSPAR)	+= unisys/
 obj-$(CONFIG_COMMON_CLK_XLNX_CLKWZRD)	+= clocking-wizard/
 obj-$(CONFIG_FB_TFT)		+= fbtft/
 obj-$(CONFIG_FSL_MC_BUS)	+= fsl-mc/
+obj-$(CONFIG_FSL_DPAA2)		+= fsl-dpaa2/
 obj-$(CONFIG_WILC1000)		+= wilc1000/
 obj-$(CONFIG_MOST)		+= most/
 obj-$(CONFIG_KS7010)		+= ks7010/
 obj-$(CONFIG_GREYBUS)		+= greybus/
 obj-$(CONFIG_BCM2835_VCHIQ)	+= vc04_services/
-obj-$(CONFIG_SND_BCM2835)	+= bcm2835-audio/
+obj-$(CONFIG_CRYPTO_DEV_CCREE)	+= ccree/
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index 6c00d6f765c6..71a50b99caff 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -14,16 +14,6 @@ config ASHMEM
 	  It is, in theory, a good memory allocator for low-memory devices,
 	  because it can discard shared memory units when under memory pressure.
 
-config ANDROID_LOW_MEMORY_KILLER
-	bool "Android Low Memory Killer"
-	---help---
-	  Registers processes to be killed when low memory conditions, this is useful
-	  as there is no particular swap space on android.
-
-	  The registered process will kill according to the priorities in android init
-	  scripts (/init.rc), and it defines priority values with minimum free memory size
-	  for each priority.
-
 source "drivers/staging/android/ion/Kconfig"
 
 endif # if ANDROID
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 7ed1be798909..7cf1564a49a5 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -3,4 +3,3 @@ ccflags-y += -I$(src)			# needed for trace events
 obj-y					+= ion/
 
 obj-$(CONFIG_ASHMEM)			+= ashmem.o
-obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
index 8f3ac37bfe12..5f14247392bf 100644
--- a/drivers/staging/android/TODO
+++ b/drivers/staging/android/TODO
@@ -7,23 +7,10 @@ TODO:
 
 
 ion/
- - Remove ION_IOC_SYNC: Flushing for devices should be purely a kernel internal
-   interface on top of dma-buf. flush_for_device needs to be added to dma-buf
-   first.
- - Remove ION_IOC_CUSTOM: Atm used for cache flushing for cpu access in some
-   vendor trees. Should be replaced with an ioctl on the dma-buf to expose the
-   begin/end_cpu_access hooks to userspace.
- - Clarify the tricks ion plays with explicitly managing coherency behind the
-   dma api's back (this is absolutely needed for high-perf gpu drivers): Add an
-   explicit coherency management mode to flush_for_device to be used by drivers
-   which want to manage caches themselves and which indicates whether cpu caches
-   need flushing.
- - With those removed there's probably no use for ION_IOC_IMPORT anymore either
-   since ion would just be the central allocator for shared buffers.
- - Add dt-binding to expose cma regions as ion heaps, with the rule that any
-   such cma regions must already be used by some device for dma. I.e. ion only
-   exposes existing cma regions and doesn't reserve unecessarily memory when
-   booting a system which doesn't use ion.
+ - Add dt-bindings for remaining heaps (chunk and carveout heaps). This would
+   involve putting appropriate bindings in a memory node for Ion to find.
+ - Split /dev/ion up into multiple nodes (e.g. /dev/ion/heap0)
+ - Better test framework (integration with VGEM was suggested)
 
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
 Arve Hjønnevåg <arve@android.com> and Riley Andrews <riandrews@android.com>
diff --git a/drivers/staging/android/ion/Kconfig b/drivers/staging/android/ion/Kconfig
index c8fb4134c55d..a517b2d29f1b 100644
--- a/drivers/staging/android/ion/Kconfig
+++ b/drivers/staging/android/ion/Kconfig
@@ -10,45 +10,35 @@ menuconfig ION
 	  If you're not using Android its probably safe to
 	  say N here.
 
-config ION_TEST
-	tristate "Ion Test Device"
+config ION_SYSTEM_HEAP
+	bool "Ion system heap"
 	depends on ION
 	help
-	  Choose this option to create a device that can be used to test the
-	  kernel and device side ION functions.
+	  Choose this option to enable the Ion system heap. The system heap
+	  is backed by pages from the buddy allocator. If in doubt, say Y.
 
-config ION_DUMMY
-	bool "Dummy Ion driver"
+config ION_CARVEOUT_HEAP
+	bool "Ion carveout heap support"
 	depends on ION
 	help
-	  Provides a dummy ION driver that registers the
-	  /dev/ion device and some basic heaps. This can
-	  be used for testing the ION infrastructure if
-	  one doesn't have access to hardware drivers that
-	  use ION.
+	  Choose this option to enable carveout heaps with Ion. Carveout heaps
+	  are backed by memory reserved from the system. Allocation times are
+	  typically faster at the cost of memory not being used. Unless you
+	  know your system has these regions, you should say N here.
 
-config ION_TEGRA
-	tristate "Ion for Tegra"
-	depends on ARCH_TEGRA && ION
+config ION_CHUNK_HEAP
+	bool "Ion chunk heap support"
+	depends on ION
 	help
-	  Choose this option if you wish to use ion on an nVidia Tegra.
+          Choose this option to enable chunk heaps with Ion. This heap is
+	  similar in function the carveout heap but memory is broken down
+	  into smaller chunk sizes, typically corresponding to a TLB size.
+	  Unless you know your system has these regions, you should say N here.
 
-config ION_HISI
-	tristate "Ion for Hisilicon"
-	depends on ARCH_HISI && ION
-	select ION_OF
+config ION_CMA_HEAP
+	bool "Ion CMA heap support"
+	depends on ION && CMA
 	help
-	  Choose this option if you wish to use ion on Hisilicon Platform.
-
-source "drivers/staging/android/ion/hisilicon/Kconfig"
-
-config ION_OF
-	bool "Devicetree support for Ion"
-	depends on ION && OF_ADDRESS
-	help
-	  Provides base support for defining Ion heaps in devicetree
-	  and setting them up. Also includes functions for platforms
-	  to parse the devicetree and expand for their own custom
-	  extensions
-
-	  If using Ion and devicetree, you should say Y here
+	  Choose this option to enable CMA heaps with Ion. This heap is backed
+	  by the Contiguous Memory Allocator (CMA). If your system has these
+	  regions, you should say Y here.
diff --git a/drivers/staging/android/ion/Makefile b/drivers/staging/android/ion/Makefile
index 5d630a088381..eb7eeed6ae40 100644
--- a/drivers/staging/android/ion/Makefile
+++ b/drivers/staging/android/ion/Makefile
@@ -1,13 +1,5 @@
-obj-$(CONFIG_ION) +=	ion.o ion-ioctl.o ion_heap.o \
-			ion_page_pool.o ion_system_heap.o \
-			ion_carveout_heap.o ion_chunk_heap.o ion_cma_heap.o
-obj-$(CONFIG_ION_TEST) += ion_test.o
-ifdef CONFIG_COMPAT
-obj-$(CONFIG_ION) += compat_ion.o
-endif
-
-obj-$(CONFIG_ION_DUMMY) += ion_dummy_driver.o
-obj-$(CONFIG_ION_TEGRA) += tegra/
-obj-$(CONFIG_ION_HISI) += hisilicon/
-obj-$(CONFIG_ION_OF) += ion_of.o
-
+obj-$(CONFIG_ION) +=	ion.o ion-ioctl.o ion_heap.o
+obj-$(CONFIG_ION_SYSTEM_HEAP) += ion_system_heap.o ion_page_pool.o
+obj-$(CONFIG_ION_CARVEOUT_HEAP) += ion_carveout_heap.o
+obj-$(CONFIG_ION_CHUNK_HEAP) += ion_chunk_heap.o
+obj-$(CONFIG_ION_CMA_HEAP) += ion_cma_heap.o
diff --git a/drivers/staging/android/ion/compat_ion.c b/drivers/staging/android/ion/compat_ion.c
deleted file mode 100644
index 9a978d21785e..000000000000
--- a/drivers/staging/android/ion/compat_ion.c
+++ /dev/null
@@ -1,195 +0,0 @@
-/*
- * drivers/staging/android/ion/compat_ion.c
- *
- * Copyright (C) 2013 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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/compat.h>
-#include <linux/fs.h>
-#include <linux/uaccess.h>
-
-#include "ion.h"
-#include "compat_ion.h"
-
-/* See drivers/staging/android/uapi/ion.h for the definition of these structs */
-struct compat_ion_allocation_data {
-	compat_size_t len;
-	compat_size_t align;
-	compat_uint_t heap_id_mask;
-	compat_uint_t flags;
-	compat_int_t handle;
-};
-
-struct compat_ion_custom_data {
-	compat_uint_t cmd;
-	compat_ulong_t arg;
-};
-
-struct compat_ion_handle_data {
-	compat_int_t handle;
-};
-
-#define COMPAT_ION_IOC_ALLOC	_IOWR(ION_IOC_MAGIC, 0, \
-				      struct compat_ion_allocation_data)
-#define COMPAT_ION_IOC_FREE	_IOWR(ION_IOC_MAGIC, 1, \
-				      struct compat_ion_handle_data)
-#define COMPAT_ION_IOC_CUSTOM	_IOWR(ION_IOC_MAGIC, 6, \
-				      struct compat_ion_custom_data)
-
-static int compat_get_ion_allocation_data(
-			struct compat_ion_allocation_data __user *data32,
-			struct ion_allocation_data __user *data)
-{
-	compat_size_t s;
-	compat_uint_t u;
-	compat_int_t i;
-	int err;
-
-	err = get_user(s, &data32->len);
-	err |= put_user(s, &data->len);
-	err |= get_user(s, &data32->align);
-	err |= put_user(s, &data->align);
-	err |= get_user(u, &data32->heap_id_mask);
-	err |= put_user(u, &data->heap_id_mask);
-	err |= get_user(u, &data32->flags);
-	err |= put_user(u, &data->flags);
-	err |= get_user(i, &data32->handle);
-	err |= put_user(i, &data->handle);
-
-	return err;
-}
-
-static int compat_get_ion_handle_data(
-			struct compat_ion_handle_data __user *data32,
-			struct ion_handle_data __user *data)
-{
-	compat_int_t i;
-	int err;
-
-	err = get_user(i, &data32->handle);
-	err |= put_user(i, &data->handle);
-
-	return err;
-}
-
-static int compat_put_ion_allocation_data(
-			struct compat_ion_allocation_data __user *data32,
-			struct ion_allocation_data __user *data)
-{
-	compat_size_t s;
-	compat_uint_t u;
-	compat_int_t i;
-	int err;
-
-	err = get_user(s, &data->len);
-	err |= put_user(s, &data32->len);
-	err |= get_user(s, &data->align);
-	err |= put_user(s, &data32->align);
-	err |= get_user(u, &data->heap_id_mask);
-	err |= put_user(u, &data32->heap_id_mask);
-	err |= get_user(u, &data->flags);
-	err |= put_user(u, &data32->flags);
-	err |= get_user(i, &data->handle);
-	err |= put_user(i, &data32->handle);
-
-	return err;
-}
-
-static int compat_get_ion_custom_data(
-			struct compat_ion_custom_data __user *data32,
-			struct ion_custom_data __user *data)
-{
-	compat_uint_t cmd;
-	compat_ulong_t arg;
-	int err;
-
-	err = get_user(cmd, &data32->cmd);
-	err |= put_user(cmd, &data->cmd);
-	err |= get_user(arg, &data32->arg);
-	err |= put_user(arg, &data->arg);
-
-	return err;
-};
-
-long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
-{
-	long ret;
-
-	if (!filp->f_op->unlocked_ioctl)
-		return -ENOTTY;
-
-	switch (cmd) {
-	case COMPAT_ION_IOC_ALLOC:
-	{
-		struct compat_ion_allocation_data __user *data32;
-		struct ion_allocation_data __user *data;
-		int err;
-
-		data32 = compat_ptr(arg);
-		data = compat_alloc_user_space(sizeof(*data));
-		if (!data)
-			return -EFAULT;
-
-		err = compat_get_ion_allocation_data(data32, data);
-		if (err)
-			return err;
-		ret = filp->f_op->unlocked_ioctl(filp, ION_IOC_ALLOC,
-							(unsigned long)data);
-		err = compat_put_ion_allocation_data(data32, data);
-		return ret ? ret : err;
-	}
-	case COMPAT_ION_IOC_FREE:
-	{
-		struct compat_ion_handle_data __user *data32;
-		struct ion_handle_data __user *data;
-		int err;
-
-		data32 = compat_ptr(arg);
-		data = compat_alloc_user_space(sizeof(*data));
-		if (!data)
-			return -EFAULT;
-
-		err = compat_get_ion_handle_data(data32, data);
-		if (err)
-			return err;
-
-		return filp->f_op->unlocked_ioctl(filp, ION_IOC_FREE,
-							(unsigned long)data);
-	}
-	case COMPAT_ION_IOC_CUSTOM: {
-		struct compat_ion_custom_data __user *data32;
-		struct ion_custom_data __user *data;
-		int err;
-
-		data32 = compat_ptr(arg);
-		data = compat_alloc_user_space(sizeof(*data));
-		if (!data)
-			return -EFAULT;
-
-		err = compat_get_ion_custom_data(data32, data);
-		if (err)
-			return err;
-
-		return filp->f_op->unlocked_ioctl(filp, ION_IOC_CUSTOM,
-							(unsigned long)data);
-	}
-	case ION_IOC_SHARE:
-	case ION_IOC_MAP:
-	case ION_IOC_IMPORT:
-	case ION_IOC_SYNC:
-		return filp->f_op->unlocked_ioctl(filp, cmd,
-						(unsigned long)compat_ptr(arg));
-	default:
-		return -ENOIOCTLCMD;
-	}
-}
diff --git a/drivers/staging/android/ion/compat_ion.h b/drivers/staging/android/ion/compat_ion.h
deleted file mode 100644
index 9da8f917670b..000000000000
--- a/drivers/staging/android/ion/compat_ion.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * drivers/staging/android/ion/compat_ion.h
- *
- * Copyright (C) 2013 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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 _LINUX_COMPAT_ION_H
-#define _LINUX_COMPAT_ION_H
-
-#if IS_ENABLED(CONFIG_COMPAT)
-
-long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
-
-#else
-
-#define compat_ion_ioctl  NULL
-
-#endif /* CONFIG_COMPAT */
-#endif /* _LINUX_COMPAT_ION_H */
diff --git a/drivers/staging/android/ion/hisilicon/Kconfig b/drivers/staging/android/ion/hisilicon/Kconfig
deleted file mode 100644
index 2b4bd0798290..000000000000
--- a/drivers/staging/android/ion/hisilicon/Kconfig
+++ /dev/null
@@ -1,5 +0,0 @@
-config HI6220_ION
-        bool "Hi6220 ION Driver"
-        depends on ARCH_HISI && ION
-        help
-          Build the Hisilicon Hi6220 ion driver.
diff --git a/drivers/staging/android/ion/hisilicon/Makefile b/drivers/staging/android/ion/hisilicon/Makefile
deleted file mode 100644
index 2a89414280ac..000000000000
--- a/drivers/staging/android/ion/hisilicon/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_HI6220_ION) += hi6220_ion.o
diff --git a/drivers/staging/android/ion/hisilicon/hi6220_ion.c b/drivers/staging/android/ion/hisilicon/hi6220_ion.c
deleted file mode 100644
index 0de7897fd4bf..000000000000
--- a/drivers/staging/android/ion/hisilicon/hi6220_ion.c
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Hisilicon Hi6220 ION Driver
- *
- * Copyright (c) 2015 Hisilicon Limited.
- *
- * Author: Chen Feng <puck.chen@hisilicon.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.
- */
-
-#define pr_fmt(fmt) "Ion: " fmt
-
-#include <linux/err.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/of.h>
-#include <linux/mm.h>
-#include "../ion_priv.h"
-#include "../ion.h"
-#include "../ion_of.h"
-
-struct hisi_ion_dev {
-	struct ion_heap	**heaps;
-	struct ion_device *idev;
-	struct ion_platform_data *data;
-};
-
-static struct ion_of_heap hisi_heaps[] = {
-	PLATFORM_HEAP("hisilicon,sys_user", 0,
-		      ION_HEAP_TYPE_SYSTEM, "sys_user"),
-	PLATFORM_HEAP("hisilicon,sys_contig", 1,
-		      ION_HEAP_TYPE_SYSTEM_CONTIG, "sys_contig"),
-	PLATFORM_HEAP("hisilicon,cma", ION_HEAP_TYPE_DMA, ION_HEAP_TYPE_DMA,
-		      "cma"),
-	{}
-};
-
-static int hi6220_ion_probe(struct platform_device *pdev)
-{
-	struct hisi_ion_dev *ipdev;
-	int i;
-
-	ipdev = devm_kzalloc(&pdev->dev, sizeof(*ipdev), GFP_KERNEL);
-	if (!ipdev)
-		return -ENOMEM;
-
-	platform_set_drvdata(pdev, ipdev);
-
-	ipdev->idev = ion_device_create(NULL);
-	if (IS_ERR(ipdev->idev))
-		return PTR_ERR(ipdev->idev);
-
-	ipdev->data = ion_parse_dt(pdev, hisi_heaps);
-	if (IS_ERR(ipdev->data))
-		return PTR_ERR(ipdev->data);
-
-	ipdev->heaps = devm_kzalloc(&pdev->dev,
-				sizeof(struct ion_heap) * ipdev->data->nr,
-				GFP_KERNEL);
-	if (!ipdev->heaps) {
-		ion_destroy_platform_data(ipdev->data);
-		return -ENOMEM;
-	}
-
-	for (i = 0; i < ipdev->data->nr; i++) {
-		ipdev->heaps[i] = ion_heap_create(&ipdev->data->heaps[i]);
-		if (!ipdev->heaps) {
-			ion_destroy_platform_data(ipdev->data);
-			return -ENOMEM;
-		}
-		ion_device_add_heap(ipdev->idev, ipdev->heaps[i]);
-	}
-	return 0;
-}
-
-static int hi6220_ion_remove(struct platform_device *pdev)
-{
-	struct hisi_ion_dev *ipdev;
-	int i;
-
-	ipdev = platform_get_drvdata(pdev);
-
-	for (i = 0; i < ipdev->data->nr; i++)
-		ion_heap_destroy(ipdev->heaps[i]);
-
-	ion_destroy_platform_data(ipdev->data);
-	ion_device_destroy(ipdev->idev);
-
-	return 0;
-}
-
-static const struct of_device_id hi6220_ion_match_table[] = {
-	{.compatible = "hisilicon,hi6220-ion"},
-	{},
-};
-
-static struct platform_driver hi6220_ion_driver = {
-	.probe = hi6220_ion_probe,
-	.remove = hi6220_ion_remove,
-	.driver = {
-		.name = "ion-hi6220",
-		.of_match_table = hi6220_ion_match_table,
-	},
-};
-
-static int __init hi6220_ion_init(void)
-{
-	return platform_driver_register(&hi6220_ion_driver);
-}
-
-subsys_initcall(hi6220_ion_init);
diff --git a/drivers/staging/android/ion/ion-ioctl.c b/drivers/staging/android/ion/ion-ioctl.c
index 9ff815ad1cb1..76427e4773a8 100644
--- a/drivers/staging/android/ion/ion-ioctl.c
+++ b/drivers/staging/android/ion/ion-ioctl.c
@@ -19,14 +19,9 @@
 #include <linux/uaccess.h>
 
 #include "ion.h"
-#include "ion_priv.h"
-#include "compat_ion.h"
 
 union ion_ioctl_arg {
-	struct ion_fd_data fd;
 	struct ion_allocation_data allocation;
-	struct ion_handle_data handle;
-	struct ion_custom_data custom;
 	struct ion_heap_query query;
 };
 
@@ -51,10 +46,6 @@ static int validate_ioctl_arg(unsigned int cmd, union ion_ioctl_arg *arg)
 static unsigned int ion_ioctl_dir(unsigned int cmd)
 {
 	switch (cmd) {
-	case ION_IOC_SYNC:
-	case ION_IOC_FREE:
-	case ION_IOC_CUSTOM:
-		return _IOC_WRITE;
 	default:
 		return _IOC_DIR(cmd);
 	}
@@ -62,9 +53,6 @@ static unsigned int ion_ioctl_dir(unsigned int cmd)
 
 long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
-	struct ion_client *client = filp->private_data;
-	struct ion_device *dev = client->dev;
-	struct ion_handle *cleanup_handle = NULL;
 	int ret = 0;
 	unsigned int dir;
 	union ion_ioctl_arg data;
@@ -92,87 +80,28 @@ long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 	switch (cmd) {
 	case ION_IOC_ALLOC:
 	{
-		struct ion_handle *handle;
+		int fd;
 
-		handle = ion_alloc(client, data.allocation.len,
-						data.allocation.align,
+		fd = ion_alloc(data.allocation.len,
 						data.allocation.heap_id_mask,
 						data.allocation.flags);
-		if (IS_ERR(handle))
-			return PTR_ERR(handle);
+		if (fd < 0)
+			return fd;
 
-		data.allocation.handle = handle->id;
+		data.allocation.fd = fd;
 
-		cleanup_handle = handle;
-		break;
-	}
-	case ION_IOC_FREE:
-	{
-		struct ion_handle *handle;
-
-		mutex_lock(&client->lock);
-		handle = ion_handle_get_by_id_nolock(client,
-						     data.handle.handle);
-		if (IS_ERR(handle)) {
-			mutex_unlock(&client->lock);
-			return PTR_ERR(handle);
-		}
-		ion_free_nolock(client, handle);
-		ion_handle_put_nolock(handle);
-		mutex_unlock(&client->lock);
-		break;
-	}
-	case ION_IOC_SHARE:
-	case ION_IOC_MAP:
-	{
-		struct ion_handle *handle;
-
-		handle = ion_handle_get_by_id(client, data.handle.handle);
-		if (IS_ERR(handle))
-			return PTR_ERR(handle);
-		data.fd.fd = ion_share_dma_buf_fd(client, handle);
-		ion_handle_put(handle);
-		if (data.fd.fd < 0)
-			ret = data.fd.fd;
-		break;
-	}
-	case ION_IOC_IMPORT:
-	{
-		struct ion_handle *handle;
-
-		handle = ion_import_dma_buf_fd(client, data.fd.fd);
-		if (IS_ERR(handle))
-			ret = PTR_ERR(handle);
-		else
-			data.handle.handle = handle->id;
-		break;
-	}
-	case ION_IOC_SYNC:
-	{
-		ret = ion_sync_for_device(client, data.fd.fd);
-		break;
-	}
-	case ION_IOC_CUSTOM:
-	{
-		if (!dev->custom_ioctl)
-			return -ENOTTY;
-		ret = dev->custom_ioctl(client, data.custom.cmd,
-						data.custom.arg);
 		break;
 	}
 	case ION_IOC_HEAP_QUERY:
-		ret = ion_query_heaps(client, &data.query);
+		ret = ion_query_heaps(&data.query);
 		break;
 	default:
 		return -ENOTTY;
 	}
 
 	if (dir & _IOC_READ) {
-		if (copy_to_user((void __user *)arg, &data, _IOC_SIZE(cmd))) {
-			if (cleanup_handle)
-				ion_free(client, cleanup_handle);
+		if (copy_to_user((void __user *)arg, &data, _IOC_SIZE(cmd)))
 			return -EFAULT;
-		}
 	}
 	return ret;
 }
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 95a7f1648c00..03d3a4fce0e2 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -39,40 +39,15 @@
 #include <linux/sched/task.h>
 
 #include "ion.h"
-#include "ion_priv.h"
-#include "compat_ion.h"
 
-bool ion_buffer_fault_user_mappings(struct ion_buffer *buffer)
-{
-	return (buffer->flags & ION_FLAG_CACHED) &&
-		!(buffer->flags & ION_FLAG_CACHED_NEEDS_SYNC);
-}
+static struct ion_device *internal_dev;
+static int heap_id;
 
 bool ion_buffer_cached(struct ion_buffer *buffer)
 {
 	return !!(buffer->flags & ION_FLAG_CACHED);
 }
 
-static inline struct page *ion_buffer_page(struct page *page)
-{
-	return (struct page *)((unsigned long)page & ~(1UL));
-}
-
-static inline bool ion_buffer_page_is_dirty(struct page *page)
-{
-	return !!((unsigned long)page & 1UL);
-}
-
-static inline void ion_buffer_page_dirty(struct page **page)
-{
-	*page = (struct page *)((unsigned long)(*page) | 1UL);
-}
-
-static inline void ion_buffer_page_clean(struct page **page)
-{
-	*page = (struct page *)((unsigned long)(*page) & ~(1UL));
-}
-
 /* this function should only be called while dev->lock is held */
 static void ion_buffer_add(struct ion_device *dev,
 			   struct ion_buffer *buffer)
@@ -103,13 +78,11 @@ static void ion_buffer_add(struct ion_device *dev,
 static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 					    struct ion_device *dev,
 					    unsigned long len,
-					    unsigned long align,
 					    unsigned long flags)
 {
 	struct ion_buffer *buffer;
 	struct sg_table *table;
-	struct scatterlist *sg;
-	int i, ret;
+	int ret;
 
 	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
 	if (!buffer)
@@ -117,17 +90,15 @@ static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 
 	buffer->heap = heap;
 	buffer->flags = flags;
-	kref_init(&buffer->ref);
 
-	ret = heap->ops->allocate(heap, buffer, len, align, flags);
+	ret = heap->ops->allocate(heap, buffer, len, flags);
 
 	if (ret) {
 		if (!(heap->flags & ION_HEAP_FLAG_DEFER_FREE))
 			goto err2;
 
 		ion_heap_freelist_drain(heap, 0);
-		ret = heap->ops->allocate(heap, buffer, len, align,
-					  flags);
+		ret = heap->ops->allocate(heap, buffer, len, flags);
 		if (ret)
 			goto err2;
 	}
@@ -142,43 +113,11 @@ static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 	buffer->dev = dev;
 	buffer->size = len;
 
-	if (ion_buffer_fault_user_mappings(buffer)) {
-		int num_pages = PAGE_ALIGN(buffer->size) / PAGE_SIZE;
-		struct scatterlist *sg;
-		int i, j, k = 0;
-
-		buffer->pages = vmalloc(sizeof(struct page *) * num_pages);
-		if (!buffer->pages) {
-			ret = -ENOMEM;
-			goto err1;
-		}
-
-		for_each_sg(table->sgl, sg, table->nents, i) {
-			struct page *page = sg_page(sg);
-
-			for (j = 0; j < sg->length / PAGE_SIZE; j++)
-				buffer->pages[k++] = page++;
-		}
-	}
-
 	buffer->dev = dev;
 	buffer->size = len;
 	INIT_LIST_HEAD(&buffer->vmas);
+	INIT_LIST_HEAD(&buffer->attachments);
 	mutex_init(&buffer->lock);
-	/*
-	 * this will set up dma addresses for the sglist -- it is not
-	 * technically correct as per the dma api -- a specific
-	 * device isn't really taking ownership here.  However, in practice on
-	 * our systems the only dma_address space is physical addresses.
-	 * Additionally, we can't afford the overhead of invalidating every
-	 * allocation via dma_map_sg. The implicit contract here is that
-	 * memory coming from the heaps is ready for dma, ie if it has a
-	 * cached mapping that mapping has been invalidated
-	 */
-	for_each_sg(buffer->sg_table->sgl, sg, buffer->sg_table->nents, i) {
-		sg_dma_address(sg) = sg_phys(sg);
-		sg_dma_len(sg) = sg->length;
-	}
 	mutex_lock(&dev->buffer_lock);
 	ion_buffer_add(dev, buffer);
 	mutex_unlock(&dev->buffer_lock);
@@ -200,9 +139,8 @@ void ion_buffer_destroy(struct ion_buffer *buffer)
 	kfree(buffer);
 }
 
-static void _ion_buffer_destroy(struct kref *kref)
+static void _ion_buffer_destroy(struct ion_buffer *buffer)
 {
-	struct ion_buffer *buffer = container_of(kref, struct ion_buffer, ref);
 	struct ion_heap *heap = buffer->heap;
 	struct ion_device *dev = buffer->dev;
 
@@ -216,273 +154,6 @@ static void _ion_buffer_destroy(struct kref *kref)
 		ion_buffer_destroy(buffer);
 }
 
-static void ion_buffer_get(struct ion_buffer *buffer)
-{
-	kref_get(&buffer->ref);
-}
-
-static int ion_buffer_put(struct ion_buffer *buffer)
-{
-	return kref_put(&buffer->ref, _ion_buffer_destroy);
-}
-
-static void ion_buffer_add_to_handle(struct ion_buffer *buffer)
-{
-	mutex_lock(&buffer->lock);
-	buffer->handle_count++;
-	mutex_unlock(&buffer->lock);
-}
-
-static void ion_buffer_remove_from_handle(struct ion_buffer *buffer)
-{
-	/*
-	 * when a buffer is removed from a handle, if it is not in
-	 * any other handles, copy the taskcomm and the pid of the
-	 * process it's being removed from into the buffer.  At this
-	 * point there will be no way to track what processes this buffer is
-	 * being used by, it only exists as a dma_buf file descriptor.
-	 * The taskcomm and pid can provide a debug hint as to where this fd
-	 * is in the system
-	 */
-	mutex_lock(&buffer->lock);
-	buffer->handle_count--;
-	BUG_ON(buffer->handle_count < 0);
-	if (!buffer->handle_count) {
-		struct task_struct *task;
-
-		task = current->group_leader;
-		get_task_comm(buffer->task_comm, task);
-		buffer->pid = task_pid_nr(task);
-	}
-	mutex_unlock(&buffer->lock);
-}
-
-static struct ion_handle *ion_handle_create(struct ion_client *client,
-					    struct ion_buffer *buffer)
-{
-	struct ion_handle *handle;
-
-	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
-	if (!handle)
-		return ERR_PTR(-ENOMEM);
-	kref_init(&handle->ref);
-	RB_CLEAR_NODE(&handle->node);
-	handle->client = client;
-	ion_buffer_get(buffer);
-	ion_buffer_add_to_handle(buffer);
-	handle->buffer = buffer;
-
-	return handle;
-}
-
-static void ion_handle_kmap_put(struct ion_handle *);
-
-static void ion_handle_destroy(struct kref *kref)
-{
-	struct ion_handle *handle = container_of(kref, struct ion_handle, ref);
-	struct ion_client *client = handle->client;
-	struct ion_buffer *buffer = handle->buffer;
-
-	mutex_lock(&buffer->lock);
-	while (handle->kmap_cnt)
-		ion_handle_kmap_put(handle);
-	mutex_unlock(&buffer->lock);
-
-	idr_remove(&client->idr, handle->id);
-	if (!RB_EMPTY_NODE(&handle->node))
-		rb_erase(&handle->node, &client->handles);
-
-	ion_buffer_remove_from_handle(buffer);
-	ion_buffer_put(buffer);
-
-	kfree(handle);
-}
-
-static void ion_handle_get(struct ion_handle *handle)
-{
-	kref_get(&handle->ref);
-}
-
-int ion_handle_put_nolock(struct ion_handle *handle)
-{
-	return kref_put(&handle->ref, ion_handle_destroy);
-}
-
-int ion_handle_put(struct ion_handle *handle)
-{
-	struct ion_client *client = handle->client;
-	int ret;
-
-	mutex_lock(&client->lock);
-	ret = ion_handle_put_nolock(handle);
-	mutex_unlock(&client->lock);
-
-	return ret;
-}
-
-static struct ion_handle *ion_handle_lookup(struct ion_client *client,
-					    struct ion_buffer *buffer)
-{
-	struct rb_node *n = client->handles.rb_node;
-
-	while (n) {
-		struct ion_handle *entry = rb_entry(n, struct ion_handle, node);
-
-		if (buffer < entry->buffer)
-			n = n->rb_left;
-		else if (buffer > entry->buffer)
-			n = n->rb_right;
-		else
-			return entry;
-	}
-	return ERR_PTR(-EINVAL);
-}
-
-struct ion_handle *ion_handle_get_by_id_nolock(struct ion_client *client,
-					       int id)
-{
-	struct ion_handle *handle;
-
-	handle = idr_find(&client->idr, id);
-	if (handle)
-		ion_handle_get(handle);
-
-	return handle ? handle : ERR_PTR(-EINVAL);
-}
-
-struct ion_handle *ion_handle_get_by_id(struct ion_client *client,
-					       int id)
-{
-	struct ion_handle *handle;
-
-	mutex_lock(&client->lock);
-	handle = ion_handle_get_by_id_nolock(client, id);
-	mutex_unlock(&client->lock);
-
-	return handle;
-}
-
-static bool ion_handle_validate(struct ion_client *client,
-				struct ion_handle *handle)
-{
-	WARN_ON(!mutex_is_locked(&client->lock));
-	return idr_find(&client->idr, handle->id) == handle;
-}
-
-static int ion_handle_add(struct ion_client *client, struct ion_handle *handle)
-{
-	int id;
-	struct rb_node **p = &client->handles.rb_node;
-	struct rb_node *parent = NULL;
-	struct ion_handle *entry;
-
-	id = idr_alloc(&client->idr, handle, 1, 0, GFP_KERNEL);
-	if (id < 0)
-		return id;
-
-	handle->id = id;
-
-	while (*p) {
-		parent = *p;
-		entry = rb_entry(parent, struct ion_handle, node);
-
-		if (handle->buffer < entry->buffer)
-			p = &(*p)->rb_left;
-		else if (handle->buffer > entry->buffer)
-			p = &(*p)->rb_right;
-		else
-			WARN(1, "%s: buffer already found.", __func__);
-	}
-
-	rb_link_node(&handle->node, parent, p);
-	rb_insert_color(&handle->node, &client->handles);
-
-	return 0;
-}
-
-struct ion_handle *ion_alloc(struct ion_client *client, size_t len,
-			     size_t align, unsigned int heap_id_mask,
-			     unsigned int flags)
-{
-	struct ion_handle *handle;
-	struct ion_device *dev = client->dev;
-	struct ion_buffer *buffer = NULL;
-	struct ion_heap *heap;
-	int ret;
-
-	pr_debug("%s: len %zu align %zu heap_id_mask %u flags %x\n", __func__,
-		 len, align, heap_id_mask, flags);
-	/*
-	 * traverse the list of heaps available in this system in priority
-	 * order.  If the heap type is supported by the client, and matches the
-	 * request of the caller allocate from it.  Repeat until allocate has
-	 * succeeded or all heaps have been tried
-	 */
-	len = PAGE_ALIGN(len);
-
-	if (!len)
-		return ERR_PTR(-EINVAL);
-
-	down_read(&dev->lock);
-	plist_for_each_entry(heap, &dev->heaps, node) {
-		/* if the caller didn't specify this heap id */
-		if (!((1 << heap->id) & heap_id_mask))
-			continue;
-		buffer = ion_buffer_create(heap, dev, len, align, flags);
-		if (!IS_ERR(buffer))
-			break;
-	}
-	up_read(&dev->lock);
-
-	if (buffer == NULL)
-		return ERR_PTR(-ENODEV);
-
-	if (IS_ERR(buffer))
-		return ERR_CAST(buffer);
-
-	handle = ion_handle_create(client, buffer);
-
-	/*
-	 * ion_buffer_create will create a buffer with a ref_cnt of 1,
-	 * and ion_handle_create will take a second reference, drop one here
-	 */
-	ion_buffer_put(buffer);
-
-	if (IS_ERR(handle))
-		return handle;
-
-	mutex_lock(&client->lock);
-	ret = ion_handle_add(client, handle);
-	mutex_unlock(&client->lock);
-	if (ret) {
-		ion_handle_put(handle);
-		handle = ERR_PTR(ret);
-	}
-
-	return handle;
-}
-EXPORT_SYMBOL(ion_alloc);
-
-void ion_free_nolock(struct ion_client *client,
-		     struct ion_handle *handle)
-{
-	if (!ion_handle_validate(client, handle)) {
-		WARN(1, "%s: invalid handle passed to free.\n", __func__);
-		return;
-	}
-	ion_handle_put_nolock(handle);
-}
-
-void ion_free(struct ion_client *client, struct ion_handle *handle)
-{
-	BUG_ON(client != handle->client);
-
-	mutex_lock(&client->lock);
-	ion_free_nolock(client, handle);
-	mutex_unlock(&client->lock);
-}
-EXPORT_SYMBOL(ion_free);
-
 static void *ion_buffer_kmap_get(struct ion_buffer *buffer)
 {
 	void *vaddr;
@@ -502,22 +173,6 @@ static void *ion_buffer_kmap_get(struct ion_buffer *buffer)
 	return vaddr;
 }
 
-static void *ion_handle_kmap_get(struct ion_handle *handle)
-{
-	struct ion_buffer *buffer = handle->buffer;
-	void *vaddr;
-
-	if (handle->kmap_cnt) {
-		handle->kmap_cnt++;
-		return buffer->vaddr;
-	}
-	vaddr = ion_buffer_kmap_get(buffer);
-	if (IS_ERR(vaddr))
-		return vaddr;
-	handle->kmap_cnt++;
-	return vaddr;
-}
-
 static void ion_buffer_kmap_put(struct ion_buffer *buffer)
 {
 	buffer->kmap_cnt--;
@@ -527,408 +182,117 @@ static void ion_buffer_kmap_put(struct ion_buffer *buffer)
 	}
 }
 
-static void ion_handle_kmap_put(struct ion_handle *handle)
+static struct sg_table *dup_sg_table(struct sg_table *table)
 {
-	struct ion_buffer *buffer = handle->buffer;
+	struct sg_table *new_table;
+	int ret, i;
+	struct scatterlist *sg, *new_sg;
 
-	if (!handle->kmap_cnt) {
-		WARN(1, "%s: Double unmap detected! bailing...\n", __func__);
-		return;
-	}
-	handle->kmap_cnt--;
-	if (!handle->kmap_cnt)
-		ion_buffer_kmap_put(buffer);
-}
-
-void *ion_map_kernel(struct ion_client *client, struct ion_handle *handle)
-{
-	struct ion_buffer *buffer;
-	void *vaddr;
-
-	mutex_lock(&client->lock);
-	if (!ion_handle_validate(client, handle)) {
-		pr_err("%s: invalid handle passed to map_kernel.\n",
-		       __func__);
-		mutex_unlock(&client->lock);
-		return ERR_PTR(-EINVAL);
-	}
-
-	buffer = handle->buffer;
-
-	if (!handle->buffer->heap->ops->map_kernel) {
-		pr_err("%s: map_kernel is not implemented by this heap.\n",
-		       __func__);
-		mutex_unlock(&client->lock);
-		return ERR_PTR(-ENODEV);
-	}
-
-	mutex_lock(&buffer->lock);
-	vaddr = ion_handle_kmap_get(handle);
-	mutex_unlock(&buffer->lock);
-	mutex_unlock(&client->lock);
-	return vaddr;
-}
-EXPORT_SYMBOL(ion_map_kernel);
-
-void ion_unmap_kernel(struct ion_client *client, struct ion_handle *handle)
-{
-	struct ion_buffer *buffer;
-
-	mutex_lock(&client->lock);
-	buffer = handle->buffer;
-	mutex_lock(&buffer->lock);
-	ion_handle_kmap_put(handle);
-	mutex_unlock(&buffer->lock);
-	mutex_unlock(&client->lock);
-}
-EXPORT_SYMBOL(ion_unmap_kernel);
-
-static struct mutex debugfs_mutex;
-static struct rb_root *ion_root_client;
-static int is_client_alive(struct ion_client *client)
-{
-	struct rb_node *node;
-	struct ion_client *tmp;
-	struct ion_device *dev;
-
-	node = ion_root_client->rb_node;
-	dev = container_of(ion_root_client, struct ion_device, clients);
-
-	down_read(&dev->lock);
-	while (node) {
-		tmp = rb_entry(node, struct ion_client, node);
-		if (client < tmp) {
-			node = node->rb_left;
-		} else if (client > tmp) {
-			node = node->rb_right;
-		} else {
-			up_read(&dev->lock);
-			return 1;
-		}
-	}
-
-	up_read(&dev->lock);
-	return 0;
-}
+	new_table = kzalloc(sizeof(*new_table), GFP_KERNEL);
+	if (!new_table)
+		return ERR_PTR(-ENOMEM);
 
-static int ion_debug_client_show(struct seq_file *s, void *unused)
-{
-	struct ion_client *client = s->private;
-	struct rb_node *n;
-	size_t sizes[ION_NUM_HEAP_IDS] = {0};
-	const char *names[ION_NUM_HEAP_IDS] = {NULL};
-	int i;
-
-	mutex_lock(&debugfs_mutex);
-	if (!is_client_alive(client)) {
-		seq_printf(s, "ion_client 0x%p dead, can't dump its buffers\n",
-			   client);
-		mutex_unlock(&debugfs_mutex);
-		return 0;
+	ret = sg_alloc_table(new_table, table->nents, GFP_KERNEL);
+	if (ret) {
+		kfree(new_table);
+		return ERR_PTR(-ENOMEM);
 	}
 
-	mutex_lock(&client->lock);
-	for (n = rb_first(&client->handles); n; n = rb_next(n)) {
-		struct ion_handle *handle = rb_entry(n, struct ion_handle,
-						     node);
-		unsigned int id = handle->buffer->heap->id;
-
-		if (!names[id])
-			names[id] = handle->buffer->heap->name;
-		sizes[id] += handle->buffer->size;
+	new_sg = new_table->sgl;
+	for_each_sg(table->sgl, sg, table->nents, i) {
+		memcpy(new_sg, sg, sizeof(*sg));
+		sg->dma_address = 0;
+		new_sg = sg_next(new_sg);
 	}
-	mutex_unlock(&client->lock);
-	mutex_unlock(&debugfs_mutex);
 
-	seq_printf(s, "%16.16s: %16.16s\n", "heap_name", "size_in_bytes");
-	for (i = 0; i < ION_NUM_HEAP_IDS; i++) {
-		if (!names[i])
-			continue;
-		seq_printf(s, "%16.16s: %16zu\n", names[i], sizes[i]);
-	}
-	return 0;
+	return new_table;
 }
 
-static int ion_debug_client_open(struct inode *inode, struct file *file)
+static void free_duped_table(struct sg_table *table)
 {
-	return single_open(file, ion_debug_client_show, inode->i_private);
+	sg_free_table(table);
+	kfree(table);
 }
 
-static const struct file_operations debug_client_fops = {
-	.open = ion_debug_client_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
+struct ion_dma_buf_attachment {
+	struct device *dev;
+	struct sg_table *table;
+	struct list_head list;
 };
 
-static int ion_get_client_serial(const struct rb_root *root,
-				 const unsigned char *name)
-{
-	int serial = -1;
-	struct rb_node *node;
-
-	for (node = rb_first(root); node; node = rb_next(node)) {
-		struct ion_client *client = rb_entry(node, struct ion_client,
-						     node);
-
-		if (strcmp(client->name, name))
-			continue;
-		serial = max(serial, client->display_serial);
-	}
-	return serial + 1;
-}
-
-struct ion_client *ion_client_create(struct ion_device *dev,
-				     const char *name)
+static int ion_dma_buf_attach(struct dma_buf *dmabuf, struct device *dev,
+				struct dma_buf_attachment *attachment)
 {
-	struct ion_client *client;
-	struct task_struct *task;
-	struct rb_node **p;
-	struct rb_node *parent = NULL;
-	struct ion_client *entry;
-	pid_t pid;
-
-	if (!name) {
-		pr_err("%s: Name cannot be null\n", __func__);
-		return ERR_PTR(-EINVAL);
-	}
+	struct ion_dma_buf_attachment *a;
+	struct sg_table *table;
+	struct ion_buffer *buffer = dmabuf->priv;
 
-	get_task_struct(current->group_leader);
-	task_lock(current->group_leader);
-	pid = task_pid_nr(current->group_leader);
-	/*
-	 * don't bother to store task struct for kernel threads,
-	 * they can't be killed anyway
-	 */
-	if (current->group_leader->flags & PF_KTHREAD) {
-		put_task_struct(current->group_leader);
-		task = NULL;
-	} else {
-		task = current->group_leader;
-	}
-	task_unlock(current->group_leader);
-
-	client = kzalloc(sizeof(*client), GFP_KERNEL);
-	if (!client)
-		goto err_put_task_struct;
-
-	client->dev = dev;
-	client->handles = RB_ROOT;
-	idr_init(&client->idr);
-	mutex_init(&client->lock);
-	client->task = task;
-	client->pid = pid;
-	client->name = kstrdup(name, GFP_KERNEL);
-	if (!client->name)
-		goto err_free_client;
+	a = kzalloc(sizeof(*a), GFP_KERNEL);
+	if (!a)
+		return -ENOMEM;
 
-	down_write(&dev->lock);
-	client->display_serial = ion_get_client_serial(&dev->clients, name);
-	client->display_name = kasprintf(
-		GFP_KERNEL, "%s-%d", name, client->display_serial);
-	if (!client->display_name) {
-		up_write(&dev->lock);
-		goto err_free_client_name;
+	table = dup_sg_table(buffer->sg_table);
+	if (IS_ERR(table)) {
+		kfree(a);
+		return -ENOMEM;
 	}
-	p = &dev->clients.rb_node;
-	while (*p) {
-		parent = *p;
-		entry = rb_entry(parent, struct ion_client, node);
 
-		if (client < entry)
-			p = &(*p)->rb_left;
-		else if (client > entry)
-			p = &(*p)->rb_right;
-	}
-	rb_link_node(&client->node, parent, p);
-	rb_insert_color(&client->node, &dev->clients);
-
-	client->debug_root = debugfs_create_file(client->display_name, 0664,
-						 dev->clients_debug_root,
-						 client, &debug_client_fops);
-	if (!client->debug_root) {
-		char buf[256], *path;
-
-		path = dentry_path(dev->clients_debug_root, buf, 256);
-		pr_err("Failed to create client debugfs at %s/%s\n",
-		       path, client->display_name);
-	}
+	a->table = table;
+	a->dev = dev;
+	INIT_LIST_HEAD(&a->list);
 
-	up_write(&dev->lock);
+	attachment->priv = a;
 
-	return client;
+	mutex_lock(&buffer->lock);
+	list_add(&a->list, &buffer->attachments);
+	mutex_unlock(&buffer->lock);
 
-err_free_client_name:
-	kfree(client->name);
-err_free_client:
-	kfree(client);
-err_put_task_struct:
-	if (task)
-		put_task_struct(current->group_leader);
-	return ERR_PTR(-ENOMEM);
+	return 0;
 }
-EXPORT_SYMBOL(ion_client_create);
 
-void ion_client_destroy(struct ion_client *client)
+static void ion_dma_buf_detatch(struct dma_buf *dmabuf,
+				struct dma_buf_attachment *attachment)
 {
-	struct ion_device *dev = client->dev;
-	struct rb_node *n;
-
-	pr_debug("%s: %d\n", __func__, __LINE__);
-	mutex_lock(&debugfs_mutex);
-	while ((n = rb_first(&client->handles))) {
-		struct ion_handle *handle = rb_entry(n, struct ion_handle,
-						     node);
-		ion_handle_destroy(&handle->ref);
-	}
-
-	idr_destroy(&client->idr);
+	struct ion_dma_buf_attachment *a = attachment->priv;
+	struct ion_buffer *buffer = dmabuf->priv;
 
-	down_write(&dev->lock);
-	if (client->task)
-		put_task_struct(client->task);
-	rb_erase(&client->node, &dev->clients);
-	debugfs_remove_recursive(client->debug_root);
-	up_write(&dev->lock);
+	free_duped_table(a->table);
+	mutex_lock(&buffer->lock);
+	list_del(&a->list);
+	mutex_unlock(&buffer->lock);
 
-	kfree(client->display_name);
-	kfree(client->name);
-	kfree(client);
-	mutex_unlock(&debugfs_mutex);
+	kfree(a);
 }
-EXPORT_SYMBOL(ion_client_destroy);
 
-static void ion_buffer_sync_for_device(struct ion_buffer *buffer,
-				       struct device *dev,
-				       enum dma_data_direction direction);
 
 static struct sg_table *ion_map_dma_buf(struct dma_buf_attachment *attachment,
 					enum dma_data_direction direction)
 {
-	struct dma_buf *dmabuf = attachment->dmabuf;
-	struct ion_buffer *buffer = dmabuf->priv;
-
-	ion_buffer_sync_for_device(buffer, attachment->dev, direction);
-	return buffer->sg_table;
-}
-
-static void ion_unmap_dma_buf(struct dma_buf_attachment *attachment,
-			      struct sg_table *table,
-			      enum dma_data_direction direction)
-{
-}
-
-void ion_pages_sync_for_device(struct device *dev, struct page *page,
-			       size_t size, enum dma_data_direction dir)
-{
-	struct scatterlist sg;
-
-	sg_init_table(&sg, 1);
-	sg_set_page(&sg, page, size, 0);
-	/*
-	 * This is not correct - sg_dma_address needs a dma_addr_t that is valid
-	 * for the targeted device, but this works on the currently targeted
-	 * hardware.
-	 */
-	sg_dma_address(&sg) = page_to_phys(page);
-	dma_sync_sg_for_device(dev, &sg, 1, dir);
-}
-
-struct ion_vma_list {
-	struct list_head list;
-	struct vm_area_struct *vma;
-};
-
-static void ion_buffer_sync_for_device(struct ion_buffer *buffer,
-				       struct device *dev,
-				       enum dma_data_direction dir)
-{
-	struct ion_vma_list *vma_list;
-	int pages = PAGE_ALIGN(buffer->size) / PAGE_SIZE;
-	int i;
-
-	pr_debug("%s: syncing for device %s\n", __func__,
-		 dev ? dev_name(dev) : "null");
-
-	if (!ion_buffer_fault_user_mappings(buffer))
-		return;
-
-	mutex_lock(&buffer->lock);
-	for (i = 0; i < pages; i++) {
-		struct page *page = buffer->pages[i];
-
-		if (ion_buffer_page_is_dirty(page))
-			ion_pages_sync_for_device(dev, ion_buffer_page(page),
-						  PAGE_SIZE, dir);
-
-		ion_buffer_page_clean(buffer->pages + i);
-	}
-	list_for_each_entry(vma_list, &buffer->vmas, list) {
-		struct vm_area_struct *vma = vma_list->vma;
-
-		zap_page_range(vma, vma->vm_start, vma->vm_end - vma->vm_start);
-	}
-	mutex_unlock(&buffer->lock);
-}
-
-static int ion_vm_fault(struct vm_fault *vmf)
-{
-	struct ion_buffer *buffer = vmf->vma->vm_private_data;
-	unsigned long pfn;
+	struct ion_dma_buf_attachment *a = attachment->priv;
+	struct sg_table *table;
 	int ret;
 
-	mutex_lock(&buffer->lock);
-	ion_buffer_page_dirty(buffer->pages + vmf->pgoff);
-	BUG_ON(!buffer->pages || !buffer->pages[vmf->pgoff]);
-
-	pfn = page_to_pfn(ion_buffer_page(buffer->pages[vmf->pgoff]));
-	ret = vm_insert_pfn(vmf->vma, vmf->address, pfn);
-	mutex_unlock(&buffer->lock);
-	if (ret)
-		return VM_FAULT_ERROR;
+	table = a->table;
 
-	return VM_FAULT_NOPAGE;
-}
-
-static void ion_vm_open(struct vm_area_struct *vma)
-{
-	struct ion_buffer *buffer = vma->vm_private_data;
-	struct ion_vma_list *vma_list;
+	if (!dma_map_sg(attachment->dev, table->sgl, table->nents,
+			direction)){
+		ret = -ENOMEM;
+		goto err;
+	}
+	return table;
 
-	vma_list = kmalloc(sizeof(*vma_list), GFP_KERNEL);
-	if (!vma_list)
-		return;
-	vma_list->vma = vma;
-	mutex_lock(&buffer->lock);
-	list_add(&vma_list->list, &buffer->vmas);
-	mutex_unlock(&buffer->lock);
-	pr_debug("%s: adding %p\n", __func__, vma);
+err:
+	free_duped_table(table);
+	return ERR_PTR(ret);
 }
 
-static void ion_vm_close(struct vm_area_struct *vma)
+static void ion_unmap_dma_buf(struct dma_buf_attachment *attachment,
+			      struct sg_table *table,
+			      enum dma_data_direction direction)
 {
-	struct ion_buffer *buffer = vma->vm_private_data;
-	struct ion_vma_list *vma_list, *tmp;
-
-	pr_debug("%s\n", __func__);
-	mutex_lock(&buffer->lock);
-	list_for_each_entry_safe(vma_list, tmp, &buffer->vmas, list) {
-		if (vma_list->vma != vma)
-			continue;
-		list_del(&vma_list->list);
-		kfree(vma_list);
-		pr_debug("%s: deleting %p\n", __func__, vma);
-		break;
-	}
-	mutex_unlock(&buffer->lock);
+	dma_unmap_sg(attachment->dev, table->sgl, table->nents, direction);
 }
 
-static const struct vm_operations_struct ion_vma_ops = {
-	.open = ion_vm_open,
-	.close = ion_vm_close,
-	.fault = ion_vm_fault,
-};
-
 static int ion_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
@@ -940,15 +304,6 @@ static int ion_mmap(struct dma_buf *dmabuf, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	if (ion_buffer_fault_user_mappings(buffer)) {
-		vma->vm_flags |= VM_IO | VM_PFNMAP | VM_DONTEXPAND |
-							VM_DONTDUMP;
-		vma->vm_private_data = buffer;
-		vma->vm_ops = &ion_vma_ops;
-		ion_vm_open(vma);
-		return 0;
-	}
-
 	if (!(buffer->flags & ION_FLAG_CACHED))
 		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 
@@ -968,7 +323,7 @@ static void ion_dma_buf_release(struct dma_buf *dmabuf)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
 
-	ion_buffer_put(buffer);
+	_ion_buffer_destroy(buffer);
 }
 
 static void *ion_dma_buf_kmap(struct dma_buf *dmabuf, unsigned long offset)
@@ -988,26 +343,45 @@ static int ion_dma_buf_begin_cpu_access(struct dma_buf *dmabuf,
 {
 	struct ion_buffer *buffer = dmabuf->priv;
 	void *vaddr;
+	struct ion_dma_buf_attachment *a;
 
-	if (!buffer->heap->ops->map_kernel) {
-		pr_err("%s: map kernel is not implemented by this heap.\n",
-		       __func__);
-		return -ENODEV;
+	/*
+	 * TODO: Move this elsewhere because we don't always need a vaddr
+	 */
+	if (buffer->heap->ops->map_kernel) {
+		mutex_lock(&buffer->lock);
+		vaddr = ion_buffer_kmap_get(buffer);
+		mutex_unlock(&buffer->lock);
 	}
 
+
 	mutex_lock(&buffer->lock);
-	vaddr = ion_buffer_kmap_get(buffer);
+	list_for_each_entry(a, &buffer->attachments, list) {
+		dma_sync_sg_for_cpu(a->dev, a->table->sgl, a->table->nents,
+					DMA_BIDIRECTIONAL);
+	}
 	mutex_unlock(&buffer->lock);
-	return PTR_ERR_OR_ZERO(vaddr);
+
+	return 0;
 }
 
 static int ion_dma_buf_end_cpu_access(struct dma_buf *dmabuf,
 				      enum dma_data_direction direction)
 {
 	struct ion_buffer *buffer = dmabuf->priv;
+	struct ion_dma_buf_attachment *a;
+
+	if (buffer->heap->ops->map_kernel) {
+		mutex_lock(&buffer->lock);
+		ion_buffer_kmap_put(buffer);
+		mutex_unlock(&buffer->lock);
+	}
 
 	mutex_lock(&buffer->lock);
-	ion_buffer_kmap_put(buffer);
+	list_for_each_entry(a, &buffer->attachments, list) {
+		dma_sync_sg_for_device(a->dev, a->table->sgl, a->table->nents,
+					DMA_BIDIRECTIONAL);
+	}
 	mutex_unlock(&buffer->lock);
 
 	return 0;
@@ -1018,6 +392,8 @@ static const struct dma_buf_ops dma_buf_ops = {
 	.unmap_dma_buf = ion_unmap_dma_buf,
 	.mmap = ion_mmap,
 	.release = ion_dma_buf_release,
+	.attach = ion_dma_buf_attach,
+	.detach = ion_dma_buf_detatch,
 	.begin_cpu_access = ion_dma_buf_begin_cpu_access,
 	.end_cpu_access = ion_dma_buf_end_cpu_access,
 	.map_atomic = ion_dma_buf_kmap,
@@ -1026,24 +402,44 @@ static const struct dma_buf_ops dma_buf_ops = {
 	.unmap = ion_dma_buf_kunmap,
 };
 
-struct dma_buf *ion_share_dma_buf(struct ion_client *client,
-				  struct ion_handle *handle)
+int ion_alloc(size_t len, unsigned int heap_id_mask, unsigned int flags)
 {
+	struct ion_device *dev = internal_dev;
+	struct ion_buffer *buffer = NULL;
+	struct ion_heap *heap;
 	DEFINE_DMA_BUF_EXPORT_INFO(exp_info);
-	struct ion_buffer *buffer;
+	int fd;
 	struct dma_buf *dmabuf;
-	bool valid_handle;
 
-	mutex_lock(&client->lock);
-	valid_handle = ion_handle_validate(client, handle);
-	if (!valid_handle) {
-		WARN(1, "%s: invalid handle passed to share.\n", __func__);
-		mutex_unlock(&client->lock);
-		return ERR_PTR(-EINVAL);
+	pr_debug("%s: len %zu heap_id_mask %u flags %x\n", __func__,
+		 len, heap_id_mask, flags);
+	/*
+	 * traverse the list of heaps available in this system in priority
+	 * order.  If the heap type is supported by the client, and matches the
+	 * request of the caller allocate from it.  Repeat until allocate has
+	 * succeeded or all heaps have been tried
+	 */
+	len = PAGE_ALIGN(len);
+
+	if (!len)
+		return -EINVAL;
+
+	down_read(&dev->lock);
+	plist_for_each_entry(heap, &dev->heaps, node) {
+		/* if the caller didn't specify this heap id */
+		if (!((1 << heap->id) & heap_id_mask))
+			continue;
+		buffer = ion_buffer_create(heap, dev, len, flags);
+		if (!IS_ERR(buffer))
+			break;
 	}
-	buffer = handle->buffer;
-	ion_buffer_get(buffer);
-	mutex_unlock(&client->lock);
+	up_read(&dev->lock);
+
+	if (buffer == NULL)
+		return -ENODEV;
+
+	if (IS_ERR(buffer))
+		return PTR_ERR(buffer);
 
 	exp_info.ops = &dma_buf_ops;
 	exp_info.size = buffer->size;
@@ -1052,22 +448,9 @@ struct dma_buf *ion_share_dma_buf(struct ion_client *client,
 
 	dmabuf = dma_buf_export(&exp_info);
 	if (IS_ERR(dmabuf)) {
-		ion_buffer_put(buffer);
-		return dmabuf;
-	}
-
-	return dmabuf;
-}
-EXPORT_SYMBOL(ion_share_dma_buf);
-
-int ion_share_dma_buf_fd(struct ion_client *client, struct ion_handle *handle)
-{
-	struct dma_buf *dmabuf;
-	int fd;
-
-	dmabuf = ion_share_dma_buf(client, handle);
-	if (IS_ERR(dmabuf))
+		_ion_buffer_destroy(buffer);
 		return PTR_ERR(dmabuf);
+	}
 
 	fd = dma_buf_fd(dmabuf, O_CLOEXEC);
 	if (fd < 0)
@@ -1075,93 +458,10 @@ int ion_share_dma_buf_fd(struct ion_client *client, struct ion_handle *handle)
 
 	return fd;
 }
-EXPORT_SYMBOL(ion_share_dma_buf_fd);
-
-struct ion_handle *ion_import_dma_buf(struct ion_client *client,
-				      struct dma_buf *dmabuf)
-{
-	struct ion_buffer *buffer;
-	struct ion_handle *handle;
-	int ret;
-
-	/* if this memory came from ion */
-
-	if (dmabuf->ops != &dma_buf_ops) {
-		pr_err("%s: can not import dmabuf from another exporter\n",
-		       __func__);
-		return ERR_PTR(-EINVAL);
-	}
-	buffer = dmabuf->priv;
-
-	mutex_lock(&client->lock);
-	/* if a handle exists for this buffer just take a reference to it */
-	handle = ion_handle_lookup(client, buffer);
-	if (!IS_ERR(handle)) {
-		ion_handle_get(handle);
-		mutex_unlock(&client->lock);
-		goto end;
-	}
-
-	handle = ion_handle_create(client, buffer);
-	if (IS_ERR(handle)) {
-		mutex_unlock(&client->lock);
-		goto end;
-	}
-
-	ret = ion_handle_add(client, handle);
-	mutex_unlock(&client->lock);
-	if (ret) {
-		ion_handle_put(handle);
-		handle = ERR_PTR(ret);
-	}
-
-end:
-	return handle;
-}
-EXPORT_SYMBOL(ion_import_dma_buf);
-
-struct ion_handle *ion_import_dma_buf_fd(struct ion_client *client, int fd)
-{
-	struct dma_buf *dmabuf;
-	struct ion_handle *handle;
-
-	dmabuf = dma_buf_get(fd);
-	if (IS_ERR(dmabuf))
-		return ERR_CAST(dmabuf);
-
-	handle = ion_import_dma_buf(client, dmabuf);
-	dma_buf_put(dmabuf);
-	return handle;
-}
-EXPORT_SYMBOL(ion_import_dma_buf_fd);
-
-int ion_sync_for_device(struct ion_client *client, int fd)
-{
-	struct dma_buf *dmabuf;
-	struct ion_buffer *buffer;
-
-	dmabuf = dma_buf_get(fd);
-	if (IS_ERR(dmabuf))
-		return PTR_ERR(dmabuf);
-
-	/* if this memory came from ion */
-	if (dmabuf->ops != &dma_buf_ops) {
-		pr_err("%s: can not sync dmabuf from another exporter\n",
-		       __func__);
-		dma_buf_put(dmabuf);
-		return -EINVAL;
-	}
-	buffer = dmabuf->priv;
-
-	dma_sync_sg_for_device(NULL, buffer->sg_table->sgl,
-			       buffer->sg_table->nents, DMA_BIDIRECTIONAL);
-	dma_buf_put(dmabuf);
-	return 0;
-}
 
-int ion_query_heaps(struct ion_client *client, struct ion_heap_query *query)
+int ion_query_heaps(struct ion_heap_query *query)
 {
-	struct ion_device *dev = client->dev;
+	struct ion_device *dev = internal_dev;
 	struct ion_heap_data __user *buffer = u64_to_user_ptr(query->heaps);
 	int ret = -EINVAL, cnt = 0, max_cnt;
 	struct ion_heap *heap;
@@ -1198,138 +498,18 @@ int ion_query_heaps(struct ion_client *client, struct ion_heap_query *query)
 	}
 
 	query->cnt = cnt;
+	ret = 0;
 out:
 	up_read(&dev->lock);
 	return ret;
 }
 
-static int ion_release(struct inode *inode, struct file *file)
-{
-	struct ion_client *client = file->private_data;
-
-	pr_debug("%s: %d\n", __func__, __LINE__);
-	ion_client_destroy(client);
-	return 0;
-}
-
-static int ion_open(struct inode *inode, struct file *file)
-{
-	struct miscdevice *miscdev = file->private_data;
-	struct ion_device *dev = container_of(miscdev, struct ion_device, dev);
-	struct ion_client *client;
-	char debug_name[64];
-
-	pr_debug("%s: %d\n", __func__, __LINE__);
-	snprintf(debug_name, 64, "%u", task_pid_nr(current->group_leader));
-	client = ion_client_create(dev, debug_name);
-	if (IS_ERR(client))
-		return PTR_ERR(client);
-	file->private_data = client;
-
-	return 0;
-}
-
 static const struct file_operations ion_fops = {
 	.owner          = THIS_MODULE,
-	.open           = ion_open,
-	.release        = ion_release,
 	.unlocked_ioctl = ion_ioctl,
-	.compat_ioctl   = compat_ion_ioctl,
-};
-
-static size_t ion_debug_heap_total(struct ion_client *client,
-				   unsigned int id)
-{
-	size_t size = 0;
-	struct rb_node *n;
-
-	mutex_lock(&client->lock);
-	for (n = rb_first(&client->handles); n; n = rb_next(n)) {
-		struct ion_handle *handle = rb_entry(n,
-						     struct ion_handle,
-						     node);
-		if (handle->buffer->heap->id == id)
-			size += handle->buffer->size;
-	}
-	mutex_unlock(&client->lock);
-	return size;
-}
-
-static int ion_debug_heap_show(struct seq_file *s, void *unused)
-{
-	struct ion_heap *heap = s->private;
-	struct ion_device *dev = heap->dev;
-	struct rb_node *n;
-	size_t total_size = 0;
-	size_t total_orphaned_size = 0;
-
-	seq_printf(s, "%16s %16s %16s\n", "client", "pid", "size");
-	seq_puts(s, "----------------------------------------------------\n");
-
-	mutex_lock(&debugfs_mutex);
-	for (n = rb_first(&dev->clients); n; n = rb_next(n)) {
-		struct ion_client *client = rb_entry(n, struct ion_client,
-						     node);
-		size_t size = ion_debug_heap_total(client, heap->id);
-
-		if (!size)
-			continue;
-		if (client->task) {
-			char task_comm[TASK_COMM_LEN];
-
-			get_task_comm(task_comm, client->task);
-			seq_printf(s, "%16s %16u %16zu\n", task_comm,
-				   client->pid, size);
-		} else {
-			seq_printf(s, "%16s %16u %16zu\n", client->name,
-				   client->pid, size);
-		}
-	}
-	mutex_unlock(&debugfs_mutex);
-
-	seq_puts(s, "----------------------------------------------------\n");
-	seq_puts(s, "orphaned allocations (info is from last known client):\n");
-	mutex_lock(&dev->buffer_lock);
-	for (n = rb_first(&dev->buffers); n; n = rb_next(n)) {
-		struct ion_buffer *buffer = rb_entry(n, struct ion_buffer,
-						     node);
-		if (buffer->heap->id != heap->id)
-			continue;
-		total_size += buffer->size;
-		if (!buffer->handle_count) {
-			seq_printf(s, "%16s %16u %16zu %d %d\n",
-				   buffer->task_comm, buffer->pid,
-				   buffer->size, buffer->kmap_cnt,
-				   kref_read(&buffer->ref));
-			total_orphaned_size += buffer->size;
-		}
-	}
-	mutex_unlock(&dev->buffer_lock);
-	seq_puts(s, "----------------------------------------------------\n");
-	seq_printf(s, "%16s %16zu\n", "total orphaned",
-		   total_orphaned_size);
-	seq_printf(s, "%16s %16zu\n", "total ", total_size);
-	if (heap->flags & ION_HEAP_FLAG_DEFER_FREE)
-		seq_printf(s, "%16s %16zu\n", "deferred free",
-			   heap->free_list_size);
-	seq_puts(s, "----------------------------------------------------\n");
-
-	if (heap->debug_show)
-		heap->debug_show(heap, s, unused);
-
-	return 0;
-}
-
-static int ion_debug_heap_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, ion_debug_heap_show, inode->i_private);
-}
-
-static const struct file_operations debug_heap_fops = {
-	.open = ion_debug_heap_open,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = single_release,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= ion_ioctl,
+#endif
 };
 
 static int debug_shrink_set(void *data, u64 val)
@@ -1367,9 +547,10 @@ static int debug_shrink_get(void *data, u64 *val)
 DEFINE_SIMPLE_ATTRIBUTE(debug_shrink_fops, debug_shrink_get,
 			debug_shrink_set, "%llu\n");
 
-void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
+void ion_device_add_heap(struct ion_heap *heap)
 {
 	struct dentry *debug_file;
+	struct ion_device *dev = internal_dev;
 
 	if (!heap->ops->allocate || !heap->ops->free)
 		pr_err("%s: can not add heap with invalid ops struct.\n",
@@ -1386,35 +567,25 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 
 	heap->dev = dev;
 	down_write(&dev->lock);
+	heap->id = heap_id++;
 	/*
 	 * use negative heap->id to reverse the priority -- when traversing
 	 * the list later attempt higher id numbers first
 	 */
 	plist_node_init(&heap->node, -heap->id);
 	plist_add(&heap->node, &dev->heaps);
-	debug_file = debugfs_create_file(heap->name, 0664,
-					 dev->heaps_debug_root, heap,
-					 &debug_heap_fops);
-
-	if (!debug_file) {
-		char buf[256], *path;
-
-		path = dentry_path(dev->heaps_debug_root, buf, 256);
-		pr_err("Failed to create heap debugfs at %s/%s\n",
-		       path, heap->name);
-	}
 
 	if (heap->shrinker.count_objects && heap->shrinker.scan_objects) {
 		char debug_name[64];
 
 		snprintf(debug_name, 64, "%s_shrink", heap->name);
 		debug_file = debugfs_create_file(
-			debug_name, 0644, dev->heaps_debug_root, heap,
+			debug_name, 0644, dev->debug_root, heap,
 			&debug_shrink_fops);
 		if (!debug_file) {
 			char buf[256], *path;
 
-			path = dentry_path(dev->heaps_debug_root, buf, 256);
+			path = dentry_path(dev->debug_root, buf, 256);
 			pr_err("Failed to create heap shrinker debugfs at %s/%s\n",
 			       path, debug_name);
 		}
@@ -1425,17 +596,14 @@ void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap)
 }
 EXPORT_SYMBOL(ion_device_add_heap);
 
-struct ion_device *ion_device_create(long (*custom_ioctl)
-				     (struct ion_client *client,
-				      unsigned int cmd,
-				      unsigned long arg))
+int ion_device_create(void)
 {
 	struct ion_device *idev;
 	int ret;
 
 	idev = kzalloc(sizeof(*idev), GFP_KERNEL);
 	if (!idev)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
 	idev->dev.minor = MISC_DYNAMIC_MINOR;
 	idev->dev.name = "ion";
@@ -1445,7 +613,7 @@ struct ion_device *ion_device_create(long (*custom_ioctl)
 	if (ret) {
 		pr_err("ion: failed to register misc device.\n");
 		kfree(idev);
-		return ERR_PTR(ret);
+		return ret;
 	}
 
 	idev->debug_root = debugfs_create_dir("ion", NULL);
@@ -1453,35 +621,13 @@ struct ion_device *ion_device_create(long (*custom_ioctl)
 		pr_err("ion: failed to create debugfs root directory.\n");
 		goto debugfs_done;
 	}
-	idev->heaps_debug_root = debugfs_create_dir("heaps", idev->debug_root);
-	if (!idev->heaps_debug_root) {
-		pr_err("ion: failed to create debugfs heaps directory.\n");
-		goto debugfs_done;
-	}
-	idev->clients_debug_root = debugfs_create_dir("clients",
-						idev->debug_root);
-	if (!idev->clients_debug_root)
-		pr_err("ion: failed to create debugfs clients directory.\n");
 
 debugfs_done:
-
-	idev->custom_ioctl = custom_ioctl;
 	idev->buffers = RB_ROOT;
 	mutex_init(&idev->buffer_lock);
 	init_rwsem(&idev->lock);
 	plist_head_init(&idev->heaps);
-	idev->clients = RB_ROOT;
-	ion_root_client = &idev->clients;
-	mutex_init(&debugfs_mutex);
-	return idev;
-}
-EXPORT_SYMBOL(ion_device_create);
-
-void ion_device_destroy(struct ion_device *dev)
-{
-	misc_deregister(&dev->dev);
-	debugfs_remove_recursive(dev->debug_root);
-	/* XXX need to free the heaps and clients ? */
-	kfree(dev);
+	internal_dev = idev;
+	return 0;
 }
-EXPORT_SYMBOL(ion_device_destroy);
+subsys_initcall(ion_device_create);
diff --git a/drivers/staging/android/ion/ion.h b/drivers/staging/android/ion/ion.h
index 93dafb4586e4..ace8416bd509 100644
--- a/drivers/staging/android/ion/ion.h
+++ b/drivers/staging/android/ion/ion.h
@@ -14,38 +14,31 @@
  *
  */
 
-#ifndef _LINUX_ION_H
-#define _LINUX_ION_H
+#ifndef _ION_H
+#define _ION_H
 
+#include <linux/device.h>
+#include <linux/dma-direction.h>
+#include <linux/kref.h>
+#include <linux/mm_types.h>
+#include <linux/mutex.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/shrinker.h>
 #include <linux/types.h>
+#include <linux/miscdevice.h>
 
 #include "../uapi/ion.h"
 
-struct ion_handle;
-struct ion_device;
-struct ion_heap;
-struct ion_mapper;
-struct ion_client;
-struct ion_buffer;
-
-/*
- * This should be removed some day when phys_addr_t's are fully
- * plumbed in the kernel, and all instances of ion_phys_addr_t should
- * be converted to phys_addr_t.  For the time being many kernel interfaces
- * do not accept phys_addr_t's that would have to
- */
-#define ion_phys_addr_t unsigned long
-
 /**
  * struct ion_platform_heap - defines a heap in the given platform
  * @type:	type of the heap from ion_heap_type enum
- * @id:		unique identifier for heap.  When allocating higher numbers
+ * @id:		unique identifier for heap.  When allocating higher numb ers
  *		will be allocated from first.  At allocation these are passed
  *		as a bit mask and therefore can not exceed ION_NUM_HEAP_IDS.
  * @name:	used for debug purposes
  * @base:	base address of heap in physical memory if applicable
  * @size:	size of the heap in bytes if applicable
- * @align:	required alignment in physical memory if applicable
  * @priv:	private info passed from the board file
  *
  * Provided by the board file.
@@ -54,123 +47,329 @@ struct ion_platform_heap {
 	enum ion_heap_type type;
 	unsigned int id;
 	const char *name;
-	ion_phys_addr_t base;
+	phys_addr_t base;
 	size_t size;
-	ion_phys_addr_t align;
+	phys_addr_t align;
 	void *priv;
 };
 
 /**
- * struct ion_platform_data - array of platform heaps passed from board file
- * @nr:		number of structures in the array
- * @heaps:	array of platform_heap structions
+ * struct ion_buffer - metadata for a particular buffer
+ * @ref:		reference count
+ * @node:		node in the ion_device buffers tree
+ * @dev:		back pointer to the ion_device
+ * @heap:		back pointer to the heap the buffer came from
+ * @flags:		buffer specific flags
+ * @private_flags:	internal buffer specific flags
+ * @size:		size of the buffer
+ * @priv_virt:		private data to the buffer representable as
+ *			a void *
+ * @lock:		protects the buffers cnt fields
+ * @kmap_cnt:		number of times the buffer is mapped to the kernel
+ * @vaddr:		the kernel mapping if kmap_cnt is not zero
+ * @sg_table:		the sg table for the buffer if dmap_cnt is not zero
+ * @pages:		flat array of pages in the buffer -- used by fault
+ *			handler and only valid for buffers that are faulted in
+ * @vmas:		list of vma's mapping this buffer
+ * @handle_count:	count of handles referencing this buffer
+ * @task_comm:		taskcomm of last client to reference this buffer in a
+ *			handle, used for debugging
+ * @pid:		pid of last client to reference this buffer in a
+ *			handle, used for debugging
+ */
+struct ion_buffer {
+	union {
+		struct rb_node node;
+		struct list_head list;
+	};
+	struct ion_device *dev;
+	struct ion_heap *heap;
+	unsigned long flags;
+	unsigned long private_flags;
+	size_t size;
+	void *priv_virt;
+	struct mutex lock;
+	int kmap_cnt;
+	void *vaddr;
+	struct sg_table *sg_table;
+	struct page **pages;
+	struct list_head vmas;
+	struct list_head attachments;
+	/* used to track orphaned buffers */
+	int handle_count;
+	char task_comm[TASK_COMM_LEN];
+	pid_t pid;
+};
+void ion_buffer_destroy(struct ion_buffer *buffer);
+
+/**
+ * struct ion_device - the metadata of the ion device node
+ * @dev:		the actual misc device
+ * @buffers:		an rb tree of all the existing buffers
+ * @buffer_lock:	lock protecting the tree of buffers
+ * @lock:		rwsem protecting the tree of heaps and clients
+ */
+struct ion_device {
+	struct miscdevice dev;
+	struct rb_root buffers;
+	struct mutex buffer_lock;
+	struct rw_semaphore lock;
+	struct plist_head heaps;
+	struct dentry *debug_root;
+	int heap_cnt;
+};
+
+/**
+ * struct ion_heap_ops - ops to operate on a given heap
+ * @allocate:		allocate memory
+ * @free:		free memory
+ * @map_kernel		map memory to the kernel
+ * @unmap_kernel	unmap memory to the kernel
+ * @map_user		map memory to userspace
  *
- * Provided by the board file in the form of platform data to a platform device.
+ * allocate, phys, and map_user return 0 on success, -errno on error.
+ * map_dma and map_kernel return pointer on success, ERR_PTR on
+ * error. @free will be called with ION_PRIV_FLAG_SHRINKER_FREE set in
+ * the buffer's private_flags when called from a shrinker. In that
+ * case, the pages being free'd must be truly free'd back to the
+ * system, not put in a page pool or otherwise cached.
  */
-struct ion_platform_data {
-	int nr;
-	struct ion_platform_heap *heaps;
+struct ion_heap_ops {
+	int (*allocate)(struct ion_heap *heap,
+			struct ion_buffer *buffer, unsigned long len,
+			unsigned long flags);
+	void (*free)(struct ion_buffer *buffer);
+	void * (*map_kernel)(struct ion_heap *heap, struct ion_buffer *buffer);
+	void (*unmap_kernel)(struct ion_heap *heap, struct ion_buffer *buffer);
+	int (*map_user)(struct ion_heap *mapper, struct ion_buffer *buffer,
+			struct vm_area_struct *vma);
+	int (*shrink)(struct ion_heap *heap, gfp_t gfp_mask, int nr_to_scan);
 };
 
 /**
- * ion_client_create() -  allocate a client and returns it
- * @dev:		the global ion device
- * @name:		used for debugging
+ * heap flags - flags between the heaps and core ion code
  */
-struct ion_client *ion_client_create(struct ion_device *dev,
-				     const char *name);
+#define ION_HEAP_FLAG_DEFER_FREE (1 << 0)
 
 /**
- * ion_client_destroy() -  free's a client and all it's handles
- * @client:	the client
+ * private flags - flags internal to ion
+ */
+/*
+ * Buffer is being freed from a shrinker function. Skip any possible
+ * heap-specific caching mechanism (e.g. page pools). Guarantees that
+ * any buffer storage that came from the system allocator will be
+ * returned to the system allocator.
+ */
+#define ION_PRIV_FLAG_SHRINKER_FREE (1 << 0)
+
+/**
+ * struct ion_heap - represents a heap in the system
+ * @node:		rb node to put the heap on the device's tree of heaps
+ * @dev:		back pointer to the ion_device
+ * @type:		type of heap
+ * @ops:		ops struct as above
+ * @flags:		flags
+ * @id:			id of heap, also indicates priority of this heap when
+ *			allocating.  These are specified by platform data and
+ *			MUST be unique
+ * @name:		used for debugging
+ * @shrinker:		a shrinker for the heap
+ * @free_list:		free list head if deferred free is used
+ * @free_list_size	size of the deferred free list in bytes
+ * @lock:		protects the free list
+ * @waitqueue:		queue to wait on from deferred free thread
+ * @task:		task struct of deferred free thread
+ * @debug_show:		called when heap debug file is read to add any
+ *			heap specific debug info to output
  *
- * Free the provided client and all it's resources including
- * any handles it is holding.
+ * Represents a pool of memory from which buffers can be made.  In some
+ * systems the only heap is regular system memory allocated via vmalloc.
+ * On others, some blocks might require large physically contiguous buffers
+ * that are allocated from a specially reserved heap.
  */
-void ion_client_destroy(struct ion_client *client);
+struct ion_heap {
+	struct plist_node node;
+	struct ion_device *dev;
+	enum ion_heap_type type;
+	struct ion_heap_ops *ops;
+	unsigned long flags;
+	unsigned int id;
+	const char *name;
+	struct shrinker shrinker;
+	struct list_head free_list;
+	size_t free_list_size;
+	spinlock_t free_lock;
+	wait_queue_head_t waitqueue;
+	struct task_struct *task;
+
+	int (*debug_show)(struct ion_heap *heap, struct seq_file *, void *);
+};
 
 /**
- * ion_alloc - allocate ion memory
- * @client:		the client
- * @len:		size of the allocation
- * @align:		requested allocation alignment, lots of hardware blocks
- *			have alignment requirements of some kind
- * @heap_id_mask:	mask of heaps to allocate from, if multiple bits are set
- *			heaps will be tried in order from highest to lowest
- *			id
- * @flags:		heap flags, the low 16 bits are consumed by ion, the
- *			high 16 bits are passed on to the respective heap and
- *			can be heap custom
+ * ion_buffer_cached - this ion buffer is cached
+ * @buffer:		buffer
  *
- * Allocate memory in one of the heaps provided in heap mask and return
- * an opaque handle to it.
+ * indicates whether this ion buffer is cached
  */
-struct ion_handle *ion_alloc(struct ion_client *client, size_t len,
-			     size_t align, unsigned int heap_id_mask,
-			     unsigned int flags);
+bool ion_buffer_cached(struct ion_buffer *buffer);
 
 /**
- * ion_free - free a handle
- * @client:	the client
- * @handle:	the handle to free
+ * ion_buffer_fault_user_mappings - fault in user mappings of this buffer
+ * @buffer:		buffer
  *
- * Free the provided handle.
+ * indicates whether userspace mappings of this buffer will be faulted
+ * in, this can affect how buffers are allocated from the heap.
  */
-void ion_free(struct ion_client *client, struct ion_handle *handle);
+bool ion_buffer_fault_user_mappings(struct ion_buffer *buffer);
 
 /**
- * ion_map_kernel - create mapping for the given handle
- * @client:	the client
- * @handle:	handle to map
+ * ion_device_add_heap - adds a heap to the ion device
+ * @heap:		the heap to add
+ */
+void ion_device_add_heap(struct ion_heap *heap);
+
+/**
+ * some helpers for common operations on buffers using the sg_table
+ * and vaddr fields
+ */
+void *ion_heap_map_kernel(struct ion_heap *heap, struct ion_buffer *buffer);
+void ion_heap_unmap_kernel(struct ion_heap *heap, struct ion_buffer *buffer);
+int ion_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
+		      struct vm_area_struct *vma);
+int ion_heap_buffer_zero(struct ion_buffer *buffer);
+int ion_heap_pages_zero(struct page *page, size_t size, pgprot_t pgprot);
+
+int ion_alloc(size_t len,
+				unsigned int heap_id_mask,
+				unsigned int flags);
+
+/**
+ * ion_heap_init_shrinker
+ * @heap:		the heap
  *
- * Map the given handle into the kernel and return a kernel address that
- * can be used to access this address.
+ * If a heap sets the ION_HEAP_FLAG_DEFER_FREE flag or defines the shrink op
+ * this function will be called to setup a shrinker to shrink the freelists
+ * and call the heap's shrink op.
  */
-void *ion_map_kernel(struct ion_client *client, struct ion_handle *handle);
+void ion_heap_init_shrinker(struct ion_heap *heap);
 
 /**
- * ion_unmap_kernel() - destroy a kernel mapping for a handle
- * @client:	the client
- * @handle:	handle to unmap
+ * ion_heap_init_deferred_free -- initialize deferred free functionality
+ * @heap:		the heap
+ *
+ * If a heap sets the ION_HEAP_FLAG_DEFER_FREE flag this function will
+ * be called to setup deferred frees. Calls to free the buffer will
+ * return immediately and the actual free will occur some time later
  */
-void ion_unmap_kernel(struct ion_client *client, struct ion_handle *handle);
+int ion_heap_init_deferred_free(struct ion_heap *heap);
 
 /**
- * ion_share_dma_buf() - share buffer as dma-buf
- * @client:	the client
- * @handle:	the handle
+ * ion_heap_freelist_add - add a buffer to the deferred free list
+ * @heap:		the heap
+ * @buffer:		the buffer
+ *
+ * Adds an item to the deferred freelist.
  */
-struct dma_buf *ion_share_dma_buf(struct ion_client *client,
-						struct ion_handle *handle);
+void ion_heap_freelist_add(struct ion_heap *heap, struct ion_buffer *buffer);
 
 /**
- * ion_share_dma_buf_fd() - given an ion client, create a dma-buf fd
- * @client:	the client
- * @handle:	the handle
+ * ion_heap_freelist_drain - drain the deferred free list
+ * @heap:		the heap
+ * @size:		amount of memory to drain in bytes
+ *
+ * Drains the indicated amount of memory from the deferred freelist immediately.
+ * Returns the total amount freed.  The total freed may be higher depending
+ * on the size of the items in the list, or lower if there is insufficient
+ * total memory on the freelist.
  */
-int ion_share_dma_buf_fd(struct ion_client *client, struct ion_handle *handle);
+size_t ion_heap_freelist_drain(struct ion_heap *heap, size_t size);
 
 /**
- * ion_import_dma_buf() - get ion_handle from dma-buf
- * @client:	the client
- * @dmabuf:	the dma-buf
+ * ion_heap_freelist_shrink - drain the deferred free
+ *				list, skipping any heap-specific
+ *				pooling or caching mechanisms
+ *
+ * @heap:		the heap
+ * @size:		amount of memory to drain in bytes
  *
- * Get the ion_buffer associated with the dma-buf and return the ion_handle.
- * If no ion_handle exists for this buffer, return newly created ion_handle.
- * If dma-buf from another exporter is passed, return ERR_PTR(-EINVAL)
+ * Drains the indicated amount of memory from the deferred freelist immediately.
+ * Returns the total amount freed.  The total freed may be higher depending
+ * on the size of the items in the list, or lower if there is insufficient
+ * total memory on the freelist.
+ *
+ * Unlike with @ion_heap_freelist_drain, don't put any pages back into
+ * page pools or otherwise cache the pages. Everything must be
+ * genuinely free'd back to the system. If you're free'ing from a
+ * shrinker you probably want to use this. Note that this relies on
+ * the heap.ops.free callback honoring the ION_PRIV_FLAG_SHRINKER_FREE
+ * flag.
  */
-struct ion_handle *ion_import_dma_buf(struct ion_client *client,
-				      struct dma_buf *dmabuf);
+size_t ion_heap_freelist_shrink(struct ion_heap *heap,
+					size_t size);
 
 /**
- * ion_import_dma_buf_fd() - given a dma-buf fd from the ion exporter get handle
- * @client:	the client
- * @fd:		the dma-buf fd
+ * ion_heap_freelist_size - returns the size of the freelist in bytes
+ * @heap:		the heap
+ */
+size_t ion_heap_freelist_size(struct ion_heap *heap);
+
+
+/**
+ * functions for creating and destroying a heap pool -- allows you
+ * to keep a pool of pre allocated memory to use from your heap.  Keeping
+ * a pool of memory that is ready for dma, ie any cached mapping have been
+ * invalidated from the cache, provides a significant performance benefit on
+ * many systems
+ */
+
+/**
+ * struct ion_page_pool - pagepool struct
+ * @high_count:		number of highmem items in the pool
+ * @low_count:		number of lowmem items in the pool
+ * @high_items:		list of highmem items
+ * @low_items:		list of lowmem items
+ * @mutex:		lock protecting this struct and especially the count
+ *			item list
+ * @gfp_mask:		gfp_mask to use from alloc
+ * @order:		order of pages in the pool
+ * @list:		plist node for list of pools
+ * @cached:		it's cached pool or not
+ *
+ * Allows you to keep a pool of pre allocated pages to use from your heap.
+ * Keeping a pool of pages that is ready for dma, ie any cached mapping have
+ * been invalidated from the cache, provides a significant performance benefit
+ * on many systems
+ */
+struct ion_page_pool {
+	int high_count;
+	int low_count;
+	bool cached;
+	struct list_head high_items;
+	struct list_head low_items;
+	struct mutex mutex;
+	gfp_t gfp_mask;
+	unsigned int order;
+	struct plist_node list;
+};
+
+struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order,
+					   bool cached);
+void ion_page_pool_destroy(struct ion_page_pool *pool);
+struct page *ion_page_pool_alloc(struct ion_page_pool *pool);
+void ion_page_pool_free(struct ion_page_pool *pool, struct page *page);
+
+/** ion_page_pool_shrink - shrinks the size of the memory cached in the pool
+ * @pool:		the pool
+ * @gfp_mask:		the memory type to reclaim
+ * @nr_to_scan:		number of items to shrink in pages
  *
- * Given an dma-buf fd that was allocated through ion via ion_share_dma_buf_fd,
- * import that fd and return a handle representing it. If a dma-buf from
- * another exporter is passed in this function will return ERR_PTR(-EINVAL)
+ * returns the number of items freed in pages
  */
-struct ion_handle *ion_import_dma_buf_fd(struct ion_client *client, int fd);
+int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
+			  int nr_to_scan);
+
+long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
+
+int ion_query_heaps(struct ion_heap_query *query);
 
-#endif /* _LINUX_ION_H */
+#endif /* _ION_H */
diff --git a/drivers/staging/android/ion/ion_carveout_heap.c b/drivers/staging/android/ion/ion_carveout_heap.c
index a8ea97391c40..5fdc1f328f61 100644
--- a/drivers/staging/android/ion/ion_carveout_heap.c
+++ b/drivers/staging/android/ion/ion_carveout_heap.c
@@ -23,19 +23,17 @@
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
 #include "ion.h"
-#include "ion_priv.h"
 
 #define ION_CARVEOUT_ALLOCATE_FAIL	-1
 
 struct ion_carveout_heap {
 	struct ion_heap heap;
 	struct gen_pool *pool;
-	ion_phys_addr_t base;
+	phys_addr_t base;
 };
 
-static ion_phys_addr_t ion_carveout_allocate(struct ion_heap *heap,
-					     unsigned long size,
-					     unsigned long align)
+static phys_addr_t ion_carveout_allocate(struct ion_heap *heap,
+					     unsigned long size)
 {
 	struct ion_carveout_heap *carveout_heap =
 		container_of(heap, struct ion_carveout_heap, heap);
@@ -47,7 +45,7 @@ static ion_phys_addr_t ion_carveout_allocate(struct ion_heap *heap,
 	return offset;
 }
 
-static void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
+static void ion_carveout_free(struct ion_heap *heap, phys_addr_t addr,
 			      unsigned long size)
 {
 	struct ion_carveout_heap *carveout_heap =
@@ -60,16 +58,13 @@ static void ion_carveout_free(struct ion_heap *heap, ion_phys_addr_t addr,
 
 static int ion_carveout_heap_allocate(struct ion_heap *heap,
 				      struct ion_buffer *buffer,
-				      unsigned long size, unsigned long align,
+				      unsigned long size,
 				      unsigned long flags)
 {
 	struct sg_table *table;
-	ion_phys_addr_t paddr;
+	phys_addr_t paddr;
 	int ret;
 
-	if (align > PAGE_SIZE)
-		return -EINVAL;
-
 	table = kmalloc(sizeof(*table), GFP_KERNEL);
 	if (!table)
 		return -ENOMEM;
@@ -77,7 +72,7 @@ static int ion_carveout_heap_allocate(struct ion_heap *heap,
 	if (ret)
 		goto err_free;
 
-	paddr = ion_carveout_allocate(heap, size, align);
+	paddr = ion_carveout_allocate(heap, size);
 	if (paddr == ION_CARVEOUT_ALLOCATE_FAIL) {
 		ret = -ENOMEM;
 		goto err_free_table;
@@ -100,14 +95,10 @@ static void ion_carveout_heap_free(struct ion_buffer *buffer)
 	struct ion_heap *heap = buffer->heap;
 	struct sg_table *table = buffer->sg_table;
 	struct page *page = sg_page(table->sgl);
-	ion_phys_addr_t paddr = PFN_PHYS(page_to_pfn(page));
+	phys_addr_t paddr = PFN_PHYS(page_to_pfn(page));
 
 	ion_heap_buffer_zero(buffer);
 
-	if (ion_buffer_cached(buffer))
-		dma_sync_sg_for_device(NULL, table->sgl, table->nents,
-				       DMA_BIDIRECTIONAL);
-
 	ion_carveout_free(heap, paddr, buffer->size);
 	sg_free_table(table);
 	kfree(table);
@@ -132,8 +123,6 @@ struct ion_heap *ion_carveout_heap_create(struct ion_platform_heap *heap_data)
 	page = pfn_to_page(PFN_DOWN(heap_data->base));
 	size = heap_data->size;
 
-	ion_pages_sync_for_device(NULL, page, size, DMA_BIDIRECTIONAL);
-
 	ret = ion_heap_pages_zero(page, size, pgprot_writecombine(PAGE_KERNEL));
 	if (ret)
 		return ERR_PTR(ret);
@@ -156,13 +145,3 @@ struct ion_heap *ion_carveout_heap_create(struct ion_platform_heap *heap_data)
 
 	return &carveout_heap->heap;
 }
-
-void ion_carveout_heap_destroy(struct ion_heap *heap)
-{
-	struct ion_carveout_heap *carveout_heap =
-	     container_of(heap, struct  ion_carveout_heap, heap);
-
-	gen_pool_destroy(carveout_heap->pool);
-	kfree(carveout_heap);
-	carveout_heap = NULL;
-}
diff --git a/drivers/staging/android/ion/ion_chunk_heap.c b/drivers/staging/android/ion/ion_chunk_heap.c
index 70495dc645ea..102c09398317 100644
--- a/drivers/staging/android/ion/ion_chunk_heap.c
+++ b/drivers/staging/android/ion/ion_chunk_heap.c
@@ -22,12 +22,11 @@
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
 #include "ion.h"
-#include "ion_priv.h"
 
 struct ion_chunk_heap {
 	struct ion_heap heap;
 	struct gen_pool *pool;
-	ion_phys_addr_t base;
+	phys_addr_t base;
 	unsigned long chunk_size;
 	unsigned long size;
 	unsigned long allocated;
@@ -35,7 +34,7 @@ struct ion_chunk_heap {
 
 static int ion_chunk_heap_allocate(struct ion_heap *heap,
 				   struct ion_buffer *buffer,
-				   unsigned long size, unsigned long align,
+				   unsigned long size,
 				   unsigned long flags)
 {
 	struct ion_chunk_heap *chunk_heap =
@@ -46,9 +45,6 @@ static int ion_chunk_heap_allocate(struct ion_heap *heap,
 	unsigned long num_chunks;
 	unsigned long allocated_size;
 
-	if (align > chunk_heap->chunk_size)
-		return -EINVAL;
-
 	allocated_size = ALIGN(size, chunk_heap->chunk_size);
 	num_chunks = allocated_size / chunk_heap->chunk_size;
 
@@ -104,10 +100,6 @@ static void ion_chunk_heap_free(struct ion_buffer *buffer)
 
 	ion_heap_buffer_zero(buffer);
 
-	if (ion_buffer_cached(buffer))
-		dma_sync_sg_for_device(NULL, table->sgl, table->nents,
-				       DMA_BIDIRECTIONAL);
-
 	for_each_sg(table->sgl, sg, table->nents, i) {
 		gen_pool_free(chunk_heap->pool, page_to_phys(sg_page(sg)),
 			      sg->length);
@@ -135,8 +127,6 @@ struct ion_heap *ion_chunk_heap_create(struct ion_platform_heap *heap_data)
 	page = pfn_to_page(PFN_DOWN(heap_data->base));
 	size = heap_data->size;
 
-	ion_pages_sync_for_device(NULL, page, size, DMA_BIDIRECTIONAL);
-
 	ret = ion_heap_pages_zero(page, size, pgprot_writecombine(PAGE_KERNEL));
 	if (ret)
 		return ERR_PTR(ret);
@@ -160,8 +150,8 @@ struct ion_heap *ion_chunk_heap_create(struct ion_platform_heap *heap_data)
 	chunk_heap->heap.ops = &chunk_heap_ops;
 	chunk_heap->heap.type = ION_HEAP_TYPE_CHUNK;
 	chunk_heap->heap.flags = ION_HEAP_FLAG_DEFER_FREE;
-	pr_debug("%s: base %lu size %zu align %ld\n", __func__,
-		 chunk_heap->base, heap_data->size, heap_data->align);
+	pr_debug("%s: base %pa size %zu\n", __func__,
+		 &chunk_heap->base, heap_data->size);
 
 	return &chunk_heap->heap;
 
@@ -170,12 +160,3 @@ error_gen_pool_create:
 	return ERR_PTR(ret);
 }
 
-void ion_chunk_heap_destroy(struct ion_heap *heap)
-{
-	struct ion_chunk_heap *chunk_heap =
-	     container_of(heap, struct  ion_chunk_heap, heap);
-
-	gen_pool_destroy(chunk_heap->pool);
-	kfree(chunk_heap);
-	chunk_heap = NULL;
-}
diff --git a/drivers/staging/android/ion/ion_cma_heap.c b/drivers/staging/android/ion/ion_cma_heap.c
index 6c4068523781..a0949bc0dcf4 100644
--- a/drivers/staging/android/ion/ion_cma_heap.c
+++ b/drivers/staging/android/ion/ion_cma_heap.c
@@ -19,124 +19,75 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/err.h>
-#include <linux/dma-mapping.h>
+#include <linux/cma.h>
+#include <linux/scatterlist.h>
 
 #include "ion.h"
-#include "ion_priv.h"
 
 struct ion_cma_heap {
 	struct ion_heap heap;
-	struct device *dev;
+	struct cma *cma;
 };
 
 #define to_cma_heap(x) container_of(x, struct ion_cma_heap, heap)
 
-struct ion_cma_buffer_info {
-	void *cpu_addr;
-	dma_addr_t handle;
-	struct sg_table *table;
-};
-
 
 /* ION CMA heap operations functions */
 static int ion_cma_allocate(struct ion_heap *heap, struct ion_buffer *buffer,
-			    unsigned long len, unsigned long align,
+			    unsigned long len,
 			    unsigned long flags)
 {
 	struct ion_cma_heap *cma_heap = to_cma_heap(heap);
-	struct device *dev = cma_heap->dev;
-	struct ion_cma_buffer_info *info;
-
-	dev_dbg(dev, "Request buffer allocation len %ld\n", len);
-
-	if (buffer->flags & ION_FLAG_CACHED)
-		return -EINVAL;
-
-	if (align > PAGE_SIZE)
-		return -EINVAL;
+	struct sg_table *table;
+	struct page *pages;
+	int ret;
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (!info)
+	pages = cma_alloc(cma_heap->cma, len, 0, GFP_KERNEL);
+	if (!pages)
 		return -ENOMEM;
 
-	info->cpu_addr = dma_alloc_coherent(dev, len, &(info->handle),
-						GFP_HIGHUSER | __GFP_ZERO);
-
-	if (!info->cpu_addr) {
-		dev_err(dev, "Fail to allocate buffer\n");
+	table = kmalloc(sizeof(struct sg_table), GFP_KERNEL);
+	if (!table)
 		goto err;
-	}
 
-	info->table = kmalloc(sizeof(*info->table), GFP_KERNEL);
-	if (!info->table)
+	ret = sg_alloc_table(table, 1, GFP_KERNEL);
+	if (ret)
 		goto free_mem;
 
-	if (dma_get_sgtable(dev, info->table, info->cpu_addr, info->handle,
-			    len))
-		goto free_table;
-	/* keep this for memory release */
-	buffer->priv_virt = info;
-	buffer->sg_table = info->table;
-	dev_dbg(dev, "Allocate buffer %p\n", buffer);
+	sg_set_page(table->sgl, pages, len, 0);
+
+	buffer->priv_virt = pages;
+	buffer->sg_table = table;
 	return 0;
 
-free_table:
-	kfree(info->table);
 free_mem:
-	dma_free_coherent(dev, len, info->cpu_addr, info->handle);
+	kfree(table);
 err:
-	kfree(info);
+	cma_release(cma_heap->cma, pages, buffer->size);
 	return -ENOMEM;
 }
 
 static void ion_cma_free(struct ion_buffer *buffer)
 {
 	struct ion_cma_heap *cma_heap = to_cma_heap(buffer->heap);
-	struct device *dev = cma_heap->dev;
-	struct ion_cma_buffer_info *info = buffer->priv_virt;
+	struct page *pages = buffer->priv_virt;
 
-	dev_dbg(dev, "Release buffer %p\n", buffer);
 	/* release memory */
-	dma_free_coherent(dev, buffer->size, info->cpu_addr, info->handle);
+	cma_release(cma_heap->cma, pages, buffer->size);
 	/* release sg table */
-	sg_free_table(info->table);
-	kfree(info->table);
-	kfree(info);
-}
-
-static int ion_cma_mmap(struct ion_heap *mapper, struct ion_buffer *buffer,
-			struct vm_area_struct *vma)
-{
-	struct ion_cma_heap *cma_heap = to_cma_heap(buffer->heap);
-	struct device *dev = cma_heap->dev;
-	struct ion_cma_buffer_info *info = buffer->priv_virt;
-
-	return dma_mmap_coherent(dev, vma, info->cpu_addr, info->handle,
-				 buffer->size);
-}
-
-static void *ion_cma_map_kernel(struct ion_heap *heap,
-				struct ion_buffer *buffer)
-{
-	struct ion_cma_buffer_info *info = buffer->priv_virt;
-	/* kernel memory mapping has been done at allocation time */
-	return info->cpu_addr;
-}
-
-static void ion_cma_unmap_kernel(struct ion_heap *heap,
-				 struct ion_buffer *buffer)
-{
+	sg_free_table(buffer->sg_table);
+	kfree(buffer->sg_table);
 }
 
 static struct ion_heap_ops ion_cma_ops = {
 	.allocate = ion_cma_allocate,
 	.free = ion_cma_free,
-	.map_user = ion_cma_mmap,
-	.map_kernel = ion_cma_map_kernel,
-	.unmap_kernel = ion_cma_unmap_kernel,
+	.map_user = ion_heap_map_user,
+	.map_kernel = ion_heap_map_kernel,
+	.unmap_kernel = ion_heap_unmap_kernel,
 };
 
-struct ion_heap *ion_cma_heap_create(struct ion_platform_heap *data)
+static struct ion_heap *__ion_cma_heap_create(struct cma *cma)
 {
 	struct ion_cma_heap *cma_heap;
 
@@ -150,14 +101,28 @@ struct ion_heap *ion_cma_heap_create(struct ion_platform_heap *data)
 	 * get device from private heaps data, later it will be
 	 * used to make the link with reserved CMA memory
 	 */
-	cma_heap->dev = data->priv;
+	cma_heap->cma = cma;
 	cma_heap->heap.type = ION_HEAP_TYPE_DMA;
 	return &cma_heap->heap;
 }
 
-void ion_cma_heap_destroy(struct ion_heap *heap)
+int __ion_add_cma_heaps(struct cma *cma, void *data)
 {
-	struct ion_cma_heap *cma_heap = to_cma_heap(heap);
+	struct ion_heap *heap;
 
-	kfree(cma_heap);
+	heap = __ion_cma_heap_create(cma);
+	if (IS_ERR(heap))
+		return PTR_ERR(heap);
+
+	heap->name = cma_get_name(cma);
+
+	ion_device_add_heap(heap);
+	return 0;
+}
+
+static int ion_add_cma_heaps(void)
+{
+	cma_for_each_area(__ion_add_cma_heaps, NULL);
+	return 0;
 }
+device_initcall(ion_add_cma_heaps);
diff --git a/drivers/staging/android/ion/ion_dummy_driver.c b/drivers/staging/android/ion/ion_dummy_driver.c
deleted file mode 100644
index cf5c010d32bc..000000000000
--- a/drivers/staging/android/ion/ion_dummy_driver.c
+++ /dev/null
@@ -1,156 +0,0 @@
-/*
- * drivers/gpu/ion/ion_dummy_driver.c
- *
- * Copyright (C) 2013 Linaro, Inc
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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/err.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/bootmem.h>
-#include <linux/memblock.h>
-#include <linux/sizes.h>
-#include <linux/io.h>
-#include "ion.h"
-#include "ion_priv.h"
-
-static struct ion_device *idev;
-static struct ion_heap **heaps;
-
-static void *carveout_ptr;
-static void *chunk_ptr;
-
-static struct ion_platform_heap dummy_heaps[] = {
-		{
-			.id	= ION_HEAP_TYPE_SYSTEM,
-			.type	= ION_HEAP_TYPE_SYSTEM,
-			.name	= "system",
-		},
-		{
-			.id	= ION_HEAP_TYPE_SYSTEM_CONTIG,
-			.type	= ION_HEAP_TYPE_SYSTEM_CONTIG,
-			.name	= "system contig",
-		},
-		{
-			.id	= ION_HEAP_TYPE_CARVEOUT,
-			.type	= ION_HEAP_TYPE_CARVEOUT,
-			.name	= "carveout",
-			.size	= SZ_4M,
-		},
-		{
-			.id	= ION_HEAP_TYPE_CHUNK,
-			.type	= ION_HEAP_TYPE_CHUNK,
-			.name	= "chunk",
-			.size	= SZ_4M,
-			.align	= SZ_16K,
-			.priv	= (void *)(SZ_16K),
-		},
-};
-
-static const struct ion_platform_data dummy_ion_pdata = {
-	.nr = ARRAY_SIZE(dummy_heaps),
-	.heaps = dummy_heaps,
-};
-
-static int __init ion_dummy_init(void)
-{
-	int i, err;
-
-	idev = ion_device_create(NULL);
-	if (IS_ERR(idev))
-		return PTR_ERR(idev);
-	heaps = kcalloc(dummy_ion_pdata.nr, sizeof(struct ion_heap *),
-			GFP_KERNEL);
-	if (!heaps)
-		return -ENOMEM;
-
-
-	/* Allocate a dummy carveout heap */
-	carveout_ptr = alloc_pages_exact(
-				dummy_heaps[ION_HEAP_TYPE_CARVEOUT].size,
-				GFP_KERNEL);
-	if (carveout_ptr)
-		dummy_heaps[ION_HEAP_TYPE_CARVEOUT].base =
-						virt_to_phys(carveout_ptr);
-	else
-		pr_err("ion_dummy: Could not allocate carveout\n");
-
-	/* Allocate a dummy chunk heap */
-	chunk_ptr = alloc_pages_exact(
-				dummy_heaps[ION_HEAP_TYPE_CHUNK].size,
-				GFP_KERNEL);
-	if (chunk_ptr)
-		dummy_heaps[ION_HEAP_TYPE_CHUNK].base = virt_to_phys(chunk_ptr);
-	else
-		pr_err("ion_dummy: Could not allocate chunk\n");
-
-	for (i = 0; i < dummy_ion_pdata.nr; i++) {
-		struct ion_platform_heap *heap_data = &dummy_ion_pdata.heaps[i];
-
-		if (heap_data->type == ION_HEAP_TYPE_CARVEOUT &&
-		    !heap_data->base)
-			continue;
-
-		if (heap_data->type == ION_HEAP_TYPE_CHUNK && !heap_data->base)
-			continue;
-
-		heaps[i] = ion_heap_create(heap_data);
-		if (IS_ERR_OR_NULL(heaps[i])) {
-			err = PTR_ERR(heaps[i]);
-			goto err;
-		}
-		ion_device_add_heap(idev, heaps[i]);
-	}
-	return 0;
-err:
-	for (i = 0; i < dummy_ion_pdata.nr; ++i)
-		ion_heap_destroy(heaps[i]);
-	kfree(heaps);
-
-	if (carveout_ptr) {
-		free_pages_exact(carveout_ptr,
-				 dummy_heaps[ION_HEAP_TYPE_CARVEOUT].size);
-		carveout_ptr = NULL;
-	}
-	if (chunk_ptr) {
-		free_pages_exact(chunk_ptr,
-				 dummy_heaps[ION_HEAP_TYPE_CHUNK].size);
-		chunk_ptr = NULL;
-	}
-	return err;
-}
-device_initcall(ion_dummy_init);
-
-static void __exit ion_dummy_exit(void)
-{
-	int i;
-
-	ion_device_destroy(idev);
-
-	for (i = 0; i < dummy_ion_pdata.nr; i++)
-		ion_heap_destroy(heaps[i]);
-	kfree(heaps);
-
-	if (carveout_ptr) {
-		free_pages_exact(carveout_ptr,
-				 dummy_heaps[ION_HEAP_TYPE_CARVEOUT].size);
-		carveout_ptr = NULL;
-	}
-	if (chunk_ptr) {
-		free_pages_exact(chunk_ptr,
-				 dummy_heaps[ION_HEAP_TYPE_CHUNK].size);
-		chunk_ptr = NULL;
-	}
-}
-__exitcall(ion_dummy_exit);
diff --git a/drivers/staging/android/ion/ion_heap.c b/drivers/staging/android/ion/ion_heap.c
index c69d0bd53693..91faa7f035b9 100644
--- a/drivers/staging/android/ion/ion_heap.c
+++ b/drivers/staging/android/ion/ion_heap.c
@@ -24,7 +24,6 @@
 #include <linux/scatterlist.h>
 #include <linux/vmalloc.h>
 #include "ion.h"
-#include "ion_priv.h"
 
 void *ion_heap_map_kernel(struct ion_heap *heap,
 			  struct ion_buffer *buffer)
@@ -315,70 +314,3 @@ void ion_heap_init_shrinker(struct ion_heap *heap)
 	heap->shrinker.batch = 0;
 	register_shrinker(&heap->shrinker);
 }
-
-struct ion_heap *ion_heap_create(struct ion_platform_heap *heap_data)
-{
-	struct ion_heap *heap = NULL;
-
-	switch (heap_data->type) {
-	case ION_HEAP_TYPE_SYSTEM_CONTIG:
-		heap = ion_system_contig_heap_create(heap_data);
-		break;
-	case ION_HEAP_TYPE_SYSTEM:
-		heap = ion_system_heap_create(heap_data);
-		break;
-	case ION_HEAP_TYPE_CARVEOUT:
-		heap = ion_carveout_heap_create(heap_data);
-		break;
-	case ION_HEAP_TYPE_CHUNK:
-		heap = ion_chunk_heap_create(heap_data);
-		break;
-	case ION_HEAP_TYPE_DMA:
-		heap = ion_cma_heap_create(heap_data);
-		break;
-	default:
-		pr_err("%s: Invalid heap type %d\n", __func__,
-		       heap_data->type);
-		return ERR_PTR(-EINVAL);
-	}
-
-	if (IS_ERR_OR_NULL(heap)) {
-		pr_err("%s: error creating heap %s type %d base %lu size %zu\n",
-		       __func__, heap_data->name, heap_data->type,
-		       heap_data->base, heap_data->size);
-		return ERR_PTR(-EINVAL);
-	}
-
-	heap->name = heap_data->name;
-	heap->id = heap_data->id;
-	return heap;
-}
-EXPORT_SYMBOL(ion_heap_create);
-
-void ion_heap_destroy(struct ion_heap *heap)
-{
-	if (!heap)
-		return;
-
-	switch (heap->type) {
-	case ION_HEAP_TYPE_SYSTEM_CONTIG:
-		ion_system_contig_heap_destroy(heap);
-		break;
-	case ION_HEAP_TYPE_SYSTEM:
-		ion_system_heap_destroy(heap);
-		break;
-	case ION_HEAP_TYPE_CARVEOUT:
-		ion_carveout_heap_destroy(heap);
-		break;
-	case ION_HEAP_TYPE_CHUNK:
-		ion_chunk_heap_destroy(heap);
-		break;
-	case ION_HEAP_TYPE_DMA:
-		ion_cma_heap_destroy(heap);
-		break;
-	default:
-		pr_err("%s: Invalid heap type %d\n", __func__,
-		       heap->type);
-	}
-}
-EXPORT_SYMBOL(ion_heap_destroy);
diff --git a/drivers/staging/android/ion/ion_of.c b/drivers/staging/android/ion/ion_of.c
deleted file mode 100644
index 7791c705ea31..000000000000
--- a/drivers/staging/android/ion/ion_of.c
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Based on work from:
- *   Andrew Andrianov <andrew@ncrmnt.org>
- *   Google
- *   The Linux Foundation
- *
- * 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.
- */
-
-#include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/of.h>
-#include <linux/of_platform.h>
-#include <linux/of_address.h>
-#include <linux/clk.h>
-#include <linux/dma-mapping.h>
-#include <linux/cma.h>
-#include <linux/dma-contiguous.h>
-#include <linux/io.h>
-#include <linux/of_reserved_mem.h>
-#include "ion.h"
-#include "ion_priv.h"
-#include "ion_of.h"
-
-static int ion_parse_dt_heap_common(struct device_node *heap_node,
-				    struct ion_platform_heap *heap,
-				    struct ion_of_heap *compatible)
-{
-	int i;
-
-	for (i = 0; compatible[i].name; i++) {
-		if (of_device_is_compatible(heap_node, compatible[i].compat))
-			break;
-	}
-
-	if (!compatible[i].name)
-		return -ENODEV;
-
-	heap->id = compatible[i].heap_id;
-	heap->type = compatible[i].type;
-	heap->name = compatible[i].name;
-	heap->align = compatible[i].align;
-
-	/* Some kind of callback function pointer? */
-
-	pr_info("%s: id %d type %d name %s align %lx\n", __func__,
-		heap->id, heap->type, heap->name, heap->align);
-	return 0;
-}
-
-static int ion_setup_heap_common(struct platform_device *parent,
-				 struct device_node *heap_node,
-				 struct ion_platform_heap *heap)
-{
-	int ret = 0;
-
-	switch (heap->type) {
-	case ION_HEAP_TYPE_CARVEOUT:
-	case ION_HEAP_TYPE_CHUNK:
-		if (heap->base && heap->size)
-			return 0;
-
-		ret = of_reserved_mem_device_init(heap->priv);
-		break;
-	default:
-		break;
-	}
-
-	return ret;
-}
-
-struct ion_platform_data *ion_parse_dt(struct platform_device *pdev,
-				       struct ion_of_heap *compatible)
-{
-	int num_heaps, ret;
-	const struct device_node *dt_node = pdev->dev.of_node;
-	struct device_node *node;
-	struct ion_platform_heap *heaps;
-	struct ion_platform_data *data;
-	int i = 0;
-
-	num_heaps = of_get_available_child_count(dt_node);
-
-	if (!num_heaps)
-		return ERR_PTR(-EINVAL);
-
-	heaps = devm_kzalloc(&pdev->dev,
-			     sizeof(struct ion_platform_heap) * num_heaps,
-			     GFP_KERNEL);
-	if (!heaps)
-		return ERR_PTR(-ENOMEM);
-
-	data = devm_kzalloc(&pdev->dev, sizeof(struct ion_platform_data),
-			    GFP_KERNEL);
-	if (!data)
-		return ERR_PTR(-ENOMEM);
-
-	for_each_available_child_of_node(dt_node, node) {
-		struct platform_device *heap_pdev;
-
-		ret = ion_parse_dt_heap_common(node, &heaps[i], compatible);
-		if (ret)
-			return ERR_PTR(ret);
-
-		heap_pdev = of_platform_device_create(node, heaps[i].name,
-						      &pdev->dev);
-		if (!heap_pdev)
-			return ERR_PTR(-ENOMEM);
-		heap_pdev->dev.platform_data = &heaps[i];
-
-		heaps[i].priv = &heap_pdev->dev;
-
-		ret = ion_setup_heap_common(pdev, node, &heaps[i]);
-		if (ret)
-			goto out_err;
-		i++;
-	}
-
-	data->heaps = heaps;
-	data->nr = num_heaps;
-	return data;
-
-out_err:
-	for ( ; i >= 0; i--)
-		if (heaps[i].priv)
-			of_device_unregister(to_platform_device(heaps[i].priv));
-
-	return ERR_PTR(ret);
-}
-
-void ion_destroy_platform_data(struct ion_platform_data *data)
-{
-	int i;
-
-	for (i = 0; i < data->nr; i++)
-		if (data->heaps[i].priv)
-			of_device_unregister(to_platform_device(
-				data->heaps[i].priv));
-}
-
-#ifdef CONFIG_OF_RESERVED_MEM
-#include <linux/of.h>
-#include <linux/of_fdt.h>
-#include <linux/of_reserved_mem.h>
-
-static int rmem_ion_device_init(struct reserved_mem *rmem, struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct ion_platform_heap *heap = pdev->dev.platform_data;
-
-	heap->base = rmem->base;
-	heap->base = rmem->size;
-	pr_debug("%s: heap %s base %pa size %pa dev %p\n", __func__,
-		 heap->name, &rmem->base, &rmem->size, dev);
-	return 0;
-}
-
-static void rmem_ion_device_release(struct reserved_mem *rmem,
-				    struct device *dev)
-{
-}
-
-static const struct reserved_mem_ops rmem_dma_ops = {
-	.device_init	= rmem_ion_device_init,
-	.device_release	= rmem_ion_device_release,
-};
-
-static int __init rmem_ion_setup(struct reserved_mem *rmem)
-{
-	phys_addr_t size = rmem->size;
-
-	size = size / 1024;
-
-	pr_info("Ion memory setup at %pa size %pa MiB\n",
-		&rmem->base, &size);
-	rmem->ops = &rmem_dma_ops;
-	return 0;
-}
-
-RESERVEDMEM_OF_DECLARE(ion, "ion-region", rmem_ion_setup);
-#endif
diff --git a/drivers/staging/android/ion/ion_of.h b/drivers/staging/android/ion/ion_of.h
deleted file mode 100644
index 8241a1770f0a..000000000000
--- a/drivers/staging/android/ion/ion_of.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * Based on work from:
- *   Andrew Andrianov <andrew@ncrmnt.org>
- *   Google
- *   The Linux Foundation
- *
- * 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.
- */
-
-#ifndef _ION_OF_H
-#define _ION_OF_H
-
-struct ion_of_heap {
-	const char *compat;
-	int heap_id;
-	int type;
-	const char *name;
-	int align;
-};
-
-#define PLATFORM_HEAP(_compat, _id, _type, _name) \
-{ \
-	.compat = _compat, \
-	.heap_id = _id, \
-	.type = _type, \
-	.name = _name, \
-	.align = PAGE_SIZE, \
-}
-
-struct ion_platform_data *ion_parse_dt(struct platform_device *pdev,
-					struct ion_of_heap *compatible);
-
-void ion_destroy_platform_data(struct ion_platform_data *data);
-
-#endif
diff --git a/drivers/staging/android/ion/ion_page_pool.c b/drivers/staging/android/ion/ion_page_pool.c
index aea89c1ec345..817849df9de3 100644
--- a/drivers/staging/android/ion/ion_page_pool.c
+++ b/drivers/staging/android/ion/ion_page_pool.c
@@ -22,7 +22,8 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/swap.h>
-#include "ion_priv.h"
+
+#include "ion.h"
 
 static void *ion_page_pool_alloc_pages(struct ion_page_pool *pool)
 {
@@ -30,9 +31,6 @@ static void *ion_page_pool_alloc_pages(struct ion_page_pool *pool)
 
 	if (!page)
 		return NULL;
-	if (!pool->cached)
-		ion_pages_sync_for_device(NULL, page, PAGE_SIZE << pool->order,
-					  DMA_BIDIRECTIONAL);
 	return page;
 }
 
diff --git a/drivers/staging/android/ion/ion_priv.h b/drivers/staging/android/ion/ion_priv.h
deleted file mode 100644
index 5b3059c78431..000000000000
--- a/drivers/staging/android/ion/ion_priv.h
+++ /dev/null
@@ -1,473 +0,0 @@
-/*
- * drivers/staging/android/ion/ion_priv.h
- *
- * Copyright (C) 2011 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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 _ION_PRIV_H
-#define _ION_PRIV_H
-
-#include <linux/device.h>
-#include <linux/dma-direction.h>
-#include <linux/kref.h>
-#include <linux/mm_types.h>
-#include <linux/mutex.h>
-#include <linux/rbtree.h>
-#include <linux/sched.h>
-#include <linux/shrinker.h>
-#include <linux/types.h>
-#include <linux/miscdevice.h>
-
-#include "ion.h"
-
-/**
- * struct ion_buffer - metadata for a particular buffer
- * @ref:		reference count
- * @node:		node in the ion_device buffers tree
- * @dev:		back pointer to the ion_device
- * @heap:		back pointer to the heap the buffer came from
- * @flags:		buffer specific flags
- * @private_flags:	internal buffer specific flags
- * @size:		size of the buffer
- * @priv_virt:		private data to the buffer representable as
- *			a void *
- * @lock:		protects the buffers cnt fields
- * @kmap_cnt:		number of times the buffer is mapped to the kernel
- * @vaddr:		the kernel mapping if kmap_cnt is not zero
- * @dmap_cnt:		number of times the buffer is mapped for dma
- * @sg_table:		the sg table for the buffer if dmap_cnt is not zero
- * @pages:		flat array of pages in the buffer -- used by fault
- *			handler and only valid for buffers that are faulted in
- * @vmas:		list of vma's mapping this buffer
- * @handle_count:	count of handles referencing this buffer
- * @task_comm:		taskcomm of last client to reference this buffer in a
- *			handle, used for debugging
- * @pid:		pid of last client to reference this buffer in a
- *			handle, used for debugging
- */
-struct ion_buffer {
-	struct kref ref;
-	union {
-		struct rb_node node;
-		struct list_head list;
-	};
-	struct ion_device *dev;
-	struct ion_heap *heap;
-	unsigned long flags;
-	unsigned long private_flags;
-	size_t size;
-	void *priv_virt;
-	struct mutex lock;
-	int kmap_cnt;
-	void *vaddr;
-	int dmap_cnt;
-	struct sg_table *sg_table;
-	struct page **pages;
-	struct list_head vmas;
-	/* used to track orphaned buffers */
-	int handle_count;
-	char task_comm[TASK_COMM_LEN];
-	pid_t pid;
-};
-void ion_buffer_destroy(struct ion_buffer *buffer);
-
-/**
- * struct ion_device - the metadata of the ion device node
- * @dev:		the actual misc device
- * @buffers:		an rb tree of all the existing buffers
- * @buffer_lock:	lock protecting the tree of buffers
- * @lock:		rwsem protecting the tree of heaps and clients
- * @heaps:		list of all the heaps in the system
- * @user_clients:	list of all the clients created from userspace
- */
-struct ion_device {
-	struct miscdevice dev;
-	struct rb_root buffers;
-	struct mutex buffer_lock;
-	struct rw_semaphore lock;
-	struct plist_head heaps;
-	long (*custom_ioctl)(struct ion_client *client, unsigned int cmd,
-			     unsigned long arg);
-	struct rb_root clients;
-	struct dentry *debug_root;
-	struct dentry *heaps_debug_root;
-	struct dentry *clients_debug_root;
-	int heap_cnt;
-};
-
-/**
- * struct ion_client - a process/hw block local address space
- * @node:		node in the tree of all clients
- * @dev:		backpointer to ion device
- * @handles:		an rb tree of all the handles in this client
- * @idr:		an idr space for allocating handle ids
- * @lock:		lock protecting the tree of handles
- * @name:		used for debugging
- * @display_name:	used for debugging (unique version of @name)
- * @display_serial:	used for debugging (to make display_name unique)
- * @task:		used for debugging
- *
- * A client represents a list of buffers this client may access.
- * The mutex stored here is used to protect both handles tree
- * as well as the handles themselves, and should be held while modifying either.
- */
-struct ion_client {
-	struct rb_node node;
-	struct ion_device *dev;
-	struct rb_root handles;
-	struct idr idr;
-	struct mutex lock;
-	const char *name;
-	char *display_name;
-	int display_serial;
-	struct task_struct *task;
-	pid_t pid;
-	struct dentry *debug_root;
-};
-
-/**
- * ion_handle - a client local reference to a buffer
- * @ref:		reference count
- * @client:		back pointer to the client the buffer resides in
- * @buffer:		pointer to the buffer
- * @node:		node in the client's handle rbtree
- * @kmap_cnt:		count of times this client has mapped to kernel
- * @id:			client-unique id allocated by client->idr
- *
- * Modifications to node, map_cnt or mapping should be protected by the
- * lock in the client.  Other fields are never changed after initialization.
- */
-struct ion_handle {
-	struct kref ref;
-	struct ion_client *client;
-	struct ion_buffer *buffer;
-	struct rb_node node;
-	unsigned int kmap_cnt;
-	int id;
-};
-
-/**
- * struct ion_heap_ops - ops to operate on a given heap
- * @allocate:		allocate memory
- * @free:		free memory
- * @map_kernel		map memory to the kernel
- * @unmap_kernel	unmap memory to the kernel
- * @map_user		map memory to userspace
- *
- * allocate, phys, and map_user return 0 on success, -errno on error.
- * map_dma and map_kernel return pointer on success, ERR_PTR on
- * error. @free will be called with ION_PRIV_FLAG_SHRINKER_FREE set in
- * the buffer's private_flags when called from a shrinker. In that
- * case, the pages being free'd must be truly free'd back to the
- * system, not put in a page pool or otherwise cached.
- */
-struct ion_heap_ops {
-	int (*allocate)(struct ion_heap *heap,
-			struct ion_buffer *buffer, unsigned long len,
-			unsigned long align, unsigned long flags);
-	void (*free)(struct ion_buffer *buffer);
-	void * (*map_kernel)(struct ion_heap *heap, struct ion_buffer *buffer);
-	void (*unmap_kernel)(struct ion_heap *heap, struct ion_buffer *buffer);
-	int (*map_user)(struct ion_heap *mapper, struct ion_buffer *buffer,
-			struct vm_area_struct *vma);
-	int (*shrink)(struct ion_heap *heap, gfp_t gfp_mask, int nr_to_scan);
-};
-
-/**
- * heap flags - flags between the heaps and core ion code
- */
-#define ION_HEAP_FLAG_DEFER_FREE (1 << 0)
-
-/**
- * private flags - flags internal to ion
- */
-/*
- * Buffer is being freed from a shrinker function. Skip any possible
- * heap-specific caching mechanism (e.g. page pools). Guarantees that
- * any buffer storage that came from the system allocator will be
- * returned to the system allocator.
- */
-#define ION_PRIV_FLAG_SHRINKER_FREE (1 << 0)
-
-/**
- * struct ion_heap - represents a heap in the system
- * @node:		rb node to put the heap on the device's tree of heaps
- * @dev:		back pointer to the ion_device
- * @type:		type of heap
- * @ops:		ops struct as above
- * @flags:		flags
- * @id:			id of heap, also indicates priority of this heap when
- *			allocating.  These are specified by platform data and
- *			MUST be unique
- * @name:		used for debugging
- * @shrinker:		a shrinker for the heap
- * @free_list:		free list head if deferred free is used
- * @free_list_size	size of the deferred free list in bytes
- * @lock:		protects the free list
- * @waitqueue:		queue to wait on from deferred free thread
- * @task:		task struct of deferred free thread
- * @debug_show:		called when heap debug file is read to add any
- *			heap specific debug info to output
- *
- * Represents a pool of memory from which buffers can be made.  In some
- * systems the only heap is regular system memory allocated via vmalloc.
- * On others, some blocks might require large physically contiguous buffers
- * that are allocated from a specially reserved heap.
- */
-struct ion_heap {
-	struct plist_node node;
-	struct ion_device *dev;
-	enum ion_heap_type type;
-	struct ion_heap_ops *ops;
-	unsigned long flags;
-	unsigned int id;
-	const char *name;
-	struct shrinker shrinker;
-	struct list_head free_list;
-	size_t free_list_size;
-	spinlock_t free_lock;
-	wait_queue_head_t waitqueue;
-	struct task_struct *task;
-
-	int (*debug_show)(struct ion_heap *heap, struct seq_file *, void *);
-};
-
-/**
- * ion_buffer_cached - this ion buffer is cached
- * @buffer:		buffer
- *
- * indicates whether this ion buffer is cached
- */
-bool ion_buffer_cached(struct ion_buffer *buffer);
-
-/**
- * ion_buffer_fault_user_mappings - fault in user mappings of this buffer
- * @buffer:		buffer
- *
- * indicates whether userspace mappings of this buffer will be faulted
- * in, this can affect how buffers are allocated from the heap.
- */
-bool ion_buffer_fault_user_mappings(struct ion_buffer *buffer);
-
-/**
- * ion_device_create - allocates and returns an ion device
- * @custom_ioctl:	arch specific ioctl function if applicable
- *
- * returns a valid device or -PTR_ERR
- */
-struct ion_device *ion_device_create(long (*custom_ioctl)
-				     (struct ion_client *client,
-				      unsigned int cmd,
-				      unsigned long arg));
-
-/**
- * ion_device_destroy - free and device and it's resource
- * @dev:		the device
- */
-void ion_device_destroy(struct ion_device *dev);
-
-/**
- * ion_device_add_heap - adds a heap to the ion device
- * @dev:		the device
- * @heap:		the heap to add
- */
-void ion_device_add_heap(struct ion_device *dev, struct ion_heap *heap);
-
-/**
- * some helpers for common operations on buffers using the sg_table
- * and vaddr fields
- */
-void *ion_heap_map_kernel(struct ion_heap *heap, struct ion_buffer *buffer);
-void ion_heap_unmap_kernel(struct ion_heap *heap, struct ion_buffer *buffer);
-int ion_heap_map_user(struct ion_heap *heap, struct ion_buffer *buffer,
-		      struct vm_area_struct *vma);
-int ion_heap_buffer_zero(struct ion_buffer *buffer);
-int ion_heap_pages_zero(struct page *page, size_t size, pgprot_t pgprot);
-
-/**
- * ion_heap_init_shrinker
- * @heap:		the heap
- *
- * If a heap sets the ION_HEAP_FLAG_DEFER_FREE flag or defines the shrink op
- * this function will be called to setup a shrinker to shrink the freelists
- * and call the heap's shrink op.
- */
-void ion_heap_init_shrinker(struct ion_heap *heap);
-
-/**
- * ion_heap_init_deferred_free -- initialize deferred free functionality
- * @heap:		the heap
- *
- * If a heap sets the ION_HEAP_FLAG_DEFER_FREE flag this function will
- * be called to setup deferred frees. Calls to free the buffer will
- * return immediately and the actual free will occur some time later
- */
-int ion_heap_init_deferred_free(struct ion_heap *heap);
-
-/**
- * ion_heap_freelist_add - add a buffer to the deferred free list
- * @heap:		the heap
- * @buffer:		the buffer
- *
- * Adds an item to the deferred freelist.
- */
-void ion_heap_freelist_add(struct ion_heap *heap, struct ion_buffer *buffer);
-
-/**
- * ion_heap_freelist_drain - drain the deferred free list
- * @heap:		the heap
- * @size:		amount of memory to drain in bytes
- *
- * Drains the indicated amount of memory from the deferred freelist immediately.
- * Returns the total amount freed.  The total freed may be higher depending
- * on the size of the items in the list, or lower if there is insufficient
- * total memory on the freelist.
- */
-size_t ion_heap_freelist_drain(struct ion_heap *heap, size_t size);
-
-/**
- * ion_heap_freelist_shrink - drain the deferred free
- *				list, skipping any heap-specific
- *				pooling or caching mechanisms
- *
- * @heap:		the heap
- * @size:		amount of memory to drain in bytes
- *
- * Drains the indicated amount of memory from the deferred freelist immediately.
- * Returns the total amount freed.  The total freed may be higher depending
- * on the size of the items in the list, or lower if there is insufficient
- * total memory on the freelist.
- *
- * Unlike with @ion_heap_freelist_drain, don't put any pages back into
- * page pools or otherwise cache the pages. Everything must be
- * genuinely free'd back to the system. If you're free'ing from a
- * shrinker you probably want to use this. Note that this relies on
- * the heap.ops.free callback honoring the ION_PRIV_FLAG_SHRINKER_FREE
- * flag.
- */
-size_t ion_heap_freelist_shrink(struct ion_heap *heap,
-					size_t size);
-
-/**
- * ion_heap_freelist_size - returns the size of the freelist in bytes
- * @heap:		the heap
- */
-size_t ion_heap_freelist_size(struct ion_heap *heap);
-
-
-/**
- * functions for creating and destroying the built in ion heaps.
- * architectures can add their own custom architecture specific
- * heaps as appropriate.
- */
-
-struct ion_heap *ion_heap_create(struct ion_platform_heap *heap_data);
-void ion_heap_destroy(struct ion_heap *heap);
-struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused);
-void ion_system_heap_destroy(struct ion_heap *heap);
-
-struct ion_heap *ion_system_contig_heap_create(struct ion_platform_heap *heap);
-void ion_system_contig_heap_destroy(struct ion_heap *heap);
-
-struct ion_heap *ion_carveout_heap_create(struct ion_platform_heap *heap_data);
-void ion_carveout_heap_destroy(struct ion_heap *heap);
-
-struct ion_heap *ion_chunk_heap_create(struct ion_platform_heap *heap_data);
-void ion_chunk_heap_destroy(struct ion_heap *heap);
-struct ion_heap *ion_cma_heap_create(struct ion_platform_heap *data);
-void ion_cma_heap_destroy(struct ion_heap *heap);
-
-/**
- * functions for creating and destroying a heap pool -- allows you
- * to keep a pool of pre allocated memory to use from your heap.  Keeping
- * a pool of memory that is ready for dma, ie any cached mapping have been
- * invalidated from the cache, provides a significant performance benefit on
- * many systems
- */
-
-/**
- * struct ion_page_pool - pagepool struct
- * @high_count:		number of highmem items in the pool
- * @low_count:		number of lowmem items in the pool
- * @high_items:		list of highmem items
- * @low_items:		list of lowmem items
- * @mutex:		lock protecting this struct and especially the count
- *			item list
- * @gfp_mask:		gfp_mask to use from alloc
- * @order:		order of pages in the pool
- * @list:		plist node for list of pools
- * @cached:		it's cached pool or not
- *
- * Allows you to keep a pool of pre allocated pages to use from your heap.
- * Keeping a pool of pages that is ready for dma, ie any cached mapping have
- * been invalidated from the cache, provides a significant performance benefit
- * on many systems
- */
-struct ion_page_pool {
-	int high_count;
-	int low_count;
-	bool cached;
-	struct list_head high_items;
-	struct list_head low_items;
-	struct mutex mutex;
-	gfp_t gfp_mask;
-	unsigned int order;
-	struct plist_node list;
-};
-
-struct ion_page_pool *ion_page_pool_create(gfp_t gfp_mask, unsigned int order,
-					   bool cached);
-void ion_page_pool_destroy(struct ion_page_pool *pool);
-struct page *ion_page_pool_alloc(struct ion_page_pool *pool);
-void ion_page_pool_free(struct ion_page_pool *pool, struct page *page);
-
-/** ion_page_pool_shrink - shrinks the size of the memory cached in the pool
- * @pool:		the pool
- * @gfp_mask:		the memory type to reclaim
- * @nr_to_scan:		number of items to shrink in pages
- *
- * returns the number of items freed in pages
- */
-int ion_page_pool_shrink(struct ion_page_pool *pool, gfp_t gfp_mask,
-			  int nr_to_scan);
-
-/**
- * ion_pages_sync_for_device - cache flush pages for use with the specified
- *                             device
- * @dev:		the device the pages will be used with
- * @page:		the first page to be flushed
- * @size:		size in bytes of region to be flushed
- * @dir:		direction of dma transfer
- */
-void ion_pages_sync_for_device(struct device *dev, struct page *page,
-		size_t size, enum dma_data_direction dir);
-
-long ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
-
-int ion_sync_for_device(struct ion_client *client, int fd);
-
-struct ion_handle *ion_handle_get_by_id_nolock(struct ion_client *client,
-						int id);
-
-void ion_free_nolock(struct ion_client *client, struct ion_handle *handle);
-
-int ion_handle_put_nolock(struct ion_handle *handle);
-
-struct ion_handle *ion_handle_get_by_id(struct ion_client *client,
-						int id);
-
-int ion_handle_put(struct ion_handle *handle);
-
-int ion_query_heaps(struct ion_client *client, struct ion_heap_query *query);
-
-#endif /* _ION_PRIV_H */
diff --git a/drivers/staging/android/ion/ion_system_heap.c b/drivers/staging/android/ion/ion_system_heap.c
index 3ebbb75746e8..c50f2d9fc58c 100644
--- a/drivers/staging/android/ion/ion_system_heap.c
+++ b/drivers/staging/android/ion/ion_system_heap.c
@@ -24,7 +24,6 @@
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
 #include "ion.h"
-#include "ion_priv.h"
 
 #define NUM_ORDERS ARRAY_SIZE(orders)
 
@@ -75,9 +74,6 @@ static struct page *alloc_buffer_page(struct ion_system_heap *heap,
 
 	page = ion_page_pool_alloc(pool);
 
-	if (cached)
-		ion_pages_sync_for_device(NULL, page, PAGE_SIZE << order,
-					  DMA_BIDIRECTIONAL);
 	return page;
 }
 
@@ -129,7 +125,7 @@ static struct page *alloc_largest_available(struct ion_system_heap *heap,
 
 static int ion_system_heap_allocate(struct ion_heap *heap,
 				    struct ion_buffer *buffer,
-				    unsigned long size, unsigned long align,
+				    unsigned long size,
 				    unsigned long flags)
 {
 	struct ion_system_heap *sys_heap = container_of(heap,
@@ -143,9 +139,6 @@ static int ion_system_heap_allocate(struct ion_heap *heap,
 	unsigned long size_remaining = PAGE_ALIGN(size);
 	unsigned int max_order = orders[0];
 
-	if (align > PAGE_SIZE)
-		return -EINVAL;
-
 	if (size / PAGE_SIZE > totalram_pages / 2)
 		return -ENOMEM;
 
@@ -327,7 +320,7 @@ err_create_pool:
 	return -ENOMEM;
 }
 
-struct ion_heap *ion_system_heap_create(struct ion_platform_heap *unused)
+static struct ion_heap *__ion_system_heap_create(void)
 {
 	struct ion_system_heap *heap;
 
@@ -355,24 +348,23 @@ free_heap:
 	return ERR_PTR(-ENOMEM);
 }
 
-void ion_system_heap_destroy(struct ion_heap *heap)
+static int ion_system_heap_create(void)
 {
-	struct ion_system_heap *sys_heap = container_of(heap,
-							struct ion_system_heap,
-							heap);
-	int i;
+	struct ion_heap *heap;
 
-	for (i = 0; i < NUM_ORDERS; i++) {
-		ion_page_pool_destroy(sys_heap->uncached_pools[i]);
-		ion_page_pool_destroy(sys_heap->cached_pools[i]);
-	}
-	kfree(sys_heap);
+	heap = __ion_system_heap_create();
+	if (IS_ERR(heap))
+		return PTR_ERR(heap);
+	heap->name = "ion_system_heap";
+
+	ion_device_add_heap(heap);
+	return 0;
 }
+device_initcall(ion_system_heap_create);
 
 static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 					   struct ion_buffer *buffer,
 					   unsigned long len,
-					   unsigned long align,
 					   unsigned long flags)
 {
 	int order = get_order(len);
@@ -381,9 +373,6 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 	unsigned long i;
 	int ret;
 
-	if (align > (PAGE_SIZE << order))
-		return -EINVAL;
-
 	page = alloc_pages(low_order_gfp_flags, order);
 	if (!page)
 		return -ENOMEM;
@@ -408,8 +397,6 @@ static int ion_system_contig_heap_allocate(struct ion_heap *heap,
 
 	buffer->sg_table = table;
 
-	ion_pages_sync_for_device(NULL, page, len, DMA_BIDIRECTIONAL);
-
 	return 0;
 
 free_table:
@@ -442,7 +429,7 @@ static struct ion_heap_ops kmalloc_ops = {
 	.map_user = ion_heap_map_user,
 };
 
-struct ion_heap *ion_system_contig_heap_create(struct ion_platform_heap *unused)
+static struct ion_heap *__ion_system_contig_heap_create(void)
 {
 	struct ion_heap *heap;
 
@@ -451,10 +438,20 @@ struct ion_heap *ion_system_contig_heap_create(struct ion_platform_heap *unused)
 		return ERR_PTR(-ENOMEM);
 	heap->ops = &kmalloc_ops;
 	heap->type = ION_HEAP_TYPE_SYSTEM_CONTIG;
+	heap->name = "ion_system_contig_heap";
 	return heap;
 }
 
-void ion_system_contig_heap_destroy(struct ion_heap *heap)
+static int ion_system_contig_heap_create(void)
 {
-	kfree(heap);
+	struct ion_heap *heap;
+
+	heap = __ion_system_contig_heap_create();
+	if (IS_ERR(heap))
+		return PTR_ERR(heap);
+
+	ion_device_add_heap(heap);
+	return 0;
 }
+device_initcall(ion_system_contig_heap_create);
+
diff --git a/drivers/staging/android/ion/ion_test.c b/drivers/staging/android/ion/ion_test.c
deleted file mode 100644
index 5abf8320a96a..000000000000
--- a/drivers/staging/android/ion/ion_test.c
+++ /dev/null
@@ -1,305 +0,0 @@
-/*
- *
- * Copyright (C) 2013 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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.
- *
- */
-
-#define pr_fmt(fmt) "ion-test: " fmt
-
-#include <linux/dma-buf.h>
-#include <linux/dma-direction.h>
-#include <linux/fs.h>
-#include <linux/miscdevice.h>
-#include <linux/mm.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/uaccess.h>
-#include <linux/vmalloc.h>
-
-#include "ion.h"
-#include "../uapi/ion_test.h"
-
-#define u64_to_uptr(x) ((void __user *)(unsigned long)(x))
-
-struct ion_test_device {
-	struct miscdevice misc;
-};
-
-struct ion_test_data {
-	struct dma_buf *dma_buf;
-	struct device *dev;
-};
-
-static int ion_handle_test_dma(struct device *dev, struct dma_buf *dma_buf,
-			       void __user *ptr, size_t offset, size_t size,
-			       bool write)
-{
-	int ret = 0;
-	struct dma_buf_attachment *attach;
-	struct sg_table *table;
-	pgprot_t pgprot = pgprot_writecombine(PAGE_KERNEL);
-	enum dma_data_direction dir = write ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-	struct sg_page_iter sg_iter;
-	unsigned long offset_page;
-
-	attach = dma_buf_attach(dma_buf, dev);
-	if (IS_ERR(attach))
-		return PTR_ERR(attach);
-
-	table = dma_buf_map_attachment(attach, dir);
-	if (IS_ERR(table))
-		return PTR_ERR(table);
-
-	offset_page = offset >> PAGE_SHIFT;
-	offset %= PAGE_SIZE;
-
-	for_each_sg_page(table->sgl, &sg_iter, table->nents, offset_page) {
-		struct page *page = sg_page_iter_page(&sg_iter);
-		void *vaddr = vmap(&page, 1, VM_MAP, pgprot);
-		size_t to_copy = PAGE_SIZE - offset;
-
-		to_copy = min(to_copy, size);
-		if (!vaddr) {
-			ret = -ENOMEM;
-			goto err;
-		}
-
-		if (write)
-			ret = copy_from_user(vaddr + offset, ptr, to_copy);
-		else
-			ret = copy_to_user(ptr, vaddr + offset, to_copy);
-
-		vunmap(vaddr);
-		if (ret) {
-			ret = -EFAULT;
-			goto err;
-		}
-		size -= to_copy;
-		if (!size)
-			break;
-		ptr += to_copy;
-		offset = 0;
-	}
-
-err:
-	dma_buf_unmap_attachment(attach, table, dir);
-	dma_buf_detach(dma_buf, attach);
-	return ret;
-}
-
-static int ion_handle_test_kernel(struct dma_buf *dma_buf, void __user *ptr,
-				  size_t offset, size_t size, bool write)
-{
-	int ret;
-	unsigned long page_offset = offset >> PAGE_SHIFT;
-	size_t copy_offset = offset % PAGE_SIZE;
-	size_t copy_size = size;
-	enum dma_data_direction dir = write ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
-
-	if (offset > dma_buf->size || size > dma_buf->size - offset)
-		return -EINVAL;
-
-	ret = dma_buf_begin_cpu_access(dma_buf, dir);
-	if (ret)
-		return ret;
-
-	while (copy_size > 0) {
-		size_t to_copy;
-		void *vaddr = dma_buf_kmap(dma_buf, page_offset);
-
-		if (!vaddr)
-			goto err;
-
-		to_copy = min_t(size_t, PAGE_SIZE - copy_offset, copy_size);
-
-		if (write)
-			ret = copy_from_user(vaddr + copy_offset, ptr, to_copy);
-		else
-			ret = copy_to_user(ptr, vaddr + copy_offset, to_copy);
-
-		dma_buf_kunmap(dma_buf, page_offset, vaddr);
-		if (ret) {
-			ret = -EFAULT;
-			goto err;
-		}
-
-		copy_size -= to_copy;
-		ptr += to_copy;
-		page_offset++;
-		copy_offset = 0;
-	}
-err:
-	dma_buf_end_cpu_access(dma_buf, dir);
-	return ret;
-}
-
-static long ion_test_ioctl(struct file *filp, unsigned int cmd,
-			   unsigned long arg)
-{
-	struct ion_test_data *test_data = filp->private_data;
-	int ret = 0;
-
-	union {
-		struct ion_test_rw_data test_rw;
-	} data;
-
-	if (_IOC_SIZE(cmd) > sizeof(data))
-		return -EINVAL;
-
-	if (_IOC_DIR(cmd) & _IOC_WRITE)
-		if (copy_from_user(&data, (void __user *)arg, _IOC_SIZE(cmd)))
-			return -EFAULT;
-
-	switch (cmd) {
-	case ION_IOC_TEST_SET_FD:
-	{
-		struct dma_buf *dma_buf = NULL;
-		int fd = arg;
-
-		if (fd >= 0) {
-			dma_buf = dma_buf_get((int)arg);
-			if (IS_ERR(dma_buf))
-				return PTR_ERR(dma_buf);
-		}
-		if (test_data->dma_buf)
-			dma_buf_put(test_data->dma_buf);
-		test_data->dma_buf = dma_buf;
-		break;
-	}
-	case ION_IOC_TEST_DMA_MAPPING:
-	{
-		ret = ion_handle_test_dma(test_data->dev, test_data->dma_buf,
-					  u64_to_uptr(data.test_rw.ptr),
-					  data.test_rw.offset,
-					  data.test_rw.size,
-					  data.test_rw.write);
-		break;
-	}
-	case ION_IOC_TEST_KERNEL_MAPPING:
-	{
-		ret = ion_handle_test_kernel(test_data->dma_buf,
-					     u64_to_uptr(data.test_rw.ptr),
-					     data.test_rw.offset,
-					     data.test_rw.size,
-					     data.test_rw.write);
-		break;
-	}
-	default:
-		return -ENOTTY;
-	}
-
-	if (_IOC_DIR(cmd) & _IOC_READ) {
-		if (copy_to_user((void __user *)arg, &data, sizeof(data)))
-			return -EFAULT;
-	}
-	return ret;
-}
-
-static int ion_test_open(struct inode *inode, struct file *file)
-{
-	struct ion_test_data *data;
-	struct miscdevice *miscdev = file->private_data;
-
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	data->dev = miscdev->parent;
-
-	file->private_data = data;
-
-	return 0;
-}
-
-static int ion_test_release(struct inode *inode, struct file *file)
-{
-	struct ion_test_data *data = file->private_data;
-
-	kfree(data);
-
-	return 0;
-}
-
-static const struct file_operations ion_test_fops = {
-	.owner = THIS_MODULE,
-	.unlocked_ioctl = ion_test_ioctl,
-	.compat_ioctl = ion_test_ioctl,
-	.open = ion_test_open,
-	.release = ion_test_release,
-};
-
-static int __init ion_test_probe(struct platform_device *pdev)
-{
-	int ret;
-	struct ion_test_device *testdev;
-
-	testdev = devm_kzalloc(&pdev->dev, sizeof(struct ion_test_device),
-			       GFP_KERNEL);
-	if (!testdev)
-		return -ENOMEM;
-
-	testdev->misc.minor = MISC_DYNAMIC_MINOR;
-	testdev->misc.name = "ion-test";
-	testdev->misc.fops = &ion_test_fops;
-	testdev->misc.parent = &pdev->dev;
-	ret = misc_register(&testdev->misc);
-	if (ret) {
-		pr_err("failed to register misc device.\n");
-		return ret;
-	}
-
-	platform_set_drvdata(pdev, testdev);
-
-	return 0;
-}
-
-static int ion_test_remove(struct platform_device *pdev)
-{
-	struct ion_test_device *testdev;
-
-	testdev = platform_get_drvdata(pdev);
-	if (!testdev)
-		return -ENODATA;
-
-	misc_deregister(&testdev->misc);
-	return 0;
-}
-
-static struct platform_device *ion_test_pdev;
-static struct platform_driver ion_test_platform_driver = {
-	.remove = ion_test_remove,
-	.driver = {
-		.name = "ion-test",
-	},
-};
-
-static int __init ion_test_init(void)
-{
-	ion_test_pdev = platform_device_register_simple("ion-test",
-							-1, NULL, 0);
-	if (IS_ERR(ion_test_pdev))
-		return PTR_ERR(ion_test_pdev);
-
-	return platform_driver_probe(&ion_test_platform_driver, ion_test_probe);
-}
-
-static void __exit ion_test_exit(void)
-{
-	platform_driver_unregister(&ion_test_platform_driver);
-	platform_device_unregister(ion_test_pdev);
-}
-
-module_init(ion_test_init);
-module_exit(ion_test_exit);
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/ion/tegra/Makefile b/drivers/staging/android/ion/tegra/Makefile
deleted file mode 100644
index 808f1f53f11a..000000000000
--- a/drivers/staging/android/ion/tegra/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-obj-$(CONFIG_ION_TEGRA) += tegra_ion.o
diff --git a/drivers/staging/android/ion/tegra/tegra_ion.c b/drivers/staging/android/ion/tegra/tegra_ion.c
deleted file mode 100644
index 49e55e5acead..000000000000
--- a/drivers/staging/android/ion/tegra/tegra_ion.c
+++ /dev/null
@@ -1,80 +0,0 @@
-/*
- * drivers/gpu/tegra/tegra_ion.c
- *
- * Copyright (C) 2011 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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/err.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include "../ion.h"
-#include "../ion_priv.h"
-
-static struct ion_device *idev;
-static int num_heaps;
-static struct ion_heap **heaps;
-
-static int tegra_ion_probe(struct platform_device *pdev)
-{
-	struct ion_platform_data *pdata = pdev->dev.platform_data;
-	int err;
-	int i;
-
-	num_heaps = pdata->nr;
-
-	heaps = devm_kcalloc(&pdev->dev, pdata->nr,
-			     sizeof(struct ion_heap *), GFP_KERNEL);
-
-	idev = ion_device_create(NULL);
-	if (IS_ERR(idev))
-		return PTR_ERR(idev);
-
-	/* create the heaps as specified in the board file */
-	for (i = 0; i < num_heaps; i++) {
-		struct ion_platform_heap *heap_data = &pdata->heaps[i];
-
-		heaps[i] = ion_heap_create(heap_data);
-		if (IS_ERR_OR_NULL(heaps[i])) {
-			err = PTR_ERR(heaps[i]);
-			goto err;
-		}
-		ion_device_add_heap(idev, heaps[i]);
-	}
-	platform_set_drvdata(pdev, idev);
-	return 0;
-err:
-	for (i = 0; i < num_heaps; ++i)
-		ion_heap_destroy(heaps[i]);
-	return err;
-}
-
-static int tegra_ion_remove(struct platform_device *pdev)
-{
-	struct ion_device *idev = platform_get_drvdata(pdev);
-	int i;
-
-	ion_device_destroy(idev);
-	for (i = 0; i < num_heaps; i++)
-		ion_heap_destroy(heaps[i]);
-	return 0;
-}
-
-static struct platform_driver ion_driver = {
-	.probe = tegra_ion_probe,
-	.remove = tegra_ion_remove,
-	.driver = { .name = "ion-tegra" }
-};
-
-module_platform_driver(ion_driver);
-
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
deleted file mode 100644
index 054660049395..000000000000
--- a/drivers/staging/android/lowmemorykiller.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/* drivers/misc/lowmemorykiller.c
- *
- * The lowmemorykiller driver lets user-space specify a set of memory thresholds
- * where processes with a range of oom_score_adj values will get killed. Specify
- * the minimum oom_score_adj values in
- * /sys/module/lowmemorykiller/parameters/adj and the number of free pages in
- * /sys/module/lowmemorykiller/parameters/minfree. Both files take a comma
- * separated list of numbers in ascending order.
- *
- * For example, write "0,8" to /sys/module/lowmemorykiller/parameters/adj and
- * "1024,4096" to /sys/module/lowmemorykiller/parameters/minfree to kill
- * processes with a oom_score_adj value of 8 or higher when the free memory
- * drops below 4096 pages and kill processes with a oom_score_adj value of 0 or
- * higher when the free memory drops below 1024 pages.
- *
- * The driver considers memory used for caches to be free, but if a large
- * percentage of the cached memory is locked this can be very inaccurate
- * and processes may not get killed until the normal oom killer is triggered.
- *
- * Copyright (C) 2007-2008 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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.
- *
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/init.h>
-#include <linux/moduleparam.h>
-#include <linux/kernel.h>
-#include <linux/mm.h>
-#include <linux/oom.h>
-#include <linux/sched/signal.h>
-#include <linux/swap.h>
-#include <linux/rcupdate.h>
-#include <linux/profile.h>
-#include <linux/notifier.h>
-
-static u32 lowmem_debug_level = 1;
-static short lowmem_adj[6] = {
-	0,
-	1,
-	6,
-	12,
-};
-
-static int lowmem_adj_size = 4;
-static int lowmem_minfree[6] = {
-	3 * 512,	/* 6MB */
-	2 * 1024,	/* 8MB */
-	4 * 1024,	/* 16MB */
-	16 * 1024,	/* 64MB */
-};
-
-static int lowmem_minfree_size = 4;
-
-static unsigned long lowmem_deathpending_timeout;
-
-#define lowmem_print(level, x...)			\
-	do {						\
-		if (lowmem_debug_level >= (level))	\
-			pr_info(x);			\
-	} while (0)
-
-static unsigned long lowmem_count(struct shrinker *s,
-				  struct shrink_control *sc)
-{
-	return global_node_page_state(NR_ACTIVE_ANON) +
-		global_node_page_state(NR_ACTIVE_FILE) +
-		global_node_page_state(NR_INACTIVE_ANON) +
-		global_node_page_state(NR_INACTIVE_FILE);
-}
-
-static unsigned long lowmem_scan(struct shrinker *s, struct shrink_control *sc)
-{
-	struct task_struct *tsk;
-	struct task_struct *selected = NULL;
-	unsigned long rem = 0;
-	int tasksize;
-	int i;
-	short min_score_adj = OOM_SCORE_ADJ_MAX + 1;
-	int minfree = 0;
-	int selected_tasksize = 0;
-	short selected_oom_score_adj;
-	int array_size = ARRAY_SIZE(lowmem_adj);
-	int other_free = global_page_state(NR_FREE_PAGES) - totalreserve_pages;
-	int other_file = global_node_page_state(NR_FILE_PAGES) -
-				global_node_page_state(NR_SHMEM) -
-				total_swapcache_pages();
-
-	if (lowmem_adj_size < array_size)
-		array_size = lowmem_adj_size;
-	if (lowmem_minfree_size < array_size)
-		array_size = lowmem_minfree_size;
-	for (i = 0; i < array_size; i++) {
-		minfree = lowmem_minfree[i];
-		if (other_free < minfree && other_file < minfree) {
-			min_score_adj = lowmem_adj[i];
-			break;
-		}
-	}
-
-	lowmem_print(3, "lowmem_scan %lu, %x, ofree %d %d, ma %hd\n",
-		     sc->nr_to_scan, sc->gfp_mask, other_free,
-		     other_file, min_score_adj);
-
-	if (min_score_adj == OOM_SCORE_ADJ_MAX + 1) {
-		lowmem_print(5, "lowmem_scan %lu, %x, return 0\n",
-			     sc->nr_to_scan, sc->gfp_mask);
-		return 0;
-	}
-
-	selected_oom_score_adj = min_score_adj;
-
-	rcu_read_lock();
-	for_each_process(tsk) {
-		struct task_struct *p;
-		short oom_score_adj;
-
-		if (tsk->flags & PF_KTHREAD)
-			continue;
-
-		p = find_lock_task_mm(tsk);
-		if (!p)
-			continue;
-
-		if (task_lmk_waiting(p) &&
-		    time_before_eq(jiffies, lowmem_deathpending_timeout)) {
-			task_unlock(p);
-			rcu_read_unlock();
-			return 0;
-		}
-		oom_score_adj = p->signal->oom_score_adj;
-		if (oom_score_adj < min_score_adj) {
-			task_unlock(p);
-			continue;
-		}
-		tasksize = get_mm_rss(p->mm);
-		task_unlock(p);
-		if (tasksize <= 0)
-			continue;
-		if (selected) {
-			if (oom_score_adj < selected_oom_score_adj)
-				continue;
-			if (oom_score_adj == selected_oom_score_adj &&
-			    tasksize <= selected_tasksize)
-				continue;
-		}
-		selected = p;
-		selected_tasksize = tasksize;
-		selected_oom_score_adj = oom_score_adj;
-		lowmem_print(2, "select '%s' (%d), adj %hd, size %d, to kill\n",
-			     p->comm, p->pid, oom_score_adj, tasksize);
-	}
-	if (selected) {
-		task_lock(selected);
-		send_sig(SIGKILL, selected, 0);
-		if (selected->mm)
-			task_set_lmk_waiting(selected);
-		task_unlock(selected);
-		lowmem_print(1, "Killing '%s' (%d), adj %hd,\n"
-				 "   to free %ldkB on behalf of '%s' (%d) because\n"
-				 "   cache %ldkB is below limit %ldkB for oom_score_adj %hd\n"
-				 "   Free memory is %ldkB above reserved\n",
-			     selected->comm, selected->pid,
-			     selected_oom_score_adj,
-			     selected_tasksize * (long)(PAGE_SIZE / 1024),
-			     current->comm, current->pid,
-			     other_file * (long)(PAGE_SIZE / 1024),
-			     minfree * (long)(PAGE_SIZE / 1024),
-			     min_score_adj,
-			     other_free * (long)(PAGE_SIZE / 1024));
-		lowmem_deathpending_timeout = jiffies + HZ;
-		rem += selected_tasksize;
-	}
-
-	lowmem_print(4, "lowmem_scan %lu, %x, return %lu\n",
-		     sc->nr_to_scan, sc->gfp_mask, rem);
-	rcu_read_unlock();
-	return rem;
-}
-
-static struct shrinker lowmem_shrinker = {
-	.scan_objects = lowmem_scan,
-	.count_objects = lowmem_count,
-	.seeks = DEFAULT_SEEKS * 16
-};
-
-static int __init lowmem_init(void)
-{
-	register_shrinker(&lowmem_shrinker);
-	return 0;
-}
-device_initcall(lowmem_init);
-
-/*
- * not really modular, but the easiest way to keep compat with existing
- * bootargs behaviour is to continue using module_param here.
- */
-module_param_named(cost, lowmem_shrinker.seeks, int, 0644);
-module_param_array_named(adj, lowmem_adj, short, &lowmem_adj_size, 0644);
-module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size,
-			 0644);
-module_param_named(debug_level, lowmem_debug_level, uint, 0644);
-
diff --git a/drivers/staging/android/uapi/ion.h b/drivers/staging/android/uapi/ion.h
index 14cd8738ecfc..b76db1b2e197 100644
--- a/drivers/staging/android/uapi/ion.h
+++ b/drivers/staging/android/uapi/ion.h
@@ -20,8 +20,6 @@
 #include <linux/ioctl.h>
 #include <linux/types.h>
 
-typedef int ion_user_handle_t;
-
 /**
  * enum ion_heap_types - list of all possible types of heaps
  * @ION_HEAP_TYPE_SYSTEM:	 memory allocated via vmalloc
@@ -76,7 +74,6 @@ enum ion_heap_type {
 /**
  * struct ion_allocation_data - metadata passed from userspace for allocations
  * @len:		size of the allocation
- * @align:		required alignment of the allocation
  * @heap_id_mask:	mask of heap ids to allocate from
  * @flags:		flags passed to heap
  * @handle:		pointer that will be populated with a cookie to use to
@@ -85,47 +82,11 @@ enum ion_heap_type {
  * Provided by userspace as an argument to the ioctl
  */
 struct ion_allocation_data {
-	size_t len;
-	size_t align;
-	unsigned int heap_id_mask;
-	unsigned int flags;
-	ion_user_handle_t handle;
-};
-
-/**
- * struct ion_fd_data - metadata passed to/from userspace for a handle/fd pair
- * @handle:	a handle
- * @fd:		a file descriptor representing that handle
- *
- * For ION_IOC_SHARE or ION_IOC_MAP userspace populates the handle field with
- * the handle returned from ion alloc, and the kernel returns the file
- * descriptor to share or map in the fd field.  For ION_IOC_IMPORT, userspace
- * provides the file descriptor and the kernel returns the handle.
- */
-struct ion_fd_data {
-	ion_user_handle_t handle;
-	int fd;
-};
-
-/**
- * struct ion_handle_data - a handle passed to/from the kernel
- * @handle:	a handle
- */
-struct ion_handle_data {
-	ion_user_handle_t handle;
-};
-
-/**
- * struct ion_custom_data - metadata passed to/from userspace for a custom ioctl
- * @cmd:	the custom ioctl function to call
- * @arg:	additional data to pass to the custom ioctl, typically a user
- *		pointer to a predefined structure
- *
- * This works just like the regular cmd and arg fields of an ioctl.
- */
-struct ion_custom_data {
-	unsigned int cmd;
-	unsigned long arg;
+	__u64 len;
+	__u32 heap_id_mask;
+	__u32 flags;
+	__u32 fd;
+	__u32 unused;
 };
 
 #define MAX_HEAP_NAME			32
@@ -177,16 +138,6 @@ struct ion_heap_query {
 #define ION_IOC_FREE		_IOWR(ION_IOC_MAGIC, 1, struct ion_handle_data)
 
 /**
- * DOC: ION_IOC_MAP - get a file descriptor to mmap
- *
- * Takes an ion_fd_data struct with the handle field populated with a valid
- * opaque handle.  Returns the struct with the fd field set to a file
- * descriptor open in the current address space.  This file descriptor
- * can then be used as an argument to mmap.
- */
-#define ION_IOC_MAP		_IOWR(ION_IOC_MAGIC, 2, struct ion_fd_data)
-
-/**
  * DOC: ION_IOC_SHARE - creates a file descriptor to use to share an allocation
  *
  * Takes an ion_fd_data struct with the handle field populated with a valid
@@ -198,33 +149,6 @@ struct ion_heap_query {
 #define ION_IOC_SHARE		_IOWR(ION_IOC_MAGIC, 4, struct ion_fd_data)
 
 /**
- * DOC: ION_IOC_IMPORT - imports a shared file descriptor
- *
- * Takes an ion_fd_data struct with the fd field populated with a valid file
- * descriptor obtained from ION_IOC_SHARE and returns the struct with the handle
- * filed set to the corresponding opaque handle.
- */
-#define ION_IOC_IMPORT		_IOWR(ION_IOC_MAGIC, 5, struct ion_fd_data)
-
-/**
- * DOC: ION_IOC_SYNC - syncs a shared file descriptors to memory
- *
- * Deprecated in favor of using the dma_buf api's correctly (syncing
- * will happen automatically when the buffer is mapped to a device).
- * If necessary should be used after touching a cached buffer from the cpu,
- * this will make the buffer in memory coherent.
- */
-#define ION_IOC_SYNC		_IOWR(ION_IOC_MAGIC, 7, struct ion_fd_data)
-
-/**
- * DOC: ION_IOC_CUSTOM - call architecture specific ion ioctl
- *
- * Takes the argument of the architecture specific ioctl to call and
- * passes appropriate userdata for that ioctl
- */
-#define ION_IOC_CUSTOM		_IOWR(ION_IOC_MAGIC, 6, struct ion_custom_data)
-
-/**
  * DOC: ION_IOC_HEAP_QUERY - information about available heaps
  *
  * Takes an ion_heap_query structure and populates information about
diff --git a/drivers/staging/android/uapi/ion_test.h b/drivers/staging/android/uapi/ion_test.h
deleted file mode 100644
index 480242e02f8d..000000000000
--- a/drivers/staging/android/uapi/ion_test.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- * drivers/staging/android/uapi/ion.h
- *
- * Copyright (C) 2011 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * 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 _UAPI_LINUX_ION_TEST_H
-#define _UAPI_LINUX_ION_TEST_H
-
-#include <linux/ioctl.h>
-#include <linux/types.h>
-
-/**
- * struct ion_test_rw_data - metadata passed to the kernel to read handle
- * @ptr:	a pointer to an area at least as large as size
- * @offset:	offset into the ion buffer to start reading
- * @size:	size to read or write
- * @write:	1 to write, 0 to read
- */
-struct ion_test_rw_data {
-	__u64 ptr;
-	__u64 offset;
-	__u64 size;
-	int write;
-	int __padding;
-};
-
-#define ION_IOC_MAGIC		'I'
-
-/**
- * DOC: ION_IOC_TEST_SET_DMA_BUF - attach a dma buf to the test driver
- *
- * Attaches a dma buf fd to the test driver.  Passing a second fd or -1 will
- * release the first fd.
- */
-#define ION_IOC_TEST_SET_FD \
-			_IO(ION_IOC_MAGIC, 0xf0)
-
-/**
- * DOC: ION_IOC_TEST_DMA_MAPPING - read or write memory from a handle as DMA
- *
- * Reads or writes the memory from a handle using an uncached mapping.  Can be
- * used by unit tests to emulate a DMA engine as close as possible.  Only
- * expected to be used for debugging and testing, may not always be available.
- */
-#define ION_IOC_TEST_DMA_MAPPING \
-			_IOW(ION_IOC_MAGIC, 0xf1, struct ion_test_rw_data)
-
-/**
- * DOC: ION_IOC_TEST_KERNEL_MAPPING - read or write memory from a handle
- *
- * Reads or writes the memory from a handle using a kernel mapping.  Can be
- * used by unit tests to test heap map_kernel functions.  Only expected to be
- * used for debugging and testing, may not always be available.
- */
-#define ION_IOC_TEST_KERNEL_MAPPING \
-			_IOW(ION_IOC_MAGIC, 0xf2, struct ion_test_rw_data)
-
-#endif /* _UAPI_LINUX_ION_H */
diff --git a/drivers/staging/bcm2835-audio/Kconfig b/drivers/staging/bcm2835-audio/Kconfig
deleted file mode 100644
index 32a2ff9ef9b2..000000000000
--- a/drivers/staging/bcm2835-audio/Kconfig
+++ /dev/null
@@ -1,7 +0,0 @@
-config SND_BCM2835
-        tristate "BCM2835 ALSA driver"
-        depends on ARCH_BCM2835 && BCM2835_VCHIQ && SND
-        select SND_PCM
-        help
-          Say Y or M if you want to support BCM2835 Alsa pcm card driver
-
diff --git a/drivers/staging/bcm2835-audio/bcm2835.c b/drivers/staging/bcm2835-audio/bcm2835.c
deleted file mode 100644
index 3a5e528e0ec6..000000000000
--- a/drivers/staging/bcm2835-audio/bcm2835.c
+++ /dev/null
@@ -1,250 +0,0 @@
-/*****************************************************************************
- * Copyright 2011 Broadcom Corporation.  All rights reserved.
- *
- * Unless you and Broadcom execute a separate written software license
- * agreement governing use of this software, this software is licensed to you
- * under the terms of the GNU General Public License version 2, available at
- * http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
- *
- * Notwithstanding the above, under no circumstances may you combine this
- * software in any way with any other Broadcom software provided under a
- * license other than the GPL, without Broadcom's express prior written
- * consent.
- *****************************************************************************/
-
-#include <linux/platform_device.h>
-
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/of.h>
-
-#include "bcm2835.h"
-
-/* HACKY global pointers needed for successive probes to work : ssp
- * But compared against the changes we will have to do in VC audio_ipc code
- * to export 8 audio_ipc devices as a single IPC device and then monitor all
- * four devices in a thread, this gets things done quickly and should be easier
- * to debug if we run into issues
- */
-
-static struct snd_card *g_card;
-static struct bcm2835_chip *g_chip;
-
-static int snd_bcm2835_free(struct bcm2835_chip *chip)
-{
-	kfree(chip);
-	return 0;
-}
-
-/* component-destructor
- * (see "Management of Cards and Components")
- */
-static int snd_bcm2835_dev_free(struct snd_device *device)
-{
-	return snd_bcm2835_free(device->device_data);
-}
-
-/* chip-specific constructor
- * (see "Management of Cards and Components")
- */
-static int snd_bcm2835_create(struct snd_card *card,
-			      struct platform_device *pdev,
-			      struct bcm2835_chip **rchip)
-{
-	struct bcm2835_chip *chip;
-	int err;
-	static struct snd_device_ops ops = {
-		.dev_free = snd_bcm2835_dev_free,
-	};
-
-	*rchip = NULL;
-
-	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
-	if (!chip)
-		return -ENOMEM;
-
-	chip->card = card;
-
-	err = snd_device_new(card, SNDRV_DEV_LOWLEVEL, chip, &ops);
-	if (err < 0) {
-		snd_bcm2835_free(chip);
-		return err;
-	}
-
-	*rchip = chip;
-	return 0;
-}
-
-static int snd_bcm2835_alsa_probe_dt(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct bcm2835_chip *chip;
-	struct snd_card *card;
-	u32 numchans;
-	int err, i;
-
-	err = of_property_read_u32(dev->of_node, "brcm,pwm-channels",
-				   &numchans);
-	if (err) {
-		dev_err(dev, "Failed to get DT property 'brcm,pwm-channels'");
-		return err;
-	}
-
-	if (numchans == 0 || numchans > MAX_SUBSTREAMS) {
-		numchans = MAX_SUBSTREAMS;
-		dev_warn(dev, "Illegal 'brcm,pwm-channels' value, will use %u\n",
-			 numchans);
-	}
-
-	err = snd_card_new(&pdev->dev, -1, NULL, THIS_MODULE, 0, &card);
-	if (err) {
-		dev_err(dev, "Failed to create soundcard structure\n");
-		return err;
-	}
-
-	snd_card_set_dev(card, dev);
-	strcpy(card->driver, "bcm2835");
-	strcpy(card->shortname, "bcm2835 ALSA");
-	sprintf(card->longname, "%s", card->shortname);
-
-	err = snd_bcm2835_create(card, pdev, &chip);
-	if (err < 0) {
-		dev_err(dev, "Failed to create bcm2835 chip\n");
-		goto err_free;
-	}
-
-	err = snd_bcm2835_new_pcm(chip);
-	if (err < 0) {
-		dev_err(dev, "Failed to create new bcm2835 pcm device\n");
-		goto err_free;
-	}
-
-	err = snd_bcm2835_new_spdif_pcm(chip);
-	if (err < 0) {
-		dev_err(dev, "Failed to create new bcm2835 spdif pcm device\n");
-		goto err_free;
-	}
-
-	err = snd_bcm2835_new_ctl(chip);
-	if (err < 0) {
-		dev_err(dev, "Failed to create new bcm2835 ctl\n");
-		goto err_free;
-	}
-
-	for (i = 0; i < numchans; i++) {
-		chip->avail_substreams |= (1 << i);
-		chip->pdev[i] = pdev;
-	}
-
-	err = snd_card_register(card);
-	if (err) {
-		dev_err(dev, "Failed to register bcm2835 ALSA card\n");
-		goto err_free;
-	}
-
-	g_card = card;
-	g_chip = chip;
-	platform_set_drvdata(pdev, card);
-	audio_info("bcm2835 ALSA card created with %u channels\n", numchans);
-
-	return 0;
-
-err_free:
-	snd_card_free(card);
-
-	return err;
-}
-
-static int snd_bcm2835_alsa_remove(struct platform_device *pdev)
-{
-	int idx;
-	void *drv_data;
-
-	drv_data = platform_get_drvdata(pdev);
-
-	if (drv_data == (void *)g_card) {
-		/* This is the card device */
-		snd_card_free((struct snd_card *)drv_data);
-		g_card = NULL;
-		g_chip = NULL;
-	} else {
-		idx = (int)(long)drv_data;
-		if (g_card) {
-			BUG_ON(!g_chip);
-			/* We pass chip device numbers in audio ipc devices
-			 * other than the one we registered our card with
-			 */
-			idx = (int)(long)drv_data;
-			BUG_ON(!idx || idx > MAX_SUBSTREAMS);
-			g_chip->avail_substreams &= ~(1 << idx);
-			/* There should be atleast one substream registered
-			 * after we are done here, as it wil be removed when
-			 * the *remove* is called for the card device
-			 */
-			BUG_ON(!g_chip->avail_substreams);
-		}
-	}
-
-	platform_set_drvdata(pdev, NULL);
-
-	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 const struct of_device_id snd_bcm2835_of_match_table[] = {
-	{ .compatible = "brcm,bcm2835-audio",},
-	{},
-};
-MODULE_DEVICE_TABLE(of, snd_bcm2835_of_match_table);
-
-static struct platform_driver bcm2835_alsa0_driver = {
-	.probe = snd_bcm2835_alsa_probe_dt,
-	.remove = snd_bcm2835_alsa_remove,
-#ifdef CONFIG_PM
-	.suspend = snd_bcm2835_alsa_suspend,
-	.resume = snd_bcm2835_alsa_resume,
-#endif
-	.driver = {
-		.name = "bcm2835_AUD0",
-		.owner = THIS_MODULE,
-		.of_match_table = snd_bcm2835_of_match_table,
-	},
-};
-
-static int bcm2835_alsa_device_init(void)
-{
-	int retval;
-
-	retval = platform_driver_register(&bcm2835_alsa0_driver);
-	if (retval)
-		pr_err("Error registering bcm2835_alsa0_driver %d .\n", retval);
-
-	return retval;
-}
-
-static void bcm2835_alsa_device_exit(void)
-{
-	platform_driver_unregister(&bcm2835_alsa0_driver);
-}
-
-late_initcall(bcm2835_alsa_device_init);
-module_exit(bcm2835_alsa_device_exit);
-
-MODULE_AUTHOR("Dom Cobley");
-MODULE_DESCRIPTION("Alsa driver for BCM2835 chip");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/ccree/Documentation/devicetree/bindings/crypto/arm-cryptocell.txt b/drivers/staging/ccree/Documentation/devicetree/bindings/crypto/arm-cryptocell.txt
new file mode 100644
index 000000000000..2ea65175c032
--- /dev/null
+++ b/drivers/staging/ccree/Documentation/devicetree/bindings/crypto/arm-cryptocell.txt
@@ -0,0 +1,27 @@
+Arm TrustZone CryptoCell cryptographic accelerators
+
+Required properties:
+- compatible: must be "arm,cryptocell-712-ree".
+- reg: shall contain base register location and length.
+	Typically length is 0x10000.
+- interrupts: shall contain the interrupt for the device.
+
+Optional properties:
+- interrupt-parent: can designate the interrupt controller the
+	device interrupt is connected to, if needed.
+- clocks: may contain the clock handling the device, if needed.
+- power-domains: may contain a reference to the PM domain, if applicable.
+
+
+Examples:
+
+Zynq FPGA device
+----------------
+
+       arm_cc7x: arm_cc7x@80000000 {
+               compatible = "arm,cryptocell-712-ree";
+               interrupt-parent = <&intc>;
+               interrupts = < 0 30 4 >;
+               reg = < 0x80000000 0x10000 >;
+       };
+
diff --git a/drivers/staging/ccree/Kconfig b/drivers/staging/ccree/Kconfig
new file mode 100644
index 000000000000..ae627049c499
--- /dev/null
+++ b/drivers/staging/ccree/Kconfig
@@ -0,0 +1,43 @@
+config CRYPTO_DEV_CCREE
+	tristate "Support for ARM TrustZone CryptoCell C7XX family of Crypto accelerators"
+	depends on CRYPTO_HW && OF && HAS_DMA
+	default n
+	select CRYPTO_HASH
+	select CRYPTO_BLKCIPHER
+	select CRYPTO_DES
+	select CRYPTO_AEAD
+	select CRYPTO_AUTHENC
+	select CRYPTO_SHA1
+	select CRYPTO_MD5
+	select CRYPTO_SHA256
+	select CRYPTO_SHA512
+	select CRYPTO_HMAC
+	select CRYPTO_AES
+	select CRYPTO_CBC
+	select CRYPTO_ECB
+	select CRYPTO_CTR
+	select CRYPTO_XTS
+	help
+	  Say 'Y' to enable a driver for the Arm TrustZone CryptoCell 
+	  C7xx. Currently only the CryptoCell 712 REE is supported.
+	  Choose this if you wish to use hardware acceleration of
+	  cryptographic operations on the system REE.
+	  If unsure say Y.
+
+config CCREE_FIPS_SUPPORT
+	bool "Turn on CryptoCell 7XX REE FIPS mode support"
+	depends on CRYPTO_DEV_CCREE
+	default n
+	help
+	  Say 'Y' to enable support for FIPS compliant mode by the
+	  CCREE driver.
+	  If unsure say N.
+
+config CCREE_DISABLE_COHERENT_DMA_OPS
+	bool "Disable Coherent DMA operations for the CCREE driver"
+	depends on CRYPTO_DEV_CCREE
+	default n
+	help
+	  Say 'Y' to disable the use of coherent DMA operations by the
+	  CCREE driver for debugging purposes.  
+	  If unsure say N.
diff --git a/drivers/staging/ccree/Makefile b/drivers/staging/ccree/Makefile
new file mode 100644
index 000000000000..44f3e3e33989
--- /dev/null
+++ b/drivers/staging/ccree/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_CRYPTO_DEV_CCREE) := ccree.o
+ccree-y := ssi_driver.o ssi_sysfs.o ssi_buffer_mgr.o ssi_request_mgr.o ssi_cipher.o ssi_hash.o ssi_aead.o ssi_ivgen.o ssi_sram_mgr.o ssi_pm.o ssi_pm_ext.o
+ccree-$(CCREE_FIPS_SUPPORT) += ssi_fips.o ssi_fips_ll.o ssi_fips_ext.o ssi_fips_local.o
diff --git a/drivers/staging/ccree/TODO b/drivers/staging/ccree/TODO
new file mode 100644
index 000000000000..c9f5754d062d
--- /dev/null
+++ b/drivers/staging/ccree/TODO
@@ -0,0 +1,30 @@
+
+
+*************************************************************************
+*									*
+* Arm Trust Zone CryptoCell REE Linux driver upstreaming TODO items	*
+*									*
+*************************************************************************
+
+ccree specific items
+a.k.a stuff fixing for this driver to move out of staging
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+1.  Move to using Crypto Engine to handle backlog queueing.
+2.  Remove synchronous algorithm support leftovers.
+3.  Separate platform specific code for FIPS and power management into separate platform modules.
+4.  Drop legacy kernel support code.
+5.  Move most (all?) #ifdef CONFIG into inline functions.
+6.  Remove all unused definitions.
+7.  Re-factor to accomediate newer/older HW revisions besides the 712.
+8.  Handle the many checkpatch errors.
+9.  Implement ahash import/export correctly.
+10. Go through a proper review of DT bindings and sysfs ABI
+11. Sort out FIPS mode: bake tests into testmgr, sort out behaviour on error, 
+    figure if 3DES weak key check is needed
+
+Kernel infrastructure items
+a.k.a stuff we either neither need to fix in the kernel or understand what we're doing wrong
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+1. ahash import/export context has a PAGE_SIZE/8 size limit.  We need more.
+2. Crypto Engine seems to be built for HW with hardware queue depth of 1, we have 600++.
diff --git a/drivers/staging/ccree/cc_bitops.h b/drivers/staging/ccree/cc_bitops.h
new file mode 100644
index 000000000000..3a39565ef73b
--- /dev/null
+++ b/drivers/staging/ccree/cc_bitops.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*!
+ * \file cc_bitops.h
+ * Bit fields operations macros.
+ */
+#ifndef _CC_BITOPS_H_
+#define _CC_BITOPS_H_
+
+#define BITMASK(mask_size) (((mask_size) < 32) ?	\
+	((1UL << (mask_size)) - 1) : 0xFFFFFFFFUL)
+#define BITMASK_AT(mask_size, mask_offset) (BITMASK(mask_size) << (mask_offset))
+
+#define BITFIELD_GET(word, bit_offset, bit_size) \
+	(((word) >> (bit_offset)) & BITMASK(bit_size))
+#define BITFIELD_SET(word, bit_offset, bit_size, new_val)   do {    \
+	word = ((word) & ~BITMASK_AT(bit_size, bit_offset)) |	    \
+		(((new_val) & BITMASK(bit_size)) << (bit_offset));  \
+} while (0)
+
+/* Is val aligned to "align" ("align" must be power of 2) */
+#ifndef IS_ALIGNED
+#define IS_ALIGNED(val, align)		\
+	(((uintptr_t)(val) & ((align) - 1)) == 0)
+#endif
+
+#define SWAP_ENDIAN(word)		\
+	(((word) >> 24) | (((word) & 0x00FF0000) >> 8) | \
+	(((word) & 0x0000FF00) << 8) | (((word) & 0x000000FF) << 24))
+
+#ifdef BIG__ENDIAN
+#define SWAP_TO_LE(word) SWAP_ENDIAN(word)
+#define SWAP_TO_BE(word) word
+#else
+#define SWAP_TO_LE(word) word
+#define SWAP_TO_BE(word) SWAP_ENDIAN(word)
+#endif
+
+
+
+/* Is val a multiple of "mult" ("mult" must be power of 2) */
+#define IS_MULT(val, mult)              \
+	(((val) & ((mult) - 1)) == 0)
+
+#define IS_NULL_ADDR(adr)		\
+	(!(adr))
+
+#endif /*_CC_BITOPS_H_*/
diff --git a/drivers/staging/ccree/cc_crypto_ctx.h b/drivers/staging/ccree/cc_crypto_ctx.h
new file mode 100644
index 000000000000..9e10b2670313
--- /dev/null
+++ b/drivers/staging/ccree/cc_crypto_ctx.h
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#ifndef _CC_CRYPTO_CTX_H_
+#define _CC_CRYPTO_CTX_H_
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#define INT32_MAX 0x7FFFFFFFL
+#else
+#include <stdint.h>
+#endif
+
+
+#ifndef max
+#define max(a, b) ((a) > (b) ? (a) : (b))
+#define min(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
+/* context size */
+#ifndef CC_CTX_SIZE_LOG2
+#if (CC_SUPPORT_SHA > 256)
+#define CC_CTX_SIZE_LOG2 8
+#else
+#define CC_CTX_SIZE_LOG2 7
+#endif
+#endif
+#define CC_CTX_SIZE (1<<CC_CTX_SIZE_LOG2)
+#define CC_DRV_CTX_SIZE_WORDS (CC_CTX_SIZE >> 2)
+
+#define CC_DRV_DES_IV_SIZE 8
+#define CC_DRV_DES_BLOCK_SIZE 8
+
+#define CC_DRV_DES_ONE_KEY_SIZE 8
+#define CC_DRV_DES_DOUBLE_KEY_SIZE 16
+#define CC_DRV_DES_TRIPLE_KEY_SIZE 24
+#define CC_DRV_DES_KEY_SIZE_MAX CC_DRV_DES_TRIPLE_KEY_SIZE
+
+#define CC_AES_IV_SIZE 16
+#define CC_AES_IV_SIZE_WORDS (CC_AES_IV_SIZE >> 2)
+
+#define CC_AES_BLOCK_SIZE 16
+#define CC_AES_BLOCK_SIZE_WORDS 4
+
+#define CC_AES_128_BIT_KEY_SIZE 16
+#define CC_AES_128_BIT_KEY_SIZE_WORDS	(CC_AES_128_BIT_KEY_SIZE >> 2)
+#define CC_AES_192_BIT_KEY_SIZE 24
+#define CC_AES_192_BIT_KEY_SIZE_WORDS	(CC_AES_192_BIT_KEY_SIZE >> 2)
+#define CC_AES_256_BIT_KEY_SIZE 32
+#define CC_AES_256_BIT_KEY_SIZE_WORDS	(CC_AES_256_BIT_KEY_SIZE >> 2)
+#define CC_AES_KEY_SIZE_MAX			CC_AES_256_BIT_KEY_SIZE
+#define CC_AES_KEY_SIZE_WORDS_MAX		(CC_AES_KEY_SIZE_MAX >> 2)
+
+#define CC_MD5_DIGEST_SIZE 	16
+#define CC_SHA1_DIGEST_SIZE 	20
+#define CC_SHA224_DIGEST_SIZE 	28
+#define CC_SHA256_DIGEST_SIZE 	32
+#define CC_SHA256_DIGEST_SIZE_IN_WORDS 8
+#define CC_SHA384_DIGEST_SIZE 	48
+#define CC_SHA512_DIGEST_SIZE 	64
+
+#define CC_SHA1_BLOCK_SIZE 64
+#define CC_SHA1_BLOCK_SIZE_IN_WORDS 16
+#define CC_MD5_BLOCK_SIZE 64
+#define CC_MD5_BLOCK_SIZE_IN_WORDS 16
+#define CC_SHA224_BLOCK_SIZE 64
+#define CC_SHA256_BLOCK_SIZE 64
+#define CC_SHA256_BLOCK_SIZE_IN_WORDS 16
+#define CC_SHA1_224_256_BLOCK_SIZE 64
+#define CC_SHA384_BLOCK_SIZE 128
+#define CC_SHA512_BLOCK_SIZE 128
+
+#if (CC_SUPPORT_SHA > 256)
+#define CC_DIGEST_SIZE_MAX CC_SHA512_DIGEST_SIZE
+#define CC_HASH_BLOCK_SIZE_MAX CC_SHA512_BLOCK_SIZE /*1024b*/
+#else /* Only up to SHA256 */
+#define CC_DIGEST_SIZE_MAX CC_SHA256_DIGEST_SIZE
+#define CC_HASH_BLOCK_SIZE_MAX CC_SHA256_BLOCK_SIZE /*512b*/
+#endif
+
+#define CC_HMAC_BLOCK_SIZE_MAX CC_HASH_BLOCK_SIZE_MAX
+
+#define CC_MULTI2_SYSTEM_KEY_SIZE 		32
+#define CC_MULTI2_DATA_KEY_SIZE 		8
+#define CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE 	(CC_MULTI2_SYSTEM_KEY_SIZE + CC_MULTI2_DATA_KEY_SIZE)
+#define	CC_MULTI2_BLOCK_SIZE					8
+#define	CC_MULTI2_IV_SIZE					8
+#define	CC_MULTI2_MIN_NUM_ROUNDS				8
+#define	CC_MULTI2_MAX_NUM_ROUNDS				128
+
+
+#define CC_DRV_ALG_MAX_BLOCK_SIZE CC_HASH_BLOCK_SIZE_MAX
+
+
+enum drv_engine_type {
+	DRV_ENGINE_NULL = 0,
+	DRV_ENGINE_AES = 1,
+	DRV_ENGINE_DES = 2,
+	DRV_ENGINE_HASH = 3,
+	DRV_ENGINE_RC4 = 4,
+	DRV_ENGINE_DOUT = 5,
+	DRV_ENGINE_RESERVE32B = INT32_MAX,
+};
+
+enum drv_crypto_alg {
+	DRV_CRYPTO_ALG_NULL = -1,
+	DRV_CRYPTO_ALG_AES  = 0,
+	DRV_CRYPTO_ALG_DES  = 1,
+	DRV_CRYPTO_ALG_HASH = 2,
+	DRV_CRYPTO_ALG_C2   = 3,
+	DRV_CRYPTO_ALG_HMAC = 4,
+	DRV_CRYPTO_ALG_AEAD = 5,
+	DRV_CRYPTO_ALG_BYPASS = 6,
+	DRV_CRYPTO_ALG_NUM = 7,
+	DRV_CRYPTO_ALG_RESERVE32B = INT32_MAX
+};
+
+enum drv_crypto_direction {
+	DRV_CRYPTO_DIRECTION_NULL = -1,
+	DRV_CRYPTO_DIRECTION_ENCRYPT = 0,
+	DRV_CRYPTO_DIRECTION_DECRYPT = 1,
+	DRV_CRYPTO_DIRECTION_DECRYPT_ENCRYPT = 3,
+	DRV_CRYPTO_DIRECTION_RESERVE32B = INT32_MAX
+};
+
+enum drv_cipher_mode {
+	DRV_CIPHER_NULL_MODE = -1,
+	DRV_CIPHER_ECB = 0,
+	DRV_CIPHER_CBC = 1,
+	DRV_CIPHER_CTR = 2,
+	DRV_CIPHER_CBC_MAC = 3,
+	DRV_CIPHER_XTS = 4,
+	DRV_CIPHER_XCBC_MAC = 5,
+	DRV_CIPHER_OFB = 6,
+	DRV_CIPHER_CMAC = 7,
+	DRV_CIPHER_CCM = 8,
+	DRV_CIPHER_CBC_CTS = 11,
+	DRV_CIPHER_GCTR = 12,
+	DRV_CIPHER_ESSIV = 13,
+	DRV_CIPHER_BITLOCKER = 14,
+	DRV_CIPHER_RESERVE32B = INT32_MAX
+};
+
+enum drv_hash_mode {
+	DRV_HASH_NULL = -1,
+	DRV_HASH_SHA1 = 0,
+	DRV_HASH_SHA256 = 1,
+	DRV_HASH_SHA224 = 2,
+	DRV_HASH_SHA512 = 3,
+	DRV_HASH_SHA384 = 4,
+	DRV_HASH_MD5 = 5,
+	DRV_HASH_CBC_MAC = 6, 
+	DRV_HASH_XCBC_MAC = 7,
+	DRV_HASH_CMAC = 8,
+	DRV_HASH_MODE_NUM = 9,
+	DRV_HASH_RESERVE32B = INT32_MAX
+};
+
+enum drv_hash_hw_mode {
+	DRV_HASH_HW_MD5 = 0,
+	DRV_HASH_HW_SHA1 = 1,
+	DRV_HASH_HW_SHA256 = 2,
+	DRV_HASH_HW_SHA224 = 10,
+	DRV_HASH_HW_SHA512 = 4,
+	DRV_HASH_HW_SHA384 = 12,
+	DRV_HASH_HW_GHASH = 6,
+	DRV_HASH_HW_RESERVE32B = INT32_MAX
+};
+
+enum drv_multi2_mode {
+	DRV_MULTI2_NULL = -1,
+	DRV_MULTI2_ECB = 0,
+	DRV_MULTI2_CBC = 1,
+	DRV_MULTI2_OFB = 2,
+	DRV_MULTI2_RESERVE32B = INT32_MAX
+};
+
+
+/* drv_crypto_key_type[1:0] is mapped to cipher_do[1:0] */
+/* drv_crypto_key_type[2] is mapped to cipher_config2 */
+enum drv_crypto_key_type {
+	DRV_NULL_KEY = -1,
+	DRV_USER_KEY = 0,		/* 0x000 */
+	DRV_ROOT_KEY = 1,		/* 0x001 */
+	DRV_PROVISIONING_KEY = 2,	/* 0x010 */
+	DRV_SESSION_KEY = 3,		/* 0x011 */
+	DRV_APPLET_KEY = 4,		/* NA */
+	DRV_PLATFORM_KEY = 5,		/* 0x101 */
+	DRV_CUSTOMER_KEY = 6,		/* 0x110 */
+	DRV_END_OF_KEYS = INT32_MAX,
+};
+
+enum drv_crypto_padding_type {
+	DRV_PADDING_NONE = 0,
+	DRV_PADDING_PKCS7 = 1,
+	DRV_PADDING_RESERVE32B = INT32_MAX
+};
+
+/*******************************************************************/
+/***************** DESCRIPTOR BASED CONTEXTS ***********************/
+/*******************************************************************/
+
+ /* Generic context ("super-class") */
+struct drv_ctx_generic {
+	enum drv_crypto_alg alg;
+} __attribute__((__may_alias__));
+
+
+struct drv_ctx_hash {
+	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_HASH */
+	enum drv_hash_mode mode;
+	uint8_t digest[CC_DIGEST_SIZE_MAX];
+	/* reserve to end of allocated context size */
+	uint8_t reserved[CC_CTX_SIZE - 2 * sizeof(uint32_t) -
+			CC_DIGEST_SIZE_MAX];
+};
+
+/* !!!! drv_ctx_hmac should have the same structure as drv_ctx_hash except
+   k0, k0_size fields */
+struct drv_ctx_hmac {
+	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_HMAC */
+	enum drv_hash_mode mode;
+	uint8_t digest[CC_DIGEST_SIZE_MAX];
+	uint32_t k0[CC_HMAC_BLOCK_SIZE_MAX/sizeof(uint32_t)];
+	uint32_t k0_size;
+	/* reserve to end of allocated context size */
+	uint8_t reserved[CC_CTX_SIZE - 3 * sizeof(uint32_t) -
+			CC_DIGEST_SIZE_MAX - CC_HMAC_BLOCK_SIZE_MAX];
+};
+
+struct drv_ctx_cipher {
+	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_AES */
+	enum drv_cipher_mode mode;
+	enum drv_crypto_direction direction;
+	enum drv_crypto_key_type crypto_key_type;
+	enum drv_crypto_padding_type padding_type;
+	uint32_t key_size; /* numeric value in bytes   */
+	uint32_t data_unit_size; /* required for XTS */
+	/* block_state is the AES engine block state.
+	*  It is used by the host to pass IV or counter at initialization.
+	*  It is used by SeP for intermediate block chaining state and for
+	*  returning MAC algorithms results.           */
+	uint8_t block_state[CC_AES_BLOCK_SIZE];
+	uint8_t key[CC_AES_KEY_SIZE_MAX];
+	uint8_t xex_key[CC_AES_KEY_SIZE_MAX];
+	/* reserve to end of allocated context size */
+	uint32_t reserved[CC_DRV_CTX_SIZE_WORDS - 7 -
+		CC_AES_BLOCK_SIZE/sizeof(uint32_t) - 2 *
+		(CC_AES_KEY_SIZE_MAX/sizeof(uint32_t))];
+};
+
+/* authentication and encryption with associated data class */
+struct drv_ctx_aead {
+	enum drv_crypto_alg alg; /* DRV_CRYPTO_ALG_AES */
+	enum drv_cipher_mode mode;
+	enum drv_crypto_direction direction;
+	uint32_t key_size; /* numeric value in bytes   */
+	uint32_t nonce_size; /* nonce size (octets) */
+	uint32_t header_size; /* finit additional data size (octets) */
+	uint32_t text_size; /* finit text data size (octets) */
+	uint32_t tag_size; /* mac size, element of {4, 6, 8, 10, 12, 14, 16} */
+	/* block_state1/2 is the AES engine block state */
+	uint8_t block_state[CC_AES_BLOCK_SIZE];
+	uint8_t mac_state[CC_AES_BLOCK_SIZE]; /* MAC result */
+	uint8_t nonce[CC_AES_BLOCK_SIZE]; /* nonce buffer */
+	uint8_t key[CC_AES_KEY_SIZE_MAX];
+	/* reserve to end of allocated context size */
+	uint32_t reserved[CC_DRV_CTX_SIZE_WORDS - 8 -
+		3 * (CC_AES_BLOCK_SIZE/sizeof(uint32_t)) -
+		CC_AES_KEY_SIZE_MAX/sizeof(uint32_t)];
+};
+
+/*******************************************************************/
+/***************** MESSAGE BASED CONTEXTS **************************/
+/*******************************************************************/
+
+
+/* Get the address of a @member within a given @ctx address
+   @ctx: The context address
+   @type: Type of context structure
+   @member: Associated context field */
+#define GET_CTX_FIELD_ADDR(ctx, type, member) (ctx + offsetof(type, member))
+
+#endif /* _CC_CRYPTO_CTX_H_ */
+
diff --git a/drivers/staging/ccree/cc_hal.h b/drivers/staging/ccree/cc_hal.h
new file mode 100644
index 000000000000..75a0ce3e80d6
--- /dev/null
+++ b/drivers/staging/ccree/cc_hal.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* pseudo cc_hal.h for cc7x_perf_test_driver (to be able to include code from CC drivers) */
+
+#ifndef __CC_HAL_H__
+#define __CC_HAL_H__
+
+#include <linux/io.h>
+
+#define READ_REGISTER(_addr) ioread32((_addr))
+#define WRITE_REGISTER(_addr, _data)  iowrite32((_data), (_addr))
+
+#define CC_HAL_WRITE_REGISTER(offset, val) WRITE_REGISTER(cc_base + offset, val)
+#define CC_HAL_READ_REGISTER(offset) READ_REGISTER(cc_base + offset)
+
+#endif
diff --git a/drivers/staging/ccree/cc_hw_queue_defs.h b/drivers/staging/ccree/cc_hw_queue_defs.h
new file mode 100644
index 000000000000..fbaf1b6fcd90
--- /dev/null
+++ b/drivers/staging/ccree/cc_hw_queue_defs.h
@@ -0,0 +1,603 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __CC_HW_QUEUE_DEFS_H__
+#define __CC_HW_QUEUE_DEFS_H__
+
+#include "cc_pal_log.h"
+#include "cc_regs.h"
+#include "dx_crys_kernel.h"
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#define UINT32_MAX 0xFFFFFFFFL
+#define INT32_MAX  0x7FFFFFFFL
+#define UINT16_MAX 0xFFFFL
+#else
+#include <stdint.h>
+#endif
+
+/******************************************************************************
+*                        	DEFINITIONS
+******************************************************************************/
+
+
+/* Dma AXI Secure bit */
+#define	AXI_SECURE	0
+#define AXI_NOT_SECURE	1
+
+#define HW_DESC_SIZE_WORDS		6
+#define HW_QUEUE_SLOTS_MAX              15 /* Max. available slots in HW queue */
+
+#define _HW_DESC_MONITOR_KICK 0x7FFFC00
+
+/******************************************************************************
+*				TYPE DEFINITIONS
+******************************************************************************/
+
+typedef struct HwDesc {
+	uint32_t word[HW_DESC_SIZE_WORDS];
+} HwDesc_s;
+
+typedef enum DescDirection {
+	DESC_DIRECTION_ILLEGAL = -1,
+	DESC_DIRECTION_ENCRYPT_ENCRYPT = 0,
+	DESC_DIRECTION_DECRYPT_DECRYPT = 1,
+	DESC_DIRECTION_DECRYPT_ENCRYPT = 3,
+	DESC_DIRECTION_END = INT32_MAX,
+}DescDirection_t;
+
+typedef enum DmaMode {
+	DMA_MODE_NULL		= -1,
+	NO_DMA 			= 0,
+	DMA_SRAM		= 1,
+	DMA_DLLI		= 2,
+	DMA_MLLI		= 3,
+	DmaMode_OPTIONTS,
+	DmaMode_END 		= INT32_MAX,
+}DmaMode_t;
+
+typedef enum FlowMode {
+	FLOW_MODE_NULL		= -1,
+	/* data flows */
+ 	BYPASS			= 0,
+	DIN_AES_DOUT		= 1,
+	AES_to_HASH		= 2,
+	AES_and_HASH		= 3,
+	DIN_DES_DOUT		= 4,
+	DES_to_HASH		= 5,
+	DES_and_HASH		= 6,
+	DIN_HASH		= 7,
+	DIN_HASH_and_BYPASS	= 8,
+	AESMAC_and_BYPASS	= 9,
+	AES_to_HASH_and_DOUT	= 10,
+	DIN_RC4_DOUT		= 11,
+	DES_to_HASH_and_DOUT	= 12,
+	AES_to_AES_to_HASH_and_DOUT	= 13,
+	AES_to_AES_to_HASH	= 14,
+	AES_to_HASH_and_AES	= 15,
+	DIN_MULTI2_DOUT		= 16,
+	DIN_AES_AESMAC		= 17,
+	HASH_to_DOUT		= 18,
+	/* setup flows */
+ 	S_DIN_to_AES 		= 32,
+	S_DIN_to_AES2		= 33,
+	S_DIN_to_DES		= 34,
+	S_DIN_to_RC4		= 35,
+ 	S_DIN_to_MULTI2		= 36,
+	S_DIN_to_HASH		= 37,
+	S_AES_to_DOUT		= 38,
+	S_AES2_to_DOUT		= 39,
+	S_RC4_to_DOUT		= 41,
+	S_DES_to_DOUT		= 42,
+	S_HASH_to_DOUT		= 43,
+	SET_FLOW_ID		= 44,
+	FlowMode_OPTIONTS,
+	FlowMode_END = INT32_MAX,
+}FlowMode_t;
+
+typedef enum TunnelOp {
+	TUNNEL_OP_INVALID = -1,
+	TUNNEL_OFF = 0,
+	TUNNEL_ON = 1,
+	TunnelOp_OPTIONS,
+	TunnelOp_END = INT32_MAX,
+} TunnelOp_t;
+
+typedef enum SetupOp {
+	SETUP_LOAD_NOP		= 0,
+	SETUP_LOAD_STATE0	= 1,
+	SETUP_LOAD_STATE1	= 2,
+	SETUP_LOAD_STATE2	= 3,
+	SETUP_LOAD_KEY0		= 4,
+	SETUP_LOAD_XEX_KEY	= 5,
+	SETUP_WRITE_STATE0	= 8, 
+	SETUP_WRITE_STATE1	= 9,
+	SETUP_WRITE_STATE2	= 10,
+	SETUP_WRITE_STATE3	= 11,
+	setupOp_OPTIONTS,
+	setupOp_END = INT32_MAX,	
+}SetupOp_t;
+
+enum AesMacSelector {
+	AES_SK = 1,
+	AES_CMAC_INIT = 2,
+	AES_CMAC_SIZE0 = 3,
+	AesMacEnd = INT32_MAX,
+};
+
+#define HW_KEY_MASK_CIPHER_DO 	  0x3
+#define HW_KEY_SHIFT_CIPHER_CFG2  2
+
+
+/* HwCryptoKey[1:0] is mapped to cipher_do[1:0] */
+/* HwCryptoKey[2:3] is mapped to cipher_config2[1:0] */
+typedef enum HwCryptoKey {
+	USER_KEY = 0,			/* 0x0000 */
+	ROOT_KEY = 1,			/* 0x0001 */
+	PROVISIONING_KEY = 2,		/* 0x0010 */ /* ==KCP */
+	SESSION_KEY = 3,		/* 0x0011 */
+	RESERVED_KEY = 4,		/* NA */
+	PLATFORM_KEY = 5,		/* 0x0101 */
+	CUSTOMER_KEY = 6,		/* 0x0110 */
+	KFDE0_KEY = 7,			/* 0x0111 */
+	KFDE1_KEY = 9,			/* 0x1001 */
+	KFDE2_KEY = 10,			/* 0x1010 */
+	KFDE3_KEY = 11,			/* 0x1011 */
+	END_OF_KEYS = INT32_MAX,
+}HwCryptoKey_t;
+
+typedef enum HwAesKeySize {
+	AES_128_KEY = 0,
+	AES_192_KEY = 1,
+	AES_256_KEY = 2,
+	END_OF_AES_KEYS = INT32_MAX,
+}HwAesKeySize_t;
+
+typedef enum HwDesKeySize {
+	DES_ONE_KEY = 0,
+	DES_TWO_KEYS = 1,
+	DES_THREE_KEYS = 2,
+	END_OF_DES_KEYS = INT32_MAX,
+}HwDesKeySize_t;
+
+/*****************************/
+/* Descriptor packing macros */
+/*****************************/
+
+#define GET_HW_Q_DESC_WORD_IDX(descWordIdx) (CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD ## descWordIdx) )
+
+#define HW_DESC_INIT(pDesc)  do { \
+	(pDesc)->word[0] = 0;     \
+	(pDesc)->word[1] = 0;     \
+	(pDesc)->word[2] = 0;     \
+	(pDesc)->word[3] = 0;     \
+	(pDesc)->word[4] = 0;     \
+	(pDesc)->word[5] = 0;     \
+} while (0)
+
+/* HW descriptor debug functions */
+int createDetailedDump(HwDesc_s *pDesc);
+void descriptor_log(HwDesc_s *desc);
+
+#if defined(HW_DESCRIPTOR_LOG) || defined(HW_DESC_DUMP_HOST_BUF)
+#define LOG_HW_DESC(pDesc) descriptor_log(pDesc)
+#else
+#define LOG_HW_DESC(pDesc) 
+#endif
+
+#if (CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE) || defined(OEMFW_LOG)
+
+#ifdef UART_PRINTF
+#define CREATE_DETAILED_DUMP(pDesc) createDetailedDump(pDesc)
+#else
+#define CREATE_DETAILED_DUMP(pDesc) 
+#endif 
+
+#define HW_DESC_DUMP(pDesc) do {            			\
+	CC_PAL_LOG_TRACE("\n---------------------------------------------------\n");	\
+	CREATE_DETAILED_DUMP(pDesc); 				\
+	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[0]);  	\
+	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[1]);  	\
+	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[2]);  	\
+	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[3]);  	\
+	CC_PAL_LOG_TRACE("0x%08X, ", (unsigned int)(pDesc)->word[4]);  	\
+	CC_PAL_LOG_TRACE("0x%08X\n", (unsigned int)(pDesc)->word[5]);  	\
+	CC_PAL_LOG_TRACE("---------------------------------------------------\n\n");    \
+} while (0)
+
+#else
+#define HW_DESC_DUMP(pDesc) do {} while (0)
+#endif
+
+
+/*!
+ * This macro indicates the end of current HW descriptors flow and release the HW engines.
+ * 
+ * \param pDesc pointer HW descriptor struct
+ */
+#define HW_DESC_SET_QUEUE_LAST_IND(pDesc) 								\
+	do {												\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, QUEUE_LAST_IND, (pDesc)->word[3], 1);	\
+	} while (0)
+
+/*!
+ * This macro signs the end of HW descriptors flow by asking for completion ack, and release the HW engines
+ * 
+ * \param pDesc pointer HW descriptor struct 
+ */
+#define HW_DESC_SET_ACK_LAST(pDesc) 									\
+	do {												\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, QUEUE_LAST_IND, (pDesc)->word[3], 1);	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, ACK_NEEDED, (pDesc)->word[4], 1);	\
+	} while (0)
+
+
+#define MSB64(_addr) (sizeof(_addr) == 4 ? 0 : ((_addr) >> 32)&UINT16_MAX)
+
+/*!
+ * This macro sets the DIN field of a HW descriptors
+ * 
+ * \param pDesc pointer HW descriptor struct 
+ * \param dmaMode The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
+ * \param dinAdr DIN address
+ * \param dinSize Data size in bytes 
+ * \param axiNs AXI secure bit
+ */
+#define HW_DESC_SET_DIN_TYPE(pDesc, dmaMode, dinAdr, dinSize, axiNs)								\
+	do {		                                                                                        		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (dinAdr)&UINT32_MAX );			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DIN_ADDR_HIGH, (pDesc)->word[5], MSB64(dinAdr) );		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], (dmaMode));			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));				\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, NS_BIT, (pDesc)->word[1], (axiNs));				\
+	} while (0)
+
+
+/*!
+ * This macro sets the DIN field of a HW descriptors to NO DMA mode. Used for NOP descriptor, register patches and 
+ * other special modes 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param dinAdr DIN address
+ * \param dinSize Data size in bytes 
+ */
+#define HW_DESC_SET_DIN_NO_DMA(pDesc, dinAdr, dinSize)									\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(dinAdr));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
+	} while (0)
+
+/*!
+ * This macro sets the DIN field of a HW descriptors to SRAM mode. 
+ * Note: No need to check SRAM alignment since host requests do not use SRAM and 
+ * adaptor will enforce alignment check. 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param dinAdr DIN address
+ * \param dinSize Data size in bytes 
+ */
+#define HW_DESC_SET_DIN_SRAM(pDesc, dinAdr, dinSize)									\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(dinAdr));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], DMA_SRAM);		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
+	} while (0)
+
+/*! This macro sets the DIN field of a HW descriptors to CONST mode 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param val DIN const value
+ * \param dinSize Data size in bytes 
+ */
+#define HW_DESC_SET_DIN_CONST(pDesc, val, dinSize)									\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0, VALUE, (pDesc)->word[0], (uint32_t)(val));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_CONST_VALUE, (pDesc)->word[1], 1);			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_DMA_MODE, (pDesc)->word[1], DMA_SRAM);		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, DIN_SIZE, (pDesc)->word[1], (dinSize));			\
+	} while (0)
+
+/*!
+ * This macro sets the DIN not last input data indicator
+ * 
+ * \param pDesc pointer HW descriptor struct
+ */
+#define HW_DESC_SET_DIN_NOT_LAST_INDICATION(pDesc)									\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD1, NOT_LAST, (pDesc)->word[1], 1);				\
+	} while (0)
+
+/*!
+ * This macro sets the DOUT field of a HW descriptors 
+ * 
+ * \param pDesc pointer HW descriptor struct 
+ * \param dmaMode The DMA mode: NO_DMA, SRAM, DLLI, MLLI, CONSTANT
+ * \param doutAdr DOUT address
+ * \param doutSize Data size in bytes 
+ * \param axiNs AXI secure bit
+ */
+#define HW_DESC_SET_DOUT_TYPE(pDesc, dmaMode, doutAdr, doutSize, axiNs)							\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], (dmaMode));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));			\
+	} while (0)
+
+/*!
+ * This macro sets the DOUT field of a HW descriptors to DLLI type 
+ * The LAST INDICATION is provided by the user 
+ * 
+ * \param pDesc pointer HW descriptor struct 
+ * \param doutAdr DOUT address
+ * \param doutSize Data size in bytes 
+ * \param lastInd The last indication bit
+ * \param axiNs AXI secure bit 
+ */
+#define HW_DESC_SET_DOUT_DLLI(pDesc, doutAdr, doutSize, axiNs ,lastInd)								\
+	do {		                                                                                        		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_DLLI);			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], lastInd);			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));				\
+	} while (0)
+
+/*!
+ * This macro sets the DOUT field of a HW descriptors to DLLI type 
+ * The LAST INDICATION is provided by the user 
+ * 
+ * \param pDesc pointer HW descriptor struct 
+ * \param doutAdr DOUT address
+ * \param doutSize Data size in bytes 
+ * \param lastInd The last indication bit
+ * \param axiNs AXI secure bit 
+ */
+#define HW_DESC_SET_DOUT_MLLI(pDesc, doutAdr, doutSize, axiNs ,lastInd)								\
+	do {		                                                                                        		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (doutAdr)&UINT32_MAX );		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD5, DOUT_ADDR_HIGH, (pDesc)->word[5], MSB64(doutAdr) );	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_MLLI);			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], lastInd);			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, NS_BIT, (pDesc)->word[3], (axiNs));				\
+	} while (0)
+
+/*!
+ * This macro sets the DOUT field of a HW descriptors to NO DMA mode. Used for NOP descriptor, register patches and 
+ * other special modes 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param doutAdr DOUT address
+ * \param doutSize Data size in bytes  
+ * \param registerWriteEnable Enables a write operation to a register
+ */
+#define HW_DESC_SET_DOUT_NO_DMA(pDesc, doutAdr, doutSize, registerWriteEnable)							\
+	do {		                                                                                        		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(doutAdr));			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));			\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_LAST_IND, (pDesc)->word[3], (registerWriteEnable));	\
+	} while (0)
+
+/*!
+ * This macro sets the word for the XOR operation. 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param xorVal xor data value
+ */
+#define HW_DESC_SET_XOR_VAL(pDesc, xorVal)										\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(xorVal));		\
+	} while (0)
+
+/*!
+ * This macro sets the XOR indicator bit in the descriptor
+ * 
+ * \param pDesc pointer HW descriptor struct
+ */
+#define HW_DESC_SET_XOR_ACTIVE(pDesc)											\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, HASH_XOR_BIT, (pDesc)->word[3], 1);			\
+	} while (0)
+
+/*!
+ * This macro selects the AES engine instead of HASH engine when setting up combined mode with AES XCBC MAC
+ * 
+ * \param pDesc pointer HW descriptor struct
+ */
+#define HW_DESC_SET_AES_NOT_HASH_MODE(pDesc)										\
+	do {		                                                                                       	 	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, AES_SEL_N_HASH, (pDesc)->word[4], 1);			\
+	} while (0)
+
+/*!
+ * This macro sets the DOUT field of a HW descriptors to SRAM mode
+ * Note: No need to check SRAM alignment since host requests do not use SRAM and 
+ * adaptor will enforce alignment check. 
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param doutAdr DOUT address
+ * \param doutSize Data size in bytes 
+ */
+#define HW_DESC_SET_DOUT_SRAM(pDesc, doutAdr, doutSize)									\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(doutAdr));		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_DMA_MODE, (pDesc)->word[3], DMA_SRAM);		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD3, DOUT_SIZE, (pDesc)->word[3], (doutSize));		\
+	} while (0)
+
+
+/*!
+ * This macro sets the data unit size for XEX mode in data_out_addr[15:0]
+ * 
+ * \param pDesc pointer HW descriptor struct
+ * \param dataUnitSize data unit size for XEX mode
+ */
+#define HW_DESC_SET_XEX_DATA_UNIT_SIZE(pDesc, dataUnitSize)								\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(dataUnitSize));	\
+	} while (0)
+
+/*!
+ * This macro sets the number of rounds for Multi2 in data_out_addr[15:0]
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param numRounds number of rounds for Multi2
+*/
+#define HW_DESC_SET_MULTI2_NUM_ROUNDS(pDesc, numRounds)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD2, VALUE, (pDesc)->word[2], (uint32_t)(numRounds));	\
+	} while (0)
+
+/*!
+ * This macro sets the flow mode.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param flowMode Any one of the modes defined in [CC7x-DESC]
+*/
+
+#define HW_DESC_SET_FLOW_MODE(pDesc, flowMode)										\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, DATA_FLOW_MODE, (pDesc)->word[4], (flowMode));		\
+	} while (0)
+
+/*!
+ * This macro sets the cipher mode.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param cipherMode Any one of the modes defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_CIPHER_MODE(pDesc, cipherMode)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_MODE, (pDesc)->word[4], (cipherMode));		\
+	} while (0)
+
+/*!
+ * This macro sets the cipher configuration fields.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param cipherConfig Any one of the modes defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_CIPHER_CONFIG0(pDesc, cipherConfig)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF0, (pDesc)->word[4], (cipherConfig));	\
+	} while (0)
+
+/*!
+ * This macro sets the cipher configuration fields.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param cipherConfig Any one of the modes defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_CIPHER_CONFIG1(pDesc, cipherConfig)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF1, (pDesc)->word[4], (cipherConfig));	\
+	} while (0)
+
+/*!
+ * This macro sets HW key configuration fields.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param hwKey The hw key number as in enun HwCryptoKey
+*/
+#define HW_DESC_SET_HW_CRYPTO_KEY(pDesc, hwKey)										\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_DO, (pDesc)->word[4], (hwKey)&HW_KEY_MASK_CIPHER_DO);		\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_CONF2, (pDesc)->word[4], (hwKey>>HW_KEY_SHIFT_CIPHER_CFG2));	\
+	} while (0)
+
+/*!
+ * This macro changes the bytes order of all setup-finalize descriptosets.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param swapConfig Any one of the modes defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_BYTES_SWAP(pDesc, swapConfig)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, BYTES_SWAP, (pDesc)->word[4], (swapConfig));		\
+	} while (0)
+
+/*!
+ * This macro sets the CMAC_SIZE0 mode.
+ *
+ * \param pDesc pointer HW descriptor struct
+*/
+#define HW_DESC_SET_CMAC_SIZE0_MODE(pDesc)										\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CMAC_SIZE0, (pDesc)->word[4], 0x1);			\
+	} while (0)
+
+/*!
+ * This macro sets the key size for AES engine.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param keySize key size in bytes (NOT size code)
+*/
+#define HW_DESC_SET_KEY_SIZE_AES(pDesc, keySize)									\
+	do {													        \
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, KEY_SIZE, (pDesc)->word[4], ((keySize) >> 3) - 2);	\
+	} while (0)
+
+/*!
+ * This macro sets the key size for DES engine.
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param keySize key size in bytes (NOT size code)
+*/
+#define HW_DESC_SET_KEY_SIZE_DES(pDesc, keySize)									\
+	do {													        \
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, KEY_SIZE, (pDesc)->word[4], ((keySize) >> 3) - 1);	\
+	} while (0)
+
+/*!
+ * This macro sets the descriptor's setup mode
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param setupMode Any one of the setup modes defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_SETUP_MODE(pDesc, setupMode)									\
+	do {														\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, SETUP_OPERATION, (pDesc)->word[4], (setupMode));	\
+	} while (0)
+
+/*!
+ * This macro sets the descriptor's cipher do
+ *
+ * \param pDesc pointer HW descriptor struct
+ * \param cipherDo Any one of the cipher do defined in [CC7x-DESC]
+*/
+#define HW_DESC_SET_CIPHER_DO(pDesc, cipherDo)											\
+	do {															\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_QUEUE_WORD4, CIPHER_DO, (pDesc)->word[4], (cipherDo)&HW_KEY_MASK_CIPHER_DO);	\
+	} while (0)
+
+/*!
+ * This macro sets the DIN field of a HW descriptors to star/stop monitor descriptor. 
+ * Used for performance measurements and debug purposes.
+ * 
+ * \param pDesc pointer HW descriptor struct
+ */
+#define HW_DESC_SET_DIN_MONITOR_CNTR(pDesc)										\
+	do {		                                                                                        	\
+		CC_REG_FLD_SET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR, VALUE, (pDesc)->word[1], _HW_DESC_MONITOR_KICK);	\
+	} while (0)
+
+
+
+#endif /*__CC_HW_QUEUE_DEFS_H__*/
diff --git a/drivers/staging/ccree/cc_lli_defs.h b/drivers/staging/ccree/cc_lli_defs.h
new file mode 100644
index 000000000000..697f1ed181e0
--- /dev/null
+++ b/drivers/staging/ccree/cc_lli_defs.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#ifndef _CC_LLI_DEFS_H_
+#define _CC_LLI_DEFS_H_
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#endif
+#include "cc_bitops.h"
+
+/* Max DLLI size */
+#define DLLI_SIZE_BIT_SIZE	0x18	// DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE
+
+#define CC_MAX_MLLI_ENTRY_SIZE 0x10000
+
+#define MSB64(_addr) (sizeof(_addr) == 4 ? 0 : ((_addr) >> 32)&UINT16_MAX)
+
+#define LLI_SET_ADDR(lli_p, addr) \
+		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD0_OFFSET], LLI_LADDR_BIT_OFFSET, LLI_LADDR_BIT_SIZE, (addr & UINT32_MAX)); \
+		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD1_OFFSET], LLI_HADDR_BIT_OFFSET, LLI_HADDR_BIT_SIZE, MSB64(addr));
+
+#define LLI_SET_SIZE(lli_p, size) \
+		BITFIELD_SET(((uint32_t *)(lli_p))[LLI_WORD1_OFFSET], LLI_SIZE_BIT_OFFSET, LLI_SIZE_BIT_SIZE, size)
+
+/* Size of entry */
+#define LLI_ENTRY_WORD_SIZE 2
+#define LLI_ENTRY_BYTE_SIZE (LLI_ENTRY_WORD_SIZE * sizeof(uint32_t))
+
+/* Word0[31:0] = ADDR[31:0] */
+#define LLI_WORD0_OFFSET 0
+#define LLI_LADDR_BIT_OFFSET 0
+#define LLI_LADDR_BIT_SIZE 32
+/* Word1[31:16] = ADDR[47:32]; Word1[15:0] = SIZE */
+#define LLI_WORD1_OFFSET 1
+#define LLI_SIZE_BIT_OFFSET 0
+#define LLI_SIZE_BIT_SIZE 16
+#define LLI_HADDR_BIT_OFFSET 16
+#define LLI_HADDR_BIT_SIZE 16
+
+
+#endif /*_CC_LLI_DEFS_H_*/
diff --git a/drivers/staging/ccree/cc_pal_log.h b/drivers/staging/ccree/cc_pal_log.h
new file mode 100644
index 000000000000..e5f5a8737833
--- /dev/null
+++ b/drivers/staging/ccree/cc_pal_log.h
@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _CC_PAL_LOG_H_
+#define _CC_PAL_LOG_H_
+
+#include "cc_pal_types.h"
+#include "cc_pal_log_plat.h"
+
+/*!
+@file 
+@brief This file contains the PAL layer log definitions, by default the log is disabled. 
+@defgroup cc_pal_log CryptoCell PAL logging APIs and definitions
+@{
+@ingroup cc_pal
+*/
+
+/* PAL log levels (to be used in CC_PAL_logLevel) */
+/*! PAL log level - disabled. */
+#define CC_PAL_LOG_LEVEL_NULL      (-1) /*!< \internal Disable logging */
+/*! PAL log level - error. */
+#define CC_PAL_LOG_LEVEL_ERR       0
+/*! PAL log level - warning. */
+#define CC_PAL_LOG_LEVEL_WARN      1
+/*! PAL log level - info. */
+#define CC_PAL_LOG_LEVEL_INFO      2
+/*! PAL log level - debug. */
+#define CC_PAL_LOG_LEVEL_DEBUG     3
+/*! PAL log level - trace. */
+#define CC_PAL_LOG_LEVEL_TRACE     4
+/*! PAL log level - data. */
+#define CC_PAL_LOG_LEVEL_DATA      5
+
+#ifndef CC_PAL_LOG_CUR_COMPONENT
+/* Setting default component mask in case caller did not define */
+/* (a mask that is always on for every log mask value but full masking) */
+/*! Default log debugged component.*/
+#define CC_PAL_LOG_CUR_COMPONENT 0xFFFFFFFF
+#endif
+#ifndef CC_PAL_LOG_CUR_COMPONENT_NAME
+/*! Default log debugged component.*/
+#define CC_PAL_LOG_CUR_COMPONENT_NAME "CC"
+#endif
+
+/* Select compile time log level (default if not explicitly specified by caller) */
+#ifndef CC_PAL_MAX_LOG_LEVEL /* Can be overriden by external definition of this constant */
+#ifdef DEBUG
+/*! Default debug log level (when debug is set to on).*/
+#define CC_PAL_MAX_LOG_LEVEL  CC_PAL_LOG_LEVEL_ERR /*CC_PAL_LOG_LEVEL_DEBUG*/
+#else /* Disable logging */
+/*! Default debug log level (when debug is set to on).*/
+#define CC_PAL_MAX_LOG_LEVEL CC_PAL_LOG_LEVEL_NULL
+#endif
+#endif /*CC_PAL_MAX_LOG_LEVEL*/
+/*! Evaluate CC_PAL_MAX_LOG_LEVEL in case provided by caller */
+#define __CC_PAL_LOG_LEVEL_EVAL(level) level
+/*! Maximal log level defintion.*/
+#define _CC_PAL_MAX_LOG_LEVEL __CC_PAL_LOG_LEVEL_EVAL(CC_PAL_MAX_LOG_LEVEL)
+
+
+#ifdef ARM_DSM
+/*! Log init function. */
+#define CC_PalLogInit() do {} while (0)
+/*! Log set level function - sets the level of logging in case of debug. */
+#define CC_PalLogLevelSet(setLevel) do {} while (0)
+/*! Log set mask function - sets the component masking in case of debug. */
+#define CC_PalLogMaskSet(setMask) do {} while (0)
+#else
+#if _CC_PAL_MAX_LOG_LEVEL > CC_PAL_LOG_LEVEL_NULL
+/*! Log init function. */
+void CC_PalLogInit(void);
+/*! Log set level function - sets the level of logging in case of debug. */
+void CC_PalLogLevelSet(int setLevel);
+/*! Log set mask function - sets the component masking in case of debug. */
+void CC_PalLogMaskSet(uint32_t setMask);
+/*! Global variable for log level */
+extern int CC_PAL_logLevel;
+/*! Global variable for log mask */
+extern uint32_t CC_PAL_logMask;
+#else /* No log */
+/*! Log init function. */
+static inline void CC_PalLogInit(void) {}
+/*! Log set level function - sets the level of logging in case of debug. */
+static inline void CC_PalLogLevelSet(int setLevel) {CC_UNUSED_PARAM(setLevel);}
+/*! Log set mask function - sets the component masking in case of debug. */
+static inline void CC_PalLogMaskSet(uint32_t setMask) {CC_UNUSED_PARAM(setMask);}
+#endif
+#endif
+
+/*! Filter logging based on logMask and dispatch to platform specific logging mechanism. */
+#define _CC_PAL_LOG(level, format, ...)  \
+	if (CC_PAL_logMask & CC_PAL_LOG_CUR_COMPONENT) \
+		__CC_PAL_LOG_PLAT(CC_PAL_LOG_LEVEL_ ## level, "%s:%s: " format, CC_PAL_LOG_CUR_COMPONENT_NAME, __func__, ##__VA_ARGS__)
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_ERR)
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_ERR(format, ... ) \
+	_CC_PAL_LOG(ERR, format, ##__VA_ARGS__)
+#else
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_ERR( ... ) do {} while (0)
+#endif
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_WARN)
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_WARN(format, ... ) \
+	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_WARN) \
+		_CC_PAL_LOG(WARN, format, ##__VA_ARGS__)
+#else
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_WARN( ... ) do {} while (0)
+#endif
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_INFO)
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_INFO(format, ... ) \
+	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_INFO) \
+		_CC_PAL_LOG(INFO, format, ##__VA_ARGS__)
+#else
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_INFO( ... ) do {} while (0)
+#endif
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_DEBUG)
+/*! Log messages according to log level.*/
+#define CC_PAL_LOG_DEBUG(format, ... ) \
+	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_DEBUG) \
+		_CC_PAL_LOG(DEBUG, format, ##__VA_ARGS__)
+
+/*! Log message buffer.*/
+#define CC_PAL_LOG_DUMP_BUF(msg, buf, size)		\
+	do {						\
+	int i;						\
+	uint8_t	*pData = (uint8_t*)buf;			\
+							\
+	PRINTF("%s (%d):\n", msg, size);		\
+	for (i = 0; i < size; i++) {			\
+		PRINTF("0x%02X ", pData[i]);		\
+		if ((i & 0xF) == 0xF) {			\
+			PRINTF("\n");			\
+		}					\
+	}						\
+	PRINTF("\n");					\
+	} while (0)
+#else
+/*! Log debug messages.*/
+#define CC_PAL_LOG_DEBUG( ... ) do {} while (0)
+/*! Log debug buffer.*/
+#define CC_PAL_LOG_DUMP_BUF(msg, buf, size)	do {} while (0)
+#endif
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE)
+/*! Log debug trace.*/
+#define CC_PAL_LOG_TRACE(format, ... ) \
+	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_TRACE) \
+		_CC_PAL_LOG(TRACE, format, ##__VA_ARGS__)
+#else
+/*! Log debug trace.*/
+#define CC_PAL_LOG_TRACE(...) do {} while (0)
+#endif
+
+#if (_CC_PAL_MAX_LOG_LEVEL >= CC_PAL_LOG_LEVEL_TRACE)
+/*! Log debug data.*/
+#define CC_PAL_LOG_DATA(format, ...) \
+	if (CC_PAL_logLevel >= CC_PAL_LOG_LEVEL_TRACE) \
+		_CC_PAL_LOG(DATA, format, ##__VA_ARGS__)
+#else
+/*! Log debug data.*/
+#define CC_PAL_LOG_DATA( ...) do {} while (0)
+#endif
+/** 
+@}
+ */
+
+#endif /*_CC_PAL_LOG_H_*/
diff --git a/drivers/staging/ccree/cc_pal_log_plat.h b/drivers/staging/ccree/cc_pal_log_plat.h
new file mode 100644
index 000000000000..a05a200cf6eb
--- /dev/null
+++ b/drivers/staging/ccree/cc_pal_log_plat.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* Dummy pal_log_plat for test driver in kernel */
+
+#ifndef _SSI_PAL_LOG_PLAT_H_
+#define _SSI_PAL_LOG_PLAT_H_
+
+#if defined(DEBUG)
+
+#define __CC_PAL_LOG_PLAT(level, format, ...) printk(level "cc7x_test::" format , ##__VA_ARGS__)
+
+#else /* Disable all prints */
+
+#define __CC_PAL_LOG_PLAT(...)  do {} while (0)
+
+#endif
+
+#endif /*_SASI_PAL_LOG_PLAT_H_*/
+
diff --git a/drivers/staging/ccree/cc_pal_types.h b/drivers/staging/ccree/cc_pal_types.h
new file mode 100644
index 000000000000..9b59bbb34515
--- /dev/null
+++ b/drivers/staging/ccree/cc_pal_types.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef CC_PAL_TYPES_H
+#define CC_PAL_TYPES_H
+
+/*! 
+@file 
+@brief This file contains platform-dependent definitions and types. 
+@defgroup cc_pal_types CryptoCell PAL platform dependant types
+@{
+@ingroup cc_pal
+
+*/
+ 
+#include "cc_pal_types_plat.h"
+
+/*! Boolean definition.*/
+typedef enum {
+	/*! Boolean false definition.*/
+	CC_FALSE = 0,
+	/*! Boolean true definition.*/
+	CC_TRUE = 1
+} CCBool;
+
+/*! Success definition. */
+#define CC_SUCCESS              0UL
+/*! Failure definition. */
+#define CC_FAIL		  	1UL
+
+/*! Defintion of 1KB in bytes. */
+#define CC_1K_SIZE_IN_BYTES	1024
+/*! Defintion of number of bits in a byte. */
+#define CC_BITS_IN_BYTE		8
+/*! Defintion of number of bits in a 32bits word. */
+#define CC_BITS_IN_32BIT_WORD	32
+/*! Defintion of number of bytes in a 32bits word. */
+#define CC_32BIT_WORD_SIZE	(sizeof(uint32_t))
+
+/*! Success (OK) defintion. */
+#define CC_OK   0
+
+/*! Macro that handles unused parameters in the code (to avoid compilation warnings).  */
+#define CC_UNUSED_PARAM(prm)  ((void)prm)
+
+/*! Maximal uint32 value.*/
+#define CC_MAX_UINT32_VAL 	(0xFFFFFFFF)
+
+
+/* Minimum and Maximum macros */
+#ifdef  min
+/*! Definition for minimum. */
+#define CC_MIN(a,b) min( a , b )
+#else
+/*! Definition for minimum. */
+#define CC_MIN( a , b ) ( ( (a) < (b) ) ? (a) : (b) )
+#endif
+
+#ifdef max    
+/*! Definition for maximum. */    
+#define CC_MAX(a,b) max( a , b )
+#else
+/*! Definition for maximum. */    
+#define CC_MAX( a , b ) ( ( (a) > (b) ) ? (a) : (b) )
+#endif
+
+/*! Macro that calculates number of full bytes from bits (i.e. 7 bits are 1 byte). */    
+#define CALC_FULL_BYTES(numBits) 		((numBits)/CC_BITS_IN_BYTE + (((numBits) & (CC_BITS_IN_BYTE-1)) > 0)) 
+/*! Macro that calculates number of full 32bits words from bits (i.e. 31 bits are 1 word). */    
+#define CALC_FULL_32BIT_WORDS(numBits) 		((numBits)/CC_BITS_IN_32BIT_WORD +  (((numBits) & (CC_BITS_IN_32BIT_WORD-1)) > 0))   
+/*! Macro that calculates number of full 32bits words from bytes (i.e. 3 bytes are 1 word). */    
+#define CALC_32BIT_WORDS_FROM_BYTES(sizeBytes)  ((sizeBytes)/CC_32BIT_WORD_SIZE + (((sizeBytes) & (CC_32BIT_WORD_SIZE-1)) > 0)) 
+/*! Macro that round up bits to 32bits words. */     
+#define ROUNDUP_BITS_TO_32BIT_WORD(numBits) 	(CALC_FULL_32BIT_WORDS(numBits) * CC_BITS_IN_32BIT_WORD)
+/*! Macro that round up bits to bytes. */    
+#define ROUNDUP_BITS_TO_BYTES(numBits) 		(CALC_FULL_BYTES(numBits) * CC_BITS_IN_BYTE)
+/*! Macro that round up bytes to 32bits words. */    
+#define ROUNDUP_BYTES_TO_32BIT_WORD(sizeBytes) 	(CALC_32BIT_WORDS_FROM_BYTES(sizeBytes) * CC_32BIT_WORD_SIZE)     
+
+
+/** 
+@}
+ */
+#endif
diff --git a/drivers/staging/ccree/cc_pal_types_plat.h b/drivers/staging/ccree/cc_pal_types_plat.h
new file mode 100644
index 000000000000..6e4211262231
--- /dev/null
+++ b/drivers/staging/ccree/cc_pal_types_plat.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+ 
+#ifndef SSI_PAL_TYPES_PLAT_H
+#define SSI_PAL_TYPES_PLAT_H
+/* Linux kernel types */
+
+#include <linux/types.h>
+
+#ifndef NULL /* Missing in Linux kernel */
+#define NULL (0x0L)
+#endif
+
+
+#endif /*SSI_PAL_TYPES_PLAT_H*/
diff --git a/drivers/staging/ccree/cc_regs.h b/drivers/staging/ccree/cc_regs.h
new file mode 100644
index 000000000000..963f8148cd28
--- /dev/null
+++ b/drivers/staging/ccree/cc_regs.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+/*!
+ * @file 
+ * @brief This file contains macro definitions for accessing ARM TrustZone CryptoCell register space.
+ */
+
+#ifndef _CC_REGS_H_
+#define _CC_REGS_H_
+
+#include "cc_bitops.h"
+
+/* Register Offset macro */
+#define CC_REG_OFFSET(unit_name, reg_name)               \
+	(DX_BASE_ ## unit_name + DX_ ## reg_name ## _REG_OFFSET)
+
+#define CC_REG_BIT_SHIFT(reg_name, field_name)               \
+	(DX_ ## reg_name ## _ ## field_name ## _BIT_SHIFT)
+
+/* Register Offset macros (from registers base address in host) */
+#include "dx_reg_base_host.h"
+
+/* Read-Modify-Write a field of a register */
+#define MODIFY_REGISTER_FLD(unitName, regName, fldName, fldVal)         \
+do {								            \
+	uint32_t regVal;						    \
+	regVal = READ_REGISTER(CC_REG_ADDR(unitName, regName));       \
+	CC_REG_FLD_SET(unitName, regName, fldName, regVal, fldVal); \
+	WRITE_REGISTER(CC_REG_ADDR(unitName, regName), regVal);       \
+} while (0)
+
+/* Registers address macros for ENV registers (development FPGA only) */
+#ifdef DX_BASE_ENV_REGS
+
+/* This offset should be added to mapping address of DX_BASE_ENV_REGS */
+#define CC_ENV_REG_OFFSET(reg_name) (DX_ENV_ ## reg_name ## _REG_OFFSET)
+
+#endif /*DX_BASE_ENV_REGS*/
+
+/*! Bit fields get */
+#define CC_REG_FLD_GET(unit_name, reg_name, fld_name, reg_val)	      \
+	(DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20 ?	      \
+	reg_val /*!< \internal Optimization for 32b fields */ :			      \
+	BITFIELD_GET(reg_val, DX_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT, \
+		     DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE))
+
+/*! Bit fields access */
+#define CC_REG_FLD_GET2(unit_name, reg_name, fld_name, reg_val)	      \
+	(CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20 ?	      \
+	reg_val /*!< \internal Optimization for 32b fields */ :			      \
+	BITFIELD_GET(reg_val, CC_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT, \
+		     CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE))
+
+/* yael TBD !!! -       				      * 
+* all HW includes should start with CC_ and not DX_ !!	      */
+
+
+/*! Bit fields set */
+#define CC_REG_FLD_SET(                                               \
+	unit_name, reg_name, fld_name, reg_shadow_var, new_fld_val)      \
+do {                                                                     \
+	if (DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20)       \
+		reg_shadow_var = new_fld_val; /*!< \internal Optimization for 32b fields */\
+	else                                                             \
+		BITFIELD_SET(reg_shadow_var,                             \
+			DX_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT,  \
+			DX_ ## reg_name ## _ ## fld_name ## _BIT_SIZE,   \
+			new_fld_val);                                    \
+} while (0)
+
+/*! Bit fields set */
+#define CC_REG_FLD_SET2(                                               \
+	unit_name, reg_name, fld_name, reg_shadow_var, new_fld_val)      \
+do {                                                                     \
+	if (CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE == 0x20)       \
+		reg_shadow_var = new_fld_val; /*!< \internal Optimization for 32b fields */\
+	else                                                             \
+		BITFIELD_SET(reg_shadow_var,                             \
+			CC_ ## reg_name ## _ ## fld_name ## _BIT_SHIFT,  \
+			CC_ ## reg_name ## _ ## fld_name ## _BIT_SIZE,   \
+			new_fld_val);                                    \
+} while (0)
+
+/* Usage example:
+   uint32_t reg_shadow = READ_REGISTER(CC_REG_ADDR(CRY_KERNEL,AES_CONTROL));
+   CC_REG_FLD_SET(CRY_KERNEL,AES_CONTROL,NK_KEY0,reg_shadow, 3);
+   CC_REG_FLD_SET(CRY_KERNEL,AES_CONTROL,NK_KEY1,reg_shadow, 1);
+   WRITE_REGISTER(CC_REG_ADDR(CRY_KERNEL,AES_CONTROL), reg_shadow);
+ */
+
+#endif /*_CC_REGS_H_*/
diff --git a/drivers/staging/ccree/dx_crys_kernel.h b/drivers/staging/ccree/dx_crys_kernel.h
new file mode 100644
index 000000000000..703469c4a828
--- /dev/null
+++ b/drivers/staging/ccree/dx_crys_kernel.h
@@ -0,0 +1,180 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DX_CRYS_KERNEL_H__
+#define __DX_CRYS_KERNEL_H__
+
+// --------------------------------------
+// BLOCK: DSCRPTR
+// --------------------------------------
+#define DX_DSCRPTR_COMPLETION_COUNTER_REG_OFFSET 	0xE00UL 
+#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_COMPLETION_COUNTER_BIT_SIZE 	0x6UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SHIFT 	0x6UL
+#define DX_DSCRPTR_COMPLETION_COUNTER_OVERFLOW_COUNTER_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_SW_RESET_REG_OFFSET 	0xE40UL 
+#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_SW_RESET_VALUE_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_REG_OFFSET 	0xE60UL 
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_NUM_OF_DSCRPTR_BIT_SIZE 	0xAUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SHIFT 	0xAUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_DSCRPTR_SRAM_SIZE_BIT_SIZE 	0xCUL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SHIFT 	0x16UL
+#define DX_DSCRPTR_QUEUE_SRAM_SIZE_SRAM_SIZE_BIT_SIZE 	0x3UL
+#define DX_DSCRPTR_SINGLE_ADDR_EN_REG_OFFSET 	0xE64UL 
+#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_SINGLE_ADDR_EN_VALUE_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_MEASURE_CNTR_REG_OFFSET 	0xE68UL 
+#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_MEASURE_CNTR_VALUE_BIT_SIZE 	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD0_REG_OFFSET 	0xE80UL 
+#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD0_VALUE_BIT_SIZE 	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD1_REG_OFFSET 	0xE84UL 
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_DMA_MODE_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SHIFT 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_SIZE_BIT_SIZE 	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SHIFT 	0x1AUL
+#define DX_DSCRPTR_QUEUE_WORD1_NS_BIT_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SHIFT 	0x1BUL
+#define DX_DSCRPTR_QUEUE_WORD1_DIN_CONST_VALUE_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SHIFT 	0x1CUL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_LAST_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SHIFT 	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD1_LOCK_QUEUE_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SHIFT 	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD1_NOT_USED_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD2_REG_OFFSET 	0xE88UL 
+#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD2_VALUE_BIT_SIZE 	0x20UL
+#define DX_DSCRPTR_QUEUE_WORD3_REG_OFFSET 	0xE8CUL 
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_DMA_MODE_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SHIFT 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_SIZE_BIT_SIZE 	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SHIFT 	0x1AUL
+#define DX_DSCRPTR_QUEUE_WORD3_NS_BIT_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SHIFT 	0x1BUL
+#define DX_DSCRPTR_QUEUE_WORD3_DOUT_LAST_IND_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SHIFT 	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD3_HASH_XOR_BIT_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SHIFT 	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD3_NOT_USED_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SHIFT 	0x1FUL
+#define DX_DSCRPTR_QUEUE_WORD3_QUEUE_LAST_IND_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_REG_OFFSET 	0xE90UL 
+#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD4_DATA_FLOW_MODE_BIT_SIZE 	0x6UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SHIFT 	0x6UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_SEL_N_HASH_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SHIFT 	0x7UL
+#define DX_DSCRPTR_QUEUE_WORD4_AES_XOR_CRYPTO_KEY_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SHIFT 	0x8UL
+#define DX_DSCRPTR_QUEUE_WORD4_ACK_NEEDED_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SHIFT 	0xAUL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_MODE_BIT_SIZE 	0x4UL
+#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SHIFT 	0xEUL
+#define DX_DSCRPTR_QUEUE_WORD4_CMAC_SIZE0_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SHIFT 	0xFUL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_DO_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SHIFT 	0x11UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF0_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SHIFT 	0x13UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF1_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SHIFT 	0x14UL
+#define DX_DSCRPTR_QUEUE_WORD4_CIPHER_CONF2_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SHIFT 	0x16UL
+#define DX_DSCRPTR_QUEUE_WORD4_KEY_SIZE_BIT_SIZE 	0x2UL
+#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SHIFT 	0x18UL
+#define DX_DSCRPTR_QUEUE_WORD4_SETUP_OPERATION_BIT_SIZE 	0x4UL
+#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SHIFT 	0x1CUL
+#define DX_DSCRPTR_QUEUE_WORD4_DIN_SRAM_ENDIANNESS_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SHIFT 	0x1DUL
+#define DX_DSCRPTR_QUEUE_WORD4_DOUT_SRAM_ENDIANNESS_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SHIFT 	0x1EUL
+#define DX_DSCRPTR_QUEUE_WORD4_WORD_SWAP_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SHIFT 	0x1FUL
+#define DX_DSCRPTR_QUEUE_WORD4_BYTES_SWAP_BIT_SIZE 	0x1UL
+#define DX_DSCRPTR_QUEUE_WORD5_REG_OFFSET 	0xE94UL 
+#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WORD5_DIN_ADDR_HIGH_BIT_SIZE 	0x10UL
+#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SHIFT 	0x10UL
+#define DX_DSCRPTR_QUEUE_WORD5_DOUT_ADDR_HIGH_BIT_SIZE 	0x10UL
+#define DX_DSCRPTR_QUEUE_WATERMARK_REG_OFFSET 	0xE98UL 
+#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_WATERMARK_VALUE_BIT_SIZE 	0xAUL
+#define DX_DSCRPTR_QUEUE_CONTENT_REG_OFFSET 	0xE9CUL 
+#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_DSCRPTR_QUEUE_CONTENT_VALUE_BIT_SIZE 	0xAUL
+// --------------------------------------
+// BLOCK: AXI_P
+// --------------------------------------
+#define DX_AXIM_MON_INFLIGHT_REG_OFFSET 	0xB00UL 
+#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_AXIM_MON_INFLIGHT_VALUE_BIT_SIZE 	0x8UL
+#define DX_AXIM_MON_INFLIGHTLAST_REG_OFFSET 	0xB40UL 
+#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SHIFT 	0x0UL
+#define DX_AXIM_MON_INFLIGHTLAST_VALUE_BIT_SIZE 	0x8UL
+#define DX_AXIM_MON_COMP_REG_OFFSET 	0xB80UL 
+#define DX_AXIM_MON_COMP_VALUE_BIT_SHIFT 	0x0UL
+#define DX_AXIM_MON_COMP_VALUE_BIT_SIZE 	0x10UL
+#define DX_AXIM_MON_ERR_REG_OFFSET 	0xBC4UL 
+#define DX_AXIM_MON_ERR_BRESP_BIT_SHIFT 	0x0UL
+#define DX_AXIM_MON_ERR_BRESP_BIT_SIZE 	0x2UL
+#define DX_AXIM_MON_ERR_BID_BIT_SHIFT 	0x2UL
+#define DX_AXIM_MON_ERR_BID_BIT_SIZE 	0x4UL
+#define DX_AXIM_MON_ERR_RRESP_BIT_SHIFT 	0x10UL
+#define DX_AXIM_MON_ERR_RRESP_BIT_SIZE 	0x2UL
+#define DX_AXIM_MON_ERR_RID_BIT_SHIFT 	0x12UL
+#define DX_AXIM_MON_ERR_RID_BIT_SIZE 	0x4UL
+#define DX_AXIM_CFG_REG_OFFSET 	0xBE8UL 
+#define DX_AXIM_CFG_BRESPMASK_BIT_SHIFT 	0x4UL
+#define DX_AXIM_CFG_BRESPMASK_BIT_SIZE 	0x1UL
+#define DX_AXIM_CFG_RRESPMASK_BIT_SHIFT 	0x5UL
+#define DX_AXIM_CFG_RRESPMASK_BIT_SIZE 	0x1UL
+#define DX_AXIM_CFG_INFLTMASK_BIT_SHIFT 	0x6UL
+#define DX_AXIM_CFG_INFLTMASK_BIT_SIZE 	0x1UL
+#define DX_AXIM_CFG_COMPMASK_BIT_SHIFT 	0x7UL
+#define DX_AXIM_CFG_COMPMASK_BIT_SIZE 	0x1UL
+#define DX_AXIM_ACE_CONST_REG_OFFSET 	0xBECUL 
+#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SHIFT 	0x0UL
+#define DX_AXIM_ACE_CONST_ARDOMAIN_BIT_SIZE 	0x2UL
+#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SHIFT 	0x2UL
+#define DX_AXIM_ACE_CONST_AWDOMAIN_BIT_SIZE 	0x2UL
+#define DX_AXIM_ACE_CONST_ARBAR_BIT_SHIFT 	0x4UL
+#define DX_AXIM_ACE_CONST_ARBAR_BIT_SIZE 	0x2UL
+#define DX_AXIM_ACE_CONST_AWBAR_BIT_SHIFT 	0x6UL
+#define DX_AXIM_ACE_CONST_AWBAR_BIT_SIZE 	0x2UL
+#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SHIFT 	0x8UL
+#define DX_AXIM_ACE_CONST_ARSNOOP_BIT_SIZE 	0x4UL
+#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SHIFT 	0xCUL
+#define DX_AXIM_ACE_CONST_AWSNOOP_NOT_ALIGNED_BIT_SIZE 	0x3UL
+#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SHIFT 	0xFUL
+#define DX_AXIM_ACE_CONST_AWSNOOP_ALIGNED_BIT_SIZE 	0x3UL
+#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SHIFT 	0x12UL
+#define DX_AXIM_ACE_CONST_AWADDR_NOT_MASKED_BIT_SIZE 	0x7UL
+#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SHIFT 	0x19UL
+#define DX_AXIM_ACE_CONST_AWLEN_VAL_BIT_SIZE 	0x4UL
+#define DX_AXIM_CACHE_PARAMS_REG_OFFSET 	0xBF0UL 
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SHIFT 	0x0UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_LAST_BIT_SIZE 	0x4UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SHIFT 	0x4UL
+#define DX_AXIM_CACHE_PARAMS_AWCACHE_BIT_SIZE 	0x4UL
+#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SHIFT 	0x8UL
+#define DX_AXIM_CACHE_PARAMS_ARCACHE_BIT_SIZE 	0x4UL
+#endif	// __DX_CRYS_KERNEL_H__
diff --git a/drivers/staging/ccree/dx_env.h b/drivers/staging/ccree/dx_env.h
new file mode 100644
index 000000000000..08040604b6da
--- /dev/null
+++ b/drivers/staging/ccree/dx_env.h
@@ -0,0 +1,224 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DX_ENV_H__
+#define __DX_ENV_H__
+
+// --------------------------------------
+// BLOCK: FPGA_ENV_REGS
+// --------------------------------------
+#define DX_ENV_PKA_DEBUG_MODE_REG_OFFSET 	0x024UL 
+#define DX_ENV_PKA_DEBUG_MODE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_PKA_DEBUG_MODE_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_SCAN_MODE_REG_OFFSET 	0x030UL 
+#define DX_ENV_SCAN_MODE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SCAN_MODE_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_ALLOW_SCAN_REG_OFFSET 	0x034UL 
+#define DX_ENV_CC_ALLOW_SCAN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_ALLOW_SCAN_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_HOST_INT_REG_OFFSET 	0x0A0UL 
+#define DX_ENV_CC_HOST_INT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_HOST_INT_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_PUB_HOST_INT_REG_OFFSET 	0x0A4UL 
+#define DX_ENV_CC_PUB_HOST_INT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_PUB_HOST_INT_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_RST_N_REG_OFFSET 	0x0A8UL 
+#define DX_ENV_CC_RST_N_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_RST_N_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_RST_OVERRIDE_REG_OFFSET 	0x0ACUL 
+#define DX_ENV_RST_OVERRIDE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_RST_OVERRIDE_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_POR_N_ADDR_REG_OFFSET 	0x0E0UL 
+#define DX_ENV_CC_POR_N_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_POR_N_ADDR_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_COLD_RST_REG_OFFSET 	0x0FCUL 
+#define DX_ENV_CC_COLD_RST_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_COLD_RST_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_DUMMY_ADDR_REG_OFFSET 	0x108UL 
+#define DX_ENV_DUMMY_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_DUMMY_ADDR_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_COUNTER_CLR_REG_OFFSET 	0x118UL 
+#define DX_ENV_COUNTER_CLR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_COUNTER_CLR_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_COUNTER_RD_REG_OFFSET 	0x11CUL 
+#define DX_ENV_COUNTER_RD_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_COUNTER_RD_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_RNG_DEBUG_ENABLE_REG_OFFSET 	0x430UL 
+#define DX_ENV_RNG_DEBUG_ENABLE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_RNG_DEBUG_ENABLE_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_LCS_REG_OFFSET 	0x43CUL 
+#define DX_ENV_CC_LCS_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_LCS_VALUE_BIT_SIZE 	0x8UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_REG_OFFSET 	0x440UL 
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_CM_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_CM_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_DM_BIT_SHIFT 	0x1UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_DM_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_SECURE_BIT_SHIFT 	0x2UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_SECURE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_RMA_BIT_SHIFT 	0x3UL
+#define DX_ENV_CC_IS_CM_DM_SECURE_RMA_IS_RMA_BIT_SIZE 	0x1UL
+#define DX_ENV_DCU_EN_REG_OFFSET 	0x444UL 
+#define DX_ENV_DCU_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_DCU_EN_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_CC_LCS_IS_VALID_REG_OFFSET 	0x448UL 
+#define DX_ENV_CC_LCS_IS_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_LCS_IS_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_POWER_DOWN_REG_OFFSET 	0x478UL 
+#define DX_ENV_POWER_DOWN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_POWER_DOWN_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_DCU_H_EN_REG_OFFSET 	0x484UL 
+#define DX_ENV_DCU_H_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_DCU_H_EN_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_VERSION_REG_OFFSET 	0x488UL 
+#define DX_ENV_VERSION_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_VERSION_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_ROSC_WRITE_REG_OFFSET 	0x48CUL 
+#define DX_ENV_ROSC_WRITE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_ROSC_WRITE_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_ROSC_ADDR_REG_OFFSET 	0x490UL 
+#define DX_ENV_ROSC_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_ROSC_ADDR_VALUE_BIT_SIZE 	0x8UL
+#define DX_ENV_RESET_SESSION_KEY_REG_OFFSET 	0x494UL 
+#define DX_ENV_RESET_SESSION_KEY_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_RESET_SESSION_KEY_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_SESSION_KEY_0_REG_OFFSET 	0x4A0UL 
+#define DX_ENV_SESSION_KEY_0_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SESSION_KEY_0_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_SESSION_KEY_1_REG_OFFSET 	0x4A4UL 
+#define DX_ENV_SESSION_KEY_1_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SESSION_KEY_1_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_SESSION_KEY_2_REG_OFFSET 	0x4A8UL 
+#define DX_ENV_SESSION_KEY_2_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SESSION_KEY_2_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_SESSION_KEY_3_REG_OFFSET 	0x4ACUL 
+#define DX_ENV_SESSION_KEY_3_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SESSION_KEY_3_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_SESSION_KEY_VALID_REG_OFFSET 	0x4B0UL 
+#define DX_ENV_SESSION_KEY_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SESSION_KEY_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_SPIDEN_REG_OFFSET 	0x4D0UL 
+#define DX_ENV_SPIDEN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_SPIDEN_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_AXIM_USER_PARAMS_REG_OFFSET 	0x600UL 
+#define DX_ENV_AXIM_USER_PARAMS_ARUSER_BIT_SHIFT 	0x0UL
+#define DX_ENV_AXIM_USER_PARAMS_ARUSER_BIT_SIZE 	0x5UL
+#define DX_ENV_AXIM_USER_PARAMS_AWUSER_BIT_SHIFT 	0x5UL
+#define DX_ENV_AXIM_USER_PARAMS_AWUSER_BIT_SIZE 	0x5UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_REG_OFFSET 	0x604UL 
+#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_BIT_BIT_SHIFT 	0x0UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_BIT_BIT_SIZE 	0x1UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_OVERRIDE_BIT_SHIFT 	0x1UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_AWPROT_NS_OVERRIDE_BIT_SIZE 	0x1UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_BIT_BIT_SHIFT 	0x2UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_BIT_BIT_SIZE 	0x1UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_OVERRIDE_BIT_SHIFT 	0x3UL
+#define DX_ENV_SECURITY_MODE_OVERRIDE_ARPROT_NS_OVERRIDE_BIT_SIZE 	0x1UL
+#define DX_ENV_AO_CC_KPLT_0_REG_OFFSET 	0x620UL 
+#define DX_ENV_AO_CC_KPLT_0_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KPLT_0_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KPLT_1_REG_OFFSET 	0x624UL 
+#define DX_ENV_AO_CC_KPLT_1_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KPLT_1_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KPLT_2_REG_OFFSET 	0x628UL 
+#define DX_ENV_AO_CC_KPLT_2_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KPLT_2_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KPLT_3_REG_OFFSET 	0x62CUL 
+#define DX_ENV_AO_CC_KPLT_3_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KPLT_3_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KCST_0_REG_OFFSET 	0x630UL 
+#define DX_ENV_AO_CC_KCST_0_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KCST_0_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KCST_1_REG_OFFSET 	0x634UL 
+#define DX_ENV_AO_CC_KCST_1_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KCST_1_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KCST_2_REG_OFFSET 	0x638UL 
+#define DX_ENV_AO_CC_KCST_2_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KCST_2_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_AO_CC_KCST_3_REG_OFFSET 	0x63CUL 
+#define DX_ENV_AO_CC_KCST_3_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_AO_CC_KCST_3_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APB_FIPS_ADDR_REG_OFFSET 	0x650UL 
+#define DX_ENV_APB_FIPS_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_ADDR_VALUE_BIT_SIZE 	0xCUL
+#define DX_ENV_APB_FIPS_VAL_REG_OFFSET 	0x654UL 
+#define DX_ENV_APB_FIPS_VAL_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_VAL_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APB_FIPS_MASK_REG_OFFSET 	0x658UL 
+#define DX_ENV_APB_FIPS_MASK_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_MASK_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APB_FIPS_CNT_REG_OFFSET 	0x65CUL 
+#define DX_ENV_APB_FIPS_CNT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_CNT_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APB_FIPS_NEW_ADDR_REG_OFFSET 	0x660UL 
+#define DX_ENV_APB_FIPS_NEW_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_NEW_ADDR_VALUE_BIT_SIZE 	0xCUL
+#define DX_ENV_APB_FIPS_NEW_VAL_REG_OFFSET 	0x664UL 
+#define DX_ENV_APB_FIPS_NEW_VAL_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APB_FIPS_NEW_VAL_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APBP_FIPS_ADDR_REG_OFFSET 	0x670UL 
+#define DX_ENV_APBP_FIPS_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_ADDR_VALUE_BIT_SIZE 	0xCUL
+#define DX_ENV_APBP_FIPS_VAL_REG_OFFSET 	0x674UL 
+#define DX_ENV_APBP_FIPS_VAL_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_VAL_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APBP_FIPS_MASK_REG_OFFSET 	0x678UL 
+#define DX_ENV_APBP_FIPS_MASK_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_MASK_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APBP_FIPS_CNT_REG_OFFSET 	0x67CUL 
+#define DX_ENV_APBP_FIPS_CNT_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_CNT_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_APBP_FIPS_NEW_ADDR_REG_OFFSET 	0x680UL 
+#define DX_ENV_APBP_FIPS_NEW_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_NEW_ADDR_VALUE_BIT_SIZE 	0xCUL
+#define DX_ENV_APBP_FIPS_NEW_VAL_REG_OFFSET 	0x684UL 
+#define DX_ENV_APBP_FIPS_NEW_VAL_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_APBP_FIPS_NEW_VAL_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_CC_POWERDOWN_EN_REG_OFFSET 	0x690UL 
+#define DX_ENV_CC_POWERDOWN_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_POWERDOWN_EN_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_CC_POWERDOWN_RST_EN_REG_OFFSET 	0x694UL 
+#define DX_ENV_CC_POWERDOWN_RST_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_CC_POWERDOWN_RST_EN_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_POWERDOWN_RST_CNTR_REG_OFFSET 	0x698UL 
+#define DX_ENV_POWERDOWN_RST_CNTR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_POWERDOWN_RST_CNTR_VALUE_BIT_SIZE 	0x20UL
+#define DX_ENV_POWERDOWN_EN_DEBUG_REG_OFFSET 	0x69CUL 
+#define DX_ENV_POWERDOWN_EN_DEBUG_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_POWERDOWN_EN_DEBUG_VALUE_BIT_SIZE 	0x1UL
+// --------------------------------------
+// BLOCK: ENV_CC_MEMORIES
+// --------------------------------------
+#define DX_ENV_FUSE_READY_REG_OFFSET 	0x000UL 
+#define DX_ENV_FUSE_READY_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_FUSE_READY_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_PERF_RAM_MASTER_REG_OFFSET 	0x0ECUL 
+#define DX_ENV_PERF_RAM_MASTER_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_PERF_RAM_MASTER_VALUE_BIT_SIZE 	0x1UL
+#define DX_ENV_PERF_RAM_ADDR_HIGH4_REG_OFFSET 	0x0F0UL 
+#define DX_ENV_PERF_RAM_ADDR_HIGH4_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_PERF_RAM_ADDR_HIGH4_VALUE_BIT_SIZE 	0x2UL
+#define DX_ENV_FUSES_RAM_REG_OFFSET 	0x3ECUL 
+#define DX_ENV_FUSES_RAM_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_FUSES_RAM_VALUE_BIT_SIZE 	0x20UL
+// --------------------------------------
+// BLOCK: ENV_PERF_RAM_BASE
+// --------------------------------------
+#define DX_ENV_PERF_RAM_BASE_REG_OFFSET 	0x000UL 
+#define DX_ENV_PERF_RAM_BASE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_ENV_PERF_RAM_BASE_VALUE_BIT_SIZE 	0x20UL
+
+#endif /*__DX_ENV_H__*/
diff --git a/drivers/staging/ccree/dx_host.h b/drivers/staging/ccree/dx_host.h
new file mode 100644
index 000000000000..4e42e748dc5f
--- /dev/null
+++ b/drivers/staging/ccree/dx_host.h
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DX_HOST_H__
+#define __DX_HOST_H__
+
+// --------------------------------------
+// BLOCK: HOST_P
+// --------------------------------------
+#define DX_HOST_IRR_REG_OFFSET 	0xA00UL 
+#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SHIFT 	0x2UL
+#define DX_HOST_IRR_DSCRPTR_COMPLETION_LOW_INT_BIT_SIZE 	0x1UL
+#define DX_HOST_IRR_AXI_ERR_INT_BIT_SHIFT 	0x8UL
+#define DX_HOST_IRR_AXI_ERR_INT_BIT_SIZE 	0x1UL
+#define DX_HOST_IRR_GPR0_BIT_SHIFT 	0xBUL
+#define DX_HOST_IRR_GPR0_BIT_SIZE 	0x1UL
+#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SHIFT 	0x13UL
+#define DX_HOST_IRR_DSCRPTR_WATERMARK_INT_BIT_SIZE 	0x1UL
+#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SHIFT 	0x17UL
+#define DX_HOST_IRR_AXIM_COMP_INT_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_REG_OFFSET 	0xA04UL 
+#define DX_HOST_IMR_NOT_USED_MASK_BIT_SHIFT 	0x1UL
+#define DX_HOST_IMR_NOT_USED_MASK_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SHIFT 	0x2UL
+#define DX_HOST_IMR_DSCRPTR_COMPLETION_MASK_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SHIFT 	0x8UL
+#define DX_HOST_IMR_AXI_ERR_MASK_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_GPR0_BIT_SHIFT 	0xBUL
+#define DX_HOST_IMR_GPR0_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SHIFT 	0x13UL
+#define DX_HOST_IMR_DSCRPTR_WATERMARK_MASK0_BIT_SIZE 	0x1UL
+#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SHIFT 	0x17UL
+#define DX_HOST_IMR_AXIM_COMP_INT_MASK_BIT_SIZE 	0x1UL
+#define DX_HOST_ICR_REG_OFFSET 	0xA08UL 
+#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SHIFT 	0x2UL
+#define DX_HOST_ICR_DSCRPTR_COMPLETION_BIT_SIZE 	0x1UL
+#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SHIFT 	0x8UL
+#define DX_HOST_ICR_AXI_ERR_CLEAR_BIT_SIZE 	0x1UL
+#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SHIFT 	0xBUL
+#define DX_HOST_ICR_GPR_INT_CLEAR_BIT_SIZE 	0x1UL
+#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SHIFT 	0x13UL
+#define DX_HOST_ICR_DSCRPTR_WATERMARK_QUEUE0_CLEAR_BIT_SIZE 	0x1UL
+#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SHIFT 	0x17UL
+#define DX_HOST_ICR_AXIM_COMP_INT_CLEAR_BIT_SIZE 	0x1UL
+#define DX_HOST_SIGNATURE_REG_OFFSET 	0xA24UL 
+#define DX_HOST_SIGNATURE_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_SIGNATURE_VALUE_BIT_SIZE 	0x20UL
+#define DX_HOST_BOOT_REG_OFFSET 	0xA28UL 
+#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SHIFT 	0x0UL
+#define DX_HOST_BOOT_SYNTHESIS_CONFIG_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SHIFT 	0x1UL
+#define DX_HOST_BOOT_LARGE_RKEK_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SHIFT 	0x2UL
+#define DX_HOST_BOOT_HASH_IN_FUSES_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SHIFT 	0x3UL
+#define DX_HOST_BOOT_EXT_MEM_SECURED_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SHIFT 	0x5UL
+#define DX_HOST_BOOT_RKEK_ECC_EXISTS_LOCAL_N_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SHIFT 	0x6UL
+#define DX_HOST_BOOT_SRAM_SIZE_LOCAL_BIT_SIZE 	0x3UL
+#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SHIFT 	0x9UL
+#define DX_HOST_BOOT_DSCRPTR_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SHIFT 	0xAUL
+#define DX_HOST_BOOT_PAU_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SHIFT 	0xBUL
+#define DX_HOST_BOOT_RNG_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SHIFT 	0xCUL
+#define DX_HOST_BOOT_PKA_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SHIFT 	0xDUL
+#define DX_HOST_BOOT_RC4_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SHIFT 	0xEUL
+#define DX_HOST_BOOT_SHA_512_PRSNT_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SHIFT 	0xFUL
+#define DX_HOST_BOOT_SHA_256_PRSNT_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SHIFT 	0x10UL
+#define DX_HOST_BOOT_MD5_PRSNT_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SHIFT 	0x11UL
+#define DX_HOST_BOOT_HASH_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SHIFT 	0x12UL
+#define DX_HOST_BOOT_C2_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SHIFT 	0x13UL
+#define DX_HOST_BOOT_DES_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SHIFT 	0x14UL
+#define DX_HOST_BOOT_AES_XCBC_MAC_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SHIFT 	0x15UL
+#define DX_HOST_BOOT_AES_CMAC_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SHIFT 	0x16UL
+#define DX_HOST_BOOT_AES_CCM_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SHIFT 	0x17UL
+#define DX_HOST_BOOT_AES_XEX_HW_T_CALC_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SHIFT 	0x18UL
+#define DX_HOST_BOOT_AES_XEX_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SHIFT 	0x19UL
+#define DX_HOST_BOOT_CTR_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SHIFT 	0x1AUL
+#define DX_HOST_BOOT_AES_DIN_BYTE_RESOLUTION_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SHIFT 	0x1BUL
+#define DX_HOST_BOOT_TUNNELING_ENB_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SHIFT 	0x1CUL
+#define DX_HOST_BOOT_SUPPORT_256_192_KEY_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SHIFT 	0x1DUL
+#define DX_HOST_BOOT_ONLY_ENCRYPT_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SHIFT 	0x1EUL
+#define DX_HOST_BOOT_AES_EXISTS_LOCAL_BIT_SIZE 	0x1UL
+#define DX_HOST_VERSION_REG_OFFSET 	0xA40UL 
+#define DX_HOST_VERSION_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_VERSION_VALUE_BIT_SIZE 	0x20UL
+#define DX_HOST_KFDE0_VALID_REG_OFFSET 	0xA60UL 
+#define DX_HOST_KFDE0_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_KFDE0_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_HOST_KFDE1_VALID_REG_OFFSET 	0xA64UL 
+#define DX_HOST_KFDE1_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_KFDE1_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_HOST_KFDE2_VALID_REG_OFFSET 	0xA68UL 
+#define DX_HOST_KFDE2_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_KFDE2_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_HOST_KFDE3_VALID_REG_OFFSET 	0xA6CUL 
+#define DX_HOST_KFDE3_VALID_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_KFDE3_VALID_VALUE_BIT_SIZE 	0x1UL
+#define DX_HOST_GPR0_REG_OFFSET 	0xA70UL 
+#define DX_HOST_GPR0_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_GPR0_VALUE_BIT_SIZE 	0x20UL
+#define DX_GPR_HOST_REG_OFFSET 	0xA74UL 
+#define DX_GPR_HOST_VALUE_BIT_SHIFT 	0x0UL
+#define DX_GPR_HOST_VALUE_BIT_SIZE 	0x20UL
+#define DX_HOST_POWER_DOWN_EN_REG_OFFSET 	0xA78UL 
+#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SHIFT 	0x0UL
+#define DX_HOST_POWER_DOWN_EN_VALUE_BIT_SIZE 	0x1UL
+// --------------------------------------
+// BLOCK: HOST_SRAM
+// --------------------------------------
+#define DX_SRAM_DATA_REG_OFFSET 	0xF00UL 
+#define DX_SRAM_DATA_VALUE_BIT_SHIFT 	0x0UL
+#define DX_SRAM_DATA_VALUE_BIT_SIZE 	0x20UL
+#define DX_SRAM_ADDR_REG_OFFSET 	0xF04UL 
+#define DX_SRAM_ADDR_VALUE_BIT_SHIFT 	0x0UL
+#define DX_SRAM_ADDR_VALUE_BIT_SIZE 	0xFUL
+#define DX_SRAM_DATA_READY_REG_OFFSET 	0xF08UL 
+#define DX_SRAM_DATA_READY_VALUE_BIT_SHIFT 	0x0UL
+#define DX_SRAM_DATA_READY_VALUE_BIT_SIZE 	0x1UL
+
+#endif //__DX_HOST_H__
diff --git a/drivers/staging/ccree/dx_reg_base_host.h b/drivers/staging/ccree/dx_reg_base_host.h
new file mode 100644
index 000000000000..58dafe05fbeb
--- /dev/null
+++ b/drivers/staging/ccree/dx_reg_base_host.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DX_REG_BASE_HOST_H__
+#define __DX_REG_BASE_HOST_H__
+
+/* Identify platform: Xilinx Zynq7000 ZC706 */
+#define DX_PLAT_ZYNQ7000 1
+#define DX_PLAT_ZYNQ7000_ZC706 1
+
+#define DX_BASE_CC 0x80000000
+
+#define DX_BASE_ENV_REGS 0x40008000
+#define DX_BASE_ENV_CC_MEMORIES 0x40008000
+#define DX_BASE_ENV_PERF_RAM 0x40009000
+
+#define DX_BASE_HOST_RGF 0x0UL
+#define DX_BASE_CRY_KERNEL     0x0UL
+#define DX_BASE_ROM     0x40000000
+
+#endif /*__DX_REG_BASE_HOST_H__*/
diff --git a/drivers/staging/ccree/dx_reg_common.h b/drivers/staging/ccree/dx_reg_common.h
new file mode 100644
index 000000000000..4ffed386521c
--- /dev/null
+++ b/drivers/staging/ccree/dx_reg_common.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __DX_REG_COMMON_H__
+#define __DX_REG_COMMON_H__
+
+#define DX_DEV_SIGNATURE 0xDCC71200UL
+
+#define CC_HW_VERSION 0xef840015UL 
+
+#define DX_DEV_SHA_MAX 512
+
+#endif /*__DX_REG_COMMON_H__*/
diff --git a/drivers/staging/ccree/hash_defs.h b/drivers/staging/ccree/hash_defs.h
new file mode 100644
index 000000000000..5ab0861fd1bb
--- /dev/null
+++ b/drivers/staging/ccree/hash_defs.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef  _HASH_DEFS_H__
+#define  _HASH_DEFS_H__
+
+#include "cc_crypto_ctx.h"
+
+/* this files provides definitions required for hash engine drivers */
+#ifndef CC_CONFIG_HASH_SHA_512_SUPPORTED
+#define SEP_HASH_LENGTH_WORDS		2
+#else
+#define SEP_HASH_LENGTH_WORDS		4
+#endif
+
+#ifdef BIG__ENDIAN
+#define OPAD_CURRENT_LENGTH 0x40000000, 0x00000000 , 0x00000000, 0x00000000
+#define HASH_LARVAL_MD5  0x76543210, 0xFEDCBA98, 0x89ABCDEF, 0x01234567
+#define HASH_LARVAL_SHA1 0xF0E1D2C3, 0x76543210, 0xFEDCBA98, 0x89ABCDEF, 0x01234567
+#define HASH_LARVAL_SHA224 0XA44FFABE, 0XA78FF964, 0X11155868, 0X310BC0FF, 0X39590EF7, 0X17DD7030, 0X07D57C36, 0XD89E05C1
+#define HASH_LARVAL_SHA256 0X19CDE05B, 0XABD9831F, 0X8C68059B, 0X7F520E51, 0X3AF54FA5, 0X72F36E3C, 0X85AE67BB, 0X67E6096A
+#define HASH_LARVAL_SHA384 0X1D48B547, 0XA44FFABE, 0X0D2E0CDB, 0XA78FF964, 0X874AB48E, 0X11155868, 0X67263367, 0X310BC0FF, 0XD8EC2F15, 0X39590EF7, 0X5A015991, 0X17DD7030, 0X2A299A62, 0X07D57C36, 0X5D9DBBCB, 0XD89E05C1
+#define HASH_LARVAL_SHA512 0X19CDE05B, 0X79217E13, 0XABD9831F, 0X6BBD41FB, 0X8C68059B, 0X1F6C3E2B, 0X7F520E51, 0XD182E6AD, 0X3AF54FA5, 0XF1361D5F, 0X72F36E3C, 0X2BF894FE, 0X85AE67BB, 0X3BA7CA84, 0X67E6096A, 0X08C9BCF3
+#else
+#define OPAD_CURRENT_LENGTH 0x00000040, 0x00000000, 0x00000000, 0x00000000
+#define HASH_LARVAL_MD5  0x10325476, 0x98BADCFE, 0xEFCDAB89, 0x67452301
+#define HASH_LARVAL_SHA1 0xC3D2E1F0, 0x10325476, 0x98BADCFE, 0xEFCDAB89, 0x67452301
+#define HASH_LARVAL_SHA224 0xbefa4fa4, 0x64f98fa7, 0x68581511, 0xffc00b31, 0xf70e5939, 0x3070dd17, 0x367cd507, 0xc1059ed8
+#define HASH_LARVAL_SHA256 0x5be0cd19, 0x1f83d9ab, 0x9b05688c, 0x510e527f, 0xa54ff53a, 0x3c6ef372, 0xbb67ae85, 0x6a09e667
+#define HASH_LARVAL_SHA384 0X47B5481D, 0XBEFA4FA4, 0XDB0C2E0D, 0X64F98FA7, 0X8EB44A87, 0X68581511, 0X67332667, 0XFFC00B31, 0X152FECD8, 0XF70E5939, 0X9159015A, 0X3070DD17, 0X629A292A, 0X367CD507, 0XCBBB9D5D, 0XC1059ED8
+#define HASH_LARVAL_SHA512 0x5be0cd19, 0x137e2179, 0x1f83d9ab, 0xfb41bd6b, 0x9b05688c, 0x2b3e6c1f, 0x510e527f, 0xade682d1, 0xa54ff53a, 0x5f1d36f1, 0x3c6ef372, 0xfe94f82b, 0xbb67ae85, 0x84caa73b, 0x6a09e667, 0xf3bcc908
+#endif
+
+enum HashConfig1Padding {
+	HASH_PADDING_DISABLED = 0,
+	HASH_PADDING_ENABLED = 1,
+	HASH_DIGEST_RESULT_LITTLE_ENDIAN = 2,
+	HASH_CONFIG1_PADDING_RESERVE32 = INT32_MAX,
+};
+
+enum HashCipherDoPadding {
+	DO_NOT_PAD = 0,
+	DO_PAD = 1,
+	HASH_CIPHER_DO_PADDING_RESERVE32 = INT32_MAX,
+};
+
+typedef struct SepHashPrivateContext {
+	/* The current length is placed at the end of the context buffer because the hash 
+	   context is used for all HMAC operations as well. HMAC context includes a 64 bytes 
+	   K0 field.  The size of struct drv_ctx_hash reserved field is  88/184 bytes depend if t
+	   he SHA512 is supported ( in this case teh context size is 256 bytes).
+	   The size of struct drv_ctx_hash reseved field is 20 or 52 depend if the SHA512 is supported.
+	   This means that this structure size (without the reserved field can be up to 20 bytes ,
+	   in case sha512 is not suppported it is 20 bytes (SEP_HASH_LENGTH_WORDS define to 2 ) and in the other
+	   case it is 28 (SEP_HASH_LENGTH_WORDS define to 4) */
+	uint32_t reserved[(sizeof(struct drv_ctx_hash)/sizeof(uint32_t)) - SEP_HASH_LENGTH_WORDS - 3];
+	uint32_t CurrentDigestedLength[SEP_HASH_LENGTH_WORDS];
+	uint32_t KeyType;
+	uint32_t dataCompleted;
+	uint32_t hmacFinalization;
+	/* no space left */
+} SepHashPrivateContext_s;
+
+#endif /*_HASH_DEFS_H__*/
+
diff --git a/drivers/staging/ccree/hw_queue_defs_plat.h b/drivers/staging/ccree/hw_queue_defs_plat.h
new file mode 100644
index 000000000000..aee02cc7588a
--- /dev/null
+++ b/drivers/staging/ccree/hw_queue_defs_plat.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HW_QUEUE_DEFS_PLAT_H__
+#define __HW_QUEUE_DEFS_PLAT_H__
+
+
+/*****************************/
+/* Descriptor packing macros */
+/*****************************/
+
+#define HW_QUEUE_FREE_SLOTS_GET() (CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_CONTENT)) & HW_QUEUE_SLOTS_MAX)
+
+#define HW_QUEUE_POLL_QUEUE_UNTIL_FREE_SLOTS(seqLen)						\
+	do {											\
+	} while (HW_QUEUE_FREE_SLOTS_GET() < (seqLen))
+
+#define HW_DESC_PUSH_TO_QUEUE(pDesc) do {        				  \
+	LOG_HW_DESC(pDesc);							  \
+	HW_DESC_DUMP(pDesc);							  \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(0), (pDesc)->word[0]); \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(1), (pDesc)->word[1]); \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(2), (pDesc)->word[2]); \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(3), (pDesc)->word[3]); \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(4), (pDesc)->word[4]); \
+	wmb();									   \
+	CC_HAL_WRITE_REGISTER(GET_HW_Q_DESC_WORD_IDX(5), (pDesc)->word[5]); \
+} while (0)
+
+#endif /*__HW_QUEUE_DEFS_PLAT_H__*/
diff --git a/drivers/staging/ccree/ssi_aead.c b/drivers/staging/ccree/ssi_aead.c
new file mode 100644
index 000000000000..038291773b59
--- /dev/null
+++ b/drivers/staging/ccree/ssi_aead.c
@@ -0,0 +1,2832 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <crypto/algapi.h>
+#include <crypto/internal/skcipher.h>
+#include <crypto/internal/hash.h>
+#include <crypto/internal/aead.h>
+#include <crypto/sha.h>
+#include <crypto/ctr.h>
+#include <crypto/authenc.h>
+#include <crypto/aes.h>
+#include <crypto/des.h>
+#include <linux/rtnetlink.h>
+#include <linux/version.h>
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_aead.h"
+#include "ssi_request_mgr.h"
+#include "ssi_hash.h"
+#include "ssi_sysfs.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_fips_local.h"
+
+#define template_aead	template_u.aead
+
+#define MAX_AEAD_SETKEY_SEQ 12
+#define MAX_AEAD_PROCESS_SEQ 23
+
+#define MAX_HMAC_DIGEST_SIZE (SHA256_DIGEST_SIZE)
+#define MAX_HMAC_BLOCK_SIZE (SHA256_BLOCK_SIZE)
+
+#define AES_CCM_RFC4309_NONCE_SIZE 3
+#define MAX_NONCE_SIZE CTR_RFC3686_NONCE_SIZE
+
+
+/* Value of each ICV_CMP byte (of 8) in case of success */
+#define ICV_VERIF_OK 0x01	
+
+struct ssi_aead_handle {
+	ssi_sram_addr_t sram_workspace_addr;
+	struct list_head aead_list;
+};
+
+struct ssi_aead_ctx {
+	struct ssi_drvdata *drvdata;
+	uint8_t ctr_nonce[MAX_NONCE_SIZE]; /* used for ctr3686 iv and aes ccm */
+	uint8_t *enckey;
+	dma_addr_t enckey_dma_addr;
+	union {
+		struct {
+			uint8_t *padded_authkey;
+			uint8_t *ipad_opad; /* IPAD, OPAD*/
+			dma_addr_t padded_authkey_dma_addr;
+			dma_addr_t ipad_opad_dma_addr;
+		} hmac;
+		struct {
+			uint8_t *xcbc_keys; /* K1,K2,K3 */
+			dma_addr_t xcbc_keys_dma_addr;
+		} xcbc;
+	} auth_state;
+	unsigned int enc_keylen;
+	unsigned int auth_keylen;
+	unsigned int authsize; /* Actual (reduced?) size of the MAC/ICv */
+	enum drv_cipher_mode cipher_mode;
+	enum FlowMode flow_mode;
+	enum drv_hash_mode auth_mode;
+};
+
+static inline bool valid_assoclen(struct aead_request *req)
+{
+	return ((req->assoclen == 16) || (req->assoclen == 20));
+}
+
+static void ssi_aead_exit(struct crypto_aead *tfm)
+{
+	struct device *dev = NULL;
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+
+	SSI_LOG_DEBUG("Clearing context @%p for %s\n",
+		crypto_aead_ctx(tfm), crypto_tfm_alg_name(&(tfm->base)));
+
+ 	dev = &ctx->drvdata->plat_dev->dev;
+	/* Unmap enckey buffer */
+	if (ctx->enckey != NULL) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->enckey_dma_addr);
+		dma_free_coherent(dev, AES_MAX_KEY_SIZE, ctx->enckey, ctx->enckey_dma_addr);
+		SSI_LOG_DEBUG("Freed enckey DMA buffer enckey_dma_addr=0x%llX\n",
+			(unsigned long long)ctx->enckey_dma_addr);
+		ctx->enckey_dma_addr = 0;
+		ctx->enckey = NULL;
+	}
+	
+	if (ctx->auth_mode == DRV_HASH_XCBC_MAC) { /* XCBC authetication */
+		if (ctx->auth_state.xcbc.xcbc_keys != NULL) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(
+				ctx->auth_state.xcbc.xcbc_keys_dma_addr);
+			dma_free_coherent(dev, CC_AES_128_BIT_KEY_SIZE * 3,
+				ctx->auth_state.xcbc.xcbc_keys, 
+				ctx->auth_state.xcbc.xcbc_keys_dma_addr);
+		}
+		SSI_LOG_DEBUG("Freed xcbc_keys DMA buffer xcbc_keys_dma_addr=0x%llX\n",
+			(unsigned long long)ctx->auth_state.xcbc.xcbc_keys_dma_addr);
+		ctx->auth_state.xcbc.xcbc_keys_dma_addr = 0;
+		ctx->auth_state.xcbc.xcbc_keys = NULL;
+	} else if (ctx->auth_mode != DRV_HASH_NULL) { /* HMAC auth. */
+		if (ctx->auth_state.hmac.ipad_opad != NULL) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(
+				ctx->auth_state.hmac.ipad_opad_dma_addr);
+			dma_free_coherent(dev, 2 * MAX_HMAC_DIGEST_SIZE,
+				ctx->auth_state.hmac.ipad_opad,
+				ctx->auth_state.hmac.ipad_opad_dma_addr);
+			SSI_LOG_DEBUG("Freed ipad_opad DMA buffer ipad_opad_dma_addr=0x%llX\n",
+				(unsigned long long)ctx->auth_state.hmac.ipad_opad_dma_addr);
+			ctx->auth_state.hmac.ipad_opad_dma_addr = 0;
+			ctx->auth_state.hmac.ipad_opad = NULL;
+		}
+		if (ctx->auth_state.hmac.padded_authkey != NULL) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(
+				ctx->auth_state.hmac.padded_authkey_dma_addr);
+			dma_free_coherent(dev, MAX_HMAC_BLOCK_SIZE,
+				ctx->auth_state.hmac.padded_authkey,
+				ctx->auth_state.hmac.padded_authkey_dma_addr);
+			SSI_LOG_DEBUG("Freed padded_authkey DMA buffer padded_authkey_dma_addr=0x%llX\n",
+				(unsigned long long)ctx->auth_state.hmac.padded_authkey_dma_addr);
+			ctx->auth_state.hmac.padded_authkey_dma_addr = 0;
+			ctx->auth_state.hmac.padded_authkey = NULL;
+		}
+	}
+}
+
+static int ssi_aead_init(struct crypto_aead *tfm)
+{
+	struct device *dev;
+	struct aead_alg *alg = crypto_aead_alg(tfm);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct ssi_crypto_alg *ssi_alg =
+			container_of(alg, struct ssi_crypto_alg, aead_alg);
+	SSI_LOG_DEBUG("Initializing context @%p for %s\n", ctx, crypto_tfm_alg_name(&(tfm->base)));
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	/* Initialize modes in instance */
+	ctx->cipher_mode = ssi_alg->cipher_mode;
+	ctx->flow_mode = ssi_alg->flow_mode;
+	ctx->auth_mode = ssi_alg->auth_mode;
+	ctx->drvdata = ssi_alg->drvdata;
+	dev = &ctx->drvdata->plat_dev->dev;
+	crypto_aead_set_reqsize(tfm,sizeof(struct aead_req_ctx));
+
+	/* Allocate key buffer, cache line aligned */
+	ctx->enckey = dma_alloc_coherent(dev, AES_MAX_KEY_SIZE,
+		&ctx->enckey_dma_addr, GFP_KERNEL);
+	if (ctx->enckey == NULL) {
+		SSI_LOG_ERR("Failed allocating key buffer\n");
+		goto init_failed;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->enckey_dma_addr, AES_MAX_KEY_SIZE);
+	SSI_LOG_DEBUG("Allocated enckey buffer in context ctx->enckey=@%p\n", ctx->enckey);
+
+	/* Set default authlen value */
+
+	if (ctx->auth_mode == DRV_HASH_XCBC_MAC) { /* XCBC authetication */
+		/* Allocate dma-coherent buffer for XCBC's K1+K2+K3 */
+		/* (and temporary for user key - up to 256b) */
+		ctx->auth_state.xcbc.xcbc_keys = dma_alloc_coherent(dev,
+			CC_AES_128_BIT_KEY_SIZE * 3,
+			&ctx->auth_state.xcbc.xcbc_keys_dma_addr, GFP_KERNEL);
+		if (ctx->auth_state.xcbc.xcbc_keys == NULL) {
+			SSI_LOG_ERR("Failed allocating buffer for XCBC keys\n");
+			goto init_failed;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(
+			ctx->auth_state.xcbc.xcbc_keys_dma_addr,
+			CC_AES_128_BIT_KEY_SIZE * 3);
+	} else if (ctx->auth_mode != DRV_HASH_NULL) { /* HMAC authentication */
+		/* Allocate dma-coherent buffer for IPAD + OPAD */
+		ctx->auth_state.hmac.ipad_opad = dma_alloc_coherent(dev,
+			2 * MAX_HMAC_DIGEST_SIZE,
+			&ctx->auth_state.hmac.ipad_opad_dma_addr, GFP_KERNEL);
+		if (ctx->auth_state.hmac.ipad_opad == NULL) {
+			SSI_LOG_ERR("Failed allocating IPAD/OPAD buffer\n");
+			goto init_failed;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(
+			ctx->auth_state.hmac.ipad_opad_dma_addr,
+			2 * MAX_HMAC_DIGEST_SIZE);
+		SSI_LOG_DEBUG("Allocated authkey buffer in context ctx->authkey=@%p\n",
+			ctx->auth_state.hmac.ipad_opad);
+	
+		ctx->auth_state.hmac.padded_authkey = dma_alloc_coherent(dev,
+			MAX_HMAC_BLOCK_SIZE,
+			&ctx->auth_state.hmac.padded_authkey_dma_addr, GFP_KERNEL);
+		if (ctx->auth_state.hmac.padded_authkey == NULL) {
+			SSI_LOG_ERR("failed to allocate padded_authkey\n");
+			goto init_failed;
+		}	
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(
+			ctx->auth_state.hmac.padded_authkey_dma_addr,
+			MAX_HMAC_BLOCK_SIZE);
+	} else {
+		ctx->auth_state.hmac.ipad_opad = NULL;
+		ctx->auth_state.hmac.padded_authkey = NULL;
+	}
+
+	return 0;
+
+init_failed:
+	ssi_aead_exit(tfm);
+	return -ENOMEM;
+}
+ 
+
+static void ssi_aead_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
+{
+	struct aead_request *areq = (struct aead_request *)ssi_req;
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(areq);
+	struct crypto_aead *tfm = crypto_aead_reqtfm(ssi_req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	int err = 0;
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	START_CYCLE_COUNT();
+
+	ssi_buffer_mgr_unmap_aead_request(dev, areq);
+
+	/* Restore ordinary iv pointer */
+	areq->iv = areq_ctx->backup_iv;
+
+	if (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		if (memcmp(areq_ctx->mac_buf, areq_ctx->icv_virt_addr,
+			ctx->authsize) != 0) {
+			SSI_LOG_DEBUG("Payload authentication failure, "
+				"(auth-size=%d, cipher=%d).\n",
+				ctx->authsize, ctx->cipher_mode);
+			/* In case of payload authentication failure, MUST NOT
+			   revealed the decrypted message --> zero its memory. */
+			ssi_buffer_mgr_zero_sgl(areq->dst, areq_ctx->cryptlen);
+			err = -EBADMSG;
+		}
+	} else { /*ENCRYPT*/
+		if (unlikely(areq_ctx->is_icv_fragmented == true))
+			ssi_buffer_mgr_copy_scatterlist_portion(
+				areq_ctx->mac_buf, areq_ctx->dstSgl, areq->cryptlen+areq_ctx->dstOffset,
+				areq->cryptlen+areq_ctx->dstOffset + ctx->authsize, SSI_SG_FROM_BUF);
+
+		/* If an IV was generated, copy it back to the user provided buffer. */
+		if (areq_ctx->backup_giv != NULL) {
+			if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+				memcpy(areq_ctx->backup_giv, areq_ctx->ctr_iv + CTR_RFC3686_NONCE_SIZE, CTR_RFC3686_IV_SIZE);
+			} else if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+				memcpy(areq_ctx->backup_giv, areq_ctx->ctr_iv + CCM_BLOCK_IV_OFFSET, CCM_BLOCK_IV_SIZE);
+			}
+		}
+	}
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_4);
+	aead_request_complete(areq, err);
+}
+
+static int xcbc_setkey(HwDesc_s *desc, struct ssi_aead_ctx *ctx)
+{
+	/* Load the AES key */
+	HW_DESC_INIT(&desc[0]);
+	/* We are using for the source/user key the same buffer as for the output keys,
+	   because after this key loading it is not needed anymore */
+	HW_DESC_SET_DIN_TYPE(&desc[0], DMA_DLLI, ctx->auth_state.xcbc.xcbc_keys_dma_addr, ctx->auth_keylen, NS_BIT);
+	HW_DESC_SET_CIPHER_MODE(&desc[0], DRV_CIPHER_ECB);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[0], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[0], ctx->auth_keylen);
+	HW_DESC_SET_FLOW_MODE(&desc[0], S_DIN_to_AES);
+	HW_DESC_SET_SETUP_MODE(&desc[0], SETUP_LOAD_KEY0);
+
+	HW_DESC_INIT(&desc[1]);
+	HW_DESC_SET_DIN_CONST(&desc[1], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[1], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[1], ctx->auth_state.xcbc.xcbc_keys_dma_addr, AES_KEYSIZE_128, NS_BIT, 0);
+
+	HW_DESC_INIT(&desc[2]);
+	HW_DESC_SET_DIN_CONST(&desc[2], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[2], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[2], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
+					 + AES_KEYSIZE_128),
+			      AES_KEYSIZE_128, NS_BIT, 0);
+
+	HW_DESC_INIT(&desc[3]);
+	HW_DESC_SET_DIN_CONST(&desc[3], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[3], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[3], (ctx->auth_state.xcbc.xcbc_keys_dma_addr
+					  + 2 * AES_KEYSIZE_128),
+			      AES_KEYSIZE_128, NS_BIT, 0);
+
+	return 4;
+}
+
+static int hmac_setkey(HwDesc_s *desc, struct ssi_aead_ctx *ctx)
+{
+	unsigned int hmacPadConst[2] = { HMAC_IPAD_CONST, HMAC_OPAD_CONST };
+	unsigned int digest_ofs = 0;
+	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+			DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+			CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
+
+	int idx = 0;
+	int i;
+
+	/* calc derived HMAC key */
+	for (i = 0; i < 2; i++) {
+		/* Load hash initial state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		HW_DESC_SET_DIN_SRAM(&desc[idx],
+			ssi_ahash_get_larval_digest_sram_addr(
+				ctx->drvdata, ctx->auth_mode),
+			digest_size);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+		/* Load the hash current length*/
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Prepare ipad key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		idx++;
+
+		/* Perform HASH update */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				   ctx->auth_state.hmac.padded_authkey_dma_addr,
+				     SHA256_BLOCK_SIZE, NS_BIT);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+
+		/* Get the digset */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+				      (ctx->auth_state.hmac.ipad_opad_dma_addr +
+				       digest_ofs),
+				      digest_size, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+		idx++;
+
+		digest_ofs += digest_size;
+	}
+
+	return idx;
+}
+
+static int validate_keys_sizes(struct ssi_aead_ctx *ctx)
+{
+	SSI_LOG_DEBUG("enc_keylen=%u  authkeylen=%u\n",
+		ctx->enc_keylen, ctx->auth_keylen);
+
+	switch (ctx->auth_mode) {
+	case DRV_HASH_SHA1:
+	case DRV_HASH_SHA256:
+		break;
+	case DRV_HASH_XCBC_MAC:
+		if ((ctx->auth_keylen != AES_KEYSIZE_128) &&
+		    (ctx->auth_keylen != AES_KEYSIZE_192) &&
+		    (ctx->auth_keylen != AES_KEYSIZE_256))
+			return -ENOTSUPP;
+		break;
+	case DRV_HASH_NULL: /* Not authenc (e.g., CCM) - no auth_key) */
+		if (ctx->auth_keylen > 0)
+			return -EINVAL;
+		break;
+	default:
+		SSI_LOG_ERR("Invalid auth_mode=%d\n", ctx->auth_mode);
+		return -EINVAL;
+	}
+	/* Check cipher key size */
+	if (unlikely(ctx->flow_mode == S_DIN_to_DES)) {
+		if (ctx->enc_keylen != DES3_EDE_KEY_SIZE) {
+			SSI_LOG_ERR("Invalid cipher(3DES) key size: %u\n",
+				ctx->enc_keylen);
+			return -EINVAL;
+		}
+	} else { /* Default assumed to be AES ciphers */
+		if ((ctx->enc_keylen != AES_KEYSIZE_128) &&
+		    (ctx->enc_keylen != AES_KEYSIZE_192) &&
+		    (ctx->enc_keylen != AES_KEYSIZE_256)) {
+			SSI_LOG_ERR("Invalid cipher(AES) key size: %u\n",
+				ctx->enc_keylen);
+			return -EINVAL;
+		}
+	}
+
+	return 0; /* All tests of keys sizes passed */
+}
+/*This function prepers the user key so it can pass to the hmac processing 
+  (copy to intenral buffer or hash in case of key longer than block */
+static int
+ssi_get_plain_hmac_key(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
+{
+	dma_addr_t key_dma_addr = 0;
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	uint32_t larval_addr = ssi_ahash_get_larval_digest_sram_addr(
+					ctx->drvdata, ctx->auth_mode);
+	struct ssi_crypto_req ssi_req = {};
+	unsigned int blocksize;
+	unsigned int digestsize;
+	unsigned int hashmode;
+	unsigned int idx = 0;
+	int rc = 0;
+	HwDesc_s desc[MAX_AEAD_SETKEY_SEQ];
+	dma_addr_t padded_authkey_dma_addr = 
+		ctx->auth_state.hmac.padded_authkey_dma_addr;
+
+	switch (ctx->auth_mode) { /* auth_key required and >0 */
+	case DRV_HASH_SHA1:
+		blocksize = SHA1_BLOCK_SIZE;
+		digestsize = SHA1_DIGEST_SIZE;
+		hashmode = DRV_HASH_HW_SHA1;
+		break;
+	case DRV_HASH_SHA256:
+	default:
+		blocksize = SHA256_BLOCK_SIZE;
+		digestsize = SHA256_DIGEST_SIZE;
+		hashmode = DRV_HASH_HW_SHA256;
+	}
+
+	if (likely(keylen != 0)) {
+		key_dma_addr = dma_map_single(dev, (void *)key, keylen, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(dev, key_dma_addr))) {
+			SSI_LOG_ERR("Mapping key va=0x%p len=%u for"
+				   " DMA failed\n", key, keylen);
+			return -ENOMEM;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(key_dma_addr, keylen);
+		if (keylen > blocksize) {
+			/* Load hash initial state */
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode);
+			HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr, digestsize);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			idx++;
+	
+			/* Load the hash current length*/
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode);
+			HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+			idx++;
+	
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+					     key_dma_addr, 
+					     keylen, NS_BIT);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+			idx++;
+	
+			/* Get hashed key */
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hashmode); 
+			HW_DESC_SET_DOUT_DLLI(&desc[idx],
+					 padded_authkey_dma_addr,
+					 digestsize,
+					 NS_BIT, 0);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx],
+							HASH_PADDING_DISABLED);
+			HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],
+						   HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+			idx++;
+	
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - digestsize));
+			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+					      (padded_authkey_dma_addr + digestsize),
+					      (blocksize - digestsize),
+					      NS_BIT, 0);
+			idx++;
+		} else {
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+					     key_dma_addr, 
+					     keylen, NS_BIT);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+					      (padded_authkey_dma_addr),
+					      keylen, NS_BIT, 0);
+			idx++;
+	
+			if ((blocksize - keylen) != 0) {
+				HW_DESC_INIT(&desc[idx]);
+				HW_DESC_SET_DIN_CONST(&desc[idx], 0,
+						      (blocksize - keylen));
+				HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+				HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+					(padded_authkey_dma_addr + keylen),
+					(blocksize - keylen),
+					NS_BIT, 0);
+				idx++;
+			}
+		}
+	} else {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0,
+				      (blocksize - keylen));
+		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+			padded_authkey_dma_addr,
+			blocksize,
+			NS_BIT, 0);
+		idx++;
+	}
+
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = STAT_OP_TYPE_SETKEY;
+#endif
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+	if (unlikely(rc != 0))
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+
+	if (likely(key_dma_addr != 0)) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(key_dma_addr);
+		dma_unmap_single(dev, key_dma_addr, keylen, DMA_TO_DEVICE);
+	}
+
+	return rc;
+}
+
+
+static int
+ssi_aead_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
+{
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct rtattr *rta = (struct rtattr *)key;
+	struct ssi_crypto_req ssi_req = {};
+	struct crypto_authenc_key_param *param;
+	HwDesc_s desc[MAX_AEAD_SETKEY_SEQ];
+	int seq_len = 0, rc = -EINVAL;
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	SSI_LOG_DEBUG("Setting key in context @%p for %s. key=%p keylen=%u\n",
+		ctx, crypto_tfm_alg_name(crypto_aead_tfm(tfm)), key, keylen);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	/* STAT_PHASE_0: Init and sanity checks */
+	START_CYCLE_COUNT();
+
+	if (ctx->auth_mode != DRV_HASH_NULL) { /* authenc() alg. */
+		if (!RTA_OK(rta, keylen))
+			goto badkey;
+		if (rta->rta_type != CRYPTO_AUTHENC_KEYA_PARAM)
+			goto badkey;
+		if (RTA_PAYLOAD(rta) < sizeof(*param))
+			goto badkey;
+		param = RTA_DATA(rta);
+		ctx->enc_keylen = be32_to_cpu(param->enckeylen);
+		key += RTA_ALIGN(rta->rta_len);
+		keylen -= RTA_ALIGN(rta->rta_len);
+		if (keylen < ctx->enc_keylen)
+			goto badkey;
+		ctx->auth_keylen = keylen - ctx->enc_keylen;
+
+		if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+			/* the nonce is stored in bytes at end of key */
+			if (ctx->enc_keylen <
+			    (AES_MIN_KEY_SIZE + CTR_RFC3686_NONCE_SIZE))
+				goto badkey;
+			/* Copy nonce from last 4 bytes in CTR key to
+			*  first 4 bytes in CTR IV */
+			memcpy(ctx->ctr_nonce, key + ctx->auth_keylen + ctx->enc_keylen -
+				CTR_RFC3686_NONCE_SIZE, CTR_RFC3686_NONCE_SIZE);
+			/* Set CTR key size */
+			ctx->enc_keylen -= CTR_RFC3686_NONCE_SIZE;
+		}
+	} else { /* non-authenc - has just one key */
+		ctx->enc_keylen = keylen;
+		ctx->auth_keylen = 0;
+	}
+
+	rc = validate_keys_sizes(ctx);
+	if (unlikely(rc != 0))
+		goto badkey;
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
+	/* STAT_PHASE_1: Copy key to ctx */
+	START_CYCLE_COUNT();
+
+	/* Get key material */
+	memcpy(ctx->enckey, key + ctx->auth_keylen, ctx->enc_keylen);
+	if (ctx->enc_keylen == 24)
+		memset(ctx->enckey + 24, 0, CC_AES_KEY_SIZE_MAX - 24);
+	if (ctx->auth_mode == DRV_HASH_XCBC_MAC) {
+		memcpy(ctx->auth_state.xcbc.xcbc_keys, key, ctx->auth_keylen);
+	} else if (ctx->auth_mode != DRV_HASH_NULL) { /* HMAC */
+		rc = ssi_get_plain_hmac_key(tfm, key, ctx->auth_keylen);
+		if (rc != 0)
+			goto badkey;
+	}
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
+	
+	/* STAT_PHASE_2: Create sequence */
+	START_CYCLE_COUNT();
+
+	switch (ctx->auth_mode) {
+	case DRV_HASH_SHA1:
+	case DRV_HASH_SHA256:
+		seq_len = hmac_setkey(desc, ctx);
+		break;
+	case DRV_HASH_XCBC_MAC:
+		seq_len = xcbc_setkey(desc, ctx);
+		break;
+	case DRV_HASH_NULL: /* non-authenc modes, e.g., CCM */
+		break; /* No auth. key setup */
+	default:
+		SSI_LOG_ERR("Unsupported authenc (%d)\n", ctx->auth_mode);
+		rc = -ENOTSUPP;
+		goto badkey;
+	}
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_2);
+
+	/* STAT_PHASE_3: Submit sequence to HW */
+	START_CYCLE_COUNT();
+	
+	if (seq_len > 0) { /* For CCM there is no sequence to setup the key */
+#ifdef ENABLE_CYCLE_COUNT
+		ssi_req.op_type = STAT_OP_TYPE_SETKEY;
+#endif
+		rc = send_request(ctx->drvdata, &ssi_req, desc, seq_len, 0);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			goto setkey_error;
+		}
+	}
+
+	/* Update STAT_PHASE_3 */
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_3);
+	return rc;
+
+badkey:
+	crypto_aead_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+
+setkey_error:
+	return rc;
+}
+
+#if SSI_CC_HAS_AES_CCM
+static int ssi_rfc4309_ccm_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
+{
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	int rc = 0;
+	
+	if (keylen < 3)
+		return -EINVAL;
+
+	keylen -= 3;
+	memcpy(ctx->ctr_nonce, key + keylen, 3);
+
+	rc = ssi_aead_setkey(tfm, key, keylen);
+
+	return rc;
+}
+#endif /*SSI_CC_HAS_AES_CCM*/
+
+static int ssi_aead_setauthsize(
+	struct crypto_aead *authenc,
+	unsigned int authsize)
+{
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(authenc);
+	
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	/* Unsupported auth. sizes */
+	if ((authsize == 0) ||
+	    (authsize >crypto_aead_maxauthsize(authenc))) {
+		return -ENOTSUPP;
+	}
+
+	ctx->authsize = authsize;
+	SSI_LOG_DEBUG("authlen=%d\n", ctx->authsize);
+
+	return 0;
+}
+
+#if SSI_CC_HAS_AES_CCM
+static int ssi_rfc4309_ccm_setauthsize(struct crypto_aead *authenc,
+				      unsigned int authsize)
+{
+	switch (authsize) {
+	case 8:
+	case 12:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ssi_aead_setauthsize(authenc, authsize);
+}
+
+static int ssi_ccm_setauthsize(struct crypto_aead *authenc,
+				      unsigned int authsize)
+{
+	switch (authsize) {
+	case 4:
+	case 6:
+	case 8:
+	case 10:
+	case 12:
+	case 14:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ssi_aead_setauthsize(authenc, authsize);
+}
+#endif /*SSI_CC_HAS_AES_CCM*/
+
+static inline void 
+ssi_aead_create_assoc_desc(
+	struct aead_request *areq, 
+	unsigned int flow_mode,
+	HwDesc_s desc[], 
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(areq);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(areq);
+	enum ssi_req_dma_buf_type assoc_dma_type = areq_ctx->assoc_buff_type;
+	unsigned int idx = *seq_size;
+
+	switch (assoc_dma_type) {
+	case SSI_DMA_BUF_DLLI:
+		SSI_LOG_DEBUG("ASSOC buffer type DLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+			sg_dma_address(areq->src),
+			areq->assoclen, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		if (ctx->auth_mode == DRV_HASH_XCBC_MAC && (areq_ctx->cryptlen > 0) )
+			HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx]);
+		break;
+	case SSI_DMA_BUF_MLLI:
+		SSI_LOG_DEBUG("ASSOC buffer type MLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
+				     areq_ctx->assoc.sram_addr,
+				     areq_ctx->assoc.mlli_nents,
+				     NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		if (ctx->auth_mode == DRV_HASH_XCBC_MAC && (areq_ctx->cryptlen > 0) )
+			HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx]);
+		break;
+	case SSI_DMA_BUF_NULL:
+	default:
+		SSI_LOG_ERR("Invalid ASSOC buffer type\n");
+	}
+
+	*seq_size = (++idx);
+}
+
+static inline void
+ssi_aead_process_authenc_data_desc(
+	struct aead_request *areq, 
+	unsigned int flow_mode,
+	HwDesc_s desc[], 
+	unsigned int *seq_size,
+	int direct)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(areq);
+	enum ssi_req_dma_buf_type data_dma_type = areq_ctx->data_buff_type;
+	unsigned int idx = *seq_size;
+
+	switch (data_dma_type) {
+	case SSI_DMA_BUF_DLLI:
+	{
+		struct scatterlist *cipher =
+			(direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ?
+			areq_ctx->dstSgl : areq_ctx->srcSgl;
+
+		unsigned int offset = 
+			(direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ?
+			areq_ctx->dstOffset : areq_ctx->srcOffset;
+		SSI_LOG_DEBUG("AUTHENC: SRC/DST buffer type DLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			(sg_dma_address(cipher)+ offset), areq_ctx->cryptlen,
+			NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		break;
+	}
+	case SSI_DMA_BUF_MLLI:
+	{
+		/* DOUBLE-PASS flow (as default)
+		 * assoc. + iv + data -compact in one table
+		 * if assoclen is ZERO only IV perform */
+		ssi_sram_addr_t mlli_addr = areq_ctx->assoc.sram_addr;
+		uint32_t mlli_nents = areq_ctx->assoc.mlli_nents;
+
+		if (likely(areq_ctx->is_single_pass == true)) {
+			if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT){
+				mlli_addr = areq_ctx->dst.sram_addr;
+				mlli_nents = areq_ctx->dst.mlli_nents;
+			} else {
+				mlli_addr = areq_ctx->src.sram_addr;
+				mlli_nents = areq_ctx->src.mlli_nents;
+			}
+		}
+
+		SSI_LOG_DEBUG("AUTHENC: SRC/DST buffer type MLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
+			mlli_addr, mlli_nents, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		break;
+	}
+	case SSI_DMA_BUF_NULL:
+	default:
+		SSI_LOG_ERR("AUTHENC: Invalid SRC/DST buffer type\n");
+	}
+
+	*seq_size = (++idx);
+}
+
+static inline void
+ssi_aead_process_cipher_data_desc(
+	struct aead_request *areq, 
+	unsigned int flow_mode,
+	HwDesc_s desc[], 
+	unsigned int *seq_size)
+{
+	unsigned int idx = *seq_size;
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(areq);
+	enum ssi_req_dma_buf_type data_dma_type = areq_ctx->data_buff_type;
+
+	if (areq_ctx->cryptlen == 0)
+		return; /*null processing*/
+
+	switch (data_dma_type) {
+	case SSI_DMA_BUF_DLLI:
+		SSI_LOG_DEBUG("CIPHER: SRC/DST buffer type DLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			(sg_dma_address(areq_ctx->srcSgl)+areq_ctx->srcOffset),
+			areq_ctx->cryptlen, NS_BIT);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx],
+			(sg_dma_address(areq_ctx->dstSgl)+areq_ctx->dstOffset),
+			areq_ctx->cryptlen, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		break;
+	case SSI_DMA_BUF_MLLI:
+		SSI_LOG_DEBUG("CIPHER: SRC/DST buffer type MLLI\n");
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI,
+			areq_ctx->src.sram_addr,
+			areq_ctx->src.mlli_nents, NS_BIT);
+		HW_DESC_SET_DOUT_MLLI(&desc[idx],
+			areq_ctx->dst.sram_addr,
+			areq_ctx->dst.mlli_nents, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		break;
+	case SSI_DMA_BUF_NULL:
+	default:
+		SSI_LOG_ERR("CIPHER: Invalid SRC/DST buffer type\n");
+	}
+
+	*seq_size = (++idx);
+}
+
+static inline void ssi_aead_process_digest_result_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int idx = *seq_size;
+	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
+				DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
+	int direct = req_ctx->gen_ctx.op_type;
+
+	/* Get final ICV result */
+	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->icv_dma_addr,
+			ctx->authsize, NS_BIT, 1);
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+		if (ctx->auth_mode == DRV_HASH_XCBC_MAC) {
+			HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC); 
+		} else {
+			HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],
+				HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		}
+	} else { /*Decrypt*/
+		/* Get ICV out from hardware */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
+			ctx->authsize, NS_BIT, 1);
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+		if (ctx->auth_mode == DRV_HASH_XCBC_MAC) {
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+			HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+		} else {
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+		}
+	}
+
+	*seq_size = (++idx);
+}
+
+static inline void ssi_aead_setup_cipher_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int hw_iv_size = req_ctx->hw_iv_size;
+	unsigned int idx = *seq_size;
+	int direct = req_ctx->gen_ctx.op_type;
+
+	/* Setup cipher state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direct);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], ctx->flow_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+		req_ctx->gen_ctx.iv_dma_addr, hw_iv_size, NS_BIT);
+	if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	} else {
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	}
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->cipher_mode);
+	idx++;
+
+	/* Setup enc. key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direct);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], ctx->flow_mode);
+	if (ctx->flow_mode == S_DIN_to_AES) {
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
+			((ctx->enc_keylen == 24) ?
+			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), NS_BIT);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	} else {
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr,
+			ctx->enc_keylen, NS_BIT);
+		HW_DESC_SET_KEY_SIZE_DES(&desc[idx], ctx->enc_keylen);
+	}
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->cipher_mode);
+	idx++;
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_process_cipher(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size,
+	unsigned int data_flow_mode)
+{
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	int direct = req_ctx->gen_ctx.op_type;
+	unsigned int idx = *seq_size;
+
+	if (req_ctx->cryptlen == 0)
+		return; /*null processing*/
+
+	ssi_aead_setup_cipher_desc(req, desc, &idx);
+	ssi_aead_process_cipher_data_desc(req, data_flow_mode, desc, &idx);
+	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		/* We must wait for DMA to write all cipher */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		idx++;
+	}
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_hmac_setup_digest_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
+				DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+				CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
+	unsigned int idx = *seq_size;
+
+	/* Loading hash ipad xor key state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+		ctx->auth_state.hmac.ipad_opad_dma_addr,
+		digest_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Load init. digest len (64 bytes) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	HW_DESC_SET_DIN_SRAM(&desc[idx],
+		ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, hash_mode),
+		HASH_LEN_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_xcbc_setup_digest_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	unsigned int idx = *seq_size;
+
+	/* Loading MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0, CC_AES_BLOCK_SIZE);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* Setup XCBC MAC K1 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     ctx->auth_state.xcbc.xcbc_keys_dma_addr,
+			     AES_KEYSIZE_128, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* Setup XCBC MAC K2 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     (ctx->auth_state.xcbc.xcbc_keys_dma_addr + 
+			      AES_KEYSIZE_128),
+			     AES_KEYSIZE_128, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* Setup XCBC MAC K3 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     (ctx->auth_state.xcbc.xcbc_keys_dma_addr +
+			      2 * AES_KEYSIZE_128),
+			     AES_KEYSIZE_128, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE2);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_process_digest_header_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	unsigned int idx = *seq_size;
+	/* Hash associated data */
+	if (req->assoclen > 0)
+		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, &idx);
+
+	/* Hash IV */
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_process_digest_scheme_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct ssi_aead_handle *aead_handle = ctx->drvdata->aead_handle;
+	unsigned int hash_mode = (ctx->auth_mode == DRV_HASH_SHA1) ?
+				DRV_HASH_HW_SHA1 : DRV_HASH_HW_SHA256;
+	unsigned int digest_size = (ctx->auth_mode == DRV_HASH_SHA1) ? 
+				CC_SHA1_DIGEST_SIZE : CC_SHA256_DIGEST_SIZE;
+	unsigned int idx = *seq_size;
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	HW_DESC_SET_DOUT_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
+			HASH_LEN_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+	idx++;
+
+	/* Get final ICV result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
+			digest_size);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	idx++;
+
+	/* Loading hash opad xor key state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+		(ctx->auth_state.hmac.ipad_opad_dma_addr + digest_size),
+		digest_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Load init. digest len (64 bytes) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hash_mode);
+	HW_DESC_SET_DIN_SRAM(&desc[idx],
+		ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, hash_mode),
+		HASH_LEN_SIZE);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Perform HASH update */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_SRAM(&desc[idx], aead_handle->sram_workspace_addr,
+			digest_size);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_load_mlli_to_sram(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+
+	if (unlikely(
+		(req_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) ||
+		(req_ctx->data_buff_type == SSI_DMA_BUF_MLLI) ||
+		(req_ctx->is_single_pass == false))) {
+		SSI_LOG_DEBUG("Copy-to-sram: mlli_dma=%08x, mlli_size=%u\n",
+			(unsigned int)ctx->drvdata->mlli_sram_addr,
+			req_ctx->mlli_params.mlli_len);
+		/* Copy MLLI table host-to-sram */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+			req_ctx->mlli_params.mlli_dma_addr,
+			req_ctx->mlli_params.mlli_len, NS_BIT);
+		HW_DESC_SET_DOUT_SRAM(&desc[*seq_size],
+			ctx->drvdata->mlli_sram_addr,
+			req_ctx->mlli_params.mlli_len);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], BYPASS);
+		(*seq_size)++;
+	}
+}
+
+static inline enum FlowMode ssi_aead_get_data_flow_mode(
+	enum drv_crypto_direction direct,
+	enum FlowMode setup_flow_mode,
+	bool is_single_pass)
+{
+	enum FlowMode data_flow_mode;
+
+	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		if (setup_flow_mode == S_DIN_to_AES)
+			data_flow_mode = likely(is_single_pass) ?
+				AES_to_HASH_and_DOUT : DIN_AES_DOUT;
+		else
+			data_flow_mode = likely(is_single_pass) ?
+				DES_to_HASH_and_DOUT : DIN_DES_DOUT;
+	} else { /* Decrypt */
+		if (setup_flow_mode == S_DIN_to_AES)
+			data_flow_mode = likely(is_single_pass) ?
+					AES_and_HASH : DIN_AES_DOUT;
+		else
+			data_flow_mode = likely(is_single_pass) ?
+					DES_and_HASH : DIN_DES_DOUT;
+	}
+
+	return data_flow_mode;
+}
+
+static inline void ssi_aead_hmac_authenc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	int direct = req_ctx->gen_ctx.op_type;
+	unsigned int data_flow_mode = ssi_aead_get_data_flow_mode(
+		direct, ctx->flow_mode, req_ctx->is_single_pass);
+
+	if (req_ctx->is_single_pass == true) {
+		/**
+		 * Single-pass flow
+		 */
+		ssi_aead_hmac_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_setup_cipher_desc(req, desc, seq_size);
+		ssi_aead_process_digest_header_desc(req, desc, seq_size);
+		ssi_aead_process_cipher_data_desc(req, data_flow_mode, desc, seq_size);
+		ssi_aead_process_digest_scheme_desc(req, desc, seq_size);
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+		return;
+	}
+
+	/** 
+	 * Double-pass flow
+	 * Fallback for unsupported single-pass modes, 
+	 * i.e. using assoc. data of non-word-multiple */
+	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		/* encrypt first.. */
+		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
+		/* authenc after..*/
+		ssi_aead_hmac_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_process_authenc_data_desc(req, DIN_HASH, desc, seq_size, direct);
+		ssi_aead_process_digest_scheme_desc(req, desc, seq_size);
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+
+	} else { /*DECRYPT*/
+		/* authenc first..*/
+		ssi_aead_hmac_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_process_authenc_data_desc(req, DIN_HASH, desc, seq_size, direct);
+		ssi_aead_process_digest_scheme_desc(req, desc, seq_size);
+		/* decrypt after.. */
+		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
+		/* read the digest result with setting the completion bit
+		   must be after the cipher operation */
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+	}
+}
+
+static inline void
+ssi_aead_xcbc_authenc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	int direct = req_ctx->gen_ctx.op_type;
+	unsigned int data_flow_mode = ssi_aead_get_data_flow_mode(
+		direct, ctx->flow_mode, req_ctx->is_single_pass);
+
+	if (req_ctx->is_single_pass == true) {
+		/**
+		 * Single-pass flow
+		 */
+		ssi_aead_xcbc_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_setup_cipher_desc(req, desc, seq_size);
+		ssi_aead_process_digest_header_desc(req, desc, seq_size);
+		ssi_aead_process_cipher_data_desc(req, data_flow_mode, desc, seq_size);
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+		return;
+	}
+
+	/** 
+	 * Double-pass flow
+	 * Fallback for unsupported single-pass modes, 
+	 * i.e. using assoc. data of non-word-multiple */
+	if (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		/* encrypt first.. */
+		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
+		/* authenc after.. */
+		ssi_aead_xcbc_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_process_authenc_data_desc(req, DIN_HASH, desc, seq_size, direct);
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+	} else { /*DECRYPT*/
+		/* authenc first.. */
+		ssi_aead_xcbc_setup_digest_desc(req, desc, seq_size);
+		ssi_aead_process_authenc_data_desc(req, DIN_HASH, desc, seq_size, direct);
+		/* decrypt after..*/
+		ssi_aead_process_cipher(req, desc, seq_size, data_flow_mode);
+		/* read the digest result with setting the completion bit
+		   must be after the cipher operation */
+		ssi_aead_process_digest_result_desc(req, desc, seq_size);
+	}
+}
+
+static int validate_data_size(struct ssi_aead_ctx *ctx,
+	enum drv_crypto_direction direct, struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	unsigned int assoclen = req->assoclen;
+	unsigned int cipherlen = (direct == DRV_CRYPTO_DIRECTION_DECRYPT) ?
+			(req->cryptlen - ctx->authsize) : req->cryptlen;
+
+	if (unlikely((direct == DRV_CRYPTO_DIRECTION_DECRYPT) &&
+		(req->cryptlen < ctx->authsize)))
+		goto data_size_err;
+
+	areq_ctx->is_single_pass = true; /*defaulted to fast flow*/
+
+	switch (ctx->flow_mode) {
+	case S_DIN_to_AES:
+		if (unlikely((ctx->cipher_mode == DRV_CIPHER_CBC) &&
+			!IS_ALIGNED(cipherlen, AES_BLOCK_SIZE)))
+			goto data_size_err;
+		if (ctx->cipher_mode == DRV_CIPHER_CCM)
+			break;
+		if (ctx->cipher_mode == DRV_CIPHER_GCTR)
+		{
+			if (areq_ctx->plaintext_authenticate_only == true)
+				areq_ctx->is_single_pass = false; 
+			break;
+		}
+
+		if (!IS_ALIGNED(assoclen, sizeof(uint32_t)))
+			areq_ctx->is_single_pass = false;
+
+		if ((ctx->cipher_mode == DRV_CIPHER_CTR) &&
+		    !IS_ALIGNED(cipherlen, sizeof(uint32_t)))
+			areq_ctx->is_single_pass = false;
+
+		break;
+	case S_DIN_to_DES:
+		if (unlikely(!IS_ALIGNED(cipherlen, DES_BLOCK_SIZE)))
+			goto data_size_err;
+		if (unlikely(!IS_ALIGNED(assoclen, DES_BLOCK_SIZE)))
+			areq_ctx->is_single_pass = false;
+		break;
+	default:
+		SSI_LOG_ERR("Unexpected flow mode (%d)\n", ctx->flow_mode);
+		goto data_size_err;
+	}
+
+	return 0;
+
+data_size_err:
+	return -EINVAL;
+}
+
+#if SSI_CC_HAS_AES_CCM
+static unsigned int format_ccm_a0(uint8_t *pA0Buff, uint32_t headerSize)
+{
+	unsigned int len = 0;
+	if ( headerSize == 0 ) {
+		return 0;
+	} 
+	if ( headerSize < ((1UL << 16) - (1UL << 8) )) {
+		len = 2;
+
+		pA0Buff[0] = (headerSize >> 8) & 0xFF;
+		pA0Buff[1] = headerSize & 0xFF;
+	} else {
+		len = 6;
+
+		pA0Buff[0] = 0xFF;
+		pA0Buff[1] = 0xFE;
+		pA0Buff[2] = (headerSize >> 24) & 0xFF;
+		pA0Buff[3] = (headerSize >> 16) & 0xFF;
+		pA0Buff[4] = (headerSize >> 8) & 0xFF;
+		pA0Buff[5] = headerSize & 0xFF;
+	}
+
+	return len;
+}
+
+static int set_msg_len(u8 *block, unsigned int msglen, unsigned int csize)
+{
+	__be32 data;
+
+	memset(block, 0, csize);
+	block += csize;
+
+	if (csize >= 4)
+		csize = 4;
+	else if (msglen > (1 << (8 * csize)))
+		return -EOVERFLOW;
+
+	data = cpu_to_be32(msglen);
+	memcpy(block - csize, (u8 *)&data + 4 - csize, csize);
+
+	return 0;
+}
+
+static inline int ssi_aead_ccm(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int idx = *seq_size;
+	unsigned int cipher_flow_mode;
+	dma_addr_t mac_result;
+
+
+	if (req_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		cipher_flow_mode = AES_to_HASH_and_DOUT;
+		mac_result = req_ctx->mac_buf_dma_addr;
+	} else { /* Encrypt */
+		cipher_flow_mode = AES_and_HASH;
+		mac_result = req_ctx->icv_dma_addr;
+	}
+
+	/* load key */
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);	
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
+			((ctx->enc_keylen == 24) ? 
+			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), 
+			 NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* load ctr state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			req_ctx->gen_ctx.iv_dma_addr, 
+			     AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* load MAC key */
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);	
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
+			((ctx->enc_keylen == 24) ? 
+			 CC_AES_KEY_SIZE_MAX : ctx->enc_keylen), 
+			 NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* load MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			req_ctx->mac_buf_dma_addr, 
+			     AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+
+	/* process assoc data */
+	if (req->assoclen > 0) {
+		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, &idx);
+	} else {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+				      sg_dma_address(&req_ctx->ccm_adata_sg),
+				     AES_BLOCK_SIZE + req_ctx->ccm_hdr_size,
+				     NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+	}
+
+	/* process the cipher */
+	if (req_ctx->cryptlen != 0) {
+		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, &idx);
+	}
+
+	/* Read temporal MAC */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
+			      ctx->authsize, NS_BIT, 0);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* load AES-CTR state (for last MAC calculation)*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     req_ctx->ccm_iv0_dma_addr ,
+			     AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* encrypt the "T" value and store MAC in mac_state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			req_ctx->mac_buf_dma_addr , ctx->authsize, NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_result , ctx->authsize, NS_BIT, 1);
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;	
+
+	*seq_size = idx;
+	return 0;
+}
+
+static int config_ccm_adata(struct aead_request *req) {
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	//unsigned int size_of_a = 0, rem_a_size = 0;
+	unsigned int lp = req->iv[0];
+	/* Note: The code assume that req->iv[0] already contains the value of L' of RFC3610 */
+	unsigned int l = lp + 1;  /* This is L' of RFC 3610. */
+	unsigned int m = ctx->authsize;  /* This is M' of RFC 3610. */
+	uint8_t *b0 = req_ctx->ccm_config + CCM_B0_OFFSET;
+	uint8_t *a0 = req_ctx->ccm_config + CCM_A0_OFFSET;
+	uint8_t *ctr_count_0 = req_ctx->ccm_config + CCM_CTR_COUNT_0_OFFSET;
+	unsigned int cryptlen = (req_ctx->gen_ctx.op_type == 
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
+				req->cryptlen : 
+				(req->cryptlen - ctx->authsize);
+	int rc;
+	memset(req_ctx->mac_buf, 0, AES_BLOCK_SIZE);
+	memset(req_ctx->ccm_config, 0, AES_BLOCK_SIZE*3);
+
+	/* taken from crypto/ccm.c */
+	/* 2 <= L <= 8, so 1 <= L' <= 7. */
+	if (2 > l || l > 8) {
+		SSI_LOG_ERR("illegal iv value %X\n",req->iv[0]);
+		return -EINVAL;
+	}
+	memcpy(b0, req->iv, AES_BLOCK_SIZE);
+
+	/* format control info per RFC 3610 and
+	 * NIST Special Publication 800-38C
+	 */
+	*b0 |= (8 * ((m - 2) / 2));
+	if (req->assoclen > 0)
+		*b0 |= 64;  /* Enable bit 6 if Adata exists. */
+	
+	rc = set_msg_len(b0 + 16 - l, cryptlen, l);  /* Write L'. */
+	if (rc != 0) {
+		return rc;
+	}
+	 /* END of "taken from crypto/ccm.c" */
+	
+	/* l(a) - size of associated data. */
+	req_ctx->ccm_hdr_size = format_ccm_a0 (a0, req->assoclen);
+
+	memset(req->iv + 15 - req->iv[0], 0, req->iv[0] + 1);
+	req->iv [15] = 1;
+
+	memcpy(ctr_count_0, req->iv, AES_BLOCK_SIZE) ;
+	ctr_count_0[15] = 0;
+
+	return 0;
+}
+
+static void ssi_rfc4309_ccm_process(struct aead_request *req)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+
+	/* L' */
+	memset(areq_ctx->ctr_iv, 0, AES_BLOCK_SIZE);
+	areq_ctx->ctr_iv[0] = 3;  /* For RFC 4309, always use 4 bytes for message length (at most 2^32-1 bytes). */
+
+	/* In RFC 4309 there is an 11-bytes nonce+IV part, that we build here. */
+	memcpy(areq_ctx->ctr_iv + CCM_BLOCK_NONCE_OFFSET, ctx->ctr_nonce, CCM_BLOCK_NONCE_SIZE);
+	memcpy(areq_ctx->ctr_iv + CCM_BLOCK_IV_OFFSET,    req->iv,        CCM_BLOCK_IV_SIZE);
+	req->iv = areq_ctx->ctr_iv;	
+	req->assoclen -= CCM_BLOCK_IV_SIZE;
+}
+#endif /*SSI_CC_HAS_AES_CCM*/
+
+#if SSI_CC_HAS_AES_GCM
+
+static inline void ssi_aead_gcm_setup_ghash_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int idx = *seq_size;
+
+	/* load key to AES*/
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);	
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
+			ctx->enc_keylen, NS_BIT); 
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* process one zero block to generate hkey */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx],
+				  req_ctx->hkey_dma_addr,
+				  AES_BLOCK_SIZE,
+				  NS_BIT, 0); 
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;
+
+	/* Memory Barrier */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* Load GHASH subkey */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			req_ctx->hkey_dma_addr, 
+				 AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Configure Hash Engine to work with GHASH.
+	   Since it was not possible to extend HASH submodes to add GHASH,
+	   The following command is necessary in order to select GHASH (according to HW designers)*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
+	HW_DESC_SET_CIPHER_DO(&desc[idx], 1); //1=AES_SK RKEK
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Load GHASH initial STATE (which is 0). (for any hash there is an initial state) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_gcm_setup_gctr_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int idx = *seq_size;
+
+	/* load key to AES*/
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);	
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->enckey_dma_addr, 
+			ctx->enc_keylen, NS_BIT); 
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	if ((req_ctx->cryptlen != 0) && (req_ctx->plaintext_authenticate_only==false)){
+		/* load AES/CTR initial CTR value inc by 2*/
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				req_ctx->gcm_iv_inc2_dma_addr, 
+					 AES_BLOCK_SIZE, NS_BIT);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		idx++;
+	}
+
+	*seq_size = idx;
+}
+
+static inline void ssi_aead_process_gcm_result_desc(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	dma_addr_t mac_result; 
+	unsigned int idx = *seq_size;
+
+	if (req_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		mac_result = req_ctx->mac_buf_dma_addr;
+	} else { /* Encrypt */
+		mac_result = req_ctx->icv_dma_addr;
+	}
+
+	/* process(ghash) gcm_block_len */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+		req_ctx->gcm_block_len_dma_addr,
+		AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	/* Store GHASH state after GHASH(Associated Data + Cipher +LenBlock) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], req_ctx->mac_buf_dma_addr,
+				  AES_BLOCK_SIZE, NS_BIT, 0);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+
+	idx++; 
+
+	/* load AES/CTR initial CTR value inc by 1*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->enc_keylen);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				 req_ctx->gcm_iv_inc1_dma_addr, 
+				 AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Memory Barrier */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* process GCTR on stored GHASH and store MAC in mac_state*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+		req_ctx->mac_buf_dma_addr,
+		AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_result, ctx->authsize, NS_BIT, 1);
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;	
+
+	*seq_size = idx;
+}
+
+static inline int ssi_aead_gcm(
+	struct aead_request *req,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	unsigned int idx = *seq_size;
+	unsigned int cipher_flow_mode;
+
+	if (req_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		cipher_flow_mode = AES_and_HASH;
+	} else { /* Encrypt */
+		cipher_flow_mode = AES_to_HASH_and_DOUT;
+	}
+
+
+	//in RFC4543 no data to encrypt. just copy data from src to dest.
+	if (req_ctx->plaintext_authenticate_only==true){     
+		ssi_aead_process_cipher_data_desc(req, BYPASS, desc, seq_size);
+		ssi_aead_gcm_setup_ghash_desc(req, desc, seq_size);
+		/* process(ghash) assoc data */
+		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, seq_size);
+		ssi_aead_gcm_setup_gctr_desc(req, desc, seq_size);
+		ssi_aead_process_gcm_result_desc(req, desc, seq_size);
+		idx = *seq_size;
+		return 0;
+	}
+
+	// for gcm and rfc4106.
+	ssi_aead_gcm_setup_ghash_desc(req, desc, seq_size);
+	/* process(ghash) assoc data */
+	if (req->assoclen > 0)
+		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, seq_size);
+	ssi_aead_gcm_setup_gctr_desc(req, desc, seq_size);
+	/* process(gctr+ghash) */
+	if (req_ctx->cryptlen != 0)
+		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size); 
+	ssi_aead_process_gcm_result_desc(req, desc, seq_size);
+
+	idx = *seq_size;
+	return 0;
+}
+
+#ifdef CC_DEBUG
+static inline void ssi_aead_dump_gcm(
+	const char* title,
+	struct aead_request *req)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+
+	if (ctx->cipher_mode != DRV_CIPHER_GCTR)
+		return;
+
+	if (title != NULL) {
+		SSI_LOG_DEBUG("----------------------------------------------------------------------------------");
+		SSI_LOG_DEBUG("%s\n", title);
+	}
+
+	SSI_LOG_DEBUG("cipher_mode %d, authsize %d, enc_keylen %d, assoclen %d, cryptlen %d \n", \
+				 ctx->cipher_mode, ctx->authsize, ctx->enc_keylen, req->assoclen, req_ctx->cryptlen );
+
+	if ( ctx->enckey != NULL ) {
+		dump_byte_array("mac key",ctx->enckey, 16);
+	}
+
+	dump_byte_array("req->iv",req->iv, AES_BLOCK_SIZE);
+
+	dump_byte_array("gcm_iv_inc1",req_ctx->gcm_iv_inc1, AES_BLOCK_SIZE);
+
+	dump_byte_array("gcm_iv_inc2",req_ctx->gcm_iv_inc2, AES_BLOCK_SIZE);
+
+	dump_byte_array("hkey",req_ctx->hkey, AES_BLOCK_SIZE);
+
+	dump_byte_array("mac_buf",req_ctx->mac_buf, AES_BLOCK_SIZE);
+
+	dump_byte_array("gcm_len_block",req_ctx->gcm_len_block.lenA, AES_BLOCK_SIZE);
+
+	if (req->src!=NULL && req->cryptlen) {
+		dump_byte_array("req->src",sg_virt(req->src), req->cryptlen+req->assoclen);
+	}
+
+	if (req->dst!=NULL) {
+		dump_byte_array("req->dst",sg_virt(req->dst), req->cryptlen+ctx->authsize+req->assoclen);
+    }
+}
+#endif
+
+static int config_gcm_context(struct aead_request *req) {
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *req_ctx = aead_request_ctx(req);
+	
+	unsigned int cryptlen = (req_ctx->gen_ctx.op_type == 
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
+				req->cryptlen : 
+				(req->cryptlen - ctx->authsize);
+	__be32 counter = cpu_to_be32(2);
+
+	SSI_LOG_DEBUG("config_gcm_context() cryptlen = %d, req->assoclen = %d ctx->authsize = %d \n", cryptlen, req->assoclen, ctx->authsize);
+
+	memset(req_ctx->hkey, 0, AES_BLOCK_SIZE);
+
+	memset(req_ctx->mac_buf, 0, AES_BLOCK_SIZE);
+
+	memcpy(req->iv + 12, &counter, 4);
+	memcpy(req_ctx->gcm_iv_inc2, req->iv, 16);
+
+	counter = cpu_to_be32(1);
+	memcpy(req->iv + 12, &counter, 4);
+	memcpy(req_ctx->gcm_iv_inc1, req->iv, 16);
+
+
+	if (req_ctx->plaintext_authenticate_only == false)
+	{
+		__be64 temp64;
+		temp64 = cpu_to_be64(req->assoclen * 8);
+		memcpy ( &req_ctx->gcm_len_block.lenA , &temp64, sizeof(temp64) );
+		temp64 = cpu_to_be64(cryptlen * 8);
+		memcpy ( &req_ctx->gcm_len_block.lenC , &temp64, 8 );
+	}
+	else { //rfc4543=>  all data(AAD,IV,Plain) are considered additional data that is nothing is encrypted.
+		__be64 temp64;
+		temp64 = cpu_to_be64((req->assoclen+GCM_BLOCK_RFC4_IV_SIZE+cryptlen) * 8);
+		memcpy ( &req_ctx->gcm_len_block.lenA , &temp64, sizeof(temp64) );
+		temp64 = 0;
+		memcpy ( &req_ctx->gcm_len_block.lenC , &temp64, 8 );
+	}
+
+	return 0;
+}
+
+static void ssi_rfc4_gcm_process(struct aead_request *req)
+{
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+
+	memcpy(areq_ctx->ctr_iv + GCM_BLOCK_RFC4_NONCE_OFFSET, ctx->ctr_nonce, GCM_BLOCK_RFC4_NONCE_SIZE);
+	memcpy(areq_ctx->ctr_iv + GCM_BLOCK_RFC4_IV_OFFSET,    req->iv, GCM_BLOCK_RFC4_IV_SIZE);
+	req->iv = areq_ctx->ctr_iv;	
+	req->assoclen -= GCM_BLOCK_RFC4_IV_SIZE;
+}
+
+
+#endif /*SSI_CC_HAS_AES_GCM*/
+
+static int ssi_aead_process(struct aead_request *req, enum drv_crypto_direction direct)
+{
+	int rc = 0;
+	int seq_len = 0;
+	HwDesc_s desc[MAX_AEAD_PROCESS_SEQ]; 
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ssi_crypto_req ssi_req = {};
+
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	SSI_LOG_DEBUG("%s context=%p req=%p iv=%p src=%p src_ofs=%d dst=%p dst_ofs=%d cryptolen=%d\n",
+		((direct==DRV_CRYPTO_DIRECTION_ENCRYPT)?"Encrypt":"Decrypt"), ctx, req, req->iv,
+		sg_virt(req->src), req->src->offset, sg_virt(req->dst), req->dst->offset, req->cryptlen);
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	/* STAT_PHASE_0: Init and sanity checks */
+	START_CYCLE_COUNT();
+	
+	/* Check data length according to mode */
+	if (unlikely(validate_data_size(ctx, direct, req) != 0)) {
+		SSI_LOG_ERR("Unsupported crypt/assoc len %d/%d.\n",
+				req->cryptlen, req->assoclen);
+		crypto_aead_set_flags(tfm, CRYPTO_TFM_RES_BAD_BLOCK_LEN);
+		return -EINVAL;
+	}
+
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_aead_complete;
+	ssi_req.user_arg = (void *)req;
+
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = (direct == DRV_CRYPTO_DIRECTION_DECRYPT) ?
+		STAT_OP_TYPE_DECODE : STAT_OP_TYPE_ENCODE;
+#endif
+	/* Setup request context */
+	areq_ctx->gen_ctx.op_type = direct;
+	areq_ctx->req_authsize = ctx->authsize;
+	areq_ctx->cipher_mode = ctx->cipher_mode;
+
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_0);
+
+	/* STAT_PHASE_1: Map buffers */
+	START_CYCLE_COUNT();
+	
+	if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+		/* Build CTR IV - Copy nonce from last 4 bytes in
+		*  CTR key to first 4 bytes in CTR IV */
+		memcpy(areq_ctx->ctr_iv, ctx->ctr_nonce, CTR_RFC3686_NONCE_SIZE);
+		if (areq_ctx->backup_giv == NULL) /*User none-generated IV*/
+			memcpy(areq_ctx->ctr_iv + CTR_RFC3686_NONCE_SIZE,
+				req->iv, CTR_RFC3686_IV_SIZE);
+		/* Initialize counter portion of counter block */
+		*(__be32 *)(areq_ctx->ctr_iv + CTR_RFC3686_NONCE_SIZE +
+			    CTR_RFC3686_IV_SIZE) = cpu_to_be32(1);
+
+		/* Replace with counter iv */
+		req->iv = areq_ctx->ctr_iv;
+		areq_ctx->hw_iv_size = CTR_RFC3686_BLOCK_SIZE;
+	} else if ((ctx->cipher_mode == DRV_CIPHER_CCM) || 
+		   (ctx->cipher_mode == DRV_CIPHER_GCTR) ) {
+		areq_ctx->hw_iv_size = AES_BLOCK_SIZE;
+		if (areq_ctx->ctr_iv != req->iv) {
+			memcpy(areq_ctx->ctr_iv, req->iv, crypto_aead_ivsize(tfm));
+			req->iv = areq_ctx->ctr_iv;
+		}
+	}  else {
+		areq_ctx->hw_iv_size = crypto_aead_ivsize(tfm);
+	}
+
+#if SSI_CC_HAS_AES_CCM
+	if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+		rc = config_ccm_adata(req);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("config_ccm_adata() returned with a failure %d!", rc);
+			goto exit; 
+		}
+	} else {
+		areq_ctx->ccm_hdr_size = ccm_header_size_null;		
+	}
+#else
+	areq_ctx->ccm_hdr_size = ccm_header_size_null;		
+#endif /*SSI_CC_HAS_AES_CCM*/
+
+#if SSI_CC_HAS_AES_GCM 
+	if (ctx->cipher_mode == DRV_CIPHER_GCTR) {
+		rc = config_gcm_context(req);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("config_gcm_context() returned with a failure %d!", rc);
+			goto exit; 
+		}
+	} 
+#endif /*SSI_CC_HAS_AES_GCM*/
+
+	rc = ssi_buffer_mgr_map_aead_request(ctx->drvdata, req);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("map_request() failed\n");
+		goto exit;
+	}
+
+	/* do we need to generate IV? */
+	if (areq_ctx->backup_giv != NULL) {
+
+		/* set the DMA mapped IV address*/
+		if (ctx->cipher_mode == DRV_CIPHER_CTR) {
+			ssi_req.ivgen_dma_addr[0] = areq_ctx->gen_ctx.iv_dma_addr + CTR_RFC3686_NONCE_SIZE;
+			ssi_req.ivgen_dma_addr_len = 1;
+		} else if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+			/* In ccm, the IV needs to exist both inside B0 and inside the counter.
+			   It is also copied to iv_dma_addr for other reasons (like returning
+			   it to the user).
+			   So, using 3 (identical) IV outputs. */
+			ssi_req.ivgen_dma_addr[0] = areq_ctx->gen_ctx.iv_dma_addr + CCM_BLOCK_IV_OFFSET;
+			ssi_req.ivgen_dma_addr[1] = sg_dma_address(&areq_ctx->ccm_adata_sg) + CCM_B0_OFFSET          + CCM_BLOCK_IV_OFFSET;
+			ssi_req.ivgen_dma_addr[2] = sg_dma_address(&areq_ctx->ccm_adata_sg) + CCM_CTR_COUNT_0_OFFSET + CCM_BLOCK_IV_OFFSET;
+			ssi_req.ivgen_dma_addr_len = 3;
+		} else {
+			ssi_req.ivgen_dma_addr[0] = areq_ctx->gen_ctx.iv_dma_addr;
+			ssi_req.ivgen_dma_addr_len = 1;
+		}
+
+		/* set the IV size (8/16 B long)*/
+		ssi_req.ivgen_size = crypto_aead_ivsize(tfm);
+	}
+
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_1);
+
+	/* STAT_PHASE_2: Create sequence */
+	START_CYCLE_COUNT();
+
+	/* Load MLLI tables to SRAM if necessary */
+	ssi_aead_load_mlli_to_sram(req, desc, &seq_len);
+
+	/*TODO: move seq len by reference */
+	switch (ctx->auth_mode) {
+	case DRV_HASH_SHA1:
+	case DRV_HASH_SHA256:
+		ssi_aead_hmac_authenc(req, desc, &seq_len);
+		break;
+	case DRV_HASH_XCBC_MAC:
+		ssi_aead_xcbc_authenc(req, desc, &seq_len);
+		break;
+#if ( SSI_CC_HAS_AES_CCM || SSI_CC_HAS_AES_GCM )
+	case DRV_HASH_NULL:
+#if SSI_CC_HAS_AES_CCM
+		if (ctx->cipher_mode == DRV_CIPHER_CCM) {
+			ssi_aead_ccm(req, desc, &seq_len);
+		}
+#endif /*SSI_CC_HAS_AES_CCM*/
+#if SSI_CC_HAS_AES_GCM
+		if (ctx->cipher_mode == DRV_CIPHER_GCTR) {
+			ssi_aead_gcm(req, desc, &seq_len);
+		}
+#endif /*SSI_CC_HAS_AES_GCM*/
+			break;
+#endif
+	default:	
+		SSI_LOG_ERR("Unsupported authenc (%d)\n", ctx->auth_mode);
+		ssi_buffer_mgr_unmap_aead_request(dev, req);
+		rc = -ENOTSUPP;
+		goto exit;
+	}
+
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_2);
+
+	/* STAT_PHASE_3: Lock HW and push sequence */
+	START_CYCLE_COUNT();
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, seq_len, 1);
+
+	if (unlikely(rc != -EINPROGRESS)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		ssi_buffer_mgr_unmap_aead_request(dev, req);
+	}
+
+	
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
+exit:
+	return rc;
+}
+
+static int ssi_aead_encrypt(struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc;
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	areq_ctx->is_gcm4543 = false;
+
+	areq_ctx->plaintext_authenticate_only = false;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_ENCRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+
+	return rc;
+}
+
+#if SSI_CC_HAS_AES_CCM
+static int ssi_rfc4309_ccm_encrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_encrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc = -EINVAL;
+
+	if (!valid_assoclen(req)) {
+		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen );
+		goto out;
+	}
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	areq_ctx->is_gcm4543 = true;
+	
+	ssi_rfc4309_ccm_process(req);
+	
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_ENCRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+out:
+	return rc;
+}
+#endif /* SSI_CC_HAS_AES_CCM */
+
+static int ssi_aead_decrypt(struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc;
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	areq_ctx->is_gcm4543 = false;
+
+	areq_ctx->plaintext_authenticate_only = false;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_DECRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+
+	return rc;
+
+}
+
+#if SSI_CC_HAS_AES_CCM
+static int ssi_rfc4309_ccm_decrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_decrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc = -EINVAL;
+
+	if (!valid_assoclen(req)) {
+		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
+		goto out;
+	}
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	
+	areq_ctx->is_gcm4543 = true;
+	ssi_rfc4309_ccm_process(req);
+	
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_DECRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+
+out:
+	return rc;
+}
+#endif /* SSI_CC_HAS_AES_CCM */
+
+#if SSI_CC_HAS_AES_GCM
+
+static int ssi_rfc4106_gcm_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
+{
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	int rc = 0;
+	
+	SSI_LOG_DEBUG("ssi_rfc4106_gcm_setkey()  keylen %d, key %p \n", keylen, key );
+
+	if (keylen < 4)
+		return -EINVAL;
+
+	keylen -= 4;
+	memcpy(ctx->ctr_nonce, key + keylen, 4);
+
+	rc = ssi_aead_setkey(tfm, key, keylen);
+
+	return rc;
+}
+
+static int ssi_rfc4543_gcm_setkey(struct crypto_aead *tfm, const u8 *key, unsigned int keylen)
+{
+	struct ssi_aead_ctx *ctx = crypto_aead_ctx(tfm);
+	int rc = 0;
+	
+	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setkey()  keylen %d, key %p \n", keylen, key );
+
+	if (keylen < 4)
+		return -EINVAL;
+
+	keylen -= 4;
+	memcpy(ctx->ctr_nonce, key + keylen, 4);
+
+	rc = ssi_aead_setkey(tfm, key, keylen);
+
+	return rc;
+}
+
+static int ssi_gcm_setauthsize(struct crypto_aead *authenc,
+				      unsigned int authsize)
+{
+	switch (authsize) {
+	case 4:
+	case 8:
+	case 12:
+	case 13:
+	case 14:
+	case 15:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ssi_aead_setauthsize(authenc, authsize);
+}
+
+static int ssi_rfc4106_gcm_setauthsize(struct crypto_aead *authenc,
+				      unsigned int authsize)
+{
+        SSI_LOG_DEBUG("ssi_rfc4106_gcm_setauthsize()  authsize %d \n", authsize );
+
+        switch (authsize) {
+        case 8:
+        case 12:
+        case 16:
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        return ssi_aead_setauthsize(authenc, authsize);
+}
+
+static int ssi_rfc4543_gcm_setauthsize(struct crypto_aead *authenc,
+				      unsigned int authsize)
+{
+	SSI_LOG_DEBUG("ssi_rfc4543_gcm_setauthsize()  authsize %d \n", authsize );
+
+	if (authsize != 16)
+		return -EINVAL;
+
+	return ssi_aead_setauthsize(authenc, authsize);
+}
+
+static int ssi_rfc4106_gcm_encrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_encrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+        int rc = -EINVAL;
+
+	if (!valid_assoclen(req)) {
+		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
+		goto out;
+	}
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	
+	areq_ctx->plaintext_authenticate_only = false;
+
+	ssi_rfc4_gcm_process(req);
+	areq_ctx->is_gcm4543 = true;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_ENCRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+out:
+	return rc;
+}
+
+static int ssi_rfc4543_gcm_encrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_encrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc;
+	
+	//plaintext is not encryped with rfc4543
+	areq_ctx->plaintext_authenticate_only = true;
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	
+	ssi_rfc4_gcm_process(req);
+	areq_ctx->is_gcm4543 = true;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_ENCRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+
+	return rc;
+}
+
+static int ssi_rfc4106_gcm_decrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_decrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+        int rc = -EINVAL;
+
+	if (!valid_assoclen(req)) {
+		SSI_LOG_ERR("invalid Assoclen:%u\n", req->assoclen);
+		goto out;
+	}
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	
+	areq_ctx->plaintext_authenticate_only = false;
+
+	ssi_rfc4_gcm_process(req);
+	areq_ctx->is_gcm4543 = true;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_DECRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+out:
+	return rc;
+}
+
+static int ssi_rfc4543_gcm_decrypt(struct aead_request *req)
+{
+	/* Very similar to ssi_aead_decrypt() above. */
+
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc;
+
+	//plaintext is not decryped with rfc4543
+	areq_ctx->plaintext_authenticate_only = true;
+
+	/* No generated IV required */
+	areq_ctx->backup_iv = req->iv;
+	areq_ctx->backup_giv = NULL;
+	
+	ssi_rfc4_gcm_process(req);
+	areq_ctx->is_gcm4543 = true;
+
+	rc = ssi_aead_process(req, DRV_CRYPTO_DIRECTION_DECRYPT);
+	if (rc != -EINPROGRESS)
+		req->iv = areq_ctx->backup_iv;
+
+	return rc;
+}
+#endif /* SSI_CC_HAS_AES_GCM */
+
+/* DX Block aead alg */
+static struct ssi_alg_template aead_algs[] = {
+	{
+		.name = "authenc(hmac(sha1),cbc(aes))",
+		.driver_name = "authenc-hmac-sha1-cbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = AES_BLOCK_SIZE,
+			.maxauthsize = SHA1_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_SHA1,
+	},
+	{
+		.name = "authenc(hmac(sha1),cbc(des3_ede))",
+		.driver_name = "authenc-hmac-sha1-cbc-des3-dx",
+		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = DES3_EDE_BLOCK_SIZE,
+			.maxauthsize = SHA1_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_DES,
+		.auth_mode = DRV_HASH_SHA1,
+	},
+	{
+		.name = "authenc(hmac(sha256),cbc(aes))",
+		.driver_name = "authenc-hmac-sha256-cbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = AES_BLOCK_SIZE,
+			.maxauthsize = SHA256_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_SHA256,
+	},
+	{
+		.name = "authenc(hmac(sha256),cbc(des3_ede))",
+		.driver_name = "authenc-hmac-sha256-cbc-des3-dx",
+		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = DES3_EDE_BLOCK_SIZE,
+			.maxauthsize = SHA256_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_DES,
+		.auth_mode = DRV_HASH_SHA256,
+	},
+	{
+		.name = "authenc(xcbc(aes),cbc(aes))",
+		.driver_name = "authenc-xcbc-aes-cbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = AES_BLOCK_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_XCBC_MAC,
+	},
+	{
+		.name = "authenc(hmac(sha1),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha1-rfc3686-ctr-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA1_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_SHA1,
+	},
+	{
+		.name = "authenc(hmac(sha256),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-hmac-sha256-rfc3686-ctr-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = SHA256_DIGEST_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_SHA256,
+	},
+	{
+		.name = "authenc(xcbc(aes),rfc3686(ctr(aes)))",
+		.driver_name = "authenc-xcbc-aes-rfc3686-ctr-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_aead_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = CTR_RFC3686_IV_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_XCBC_MAC,
+	},
+#if SSI_CC_HAS_AES_CCM
+	{
+		.name = "ccm(aes)",
+		.driver_name = "ccm-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_ccm_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = AES_BLOCK_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CCM,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_NULL,
+	},
+	{
+		.name = "rfc4309(ccm(aes))",
+		.driver_name = "rfc4309-ccm-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_rfc4309_ccm_setkey,
+			.setauthsize = ssi_rfc4309_ccm_setauthsize,
+			.encrypt = ssi_rfc4309_ccm_encrypt,
+			.decrypt = ssi_rfc4309_ccm_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = CCM_BLOCK_IV_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_CCM,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_NULL,
+	},
+#endif /*SSI_CC_HAS_AES_CCM*/
+#if SSI_CC_HAS_AES_GCM
+	{
+		.name = "gcm(aes)",
+		.driver_name = "gcm-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_aead_setkey,
+			.setauthsize = ssi_gcm_setauthsize,
+			.encrypt = ssi_aead_encrypt,
+			.decrypt = ssi_aead_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = 12,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_GCTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_NULL,
+	},
+	{
+		.name = "rfc4106(gcm(aes))",
+		.driver_name = "rfc4106-gcm-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_rfc4106_gcm_setkey,
+			.setauthsize = ssi_rfc4106_gcm_setauthsize,
+			.encrypt = ssi_rfc4106_gcm_encrypt,
+			.decrypt = ssi_rfc4106_gcm_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = GCM_BLOCK_RFC4_IV_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_GCTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_NULL,
+	},
+	{
+		.name = "rfc4543(gcm(aes))",
+		.driver_name = "rfc4543-gcm-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = ssi_rfc4543_gcm_setkey,
+			.setauthsize = ssi_rfc4543_gcm_setauthsize,
+			.encrypt = ssi_rfc4543_gcm_encrypt,
+			.decrypt = ssi_rfc4543_gcm_decrypt,
+			.init = ssi_aead_init,
+			.exit = ssi_aead_exit,
+			.ivsize = GCM_BLOCK_RFC4_IV_SIZE,
+			.maxauthsize = AES_BLOCK_SIZE,
+		},
+		.cipher_mode = DRV_CIPHER_GCTR,
+		.flow_mode = S_DIN_to_AES,
+		.auth_mode = DRV_HASH_NULL,
+	}, 
+#endif /*SSI_CC_HAS_AES_GCM*/
+};
+
+static struct ssi_crypto_alg *ssi_aead_create_alg(struct ssi_alg_template *template)
+{
+	struct ssi_crypto_alg *t_alg;
+	struct aead_alg *alg;
+
+	t_alg = kzalloc(sizeof(struct ssi_crypto_alg), GFP_KERNEL);
+	if (!t_alg) {
+		SSI_LOG_ERR("failed to allocate t_alg\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	alg = &template->template_aead;
+
+	snprintf(alg->base.cra_name, CRYPTO_MAX_ALG_NAME, "%s", template->name);
+	snprintf(alg->base.cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+		 template->driver_name);
+	alg->base.cra_module = THIS_MODULE;
+	alg->base.cra_priority = SSI_CRA_PRIO;
+
+	alg->base.cra_ctxsize = sizeof(struct ssi_aead_ctx);
+	alg->base.cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY |
+			 template->type;
+	alg->init = ssi_aead_init;
+	alg->exit = ssi_aead_exit;
+
+	t_alg->aead_alg = *alg;
+
+	t_alg->cipher_mode = template->cipher_mode;
+	t_alg->flow_mode = template->flow_mode;
+	t_alg->auth_mode = template->auth_mode;
+
+	return t_alg;
+}
+
+int ssi_aead_free(struct ssi_drvdata *drvdata)
+{
+	struct ssi_crypto_alg *t_alg, *n;
+	struct ssi_aead_handle *aead_handle =
+		(struct ssi_aead_handle *)drvdata->aead_handle;
+
+	if (aead_handle != NULL) {
+		/* Remove registered algs */
+		list_for_each_entry_safe(t_alg, n, &aead_handle->aead_list, entry) {
+			crypto_unregister_aead(&t_alg->aead_alg);
+			list_del(&t_alg->entry);
+			kfree(t_alg);
+		}
+		kfree(aead_handle);
+		drvdata->aead_handle = NULL;
+	}
+
+	return 0;
+}
+
+int ssi_aead_alloc(struct ssi_drvdata *drvdata)
+{
+	struct ssi_aead_handle *aead_handle;
+	struct ssi_crypto_alg *t_alg;
+	int rc = -ENOMEM;
+	int alg;
+
+	aead_handle = kmalloc(sizeof(struct ssi_aead_handle), GFP_KERNEL);
+	if (aead_handle == NULL) {
+		rc = -ENOMEM;
+		goto fail0;
+	}
+
+	drvdata->aead_handle = aead_handle;
+
+	aead_handle->sram_workspace_addr = ssi_sram_mgr_alloc(
+		drvdata, MAX_HMAC_DIGEST_SIZE);
+	if (aead_handle->sram_workspace_addr == NULL_SRAM_ADDR) {
+		SSI_LOG_ERR("SRAM pool exhausted\n");
+		rc = -ENOMEM;
+		goto fail1;
+	}
+
+	INIT_LIST_HEAD(&aead_handle->aead_list);
+
+	/* Linux crypto */
+	for (alg = 0; alg < ARRAY_SIZE(aead_algs); alg++) {
+		t_alg = ssi_aead_create_alg(&aead_algs[alg]);
+		if (IS_ERR(t_alg)) {
+			rc = PTR_ERR(t_alg);
+			SSI_LOG_ERR("%s alg allocation failed\n",
+				 aead_algs[alg].driver_name);
+			goto fail1;
+		}
+		t_alg->drvdata = drvdata;
+		rc = crypto_register_aead(&t_alg->aead_alg);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("%s alg registration failed\n",
+				t_alg->aead_alg.base.cra_driver_name);
+			goto fail2;
+		} else {
+			list_add_tail(&t_alg->entry, &aead_handle->aead_list);
+			SSI_LOG_DEBUG("Registered %s\n", t_alg->aead_alg.base.cra_driver_name);
+		}
+	}
+
+	return 0;
+
+fail2:
+	kfree(t_alg);
+fail1:
+	ssi_aead_free(drvdata);
+fail0:
+	return rc;
+}
+
+
+
diff --git a/drivers/staging/ccree/ssi_aead.h b/drivers/staging/ccree/ssi_aead.h
new file mode 100644
index 000000000000..fe88c9e04f88
--- /dev/null
+++ b/drivers/staging/ccree/ssi_aead.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_aead.h
+   ARM CryptoCell AEAD Crypto API
+ */
+
+#ifndef __SSI_AEAD_H__
+#define __SSI_AEAD_H__
+
+#include <linux/kernel.h>
+#include <crypto/algapi.h>
+#include <crypto/ctr.h>
+
+
+/* mac_cmp - HW writes 8 B but all bytes hold the same value */
+#define ICV_CMP_SIZE 8
+#define CCM_CONFIG_BUF_SIZE (AES_BLOCK_SIZE*3)
+#define MAX_MAC_SIZE MAX(SHA256_DIGEST_SIZE, AES_BLOCK_SIZE)
+
+
+/* defines for AES GCM configuration buffer */
+#define GCM_BLOCK_LEN_SIZE 8
+
+#define GCM_BLOCK_RFC4_IV_OFFSET    	4  
+#define GCM_BLOCK_RFC4_IV_SIZE  	    8  /* IV size for rfc's */
+#define GCM_BLOCK_RFC4_NONCE_OFFSET 	0  
+#define GCM_BLOCK_RFC4_NONCE_SIZE   	4  
+
+
+
+/* Offsets into AES CCM configuration buffer */
+#define CCM_B0_OFFSET 0
+#define CCM_A0_OFFSET 16
+#define CCM_CTR_COUNT_0_OFFSET 32
+/* CCM B0 and CTR_COUNT constants. */
+#define CCM_BLOCK_NONCE_OFFSET 1  /* Nonce offset inside B0 and CTR_COUNT */
+#define CCM_BLOCK_NONCE_SIZE   3  /* Nonce size inside B0 and CTR_COUNT */
+#define CCM_BLOCK_IV_OFFSET    4  /* IV offset inside B0 and CTR_COUNT */
+#define CCM_BLOCK_IV_SIZE      8  /* IV size inside B0 and CTR_COUNT */
+
+enum aead_ccm_header_size {
+	ccm_header_size_null = -1,
+	ccm_header_size_zero = 0,
+	ccm_header_size_2 = 2,
+	ccm_header_size_6 = 6,
+	ccm_header_size_max = INT32_MAX
+};
+
+struct aead_req_ctx {
+	/* Allocate cache line although only 4 bytes are needed to
+	*  assure next field falls @ cache line 
+	*  Used for both: digest HW compare and CCM/GCM MAC value */
+	uint8_t mac_buf[MAX_MAC_SIZE] ____cacheline_aligned;
+	uint8_t ctr_iv[AES_BLOCK_SIZE] ____cacheline_aligned;
+
+	//used in gcm 
+	uint8_t gcm_iv_inc1[AES_BLOCK_SIZE] ____cacheline_aligned;
+	uint8_t gcm_iv_inc2[AES_BLOCK_SIZE] ____cacheline_aligned;
+	uint8_t hkey[AES_BLOCK_SIZE] ____cacheline_aligned;
+	struct {
+		uint8_t lenA[GCM_BLOCK_LEN_SIZE] ____cacheline_aligned;
+		uint8_t lenC[GCM_BLOCK_LEN_SIZE] ;
+	} gcm_len_block;
+
+	uint8_t ccm_config[CCM_CONFIG_BUF_SIZE] ____cacheline_aligned;
+	unsigned int hw_iv_size ____cacheline_aligned; /*HW actual size input*/
+	uint8_t backup_mac[MAX_MAC_SIZE]; /*used to prevent cache coherence problem*/
+	uint8_t *backup_iv; /*store iv for generated IV flow*/
+	uint8_t *backup_giv; /*store iv for rfc3686(ctr) flow*/
+	dma_addr_t mac_buf_dma_addr; /* internal ICV DMA buffer */
+	dma_addr_t ccm_iv0_dma_addr; /* buffer for internal ccm configurations */
+	dma_addr_t icv_dma_addr; /* Phys. address of ICV */
+
+	//used in gcm 
+	dma_addr_t gcm_iv_inc1_dma_addr; /* buffer for internal gcm configurations */
+	dma_addr_t gcm_iv_inc2_dma_addr; /* buffer for internal gcm configurations */
+	dma_addr_t hkey_dma_addr; /* Phys. address of hkey */
+	dma_addr_t gcm_block_len_dma_addr; /* Phys. address of gcm block len */
+	bool is_gcm4543;
+
+	uint8_t *icv_virt_addr; /* Virt. address of ICV */
+	struct async_gen_req_ctx gen_ctx;
+	struct ssi_mlli assoc;
+	struct ssi_mlli src;
+	struct ssi_mlli dst;
+	struct scatterlist* srcSgl;
+	struct scatterlist* dstSgl;
+	unsigned int srcOffset;
+	unsigned int dstOffset;
+	enum ssi_req_dma_buf_type assoc_buff_type;
+	enum ssi_req_dma_buf_type data_buff_type;
+	struct mlli_params mlli_params;
+	unsigned int cryptlen;
+	struct scatterlist ccm_adata_sg;
+	enum aead_ccm_header_size ccm_hdr_size;
+	unsigned int req_authsize;
+	enum drv_cipher_mode cipher_mode;
+	bool is_icv_fragmented;
+	bool is_single_pass;
+	bool plaintext_authenticate_only; //for gcm_rfc4543
+};
+
+int ssi_aead_alloc(struct ssi_drvdata *drvdata);
+int ssi_aead_free(struct ssi_drvdata *drvdata);
+
+#endif /*__SSI_AEAD_H__*/
diff --git a/drivers/staging/ccree/ssi_buffer_mgr.c b/drivers/staging/ccree/ssi_buffer_mgr.c
new file mode 100644
index 000000000000..038e2ff5e545
--- /dev/null
+++ b/drivers/staging/ccree/ssi_buffer_mgr.c
@@ -0,0 +1,1873 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/crypto.h>
+#include <linux/version.h>
+#include <crypto/algapi.h>
+#include <crypto/internal/aead.h>
+#include <crypto/hash.h>
+#include <crypto/authenc.h>
+#include <crypto/scatterwalk.h>
+#include <linux/dmapool.h>
+#include <linux/dma-mapping.h>
+#include <linux/crypto.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "ssi_buffer_mgr.h"
+#include "cc_lli_defs.h"
+#include "ssi_cipher.h"
+#include "ssi_hash.h"
+#include "ssi_aead.h"
+
+#define LLI_MAX_NUM_OF_DATA_ENTRIES 128
+#define LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES 4
+#define MLLI_TABLE_MIN_ALIGNMENT 4 /*Force the MLLI table to be align to uint32 */
+#define MAX_NUM_OF_BUFFERS_IN_MLLI 4
+#define MAX_NUM_OF_TOTAL_MLLI_ENTRIES (2*LLI_MAX_NUM_OF_DATA_ENTRIES + \
+					LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES )
+
+#ifdef CC_DEBUG
+#define DUMP_SGL(sg) \
+	while (sg) { \
+		SSI_LOG_DEBUG("page=%lu offset=%u length=%u (dma_len=%u) " \
+			     "dma_addr=%08x\n", (sg)->page_link, (sg)->offset, \
+			(sg)->length, sg_dma_len(sg), (sg)->dma_address); \
+		(sg) = sg_next(sg); \
+	}
+#define DUMP_MLLI_TABLE(mlli_p, nents) \
+	do { \
+		SSI_LOG_DEBUG("mlli=%pK nents=%u\n", (mlli_p), (nents)); \
+		while((nents)--) { \
+			SSI_LOG_DEBUG("addr=0x%08X size=0x%08X\n", \
+			     (mlli_p)[LLI_WORD0_OFFSET], \
+			     (mlli_p)[LLI_WORD1_OFFSET]); \
+			(mlli_p) += LLI_ENTRY_WORD_SIZE; \
+		} \
+	} while (0)
+#define GET_DMA_BUFFER_TYPE(buff_type) ( \
+	((buff_type) == SSI_DMA_BUF_NULL) ? "BUF_NULL" : \
+	((buff_type) == SSI_DMA_BUF_DLLI) ? "BUF_DLLI" : \
+	((buff_type) == SSI_DMA_BUF_MLLI) ? "BUF_MLLI" : "BUF_INVALID")
+#else
+#define DX_BUFFER_MGR_DUMP_SGL(sg)
+#define DX_BUFFER_MGR_DUMP_MLLI_TABLE(mlli_p, nents)
+#define GET_DMA_BUFFER_TYPE(buff_type)
+#endif
+
+
+enum dma_buffer_type {
+	DMA_NULL_TYPE = -1,
+	DMA_SGL_TYPE = 1,
+	DMA_BUFF_TYPE = 2,
+};
+
+struct buff_mgr_handle {
+	struct dma_pool *mlli_buffs_pool;
+};
+
+union buffer_array_entry {
+	struct scatterlist *sgl;
+	dma_addr_t buffer_dma;
+};
+
+struct buffer_array {
+	unsigned int num_of_buffers;
+	union buffer_array_entry entry[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	unsigned int offset[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	int nents[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	int total_data_len[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	enum dma_buffer_type type[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	bool is_last[MAX_NUM_OF_BUFFERS_IN_MLLI];
+	uint32_t * mlli_nents[MAX_NUM_OF_BUFFERS_IN_MLLI];
+};
+
+#ifdef CC_DMA_48BIT_SIM
+dma_addr_t ssi_buff_mgr_update_dma_addr(dma_addr_t orig_addr, uint32_t data_len)
+{
+	dma_addr_t tmp_dma_addr;
+#ifdef CC_DMA_48BIT_SIM_FULL
+	/* With this code all addresses will be switched to 48 bits. */
+	/* The if condition protects from double expention */
+	if((((orig_addr >> 16) & 0xFFFF) != 0xFFFF) && 
+		(data_len <= CC_MAX_MLLI_ENTRY_SIZE)) {
+#else
+	if((!(((orig_addr >> 16) & 0xFF) % 2)) && 
+		(data_len <= CC_MAX_MLLI_ENTRY_SIZE)) {
+#endif
+		tmp_dma_addr = ((orig_addr<<16) | 0xFFFF0000 | 
+				(orig_addr & UINT16_MAX));
+			SSI_LOG_DEBUG("MAP DMA: orig address=0x%llX "
+				    "dma_address=0x%llX\n",
+				     orig_addr, tmp_dma_addr);
+			return tmp_dma_addr;	
+	}
+	return orig_addr;
+}
+
+dma_addr_t ssi_buff_mgr_restore_dma_addr(dma_addr_t orig_addr)
+{
+	dma_addr_t tmp_dma_addr;
+#ifdef CC_DMA_48BIT_SIM_FULL
+	/* With this code all addresses will be restored from 48 bits. */
+	/* The if condition protects from double restoring */
+	if((orig_addr >> 32) & 0xFFFF ) {
+#else
+	if(((orig_addr >> 32) & 0xFFFF) && 
+		!(((orig_addr >> 32) & 0xFF) % 2) ) {
+#endif
+		/*return high 16 bits*/
+		tmp_dma_addr = ((orig_addr >> 16));
+		/*clean the 0xFFFF in the lower bits (set in the add expansion)*/
+		tmp_dma_addr &= 0xFFFF0000; 
+		/* Set the original 16 bits */
+		tmp_dma_addr |= (orig_addr & UINT16_MAX); 
+		SSI_LOG_DEBUG("Release DMA: orig address=0x%llX "
+			     "dma_address=0x%llX\n",
+			     orig_addr, tmp_dma_addr);
+			return tmp_dma_addr;	
+	}
+	return orig_addr;
+}
+#endif
+/**
+ * ssi_buffer_mgr_get_sgl_nents() - Get scatterlist number of entries.
+ * 
+ * @sg_list: SG list
+ * @nbytes: [IN] Total SGL data bytes.
+ * @lbytes: [OUT] Returns the amount of bytes at the last entry 
+ */
+static unsigned int ssi_buffer_mgr_get_sgl_nents(
+	struct scatterlist *sg_list, unsigned int nbytes, uint32_t *lbytes, bool *is_chained)
+{
+	unsigned int nents = 0;
+	while (nbytes != 0) {
+		if (sg_is_chain(sg_list)) {
+			SSI_LOG_ERR("Unexpected chanined entry "
+				   "in sg (entry =0x%X) \n", nents);
+			BUG();
+		}
+		if (sg_list->length != 0) {
+			nents++;
+			/* get the number of bytes in the last entry */
+			*lbytes = nbytes;
+			nbytes -= ( sg_list->length > nbytes ) ? nbytes : sg_list->length;
+			sg_list = sg_next(sg_list);
+		} else {
+			sg_list = (struct scatterlist *)sg_page(sg_list);
+			if (is_chained != NULL) {
+				*is_chained = true;
+			}
+		}
+	}
+	SSI_LOG_DEBUG("nents %d last bytes %d\n",nents, *lbytes);
+	return nents;
+}
+
+/**
+ * ssi_buffer_mgr_zero_sgl() - Zero scatter scatter list data.
+ * 
+ * @sgl:
+ */
+void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len)
+{
+	struct scatterlist *current_sg = sgl;
+	int sg_index = 0;
+
+	while (sg_index <= data_len) {
+		if (current_sg == NULL) {
+			/* reached the end of the sgl --> just return back */
+			return;
+		}
+		memset(sg_virt(current_sg), 0, current_sg->length);
+		sg_index += current_sg->length;
+		current_sg = sg_next(current_sg);
+	}
+}
+
+/**
+ * ssi_buffer_mgr_copy_scatterlist_portion() - Copy scatter list data,
+ * from to_skip to end, to dest and vice versa
+ * 
+ * @dest:
+ * @sg:
+ * @to_skip:
+ * @end:
+ * @direct:
+ */
+void ssi_buffer_mgr_copy_scatterlist_portion(
+	u8 *dest, struct scatterlist *sg,
+	uint32_t to_skip,  uint32_t end,
+	enum ssi_sg_cpy_direct direct)
+{
+	uint32_t nents, lbytes;
+
+	nents = ssi_buffer_mgr_get_sgl_nents(sg, end, &lbytes, NULL);
+	sg_copy_buffer(sg, nents, (void *)dest, (end - to_skip), 0, (direct == SSI_SG_TO_BUF));
+}
+
+static inline int ssi_buffer_mgr_render_buff_to_mlli(
+	dma_addr_t buff_dma, uint32_t buff_size, uint32_t *curr_nents,
+	uint32_t **mlli_entry_pp)
+{
+	uint32_t *mlli_entry_p = *mlli_entry_pp;
+	uint32_t new_nents;;
+
+	/* Verify there is no memory overflow*/
+	new_nents = (*curr_nents + buff_size/CC_MAX_MLLI_ENTRY_SIZE + 1);
+	if (new_nents > MAX_NUM_OF_TOTAL_MLLI_ENTRIES ) {
+		return -ENOMEM;
+	}
+
+	/*handle buffer longer than 64 kbytes */
+	while (buff_size > CC_MAX_MLLI_ENTRY_SIZE ) {
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(buff_dma, CC_MAX_MLLI_ENTRY_SIZE);
+		LLI_SET_ADDR(mlli_entry_p,buff_dma);
+		LLI_SET_SIZE(mlli_entry_p, CC_MAX_MLLI_ENTRY_SIZE);
+		SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n",*curr_nents,
+			   mlli_entry_p[LLI_WORD0_OFFSET],
+			   mlli_entry_p[LLI_WORD1_OFFSET]);
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(buff_dma);
+		buff_dma += CC_MAX_MLLI_ENTRY_SIZE;
+		buff_size -= CC_MAX_MLLI_ENTRY_SIZE;
+		mlli_entry_p = mlli_entry_p + 2;
+		(*curr_nents)++;
+	}
+	/*Last entry */
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(buff_dma, buff_size);
+	LLI_SET_ADDR(mlli_entry_p,buff_dma);
+	LLI_SET_SIZE(mlli_entry_p, buff_size);
+	SSI_LOG_DEBUG("entry[%d]: single_buff=0x%08X size=%08X\n",*curr_nents,
+		   mlli_entry_p[LLI_WORD0_OFFSET],
+		   mlli_entry_p[LLI_WORD1_OFFSET]);
+	mlli_entry_p = mlli_entry_p + 2;
+	*mlli_entry_pp = mlli_entry_p;
+	(*curr_nents)++;
+	return 0;
+}
+
+
+static inline int ssi_buffer_mgr_render_scatterlist_to_mlli(
+	struct scatterlist *sgl, uint32_t sgl_data_len, uint32_t sglOffset, uint32_t *curr_nents,
+	uint32_t **mlli_entry_pp)
+{
+	struct scatterlist *curr_sgl = sgl;
+	uint32_t *mlli_entry_p = *mlli_entry_pp;
+	int32_t rc = 0;
+
+	for ( ; (curr_sgl != NULL) && (sgl_data_len != 0);
+	      curr_sgl = sg_next(curr_sgl)) {
+		uint32_t entry_data_len =
+			(sgl_data_len > sg_dma_len(curr_sgl) - sglOffset) ?
+				sg_dma_len(curr_sgl) - sglOffset : sgl_data_len ;
+		sgl_data_len -= entry_data_len;
+		rc = ssi_buffer_mgr_render_buff_to_mlli(
+			sg_dma_address(curr_sgl) + sglOffset, entry_data_len, curr_nents,
+			&mlli_entry_p);
+		if(rc != 0) {
+			return rc;
+		}
+		sglOffset=0;
+	}
+	*mlli_entry_pp = mlli_entry_p;
+	return 0;
+}
+
+static int ssi_buffer_mgr_generate_mlli(
+	struct device *dev,
+	struct buffer_array *sg_data,
+	struct mlli_params *mlli_params)
+{
+	uint32_t *mlli_p;
+	uint32_t total_nents = 0,prev_total_nents = 0;
+	int rc = 0, i;
+
+	SSI_LOG_DEBUG("NUM of SG's = %d\n", sg_data->num_of_buffers);
+
+	/* Allocate memory from the pointed pool */
+	mlli_params->mlli_virt_addr = dma_pool_alloc(
+			mlli_params->curr_pool, GFP_KERNEL,
+			&(mlli_params->mlli_dma_addr));
+	if (unlikely(mlli_params->mlli_virt_addr == NULL)) {
+		SSI_LOG_ERR("dma_pool_alloc() failed\n");
+		rc =-ENOMEM;
+		goto build_mlli_exit;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(mlli_params->mlli_dma_addr, 
+						(MAX_NUM_OF_TOTAL_MLLI_ENTRIES*
+						LLI_ENTRY_BYTE_SIZE));
+	/* Point to start of MLLI */
+	mlli_p = (uint32_t *)mlli_params->mlli_virt_addr;
+	/* go over all SG's and link it to one MLLI table */
+	for (i = 0; i < sg_data->num_of_buffers; i++) {
+		if (sg_data->type[i] == DMA_SGL_TYPE)
+			rc = ssi_buffer_mgr_render_scatterlist_to_mlli(
+				sg_data->entry[i].sgl, 
+				sg_data->total_data_len[i], sg_data->offset[i], &total_nents,
+				&mlli_p);
+		else /*DMA_BUFF_TYPE*/
+			rc = ssi_buffer_mgr_render_buff_to_mlli(
+				sg_data->entry[i].buffer_dma,
+				sg_data->total_data_len[i], &total_nents,
+				&mlli_p);
+		if(rc != 0) {
+			return rc;
+		}
+
+		/* set last bit in the current table */
+		if (sg_data->mlli_nents[i] != NULL) {
+			/*Calculate the current MLLI table length for the 
+			length field in the descriptor*/
+			*(sg_data->mlli_nents[i]) += 
+				(total_nents - prev_total_nents);
+			prev_total_nents = total_nents;
+		}
+	}
+
+	/* Set MLLI size for the bypass operation */
+	mlli_params->mlli_len = (total_nents * LLI_ENTRY_BYTE_SIZE);
+
+	SSI_LOG_DEBUG("MLLI params: "
+		     "virt_addr=%pK dma_addr=0x%llX mlli_len=0x%X\n",
+		   mlli_params->mlli_virt_addr,
+		   (unsigned long long)mlli_params->mlli_dma_addr,
+		   mlli_params->mlli_len);
+
+build_mlli_exit:
+	return rc;
+}
+
+static inline void ssi_buffer_mgr_add_buffer_entry(
+	struct buffer_array *sgl_data,
+	dma_addr_t buffer_dma, unsigned int buffer_len,
+	bool is_last_entry, uint32_t *mlli_nents)
+{
+	unsigned int index = sgl_data->num_of_buffers;
+
+	SSI_LOG_DEBUG("index=%u single_buff=0x%llX "
+		     "buffer_len=0x%08X is_last=%d\n",
+		     index, (unsigned long long)buffer_dma, buffer_len, is_last_entry);
+	sgl_data->nents[index] = 1;
+	sgl_data->entry[index].buffer_dma = buffer_dma;
+	sgl_data->offset[index] = 0;
+	sgl_data->total_data_len[index] = buffer_len;
+	sgl_data->type[index] = DMA_BUFF_TYPE;
+	sgl_data->is_last[index] = is_last_entry;
+	sgl_data->mlli_nents[index] = mlli_nents;
+	if (sgl_data->mlli_nents[index] != NULL)
+		*sgl_data->mlli_nents[index] = 0;
+	sgl_data->num_of_buffers++;
+}
+
+static inline void ssi_buffer_mgr_add_scatterlist_entry(
+	struct buffer_array *sgl_data,
+	unsigned int nents,
+	struct scatterlist *sgl,
+	unsigned int data_len,
+	unsigned int data_offset,
+	bool is_last_table,
+	uint32_t *mlli_nents)
+{
+	unsigned int index = sgl_data->num_of_buffers;
+
+	SSI_LOG_DEBUG("index=%u nents=%u sgl=%pK data_len=0x%08X is_last=%d\n",
+		     index, nents, sgl, data_len, is_last_table);
+	sgl_data->nents[index] = nents;
+	sgl_data->entry[index].sgl = sgl;
+	sgl_data->offset[index] = data_offset;
+	sgl_data->total_data_len[index] = data_len;
+	sgl_data->type[index] = DMA_SGL_TYPE;
+	sgl_data->is_last[index] = is_last_table;
+	sgl_data->mlli_nents[index] = mlli_nents;
+	if (sgl_data->mlli_nents[index] != NULL)
+		*sgl_data->mlli_nents[index] = 0;
+	sgl_data->num_of_buffers++;
+}
+
+static int
+ssi_buffer_mgr_dma_map_sg(struct device *dev, struct scatterlist *sg, uint32_t nents,
+			 enum dma_data_direction direction)
+{
+	uint32_t i , j;
+	struct scatterlist *l_sg = sg;
+	for (i = 0; i < nents; i++) {
+		if (l_sg == NULL) {
+			break;
+		}
+		if (unlikely(dma_map_sg(dev, l_sg, 1, direction) != 1)){
+			SSI_LOG_ERR("dma_map_page() sg buffer failed\n");
+			goto err;
+		}
+		l_sg = sg_next(l_sg);
+	}
+	return nents;
+
+err:
+	/* Restore mapped parts */
+	for (j = 0; j < i; j++) {
+		if (sg == NULL) {
+			break;
+		}
+		dma_unmap_sg(dev,sg,1,direction);
+		sg = sg_next(sg);
+	}
+	return 0;
+}
+
+static int ssi_buffer_mgr_map_scatterlist(
+	struct device *dev, struct scatterlist *sg,
+	unsigned int nbytes, int direction,
+	uint32_t *nents, uint32_t max_sg_nents,
+	uint32_t *lbytes, uint32_t *mapped_nents)
+{
+	bool is_chained = false;
+
+	if (sg_is_last(sg)) {
+		/* One entry only case -set to DLLI */
+		if (unlikely(dma_map_sg(dev, sg, 1, direction) != 1)) {
+			SSI_LOG_ERR("dma_map_sg() single buffer failed\n");
+			return -ENOMEM;
+		} 
+		SSI_LOG_DEBUG("Mapped sg: dma_address=0x%llX "
+			     "page_link=0x%08lX addr=%pK offset=%u "
+			     "length=%u\n",
+			     (unsigned long long)sg_dma_address(sg), 
+			     sg->page_link, 
+			     sg_virt(sg), 
+			     sg->offset, sg->length);
+		*lbytes = nbytes;
+		*nents = 1;
+		*mapped_nents = 1;
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(sg_dma_address(sg), sg_dma_len(sg));
+	} else {  /*sg_is_last*/
+		*nents = ssi_buffer_mgr_get_sgl_nents(sg, nbytes, lbytes, 
+						     &is_chained);
+		if (*nents > max_sg_nents) {
+			*nents = 0;
+			SSI_LOG_ERR("Too many fragments. current %d max %d\n",
+				   *nents, max_sg_nents);
+			return -ENOMEM;
+		}
+		if (!is_chained) {
+			/* In case of mmu the number of mapped nents might
+			be changed from the original sgl nents */
+			*mapped_nents = dma_map_sg(dev, sg, *nents, direction);
+			if (unlikely(*mapped_nents == 0)){
+				*nents = 0;
+				SSI_LOG_ERR("dma_map_sg() sg buffer failed\n");
+				return -ENOMEM;
+			}
+		} else {
+			/*In this case the driver maps entry by entry so it
+			must have the same nents before and after map */
+			*mapped_nents = ssi_buffer_mgr_dma_map_sg(dev,
+								 sg,
+								 *nents,
+								 direction);
+			if (unlikely(*mapped_nents != *nents)){
+				*nents = *mapped_nents;
+				SSI_LOG_ERR("dma_map_sg() sg buffer failed\n");
+				return -ENOMEM;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static inline int
+ssi_aead_handle_config_buf(struct device *dev,
+	struct aead_req_ctx *areq_ctx,
+	uint8_t* config_data,
+	struct buffer_array *sg_data,
+	unsigned int assoclen)
+{
+	SSI_LOG_DEBUG(" handle additional data config set to   DLLI \n");
+	/* create sg for the current buffer */
+	sg_init_one(&areq_ctx->ccm_adata_sg, config_data, AES_BLOCK_SIZE + areq_ctx->ccm_hdr_size);
+	if (unlikely(dma_map_sg(dev, &areq_ctx->ccm_adata_sg, 1, 
+				DMA_TO_DEVICE) != 1)) {
+			SSI_LOG_ERR("dma_map_sg() "
+			   "config buffer failed\n");
+			return -ENOMEM;
+	}
+	SSI_LOG_DEBUG("Mapped curr_buff: dma_address=0x%llX "
+		     "page_link=0x%08lX addr=%pK "
+		     "offset=%u length=%u\n",
+		     (unsigned long long)sg_dma_address(&areq_ctx->ccm_adata_sg), 
+		     areq_ctx->ccm_adata_sg.page_link, 
+		     sg_virt(&areq_ctx->ccm_adata_sg),
+		     areq_ctx->ccm_adata_sg.offset, 
+		     areq_ctx->ccm_adata_sg.length);
+	/* prepare for case of MLLI */
+	if (assoclen > 0) {
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data, 1, 
+						    &areq_ctx->ccm_adata_sg,
+						    (AES_BLOCK_SIZE + 
+						    areq_ctx->ccm_hdr_size), 0,
+						    false, NULL);
+	}
+	return 0;
+}
+
+
+static inline int ssi_ahash_handle_curr_buf(struct device *dev,
+					   struct ahash_req_ctx *areq_ctx,
+					   uint8_t* curr_buff,
+					   uint32_t curr_buff_cnt,
+					   struct buffer_array *sg_data)
+{
+	SSI_LOG_DEBUG(" handle curr buff %x set to   DLLI \n", curr_buff_cnt);
+	/* create sg for the current buffer */
+	sg_init_one(areq_ctx->buff_sg,curr_buff, curr_buff_cnt);
+	if (unlikely(dma_map_sg(dev, areq_ctx->buff_sg, 1,
+				DMA_TO_DEVICE) != 1)) {
+			SSI_LOG_ERR("dma_map_sg() "
+			   "src buffer failed\n");
+			return -ENOMEM;
+	}
+	SSI_LOG_DEBUG("Mapped curr_buff: dma_address=0x%llX "
+		     "page_link=0x%08lX addr=%pK "
+		     "offset=%u length=%u\n",
+		     (unsigned long long)sg_dma_address(areq_ctx->buff_sg), 
+		     areq_ctx->buff_sg->page_link, 
+		     sg_virt(areq_ctx->buff_sg),
+		     areq_ctx->buff_sg->offset, 
+		     areq_ctx->buff_sg->length);
+	areq_ctx->data_dma_buf_type = SSI_DMA_BUF_DLLI;
+	areq_ctx->curr_sg = areq_ctx->buff_sg;
+	areq_ctx->in_nents = 0;
+	/* prepare for case of MLLI */
+	ssi_buffer_mgr_add_scatterlist_entry(sg_data, 1, areq_ctx->buff_sg,
+				curr_buff_cnt, 0, false, NULL);
+	return 0;
+}
+
+void ssi_buffer_mgr_unmap_blkcipher_request(
+	struct device *dev,
+	void *ctx,
+	unsigned int ivsize,
+	struct scatterlist *src,
+	struct scatterlist *dst)
+{
+	struct blkcipher_req_ctx *req_ctx = (struct blkcipher_req_ctx *)ctx;
+
+	if (likely(req_ctx->gen_ctx.iv_dma_addr != 0)) {
+		SSI_LOG_DEBUG("Unmapped iv: iv_dma_addr=0x%llX iv_size=%u\n", 
+			(unsigned long long)req_ctx->gen_ctx.iv_dma_addr,
+			ivsize);
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_ctx->gen_ctx.iv_dma_addr);
+		dma_unmap_single(dev, req_ctx->gen_ctx.iv_dma_addr, 
+				 ivsize, 
+				 req_ctx->is_giv ? DMA_BIDIRECTIONAL :
+				 DMA_TO_DEVICE);
+	}
+	/* Release pool */
+	if (req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_ctx->mlli_params.mlli_dma_addr);
+		dma_pool_free(req_ctx->mlli_params.curr_pool,
+			      req_ctx->mlli_params.mlli_virt_addr,
+			      req_ctx->mlli_params.mlli_dma_addr);
+	}
+
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(src));
+	dma_unmap_sg(dev, src, req_ctx->in_nents,
+		DMA_BIDIRECTIONAL);
+	SSI_LOG_DEBUG("Unmapped req->src=%pK\n", 
+		     sg_virt(src));
+
+	if (src != dst) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(dst));
+		dma_unmap_sg(dev, dst, req_ctx->out_nents, 
+			DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped req->dst=%pK\n",
+			sg_virt(dst));
+	}
+}
+
+int ssi_buffer_mgr_map_blkcipher_request(
+	struct ssi_drvdata *drvdata,
+	void *ctx,
+	unsigned int ivsize,
+	unsigned int nbytes,
+	void *info,
+	struct scatterlist *src,
+	struct scatterlist *dst)
+{
+	struct blkcipher_req_ctx *req_ctx = (struct blkcipher_req_ctx *)ctx;
+	struct mlli_params *mlli_params = &req_ctx->mlli_params;	
+	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
+	struct device *dev = &drvdata->plat_dev->dev;
+	struct buffer_array sg_data;
+	uint32_t dummy = 0;
+	int rc = 0;
+	uint32_t mapped_nents = 0;
+
+	req_ctx->dma_buf_type = SSI_DMA_BUF_DLLI;
+	mlli_params->curr_pool = NULL;
+	sg_data.num_of_buffers = 0;
+
+	/* Map IV buffer */
+	if (likely(ivsize != 0) ) {
+		dump_byte_array("iv", (uint8_t *)info, ivsize);
+		req_ctx->gen_ctx.iv_dma_addr = 
+			dma_map_single(dev, (void *)info, 
+				       ivsize, 
+				       req_ctx->is_giv ? DMA_BIDIRECTIONAL:
+				       DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(dev, 
+					req_ctx->gen_ctx.iv_dma_addr))) {
+			SSI_LOG_ERR("Mapping iv %u B at va=%pK "
+				   "for DMA failed\n", ivsize, info);
+			return -ENOMEM;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(req_ctx->gen_ctx.iv_dma_addr,
+								ivsize);
+		SSI_LOG_DEBUG("Mapped iv %u B at va=%pK to dma=0x%llX\n",
+			ivsize, info,
+			(unsigned long long)req_ctx->gen_ctx.iv_dma_addr);
+	} else
+		req_ctx->gen_ctx.iv_dma_addr = 0;
+	
+	/* Map the src SGL */
+	rc = ssi_buffer_mgr_map_scatterlist(dev, src,
+		nbytes, DMA_BIDIRECTIONAL, &req_ctx->in_nents,
+		LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy, &mapped_nents);
+	if (unlikely(rc != 0)) {
+		rc = -ENOMEM;
+		goto ablkcipher_exit;
+	}
+	if (mapped_nents > 1)
+		req_ctx->dma_buf_type = SSI_DMA_BUF_MLLI;
+
+	if (unlikely(src == dst)) {
+		/* Handle inplace operation */
+		if (unlikely(req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI)) {
+			req_ctx->out_nents = 0;
+			ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
+				req_ctx->in_nents, src,
+				nbytes, 0, true, &req_ctx->in_mlli_nents);
+		}
+	} else {
+		/* Map the dst sg */
+		if (unlikely(ssi_buffer_mgr_map_scatterlist(
+			dev,dst, nbytes,
+			DMA_BIDIRECTIONAL, &req_ctx->out_nents,
+			LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy,
+			&mapped_nents))){
+			rc = -ENOMEM;
+			goto ablkcipher_exit;
+		}
+		if (mapped_nents > 1)
+			req_ctx->dma_buf_type = SSI_DMA_BUF_MLLI;
+
+		if (unlikely((req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI))) {
+			ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
+				req_ctx->in_nents, src,
+				nbytes, 0, true,
+				&req_ctx->in_mlli_nents);
+			ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
+				req_ctx->out_nents, dst,
+				nbytes, 0, true, 
+				&req_ctx->out_mlli_nents);
+		}
+	}
+	
+	if (unlikely(req_ctx->dma_buf_type == SSI_DMA_BUF_MLLI)) {
+		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
+		rc = ssi_buffer_mgr_generate_mlli(dev, &sg_data, mlli_params);
+		if (unlikely(rc!= 0))
+			goto ablkcipher_exit;
+
+	}
+
+	SSI_LOG_DEBUG("areq_ctx->dma_buf_type = %s\n",
+		GET_DMA_BUFFER_TYPE(req_ctx->dma_buf_type));
+
+	return 0;
+
+ablkcipher_exit:
+	ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
+	return rc;
+}
+
+void ssi_buffer_mgr_unmap_aead_request(
+	struct device *dev, struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	unsigned int hw_iv_size = areq_ctx->hw_iv_size;
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	uint32_t dummy;
+	bool chained;
+	uint32_t size_to_unmap = 0;
+
+	if (areq_ctx->mac_buf_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mac_buf_dma_addr);
+		dma_unmap_single(dev, areq_ctx->mac_buf_dma_addr, 
+			MAX_MAC_SIZE, DMA_BIDIRECTIONAL);
+	}
+
+#if SSI_CC_HAS_AES_GCM
+	if (areq_ctx->cipher_mode == DRV_CIPHER_GCTR) {
+		if (areq_ctx->hkey_dma_addr != 0) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->hkey_dma_addr);
+			dma_unmap_single(dev, areq_ctx->hkey_dma_addr,
+					 AES_BLOCK_SIZE, DMA_BIDIRECTIONAL);
+		}
+	
+		if (areq_ctx->gcm_block_len_dma_addr != 0) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_block_len_dma_addr);
+			dma_unmap_single(dev, areq_ctx->gcm_block_len_dma_addr,
+					 AES_BLOCK_SIZE, DMA_TO_DEVICE);
+		}
+	
+		if (areq_ctx->gcm_iv_inc1_dma_addr != 0) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc1_dma_addr);
+			dma_unmap_single(dev, areq_ctx->gcm_iv_inc1_dma_addr, 
+				AES_BLOCK_SIZE, DMA_TO_DEVICE);
+		}
+	
+		if (areq_ctx->gcm_iv_inc2_dma_addr != 0) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc2_dma_addr);
+			dma_unmap_single(dev, areq_ctx->gcm_iv_inc2_dma_addr, 
+				AES_BLOCK_SIZE, DMA_TO_DEVICE);
+		}
+	}
+#endif
+
+	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
+		if (areq_ctx->ccm_iv0_dma_addr != 0) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->ccm_iv0_dma_addr);
+			dma_unmap_single(dev, areq_ctx->ccm_iv0_dma_addr, 
+				AES_BLOCK_SIZE, DMA_TO_DEVICE);
+		}
+
+		dma_unmap_sg(dev, &areq_ctx->ccm_adata_sg, 1, DMA_TO_DEVICE);
+	}
+	if (areq_ctx->gen_ctx.iv_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->gen_ctx.iv_dma_addr);
+		dma_unmap_single(dev, areq_ctx->gen_ctx.iv_dma_addr,
+				 hw_iv_size, DMA_BIDIRECTIONAL);
+	}
+
+	/*In case a pool was set, a table was 
+	  allocated and should be released */
+	if (areq_ctx->mlli_params.curr_pool != NULL) {
+		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%08llX virt=%pK\n", 
+			(unsigned long long)areq_ctx->mlli_params.mlli_dma_addr,
+			areq_ctx->mlli_params.mlli_virt_addr);
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mlli_params.mlli_dma_addr);
+		dma_pool_free(areq_ctx->mlli_params.curr_pool,
+			      areq_ctx->mlli_params.mlli_virt_addr,
+			      areq_ctx->mlli_params.mlli_dma_addr);
+	}
+
+	SSI_LOG_DEBUG("Unmapping src sgl: req->src=%pK areq_ctx->src.nents=%u areq_ctx->assoc.nents=%u assoclen:%u cryptlen=%u\n", sg_virt(req->src),areq_ctx->src.nents,areq_ctx->assoc.nents,req->assoclen,req->cryptlen);
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(req->src));
+	size_to_unmap = req->assoclen+req->cryptlen;
+	if(areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT){
+		size_to_unmap += areq_ctx->req_authsize;
+	}
+	if (areq_ctx->is_gcm4543)
+		size_to_unmap += crypto_aead_ivsize(tfm);
+
+	dma_unmap_sg(dev, req->src, ssi_buffer_mgr_get_sgl_nents(req->src,size_to_unmap,&dummy,&chained) , DMA_BIDIRECTIONAL);
+	if (unlikely(req->src != req->dst)) {
+		SSI_LOG_DEBUG("Unmapping dst sgl: req->dst=%pK\n", 
+			sg_virt(req->dst));
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(req->dst));
+		dma_unmap_sg(dev, req->dst, ssi_buffer_mgr_get_sgl_nents(req->dst,size_to_unmap,&dummy,&chained),
+			DMA_BIDIRECTIONAL);
+	}
+#if DX_HAS_ACP
+	if ((areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
+	    likely(req->src == req->dst))
+	{
+		uint32_t size_to_skip = req->assoclen;
+		if (areq_ctx->is_gcm4543) {
+			size_to_skip += crypto_aead_ivsize(tfm);
+		}
+		/* copy mac to a temporary location to deal with possible
+		  data memory overriding that caused by cache coherence problem. */
+		ssi_buffer_mgr_copy_scatterlist_portion(
+			areq_ctx->backup_mac, req->src,
+			size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
+			size_to_skip+ req->cryptlen, SSI_SG_FROM_BUF);
+	}
+#endif
+}
+
+static inline int ssi_buffer_mgr_get_aead_icv_nents(
+	struct scatterlist *sgl,
+	unsigned int sgl_nents,
+	unsigned int authsize,
+	uint32_t last_entry_data_size,
+	bool *is_icv_fragmented)
+{
+	unsigned int icv_max_size = 0;
+	unsigned int icv_required_size = authsize > last_entry_data_size ? (authsize - last_entry_data_size) : authsize;
+	unsigned int nents;
+	unsigned int i;
+	
+	if (sgl_nents < MAX_ICV_NENTS_SUPPORTED) {
+		*is_icv_fragmented = false;
+		return 0;
+	}
+	
+	for( i = 0 ; i < (sgl_nents - MAX_ICV_NENTS_SUPPORTED) ; i++) {
+		if (sgl == NULL) {
+			break;
+		}
+		sgl = sg_next(sgl);
+	}
+
+	if (sgl != NULL) {
+		icv_max_size = sgl->length;
+	}
+
+	if (last_entry_data_size > authsize) {
+		nents = 0; /* ICV attached to data in last entry (not fragmented!) */
+		*is_icv_fragmented = false;
+	} else if (last_entry_data_size == authsize) {
+		nents = 1; /* ICV placed in whole last entry (not fragmented!) */
+		*is_icv_fragmented = false;
+	} else if (icv_max_size > icv_required_size) {
+		nents = 1;
+		*is_icv_fragmented = true;
+	} else if (icv_max_size == icv_required_size) {
+		nents = 2;
+		*is_icv_fragmented = true;
+	} else {
+		SSI_LOG_ERR("Unsupported num. of ICV fragments (> %d)\n",
+			MAX_ICV_NENTS_SUPPORTED);
+		nents = -1; /*unsupported*/
+	}
+	SSI_LOG_DEBUG("is_frag=%s icv_nents=%u\n",
+		(*is_icv_fragmented ? "true" : "false"), nents);
+
+	return nents;
+}
+
+static inline int ssi_buffer_mgr_aead_chain_iv(
+	struct ssi_drvdata *drvdata,
+	struct aead_request *req,
+	struct buffer_array *sg_data,
+	bool is_last, bool do_chain)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	unsigned int hw_iv_size = areq_ctx->hw_iv_size;
+	struct device *dev = &drvdata->plat_dev->dev;
+	int rc = 0;
+
+	if (unlikely(req->iv == NULL)) {
+		areq_ctx->gen_ctx.iv_dma_addr = 0;
+		goto chain_iv_exit;
+	}
+
+	areq_ctx->gen_ctx.iv_dma_addr = dma_map_single(dev, req->iv,
+		hw_iv_size, DMA_BIDIRECTIONAL);
+	if (unlikely(dma_mapping_error(dev, areq_ctx->gen_ctx.iv_dma_addr))) {
+		SSI_LOG_ERR("Mapping iv %u B at va=%pK for DMA failed\n",
+			hw_iv_size, req->iv);
+		rc = -ENOMEM;
+		goto chain_iv_exit; 
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gen_ctx.iv_dma_addr, hw_iv_size);
+
+	SSI_LOG_DEBUG("Mapped iv %u B at va=%pK to dma=0x%llX\n",
+		hw_iv_size, req->iv, 
+		(unsigned long long)areq_ctx->gen_ctx.iv_dma_addr);
+	if (do_chain == true && areq_ctx->plaintext_authenticate_only == true){  // TODO: what about CTR?? ask Ron
+		struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+		unsigned int iv_size_to_authenc = crypto_aead_ivsize(tfm);
+		unsigned int iv_ofs = GCM_BLOCK_RFC4_IV_OFFSET;
+		/* Chain to given list */
+		ssi_buffer_mgr_add_buffer_entry(
+			sg_data, areq_ctx->gen_ctx.iv_dma_addr + iv_ofs,
+			iv_size_to_authenc, is_last,
+			&areq_ctx->assoc.mlli_nents);
+		areq_ctx->assoc_buff_type = SSI_DMA_BUF_MLLI;
+	}
+
+chain_iv_exit:
+	return rc;
+}
+
+static inline int ssi_buffer_mgr_aead_chain_assoc(
+	struct ssi_drvdata *drvdata,
+	struct aead_request *req,
+	struct buffer_array *sg_data,
+	bool is_last, bool do_chain)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	int rc = 0;
+	uint32_t mapped_nents = 0;
+	struct scatterlist *current_sg = req->src;
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	unsigned int sg_index = 0;
+	uint32_t size_of_assoc = req->assoclen;
+
+	if (areq_ctx->is_gcm4543) {
+		size_of_assoc += crypto_aead_ivsize(tfm);
+	}
+
+	if (sg_data == NULL) {
+		rc = -EINVAL;
+		goto chain_assoc_exit;
+	}
+
+	if (unlikely(req->assoclen == 0)) {
+		areq_ctx->assoc_buff_type = SSI_DMA_BUF_NULL;
+		areq_ctx->assoc.nents = 0;
+		areq_ctx->assoc.mlli_nents = 0;
+		SSI_LOG_DEBUG("Chain assoc of length 0: buff_type=%s nents=%u\n",
+			GET_DMA_BUFFER_TYPE(areq_ctx->assoc_buff_type),
+			areq_ctx->assoc.nents);
+		goto chain_assoc_exit;
+	}
+
+	//iterate over the sgl to see how many entries are for associated data
+	//it is assumed that if we reach here , the sgl is already mapped
+	sg_index = current_sg->length;
+	if (sg_index > size_of_assoc) { //the first entry in the scatter list contains all the associated data
+		mapped_nents++;        
+	}
+	else{
+		while (sg_index <= size_of_assoc) {
+			current_sg = sg_next(current_sg);
+			//if have reached the end of the sgl, then this is unexpected
+			if (current_sg == NULL) {
+				SSI_LOG_ERR("reached end of sg list. unexpected \n");
+				BUG();
+			}
+			sg_index += current_sg->length;
+			mapped_nents++;
+		}
+	}
+	if (unlikely(mapped_nents > LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES)) {
+		SSI_LOG_ERR("Too many fragments. current %d max %d\n",
+			    mapped_nents, LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES);
+		return -ENOMEM;
+	}
+	areq_ctx->assoc.nents = mapped_nents;
+
+	/* in CCM case we have additional entry for
+	*  ccm header configurations */
+	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
+		if (unlikely((mapped_nents + 1) >
+			LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES)) {
+
+			SSI_LOG_ERR("CCM case.Too many fragments. "
+				"Current %d max %d\n",
+				(areq_ctx->assoc.nents + 1),
+				LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES);
+			rc = -ENOMEM;
+			goto chain_assoc_exit;
+		}
+	}
+
+	if (likely(mapped_nents == 1) &&
+	    (areq_ctx->ccm_hdr_size == ccm_header_size_null))
+		areq_ctx->assoc_buff_type = SSI_DMA_BUF_DLLI;
+	else
+		areq_ctx->assoc_buff_type = SSI_DMA_BUF_MLLI;
+
+	if (unlikely((do_chain == true) ||
+		(areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI))) {
+
+		SSI_LOG_DEBUG("Chain assoc: buff_type=%s nents=%u\n",
+			GET_DMA_BUFFER_TYPE(areq_ctx->assoc_buff_type),
+			areq_ctx->assoc.nents);
+		ssi_buffer_mgr_add_scatterlist_entry(
+			sg_data, areq_ctx->assoc.nents,
+			req->src, req->assoclen, 0, is_last,
+			&areq_ctx->assoc.mlli_nents);
+		areq_ctx->assoc_buff_type = SSI_DMA_BUF_MLLI;
+	}
+
+chain_assoc_exit:
+	return rc;
+}
+
+static inline void ssi_buffer_mgr_prepare_aead_data_dlli(
+	struct aead_request *req,
+	uint32_t *src_last_bytes, uint32_t *dst_last_bytes)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	enum drv_crypto_direction direct = areq_ctx->gen_ctx.op_type;
+	unsigned int authsize = areq_ctx->req_authsize;
+
+	areq_ctx->is_icv_fragmented = false;
+	if (likely(req->src == req->dst)) {
+		/*INPLACE*/
+		areq_ctx->icv_dma_addr = sg_dma_address(
+			areq_ctx->srcSgl)+
+			(*src_last_bytes - authsize);
+		areq_ctx->icv_virt_addr = sg_virt(
+			areq_ctx->srcSgl) +
+			(*src_last_bytes - authsize);
+	} else if (direct == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		/*NON-INPLACE and DECRYPT*/
+		areq_ctx->icv_dma_addr = sg_dma_address(
+			areq_ctx->srcSgl) +
+			(*src_last_bytes - authsize);
+		areq_ctx->icv_virt_addr = sg_virt(
+			areq_ctx->srcSgl) +
+			(*src_last_bytes - authsize);
+	} else {
+		/*NON-INPLACE and ENCRYPT*/
+		areq_ctx->icv_dma_addr = sg_dma_address(
+			areq_ctx->dstSgl) +
+			(*dst_last_bytes - authsize);
+		areq_ctx->icv_virt_addr = sg_virt(
+			areq_ctx->dstSgl)+
+			(*dst_last_bytes - authsize);
+	}
+}
+
+static inline int ssi_buffer_mgr_prepare_aead_data_mlli(
+	struct ssi_drvdata *drvdata,
+	struct aead_request *req,
+	struct buffer_array *sg_data,
+	uint32_t *src_last_bytes, uint32_t *dst_last_bytes,
+	bool is_last_table)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	enum drv_crypto_direction direct = areq_ctx->gen_ctx.op_type;
+	unsigned int authsize = areq_ctx->req_authsize;
+	int rc = 0, icv_nents;
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+
+	if (likely(req->src == req->dst)) {
+		/*INPLACE*/
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
+			areq_ctx->src.nents, areq_ctx->srcSgl,
+			areq_ctx->cryptlen,areq_ctx->srcOffset, is_last_table,
+			&areq_ctx->src.mlli_nents);
+
+		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->srcSgl,
+			areq_ctx->src.nents, authsize, *src_last_bytes,
+			&areq_ctx->is_icv_fragmented);
+		if (unlikely(icv_nents < 0)) {
+			rc = -ENOTSUPP;
+			goto prepare_data_mlli_exit;
+		}
+
+		if (unlikely(areq_ctx->is_icv_fragmented == true)) {
+			/* Backup happens only when ICV is fragmented, ICV
+			   verification is made by CPU compare in order to simplify
+			   MAC verification upon request completion */
+			if (direct == DRV_CRYPTO_DIRECTION_DECRYPT) {
+#if !DX_HAS_ACP
+				/* In ACP platform we already copying ICV
+				   for any INPLACE-DECRYPT operation, hence
+				   we must neglect this code. */
+				uint32_t size_to_skip = req->assoclen;
+				if (areq_ctx->is_gcm4543) {
+					size_to_skip += crypto_aead_ivsize(tfm);
+				}
+				ssi_buffer_mgr_copy_scatterlist_portion(
+					areq_ctx->backup_mac, req->src,
+					size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
+					size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
+#endif
+				areq_ctx->icv_virt_addr = areq_ctx->backup_mac;
+			} else {
+				areq_ctx->icv_virt_addr = areq_ctx->mac_buf;
+				areq_ctx->icv_dma_addr = areq_ctx->mac_buf_dma_addr;
+			}
+		} else { /* Contig. ICV */
+			/*Should hanlde if the sg is not contig.*/
+			areq_ctx->icv_dma_addr = sg_dma_address(
+				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) +
+				(*src_last_bytes - authsize);
+			areq_ctx->icv_virt_addr = sg_virt(
+				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) + 
+				(*src_last_bytes - authsize);
+		}
+
+	} else if (direct == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		/*NON-INPLACE and DECRYPT*/
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
+			areq_ctx->src.nents, areq_ctx->srcSgl,
+			areq_ctx->cryptlen, areq_ctx->srcOffset,is_last_table,
+			&areq_ctx->src.mlli_nents);
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
+			areq_ctx->dst.nents, areq_ctx->dstSgl,
+			areq_ctx->cryptlen,areq_ctx->dstOffset, is_last_table,
+			&areq_ctx->dst.mlli_nents);
+
+		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->srcSgl,
+			areq_ctx->src.nents, authsize, *src_last_bytes,
+			&areq_ctx->is_icv_fragmented);
+		if (unlikely(icv_nents < 0)) {
+			rc = -ENOTSUPP;
+			goto prepare_data_mlli_exit;
+		}
+
+		if (unlikely(areq_ctx->is_icv_fragmented == true)) {
+			/* Backup happens only when ICV is fragmented, ICV
+			   verification is made by CPU compare in order to simplify
+			   MAC verification upon request completion */
+			  uint32_t size_to_skip = req->assoclen;
+			  if (areq_ctx->is_gcm4543) {
+				  size_to_skip += crypto_aead_ivsize(tfm);
+			  }
+			  ssi_buffer_mgr_copy_scatterlist_portion(
+				  areq_ctx->backup_mac, req->src,
+				  size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
+				  size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
+			areq_ctx->icv_virt_addr = areq_ctx->backup_mac;
+		} else { /* Contig. ICV */
+			/*Should hanlde if the sg is not contig.*/
+			areq_ctx->icv_dma_addr = sg_dma_address(
+				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) +
+				(*src_last_bytes - authsize);
+			areq_ctx->icv_virt_addr = sg_virt(
+				&areq_ctx->srcSgl[areq_ctx->src.nents - 1]) +
+				(*src_last_bytes - authsize);
+		}
+
+	} else {
+		/*NON-INPLACE and ENCRYPT*/
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
+			areq_ctx->dst.nents, areq_ctx->dstSgl,
+			areq_ctx->cryptlen,areq_ctx->dstOffset, is_last_table,
+			&areq_ctx->dst.mlli_nents);
+		ssi_buffer_mgr_add_scatterlist_entry(sg_data,
+			areq_ctx->src.nents, areq_ctx->srcSgl,
+			areq_ctx->cryptlen, areq_ctx->srcOffset,is_last_table,
+			&areq_ctx->src.mlli_nents);
+
+		icv_nents = ssi_buffer_mgr_get_aead_icv_nents(areq_ctx->dstSgl,
+			areq_ctx->dst.nents, authsize, *dst_last_bytes,
+			&areq_ctx->is_icv_fragmented);
+		if (unlikely(icv_nents < 0)) {
+			rc = -ENOTSUPP;
+			goto prepare_data_mlli_exit;
+		}
+
+		if (likely(areq_ctx->is_icv_fragmented == false)) {
+			/* Contig. ICV */
+			areq_ctx->icv_dma_addr = sg_dma_address(
+				&areq_ctx->dstSgl[areq_ctx->dst.nents - 1]) +
+				(*dst_last_bytes - authsize);
+			areq_ctx->icv_virt_addr = sg_virt(
+				&areq_ctx->dstSgl[areq_ctx->dst.nents - 1]) +
+				(*dst_last_bytes - authsize);
+		} else {
+			areq_ctx->icv_dma_addr = areq_ctx->mac_buf_dma_addr;
+			areq_ctx->icv_virt_addr = areq_ctx->mac_buf;
+		}
+	}
+
+prepare_data_mlli_exit:
+	return rc;
+}
+
+static inline int ssi_buffer_mgr_aead_chain_data(
+	struct ssi_drvdata *drvdata,
+	struct aead_request *req,
+	struct buffer_array *sg_data,
+	bool is_last_table, bool do_chain)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	struct device *dev = &drvdata->plat_dev->dev;
+	enum drv_crypto_direction direct = areq_ctx->gen_ctx.op_type;
+	unsigned int authsize = areq_ctx->req_authsize;
+	int src_last_bytes = 0, dst_last_bytes = 0;
+	int rc = 0;
+	uint32_t src_mapped_nents = 0, dst_mapped_nents = 0;
+	uint32_t offset = 0;
+	unsigned int size_for_map = req->assoclen +req->cryptlen; /*non-inplace mode*/
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	uint32_t sg_index = 0;
+	bool chained = false;
+	bool is_gcm4543 = areq_ctx->is_gcm4543;
+	uint32_t size_to_skip = req->assoclen;
+	if (is_gcm4543) {
+		size_to_skip += crypto_aead_ivsize(tfm);
+	}
+	offset = size_to_skip;
+
+	if (sg_data == NULL) {
+		rc = -EINVAL;
+		goto chain_data_exit;
+	}
+	areq_ctx->srcSgl = req->src;
+	areq_ctx->dstSgl = req->dst;
+
+	if (is_gcm4543) {
+		size_for_map += crypto_aead_ivsize(tfm);
+	}
+
+	size_for_map += (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? authsize:0;	
+	src_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->src,size_for_map,&src_last_bytes, &chained);  
+	sg_index = areq_ctx->srcSgl->length;
+	//check where the data starts
+	while (sg_index <= size_to_skip) {
+		offset -= areq_ctx->srcSgl->length;
+		areq_ctx->srcSgl = sg_next(areq_ctx->srcSgl);
+		//if have reached the end of the sgl, then this is unexpected
+		if (areq_ctx->srcSgl == NULL) {
+			SSI_LOG_ERR("reached end of sg list. unexpected \n");
+			BUG();
+		}
+		sg_index += areq_ctx->srcSgl->length;
+		src_mapped_nents--;
+	}
+	if (unlikely(src_mapped_nents > LLI_MAX_NUM_OF_DATA_ENTRIES))
+	{
+		SSI_LOG_ERR("Too many fragments. current %d max %d\n",
+				src_mapped_nents, LLI_MAX_NUM_OF_DATA_ENTRIES);
+			return -ENOMEM;
+	}
+
+	areq_ctx->src.nents = src_mapped_nents;
+
+	areq_ctx->srcOffset = offset;  
+
+	if (req->src != req->dst) {
+		size_for_map = req->assoclen +req->cryptlen;
+		size_for_map += (direct == DRV_CRYPTO_DIRECTION_ENCRYPT) ? authsize : 0;
+		if (is_gcm4543) {
+			size_for_map += crypto_aead_ivsize(tfm);
+		}
+
+		rc = ssi_buffer_mgr_map_scatterlist(dev, req->dst, size_for_map,
+			 DMA_BIDIRECTIONAL, &(areq_ctx->dst.nents),
+			 LLI_MAX_NUM_OF_DATA_ENTRIES, &dst_last_bytes,
+						   &dst_mapped_nents);
+		if (unlikely(rc != 0)) {
+			rc = -ENOMEM;
+			goto chain_data_exit; 
+		}
+	}
+
+	dst_mapped_nents = ssi_buffer_mgr_get_sgl_nents(req->dst,size_for_map,&dst_last_bytes, &chained);
+	sg_index = areq_ctx->dstSgl->length;
+	offset = size_to_skip;
+
+	//check where the data starts
+	while (sg_index <= size_to_skip) {
+
+		offset -= areq_ctx->dstSgl->length;
+		areq_ctx->dstSgl = sg_next(areq_ctx->dstSgl);
+		//if have reached the end of the sgl, then this is unexpected
+		if (areq_ctx->dstSgl == NULL) {
+			SSI_LOG_ERR("reached end of sg list. unexpected \n");
+			BUG();
+		}
+		sg_index += areq_ctx->dstSgl->length;
+		dst_mapped_nents--;
+	}
+	if (unlikely(dst_mapped_nents > LLI_MAX_NUM_OF_DATA_ENTRIES))
+	{
+		SSI_LOG_ERR("Too many fragments. current %d max %d\n",
+			    dst_mapped_nents, LLI_MAX_NUM_OF_DATA_ENTRIES);
+		return -ENOMEM;
+	}
+	areq_ctx->dst.nents = dst_mapped_nents;
+	areq_ctx->dstOffset = offset;
+	if ((src_mapped_nents > 1) ||
+	    (dst_mapped_nents  > 1) ||
+	    (do_chain == true)) {
+		areq_ctx->data_buff_type = SSI_DMA_BUF_MLLI;
+		rc = ssi_buffer_mgr_prepare_aead_data_mlli(drvdata, req, sg_data,
+			&src_last_bytes, &dst_last_bytes, is_last_table);
+	} else {
+		areq_ctx->data_buff_type = SSI_DMA_BUF_DLLI;
+		ssi_buffer_mgr_prepare_aead_data_dlli(
+				req, &src_last_bytes, &dst_last_bytes);
+	}
+
+chain_data_exit:
+	return rc;
+}
+
+static void ssi_buffer_mgr_update_aead_mlli_nents( struct ssi_drvdata *drvdata,
+					   struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	uint32_t curr_mlli_size = 0;
+	
+	if (areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) {
+		areq_ctx->assoc.sram_addr = drvdata->mlli_sram_addr;
+		curr_mlli_size = areq_ctx->assoc.mlli_nents * 
+						LLI_ENTRY_BYTE_SIZE;
+	}
+
+	if (areq_ctx->data_buff_type == SSI_DMA_BUF_MLLI) {
+		/*Inplace case dst nents equal to src nents*/
+		if (req->src == req->dst) {
+			areq_ctx->dst.mlli_nents = areq_ctx->src.mlli_nents;
+			areq_ctx->src.sram_addr = drvdata->mlli_sram_addr +
+								curr_mlli_size;
+			areq_ctx->dst.sram_addr = areq_ctx->src.sram_addr;
+			if (areq_ctx->is_single_pass == false)
+				areq_ctx->assoc.mlli_nents += 
+					areq_ctx->src.mlli_nents;
+		} else {
+			if (areq_ctx->gen_ctx.op_type == 
+					DRV_CRYPTO_DIRECTION_DECRYPT) {
+				areq_ctx->src.sram_addr = 
+						drvdata->mlli_sram_addr +
+								curr_mlli_size;
+				areq_ctx->dst.sram_addr = 
+						areq_ctx->src.sram_addr + 
+						areq_ctx->src.mlli_nents * 
+						LLI_ENTRY_BYTE_SIZE;
+				if (areq_ctx->is_single_pass == false)
+					areq_ctx->assoc.mlli_nents += 
+						areq_ctx->src.mlli_nents;
+			} else {
+				areq_ctx->dst.sram_addr = 
+						drvdata->mlli_sram_addr +
+								curr_mlli_size;
+				areq_ctx->src.sram_addr = 
+						areq_ctx->dst.sram_addr +
+						areq_ctx->dst.mlli_nents * 
+						LLI_ENTRY_BYTE_SIZE;
+				if (areq_ctx->is_single_pass == false)
+					areq_ctx->assoc.mlli_nents += 
+						areq_ctx->dst.mlli_nents;
+			}
+		}
+	}
+}
+
+int ssi_buffer_mgr_map_aead_request(
+	struct ssi_drvdata *drvdata, struct aead_request *req)
+{
+	struct aead_req_ctx *areq_ctx = aead_request_ctx(req);
+	struct mlli_params *mlli_params = &areq_ctx->mlli_params;
+	struct device *dev = &drvdata->plat_dev->dev;
+	struct buffer_array sg_data;
+	unsigned int authsize = areq_ctx->req_authsize;
+	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
+	int rc = 0;
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	bool is_gcm4543 = areq_ctx->is_gcm4543;
+
+	uint32_t mapped_nents = 0;
+	uint32_t dummy = 0; /*used for the assoc data fragments */
+	uint32_t size_to_map = 0;
+
+	mlli_params->curr_pool = NULL;
+	sg_data.num_of_buffers = 0;
+
+#if DX_HAS_ACP
+	if ((areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_DECRYPT) &&
+	    likely(req->src == req->dst))
+	{
+		uint32_t size_to_skip = req->assoclen;
+		if (is_gcm4543) {
+			size_to_skip += crypto_aead_ivsize(tfm);
+		}
+		/* copy mac to a temporary location to deal with possible
+		   data memory overriding that caused by cache coherence problem. */
+		ssi_buffer_mgr_copy_scatterlist_portion(
+			areq_ctx->backup_mac, req->src,
+			size_to_skip+ req->cryptlen - areq_ctx->req_authsize,
+			size_to_skip+ req->cryptlen, SSI_SG_TO_BUF);
+	}
+#endif
+
+	/* cacluate the size for cipher remove ICV in decrypt*/
+	areq_ctx->cryptlen = (areq_ctx->gen_ctx.op_type == 
+				 DRV_CRYPTO_DIRECTION_ENCRYPT) ? 
+				req->cryptlen :
+				(req->cryptlen - authsize);
+
+	areq_ctx->mac_buf_dma_addr = dma_map_single(dev,
+		areq_ctx->mac_buf, MAX_MAC_SIZE, DMA_BIDIRECTIONAL);
+	if (unlikely(dma_mapping_error(dev, areq_ctx->mac_buf_dma_addr))) {
+		SSI_LOG_ERR("Mapping mac_buf %u B at va=%pK for DMA failed\n",
+			MAX_MAC_SIZE, areq_ctx->mac_buf);
+		rc = -ENOMEM;
+		goto aead_map_failure;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->mac_buf_dma_addr, MAX_MAC_SIZE);
+
+	if (areq_ctx->ccm_hdr_size != ccm_header_size_null) {
+		areq_ctx->ccm_iv0_dma_addr = dma_map_single(dev,
+			(areq_ctx->ccm_config + CCM_CTR_COUNT_0_OFFSET),
+			AES_BLOCK_SIZE, DMA_TO_DEVICE);
+
+		if (unlikely(dma_mapping_error(dev, areq_ctx->ccm_iv0_dma_addr))) {
+			SSI_LOG_ERR("Mapping mac_buf %u B at va=%pK "
+			"for DMA failed\n", AES_BLOCK_SIZE,
+			(areq_ctx->ccm_config + CCM_CTR_COUNT_0_OFFSET));
+			areq_ctx->ccm_iv0_dma_addr = 0;
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->ccm_iv0_dma_addr,
+								AES_BLOCK_SIZE);
+		if (ssi_aead_handle_config_buf(dev, areq_ctx,
+			areq_ctx->ccm_config, &sg_data, req->assoclen) != 0) {
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+	}
+
+#if SSI_CC_HAS_AES_GCM
+	if (areq_ctx->cipher_mode == DRV_CIPHER_GCTR) {
+		areq_ctx->hkey_dma_addr = dma_map_single(dev,
+			areq_ctx->hkey, AES_BLOCK_SIZE, DMA_BIDIRECTIONAL);
+		if (unlikely(dma_mapping_error(dev, areq_ctx->hkey_dma_addr))) {
+			SSI_LOG_ERR("Mapping hkey %u B at va=%pK for DMA failed\n",
+				AES_BLOCK_SIZE, areq_ctx->hkey);
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->hkey_dma_addr, AES_BLOCK_SIZE);
+
+		areq_ctx->gcm_block_len_dma_addr = dma_map_single(dev,
+			&areq_ctx->gcm_len_block, AES_BLOCK_SIZE, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(dev, areq_ctx->gcm_block_len_dma_addr))) {
+			SSI_LOG_ERR("Mapping gcm_len_block %u B at va=%pK for DMA failed\n",
+				AES_BLOCK_SIZE, &areq_ctx->gcm_len_block);
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_block_len_dma_addr, AES_BLOCK_SIZE);
+
+		areq_ctx->gcm_iv_inc1_dma_addr = dma_map_single(dev,
+			areq_ctx->gcm_iv_inc1,
+			AES_BLOCK_SIZE, DMA_TO_DEVICE);
+
+		if (unlikely(dma_mapping_error(dev, areq_ctx->gcm_iv_inc1_dma_addr))) {
+			SSI_LOG_ERR("Mapping gcm_iv_inc1 %u B at va=%pK "
+			"for DMA failed\n", AES_BLOCK_SIZE,
+			(areq_ctx->gcm_iv_inc1));
+			areq_ctx->gcm_iv_inc1_dma_addr = 0;
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc1_dma_addr,
+								AES_BLOCK_SIZE);
+
+		areq_ctx->gcm_iv_inc2_dma_addr = dma_map_single(dev,
+			areq_ctx->gcm_iv_inc2,
+			AES_BLOCK_SIZE, DMA_TO_DEVICE);
+
+		if (unlikely(dma_mapping_error(dev, areq_ctx->gcm_iv_inc2_dma_addr))) {
+			SSI_LOG_ERR("Mapping gcm_iv_inc2 %u B at va=%pK "
+			"for DMA failed\n", AES_BLOCK_SIZE,
+			(areq_ctx->gcm_iv_inc2));
+			areq_ctx->gcm_iv_inc2_dma_addr = 0;
+			rc = -ENOMEM;
+			goto aead_map_failure;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(areq_ctx->gcm_iv_inc2_dma_addr,
+								AES_BLOCK_SIZE);
+	}
+#endif /*SSI_CC_HAS_AES_GCM*/
+
+	size_to_map = req->cryptlen + req->assoclen;
+	if (areq_ctx->gen_ctx.op_type == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+		size_to_map += authsize;
+	}
+	if (is_gcm4543)
+		size_to_map += crypto_aead_ivsize(tfm);
+	rc = ssi_buffer_mgr_map_scatterlist(dev, req->src,
+					    size_to_map, DMA_BIDIRECTIONAL, &(areq_ctx->src.nents),
+					    LLI_MAX_NUM_OF_ASSOC_DATA_ENTRIES+LLI_MAX_NUM_OF_DATA_ENTRIES, &dummy, &mapped_nents);
+	if (unlikely(rc != 0)) {
+		rc = -ENOMEM;
+		goto aead_map_failure; 
+	}
+
+	if (likely(areq_ctx->is_single_pass == true)) {
+		/*
+		* Create MLLI table for: 
+		*   (1) Assoc. data
+		*   (2) Src/Dst SGLs
+		*   Note: IV is contg. buffer (not an SGL) 
+		*/
+		rc = ssi_buffer_mgr_aead_chain_assoc(drvdata, req, &sg_data, true, false);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+		rc = ssi_buffer_mgr_aead_chain_iv(drvdata, req, &sg_data, true, false);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+		rc = ssi_buffer_mgr_aead_chain_data(drvdata, req, &sg_data, true, false);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+	} else { /* DOUBLE-PASS flow */
+		/*
+		* Prepare MLLI table(s) in this order:
+		*  
+		* If ENCRYPT/DECRYPT (inplace):
+		*   (1) MLLI table for assoc
+		*   (2) IV entry (chained right after end of assoc)
+		*   (3) MLLI for src/dst (inplace operation)
+		*  
+		* If ENCRYPT (non-inplace) 
+		*   (1) MLLI table for assoc
+		*   (2) IV entry (chained right after end of assoc)
+		*   (3) MLLI for dst
+		*   (4) MLLI for src
+		*  
+		* If DECRYPT (non-inplace) 
+		*   (1) MLLI table for assoc
+		*   (2) IV entry (chained right after end of assoc)
+		*   (3) MLLI for src
+		*   (4) MLLI for dst
+		*/
+		rc = ssi_buffer_mgr_aead_chain_assoc(drvdata, req, &sg_data, false, true);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+		rc = ssi_buffer_mgr_aead_chain_iv(drvdata, req, &sg_data, false, true);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+		rc = ssi_buffer_mgr_aead_chain_data(drvdata, req, &sg_data, true, true);
+		if (unlikely(rc != 0))
+			goto aead_map_failure;
+	}
+
+	/* Mlli support -start building the MLLI according to the above results */
+	if (unlikely(
+		(areq_ctx->assoc_buff_type == SSI_DMA_BUF_MLLI) ||
+		(areq_ctx->data_buff_type == SSI_DMA_BUF_MLLI))) {
+
+		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
+		rc = ssi_buffer_mgr_generate_mlli(dev, &sg_data, mlli_params);
+		if (unlikely(rc != 0)) {
+			goto aead_map_failure;
+		}
+
+		ssi_buffer_mgr_update_aead_mlli_nents(drvdata, req);
+		SSI_LOG_DEBUG("assoc params mn %d\n",areq_ctx->assoc.mlli_nents);
+		SSI_LOG_DEBUG("src params mn %d\n",areq_ctx->src.mlli_nents);
+		SSI_LOG_DEBUG("dst params mn %d\n",areq_ctx->dst.mlli_nents);
+	}
+	return 0;
+
+aead_map_failure:
+	ssi_buffer_mgr_unmap_aead_request(dev, req);
+	return rc;
+}
+
+int ssi_buffer_mgr_map_hash_request_final(
+	struct ssi_drvdata *drvdata, void *ctx, struct scatterlist *src, unsigned int nbytes, bool do_update)
+{
+	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
+	struct device *dev = &drvdata->plat_dev->dev;
+	uint8_t* curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
+			areq_ctx->buff0;
+	uint32_t *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
+			&areq_ctx->buff0_cnt;
+	struct mlli_params *mlli_params = &areq_ctx->mlli_params;	
+	struct buffer_array sg_data;
+	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
+	uint32_t dummy = 0;
+	uint32_t mapped_nents = 0;
+
+	SSI_LOG_DEBUG(" final params : curr_buff=%pK "
+		     "curr_buff_cnt=0x%X nbytes = 0x%X "
+		     "src=%pK curr_index=%u\n",
+		     curr_buff, *curr_buff_cnt, nbytes,
+		     src, areq_ctx->buff_index);
+	/* Init the type of the dma buffer */
+	areq_ctx->data_dma_buf_type = SSI_DMA_BUF_NULL;
+	mlli_params->curr_pool = NULL;
+	sg_data.num_of_buffers = 0;
+	areq_ctx->in_nents = 0;
+
+	if (unlikely(nbytes == 0 && *curr_buff_cnt == 0)) {
+		/* nothing to do */
+		return 0;
+	}
+	
+	/*TODO: copy data in case that buffer is enough for operation */
+	/* map the previous buffer */
+	if (*curr_buff_cnt != 0 ) {
+		if (ssi_ahash_handle_curr_buf(dev, areq_ctx, curr_buff,
+					    *curr_buff_cnt, &sg_data) != 0) {
+			return -ENOMEM;
+		}
+	}
+
+	if (src && (nbytes > 0) && do_update) {
+		if ( unlikely( ssi_buffer_mgr_map_scatterlist( dev,src,
+					  nbytes,
+					  DMA_TO_DEVICE,
+					  &areq_ctx->in_nents,
+					  LLI_MAX_NUM_OF_DATA_ENTRIES,
+					  &dummy, &mapped_nents))){
+			goto unmap_curr_buff;
+		}
+		if ( src && (mapped_nents == 1) 
+		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) ) {
+			memcpy(areq_ctx->buff_sg,src,
+			       sizeof(struct scatterlist));
+			areq_ctx->buff_sg->length = nbytes;
+			areq_ctx->curr_sg = areq_ctx->buff_sg;
+			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_DLLI;
+		} else {
+			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_MLLI;
+		}
+
+	}
+
+	/*build mlli */
+	if (unlikely(areq_ctx->data_dma_buf_type == SSI_DMA_BUF_MLLI)) {
+		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
+		/* add the src data to the sg_data */
+		ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
+					areq_ctx->in_nents,
+					src,
+					nbytes, 0,
+					true, &areq_ctx->mlli_nents);
+		if (unlikely(ssi_buffer_mgr_generate_mlli(dev, &sg_data,
+						  mlli_params) != 0)) {
+			goto fail_unmap_din;
+		}
+	}
+	/* change the buffer index for the unmap function */
+	areq_ctx->buff_index = (areq_ctx->buff_index^1);
+	SSI_LOG_DEBUG("areq_ctx->data_dma_buf_type = %s\n",
+		GET_DMA_BUFFER_TYPE(areq_ctx->data_dma_buf_type));
+	return 0;
+
+fail_unmap_din:
+	dma_unmap_sg(dev, src, areq_ctx->in_nents, DMA_TO_DEVICE);
+
+unmap_curr_buff:
+	if (*curr_buff_cnt != 0 ) {
+		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
+	}
+	return -ENOMEM;
+}
+
+int ssi_buffer_mgr_map_hash_request_update(
+	struct ssi_drvdata *drvdata, void *ctx, struct scatterlist *src, unsigned int nbytes, unsigned int block_size)
+{
+	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
+	struct device *dev = &drvdata->plat_dev->dev;
+	uint8_t* curr_buff = areq_ctx->buff_index ? areq_ctx->buff1 :
+			areq_ctx->buff0;
+	uint32_t *curr_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff1_cnt :
+			&areq_ctx->buff0_cnt;
+	uint8_t* next_buff = areq_ctx->buff_index ? areq_ctx->buff0 :
+			areq_ctx->buff1;
+	uint32_t *next_buff_cnt = areq_ctx->buff_index ? &areq_ctx->buff0_cnt :
+			&areq_ctx->buff1_cnt;
+	struct mlli_params *mlli_params = &areq_ctx->mlli_params;	
+	unsigned int update_data_len;
+	uint32_t total_in_len = nbytes + *curr_buff_cnt;
+	struct buffer_array sg_data;
+	struct buff_mgr_handle *buff_mgr = drvdata->buff_mgr_handle;
+	unsigned int swap_index = 0;
+	uint32_t dummy = 0;
+	uint32_t mapped_nents = 0;
+		
+	SSI_LOG_DEBUG(" update params : curr_buff=%pK "
+		     "curr_buff_cnt=0x%X nbytes=0x%X "
+		     "src=%pK curr_index=%u \n",
+		     curr_buff, *curr_buff_cnt, nbytes,
+		     src, areq_ctx->buff_index);
+	/* Init the type of the dma buffer */
+	areq_ctx->data_dma_buf_type = SSI_DMA_BUF_NULL;
+	mlli_params->curr_pool = NULL;
+	areq_ctx->curr_sg = NULL;
+	sg_data.num_of_buffers = 0;
+	areq_ctx->in_nents = 0;
+
+	if (unlikely(total_in_len < block_size)) {
+		SSI_LOG_DEBUG(" less than one block: curr_buff=%pK "
+			     "*curr_buff_cnt=0x%X copy_to=%pK\n",
+			curr_buff, *curr_buff_cnt,
+			&curr_buff[*curr_buff_cnt]);
+		areq_ctx->in_nents = 
+			ssi_buffer_mgr_get_sgl_nents(src,
+						    nbytes,
+						    &dummy, NULL);
+		sg_copy_to_buffer(src, areq_ctx->in_nents,
+				  &curr_buff[*curr_buff_cnt], nbytes); 
+		*curr_buff_cnt += nbytes;
+		return 1;
+	}
+
+	/* Calculate the residue size*/
+	*next_buff_cnt = total_in_len & (block_size - 1);
+	/* update data len */
+	update_data_len = total_in_len - *next_buff_cnt;
+
+	SSI_LOG_DEBUG(" temp length : *next_buff_cnt=0x%X "
+		     "update_data_len=0x%X\n",
+		*next_buff_cnt, update_data_len);
+
+	/* Copy the new residue to next buffer */
+	if (*next_buff_cnt != 0) {
+		SSI_LOG_DEBUG(" handle residue: next buff %pK skip data %u"
+			     " residue %u \n", next_buff,
+			     (update_data_len - *curr_buff_cnt),
+			     *next_buff_cnt);
+		ssi_buffer_mgr_copy_scatterlist_portion(next_buff, src,
+			     (update_data_len -*curr_buff_cnt),
+			     nbytes,SSI_SG_TO_BUF);
+		/* change the buffer index for next operation */
+		swap_index = 1;
+	}
+
+	if (*curr_buff_cnt != 0) {
+		if (ssi_ahash_handle_curr_buf(dev, areq_ctx, curr_buff,
+					    *curr_buff_cnt, &sg_data) != 0) {
+			return -ENOMEM;
+		}
+		/* change the buffer index for next operation */
+		swap_index = 1;
+	}
+	
+	if ( update_data_len > *curr_buff_cnt ) {
+		if ( unlikely( ssi_buffer_mgr_map_scatterlist( dev,src,
+					  (update_data_len -*curr_buff_cnt),
+					  DMA_TO_DEVICE,
+					  &areq_ctx->in_nents,
+					  LLI_MAX_NUM_OF_DATA_ENTRIES,
+					  &dummy, &mapped_nents))){
+			goto unmap_curr_buff;
+		}
+		if ( (mapped_nents == 1) 
+		     && (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) ) {
+			/* only one entry in the SG and no previous data */
+			memcpy(areq_ctx->buff_sg,src,
+			       sizeof(struct scatterlist));
+			areq_ctx->buff_sg->length = update_data_len;
+			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_DLLI;
+			areq_ctx->curr_sg = areq_ctx->buff_sg;
+		} else {
+			areq_ctx->data_dma_buf_type = SSI_DMA_BUF_MLLI;
+		}
+	}
+
+	if (unlikely(areq_ctx->data_dma_buf_type == SSI_DMA_BUF_MLLI)) {
+		mlli_params->curr_pool = buff_mgr->mlli_buffs_pool;
+		/* add the src data to the sg_data */
+		ssi_buffer_mgr_add_scatterlist_entry(&sg_data,
+					areq_ctx->in_nents,
+					src,
+					(update_data_len - *curr_buff_cnt), 0,
+					true, &areq_ctx->mlli_nents);
+		if (unlikely(ssi_buffer_mgr_generate_mlli(dev, &sg_data,
+						  mlli_params) != 0)) {
+			goto fail_unmap_din;
+		}
+
+	}
+	areq_ctx->buff_index = (areq_ctx->buff_index^swap_index);
+
+	return 0;
+
+fail_unmap_din:
+	dma_unmap_sg(dev, src, areq_ctx->in_nents, DMA_TO_DEVICE);
+
+unmap_curr_buff:
+	if (*curr_buff_cnt != 0 ) {
+		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
+	}
+	return -ENOMEM;
+}
+
+void ssi_buffer_mgr_unmap_hash_request(
+	struct device *dev, void *ctx, struct scatterlist *src, bool do_revert)
+{
+	struct ahash_req_ctx *areq_ctx = (struct ahash_req_ctx *)ctx;
+	uint32_t *prev_len = areq_ctx->buff_index ?  &areq_ctx->buff0_cnt :
+						&areq_ctx->buff1_cnt;
+
+	/*In case a pool was set, a table was 
+	  allocated and should be released */
+	if (areq_ctx->mlli_params.curr_pool != NULL) {
+		SSI_LOG_DEBUG("free MLLI buffer: dma=0x%llX virt=%pK\n", 
+			     (unsigned long long)areq_ctx->mlli_params.mlli_dma_addr,
+			     areq_ctx->mlli_params.mlli_virt_addr);
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(areq_ctx->mlli_params.mlli_dma_addr);
+		dma_pool_free(areq_ctx->mlli_params.curr_pool,
+			      areq_ctx->mlli_params.mlli_virt_addr,
+			      areq_ctx->mlli_params.mlli_dma_addr);
+	}
+	
+	if ((src) && likely(areq_ctx->in_nents != 0)) {
+		SSI_LOG_DEBUG("Unmapped sg src: virt=%pK dma=0x%llX len=0x%X\n",
+			     sg_virt(src),
+			     (unsigned long long)sg_dma_address(src), 
+			     sg_dma_len(src));
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(sg_dma_address(src));
+		dma_unmap_sg(dev, src, 
+			     areq_ctx->in_nents, DMA_TO_DEVICE);
+	}
+
+	if (*prev_len != 0) {
+		SSI_LOG_DEBUG("Unmapped buffer: areq_ctx->buff_sg=%pK"
+			     "dma=0x%llX len 0x%X\n", 
+				sg_virt(areq_ctx->buff_sg),
+				(unsigned long long)sg_dma_address(areq_ctx->buff_sg), 
+				sg_dma_len(areq_ctx->buff_sg));
+		dma_unmap_sg(dev, areq_ctx->buff_sg, 1, DMA_TO_DEVICE);
+		if (!do_revert) {
+			/* clean the previous data length for update operation */
+			*prev_len = 0;
+		} else {
+			areq_ctx->buff_index ^= 1;
+		}
+	}
+}
+
+int ssi_buffer_mgr_init(struct ssi_drvdata *drvdata)
+{
+	struct buff_mgr_handle *buff_mgr_handle;
+	struct device *dev = &drvdata->plat_dev->dev;
+
+	buff_mgr_handle = (struct buff_mgr_handle *)
+		kmalloc(sizeof(struct buff_mgr_handle), GFP_KERNEL);
+	if (buff_mgr_handle == NULL)
+		return -ENOMEM;
+
+	drvdata->buff_mgr_handle = buff_mgr_handle;
+
+	buff_mgr_handle->mlli_buffs_pool = dma_pool_create(
+				"dx_single_mlli_tables", dev,
+				MAX_NUM_OF_TOTAL_MLLI_ENTRIES * 
+				LLI_ENTRY_BYTE_SIZE,
+				MLLI_TABLE_MIN_ALIGNMENT, 0);
+
+	if (unlikely(buff_mgr_handle->mlli_buffs_pool == NULL))
+		goto error;
+
+	return 0;
+
+error:
+	ssi_buffer_mgr_fini(drvdata);
+	return -ENOMEM;
+}
+
+int ssi_buffer_mgr_fini(struct ssi_drvdata *drvdata)
+{
+	struct buff_mgr_handle *buff_mgr_handle = drvdata->buff_mgr_handle;
+
+	if (buff_mgr_handle  != NULL) {
+		dma_pool_destroy(buff_mgr_handle->mlli_buffs_pool);
+		kfree(drvdata->buff_mgr_handle);
+		drvdata->buff_mgr_handle = NULL;
+
+	}
+	return 0;
+}
+
diff --git a/drivers/staging/ccree/ssi_buffer_mgr.h b/drivers/staging/ccree/ssi_buffer_mgr.h
new file mode 100644
index 000000000000..5f4b032389f1
--- /dev/null
+++ b/drivers/staging/ccree/ssi_buffer_mgr.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file buffer_mgr.h
+   Buffer Manager
+ */
+
+#ifndef __SSI_BUFFER_MGR_H__
+#define __SSI_BUFFER_MGR_H__
+
+#include <crypto/algapi.h>
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+
+
+enum ssi_req_dma_buf_type {
+	SSI_DMA_BUF_NULL = 0,
+	SSI_DMA_BUF_DLLI,
+	SSI_DMA_BUF_MLLI
+};
+
+enum ssi_sg_cpy_direct {
+	SSI_SG_TO_BUF = 0,
+	SSI_SG_FROM_BUF = 1
+};
+
+struct ssi_mlli {
+	ssi_sram_addr_t sram_addr;
+	unsigned int nents; //sg nents
+	unsigned int mlli_nents; //mlli nents might be different than the above
+};
+
+struct mlli_params {
+	struct dma_pool *curr_pool;
+	uint8_t *mlli_virt_addr;
+	dma_addr_t mlli_dma_addr;
+	uint32_t mlli_len;  
+};
+
+int ssi_buffer_mgr_init(struct ssi_drvdata *drvdata);
+
+int ssi_buffer_mgr_fini(struct ssi_drvdata *drvdata);
+
+int ssi_buffer_mgr_map_blkcipher_request(
+	struct ssi_drvdata *drvdata,
+	void *ctx,
+	unsigned int ivsize,
+	unsigned int nbytes,
+	void *info,
+	struct scatterlist *src,
+	struct scatterlist *dst);
+
+void ssi_buffer_mgr_unmap_blkcipher_request(
+	struct device *dev, 
+	void *ctx,
+	unsigned int ivsize,
+	struct scatterlist *src,
+	struct scatterlist *dst);
+
+int ssi_buffer_mgr_map_aead_request(struct ssi_drvdata *drvdata, struct aead_request *req);
+
+void ssi_buffer_mgr_unmap_aead_request(struct device *dev, struct aead_request *req);
+
+int ssi_buffer_mgr_map_hash_request_final(struct ssi_drvdata *drvdata, void *ctx, struct scatterlist *src, unsigned int nbytes, bool do_update);
+
+int ssi_buffer_mgr_map_hash_request_update(struct ssi_drvdata *drvdata, void *ctx, struct scatterlist *src, unsigned int nbytes, unsigned int block_size);
+
+void ssi_buffer_mgr_unmap_hash_request(struct device *dev, void *ctx, struct scatterlist *src, bool do_revert);
+
+void ssi_buffer_mgr_copy_scatterlist_portion(u8 *dest, struct scatterlist *sg, uint32_t to_skip, uint32_t end, enum ssi_sg_cpy_direct direct);
+
+void ssi_buffer_mgr_zero_sgl(struct scatterlist *sgl, uint32_t data_len);
+
+
+#ifdef CC_DMA_48BIT_SIM
+dma_addr_t ssi_buff_mgr_update_dma_addr(dma_addr_t orig_addr, uint32_t data_len);
+dma_addr_t ssi_buff_mgr_restore_dma_addr(dma_addr_t orig_addr);
+
+#define SSI_UPDATE_DMA_ADDR_TO_48BIT(addr,size) addr = \
+					ssi_buff_mgr_update_dma_addr(addr,size)
+#define SSI_RESTORE_DMA_ADDR_TO_48BIT(addr) addr = \
+					ssi_buff_mgr_restore_dma_addr(addr)
+#else
+
+#define SSI_UPDATE_DMA_ADDR_TO_48BIT(addr,size) addr = addr
+#define SSI_RESTORE_DMA_ADDR_TO_48BIT(addr) addr = addr
+
+#endif
+
+#endif /*__BUFFER_MGR_H__*/
+
diff --git a/drivers/staging/ccree/ssi_cipher.c b/drivers/staging/ccree/ssi_cipher.c
new file mode 100644
index 000000000000..664ed7e52cf2
--- /dev/null
+++ b/drivers/staging/ccree/ssi_cipher.c
@@ -0,0 +1,1503 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <crypto/algapi.h>
+#include <crypto/internal/skcipher.h>
+#include <crypto/aes.h>
+#include <crypto/ctr.h>
+#include <crypto/des.h>
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "cc_lli_defs.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_cipher.h"
+#include "ssi_request_mgr.h"
+#include "ssi_sysfs.h"
+#include "ssi_fips_local.h"
+
+#define MAX_ABLKCIPHER_SEQ_LEN 6
+
+#define template_ablkcipher	template_u.ablkcipher
+#define template_sblkcipher	template_u.blkcipher
+
+#define SSI_MIN_AES_XTS_SIZE 0x10
+#define SSI_MAX_AES_XTS_SIZE 0x2000
+struct ssi_blkcipher_handle {
+	struct list_head blkcipher_alg_list;
+};
+
+struct cc_user_key_info {
+	uint8_t *key;
+	dma_addr_t key_dma_addr;
+};
+struct cc_hw_key_info {
+	enum HwCryptoKey key1_slot;
+	enum HwCryptoKey key2_slot;
+};
+
+struct ssi_ablkcipher_ctx {
+	struct ssi_drvdata *drvdata;
+	int keylen;
+	int key_round_number;
+	int cipher_mode;
+	int flow_mode;
+	unsigned int flags;
+	struct blkcipher_req_ctx *sync_ctx;
+	struct cc_user_key_info user;
+	struct cc_hw_key_info hw;
+	struct crypto_shash *shash_tfm;
+};
+
+static void ssi_ablkcipher_complete(struct device *dev, void *ssi_req, void __iomem *cc_base);
+
+
+static int validate_keys_sizes(struct ssi_ablkcipher_ctx *ctx_p, uint32_t size) {
+	switch (ctx_p->flow_mode){
+	case S_DIN_to_AES:
+		switch (size){
+		case CC_AES_128_BIT_KEY_SIZE:
+		case CC_AES_192_BIT_KEY_SIZE:
+			if (likely((ctx_p->cipher_mode != DRV_CIPHER_XTS) &&
+				   (ctx_p->cipher_mode != DRV_CIPHER_ESSIV) &&
+				   (ctx_p->cipher_mode != DRV_CIPHER_BITLOCKER)))
+				return 0;
+			break;
+		case CC_AES_256_BIT_KEY_SIZE:
+			return 0;
+		case (CC_AES_192_BIT_KEY_SIZE*2):
+		case (CC_AES_256_BIT_KEY_SIZE*2):
+			if (likely((ctx_p->cipher_mode == DRV_CIPHER_XTS) ||
+				   (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) ||
+				   (ctx_p->cipher_mode == DRV_CIPHER_BITLOCKER)))
+				return 0;
+			break;
+		default:
+			break;
+		}
+	case S_DIN_to_DES:
+		if (likely(size == DES3_EDE_KEY_SIZE ||
+		    size == DES_KEY_SIZE))
+			return 0;
+		break;
+#if SSI_CC_HAS_MULTI2
+	case S_DIN_to_MULTI2:
+		if (likely(size == CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE))
+			return 0;
+		break;
+#endif
+	default:
+		break;
+
+	}
+	return -EINVAL;
+}
+
+
+static int validate_data_size(struct ssi_ablkcipher_ctx *ctx_p, unsigned int size) {
+	switch (ctx_p->flow_mode){
+	case S_DIN_to_AES:
+		switch (ctx_p->cipher_mode){
+		case DRV_CIPHER_XTS:
+			if ((size >= SSI_MIN_AES_XTS_SIZE) &&
+			    (size <= SSI_MAX_AES_XTS_SIZE) && 
+			    IS_ALIGNED(size, AES_BLOCK_SIZE))
+				return 0;
+			break;
+		case DRV_CIPHER_CBC_CTS:
+			if (likely(size >= AES_BLOCK_SIZE))
+				return 0;
+			break;
+		case DRV_CIPHER_OFB:
+		case DRV_CIPHER_CTR:
+				return 0;
+		case DRV_CIPHER_ECB:
+		case DRV_CIPHER_CBC:
+		case DRV_CIPHER_ESSIV:
+		case DRV_CIPHER_BITLOCKER:
+			if (likely(IS_ALIGNED(size, AES_BLOCK_SIZE)))
+				return 0;
+			break;
+		default:
+			break;
+		}
+		break;
+	case S_DIN_to_DES:
+		if (likely(IS_ALIGNED(size, DES_BLOCK_SIZE)))
+				return 0;
+		break;
+#if SSI_CC_HAS_MULTI2
+	case S_DIN_to_MULTI2:
+		switch (ctx_p->cipher_mode) {
+		case DRV_MULTI2_CBC:
+			if (likely(IS_ALIGNED(size, CC_MULTI2_BLOCK_SIZE)))
+				return 0;
+			break;
+		case DRV_MULTI2_OFB:
+			return 0;
+		default:
+			break;
+		}
+		break;
+#endif /*SSI_CC_HAS_MULTI2*/
+	default:
+		break;
+
+	}
+	return -EINVAL;
+}
+
+static unsigned int get_max_keysize(struct crypto_tfm *tfm)
+{
+	struct ssi_crypto_alg *ssi_alg = container_of(tfm->__crt_alg, struct ssi_crypto_alg, crypto_alg);
+
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_ABLKCIPHER) {
+		return ssi_alg->crypto_alg.cra_ablkcipher.max_keysize;
+	}
+
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_TYPE_MASK) == CRYPTO_ALG_TYPE_BLKCIPHER) {
+		return ssi_alg->crypto_alg.cra_blkcipher.max_keysize;
+	}
+
+	return 0;
+}
+
+static int ssi_blkcipher_init(struct crypto_tfm *tfm)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct ssi_crypto_alg *ssi_alg =
+			container_of(alg, struct ssi_crypto_alg, crypto_alg);
+	struct device *dev;
+	int rc = 0;
+	unsigned int max_key_buf_size = get_max_keysize(tfm);
+
+	SSI_LOG_DEBUG("Initializing context @%p for %s\n", ctx_p, 
+						crypto_tfm_alg_name(tfm));
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	ctx_p->cipher_mode = ssi_alg->cipher_mode;
+	ctx_p->flow_mode = ssi_alg->flow_mode;
+	ctx_p->drvdata = ssi_alg->drvdata;
+	dev = &ctx_p->drvdata->plat_dev->dev;
+
+	/* Allocate key buffer, cache line aligned */
+	ctx_p->user.key = kmalloc(max_key_buf_size, GFP_KERNEL|GFP_DMA);
+	if (!ctx_p->user.key) {
+		SSI_LOG_ERR("Allocating key buffer in context failed\n");
+		rc = -ENOMEM;
+	}
+	SSI_LOG_DEBUG("Allocated key buffer in context. key=@%p\n",
+		      ctx_p->user.key);
+
+	/* Map key buffer */
+	ctx_p->user.key_dma_addr = dma_map_single(dev, (void *)ctx_p->user.key,
+					     max_key_buf_size, DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, ctx_p->user.key_dma_addr)) {
+		SSI_LOG_ERR("Mapping Key %u B at va=%pK for DMA failed\n",
+			max_key_buf_size, ctx_p->user.key);
+		return -ENOMEM;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr, max_key_buf_size);
+	SSI_LOG_DEBUG("Mapped key %u B at va=%pK to dma=0x%llX\n",
+		max_key_buf_size, ctx_p->user.key,
+		(unsigned long long)ctx_p->user.key_dma_addr);
+
+	if (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) {
+		/* Alloc hash tfm for essiv */
+		ctx_p->shash_tfm = crypto_alloc_shash("sha256-generic", 0, 0);
+		if (IS_ERR(ctx_p->shash_tfm)) {
+			SSI_LOG_ERR("Error allocating hash tfm for ESSIV.\n");
+			return PTR_ERR(ctx_p->shash_tfm);
+		}
+	}
+
+	return rc;
+}
+
+static void ssi_blkcipher_exit(struct crypto_tfm *tfm)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct device *dev = &ctx_p->drvdata->plat_dev->dev;
+	unsigned int max_key_buf_size = get_max_keysize(tfm);
+
+	SSI_LOG_DEBUG("Clearing context @%p for %s\n",
+		crypto_tfm_ctx(tfm), crypto_tfm_alg_name(tfm));
+
+	if (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) {
+		/* Free hash tfm for essiv */
+		crypto_free_shash(ctx_p->shash_tfm);
+		ctx_p->shash_tfm = NULL;
+	}
+
+	/* Unmap key buffer */
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr);
+	dma_unmap_single(dev, ctx_p->user.key_dma_addr, max_key_buf_size,
+								DMA_TO_DEVICE);
+	SSI_LOG_DEBUG("Unmapped key buffer key_dma_addr=0x%llX\n", 
+		(unsigned long long)ctx_p->user.key_dma_addr);
+
+	/* Free key buffer in context */
+	kfree(ctx_p->user.key);
+	SSI_LOG_DEBUG("Free key buffer in context. key=@%p\n", ctx_p->user.key);
+}
+
+
+typedef struct tdes_keys{
+        u8      key1[DES_KEY_SIZE];
+        u8      key2[DES_KEY_SIZE];
+        u8      key3[DES_KEY_SIZE];
+}tdes_keys_t;
+
+static const u8 zero_buff[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 
+                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 
+                               0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+
+/* The function verifies that tdes keys are not weak.*/
+static int ssi_fips_verify_3des_keys(const u8 *key, unsigned int keylen)
+{
+#ifdef CCREE_FIPS_SUPPORT
+        tdes_keys_t *tdes_key = (tdes_keys_t*)key;
+
+	/* verify key1 != key2 and key3 != key2*/
+        if (unlikely( (memcmp((u8*)tdes_key->key1, (u8*)tdes_key->key2, sizeof(tdes_key->key1)) == 0) || 
+		      (memcmp((u8*)tdes_key->key3, (u8*)tdes_key->key2, sizeof(tdes_key->key3)) == 0) )) {
+                return -ENOEXEC;
+        }
+#endif /* CCREE_FIPS_SUPPORT */
+
+        return 0;
+}
+
+/* The function verifies that xts keys are not weak.*/
+static int ssi_fips_verify_xts_keys(const u8 *key, unsigned int keylen)
+{
+#ifdef CCREE_FIPS_SUPPORT
+        /* Weak key is define as key that its first half (128/256 lsb) equals its second half (128/256 msb) */
+        int singleKeySize = keylen >> 1;
+
+	if (unlikely(memcmp(key, &key[singleKeySize], singleKeySize) == 0)) {
+		return -ENOEXEC;
+	}
+#endif /* CCREE_FIPS_SUPPORT */
+
+        return 0;
+}
+
+static enum HwCryptoKey hw_key_to_cc_hw_key(int slot_num)
+{
+	switch (slot_num) {
+	case 0:
+		return KFDE0_KEY;
+	case 1:
+		return KFDE1_KEY;
+	case 2:
+		return KFDE2_KEY;
+	case 3:
+		return KFDE3_KEY;
+	}
+	return END_OF_KEYS;
+}
+
+static int ssi_blkcipher_setkey(struct crypto_tfm *tfm, 
+				const u8 *key, 
+				unsigned int keylen)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct device *dev = &ctx_p->drvdata->plat_dev->dev;
+	u32 tmp[DES_EXPKEY_WORDS];
+	unsigned int max_key_buf_size = get_max_keysize(tfm);
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	SSI_LOG_DEBUG("Setting key in context @%p for %s. keylen=%u\n",
+		ctx_p, crypto_tfm_alg_name(tfm), keylen);
+	dump_byte_array("key", (uint8_t *)key, keylen);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	SSI_LOG_DEBUG("ssi_blkcipher_setkey: after FIPS check");
+	
+	/* STAT_PHASE_0: Init and sanity checks */
+	START_CYCLE_COUNT();
+
+#if SSI_CC_HAS_MULTI2
+	/*last byte of key buffer is round number and should not be a part of key size*/
+	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
+		keylen -=1;
+	}
+#endif /*SSI_CC_HAS_MULTI2*/
+
+	if (unlikely(validate_keys_sizes(ctx_p,keylen) != 0)) {
+		SSI_LOG_ERR("Unsupported key size %d.\n", keylen);
+		crypto_tfm_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	if (ssi_is_hw_key(tfm)) {
+		/* setting HW key slots */
+		struct arm_hw_key_info *hki = (struct arm_hw_key_info*)key;
+
+		if (unlikely(ctx_p->flow_mode != S_DIN_to_AES)) {
+			SSI_LOG_ERR("HW key not supported for non-AES flows\n");
+			return -EINVAL;
+		}
+
+		ctx_p->hw.key1_slot = hw_key_to_cc_hw_key(hki->hw_key1);
+		if (unlikely(ctx_p->hw.key1_slot == END_OF_KEYS)) {
+			SSI_LOG_ERR("Unsupported hw key1 number (%d)\n", hki->hw_key1);
+			return -EINVAL;
+		}
+
+		if ((ctx_p->cipher_mode == DRV_CIPHER_XTS) ||
+		    (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) ||
+		    (ctx_p->cipher_mode == DRV_CIPHER_BITLOCKER)) {
+			if (unlikely(hki->hw_key1 == hki->hw_key2)) {
+				SSI_LOG_ERR("Illegal hw key numbers (%d,%d)\n", hki->hw_key1, hki->hw_key2);
+				return -EINVAL;
+			}
+			ctx_p->hw.key2_slot = hw_key_to_cc_hw_key(hki->hw_key2);
+			if (unlikely(ctx_p->hw.key2_slot == END_OF_KEYS)) {
+				SSI_LOG_ERR("Unsupported hw key2 number (%d)\n", hki->hw_key2);
+				return -EINVAL;
+			}
+		}
+
+		ctx_p->keylen = keylen;
+		END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
+		SSI_LOG_DEBUG("ssi_blkcipher_setkey: ssi_is_hw_key ret 0");
+
+		return 0;
+	}
+
+	// verify weak keys
+	if (ctx_p->flow_mode == S_DIN_to_DES) {
+		if (unlikely(!des_ekey(tmp, key)) &&
+		    (crypto_tfm_get_flags(tfm) & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			SSI_LOG_DEBUG("ssi_blkcipher_setkey:  weak DES key");
+			return -EINVAL;
+		}
+	}
+	if ((ctx_p->cipher_mode == DRV_CIPHER_XTS) && 
+	    ssi_fips_verify_xts_keys(key, keylen) != 0) {
+		SSI_LOG_DEBUG("ssi_blkcipher_setkey: weak XTS key");
+		return -EINVAL;
+	}
+	if ((ctx_p->flow_mode == S_DIN_to_DES) && 
+	    (keylen == DES3_EDE_KEY_SIZE) && 
+	    ssi_fips_verify_3des_keys(key, keylen) != 0) {
+		SSI_LOG_DEBUG("ssi_blkcipher_setkey: weak 3DES key");
+		return -EINVAL;
+	}
+
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_0);
+
+	/* STAT_PHASE_1: Copy key to ctx */
+	START_CYCLE_COUNT();
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr);
+	dma_sync_single_for_cpu(dev, ctx_p->user.key_dma_addr, 
+					max_key_buf_size, DMA_TO_DEVICE);
+#if SSI_CC_HAS_MULTI2
+	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
+		memcpy(ctx_p->user.key, key, CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE);
+		ctx_p->key_round_number = key[CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE];
+		if (ctx_p->key_round_number < CC_MULTI2_MIN_NUM_ROUNDS ||
+		    ctx_p->key_round_number > CC_MULTI2_MAX_NUM_ROUNDS) {
+			crypto_tfm_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+			SSI_LOG_DEBUG("ssi_blkcipher_setkey: SSI_CC_HAS_MULTI2 einval");
+			return -EINVAL;
+		}
+	} else 
+#endif /*SSI_CC_HAS_MULTI2*/
+	{
+		memcpy(ctx_p->user.key, key, keylen);
+		if (keylen == 24)
+			memset(ctx_p->user.key + 24, 0, CC_AES_KEY_SIZE_MAX - 24);
+
+		if (ctx_p->cipher_mode == DRV_CIPHER_ESSIV) {
+			/* sha256 for key2 - use sw implementation */
+			int key_len = keylen >> 1;
+			int err;
+			SHASH_DESC_ON_STACK(desc, ctx_p->shash_tfm);
+			desc->tfm = ctx_p->shash_tfm;
+
+			err = crypto_shash_digest(desc, ctx_p->user.key, key_len, ctx_p->user.key + key_len);
+			if (err) {
+				SSI_LOG_ERR("Failed to hash ESSIV key.\n");
+				return err;
+			}
+		}
+	}
+	dma_sync_single_for_device(dev, ctx_p->user.key_dma_addr, 
+					max_key_buf_size, DMA_TO_DEVICE);
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx_p->user.key_dma_addr ,max_key_buf_size);
+	ctx_p->keylen = keylen;
+	
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
+
+	 SSI_LOG_DEBUG("ssi_blkcipher_setkey: return safely");
+	return 0;
+}
+
+static inline void
+ssi_blkcipher_create_setup_desc(
+	struct crypto_tfm *tfm,
+	struct blkcipher_req_ctx *req_ctx,
+	unsigned int ivsize,
+	unsigned int nbytes,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	int cipher_mode = ctx_p->cipher_mode;
+	int flow_mode = ctx_p->flow_mode;
+	int direction = req_ctx->gen_ctx.op_type;
+	dma_addr_t key_dma_addr = ctx_p->user.key_dma_addr;
+	unsigned int key_len = ctx_p->keylen;
+	dma_addr_t iv_dma_addr = req_ctx->gen_ctx.iv_dma_addr;
+	unsigned int du_size = nbytes;
+
+	struct ssi_crypto_alg *ssi_alg = container_of(tfm->__crt_alg, struct ssi_crypto_alg, crypto_alg);
+
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_BULK_MASK) == CRYPTO_ALG_BULK_DU_512)
+		du_size = 512;
+	if ((ssi_alg->crypto_alg.cra_flags & CRYPTO_ALG_BULK_MASK) == CRYPTO_ALG_BULK_DU_4096)
+		du_size = 4096;
+
+	switch (cipher_mode) {
+	case DRV_CIPHER_CBC:
+	case DRV_CIPHER_CBC_CTS:
+	case DRV_CIPHER_CTR:
+	case DRV_CIPHER_OFB:
+		/* Load cipher state */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+				     iv_dma_addr, ivsize,
+				     NS_BIT);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
+		if ((cipher_mode == DRV_CIPHER_CTR) || 
+		    (cipher_mode == DRV_CIPHER_OFB) ) {
+			HW_DESC_SET_SETUP_MODE(&desc[*seq_size],
+					       SETUP_LOAD_STATE1);
+		} else {
+			HW_DESC_SET_SETUP_MODE(&desc[*seq_size],
+					       SETUP_LOAD_STATE0);
+		}
+		(*seq_size)++;
+		/*FALLTHROUGH*/
+	case DRV_CIPHER_ECB:
+		/* Load key */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		if (flow_mode == S_DIN_to_AES) {
+
+			if (ssi_is_hw_key(tfm)) {
+				HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key1_slot);
+			} else {
+				HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+						     key_dma_addr, 
+						     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len),
+						     NS_BIT);
+			}
+			HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len);
+		} else {
+			/*des*/
+			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+					     key_dma_addr, key_len,
+					     NS_BIT);
+			HW_DESC_SET_KEY_SIZE_DES(&desc[*seq_size], key_len);
+		}
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+		(*seq_size)++;
+		break;
+	case DRV_CIPHER_XTS:
+	case DRV_CIPHER_ESSIV:
+	case DRV_CIPHER_BITLOCKER:
+		/* Load AES key */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		if (ssi_is_hw_key(tfm)) {
+			HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key1_slot);
+		} else {
+			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+					     key_dma_addr, key_len/2,
+					     NS_BIT);
+		}
+		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+		(*seq_size)++;
+
+		/* load XEX key */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		if (ssi_is_hw_key(tfm)) {
+			HW_DESC_SET_HW_CRYPTO_KEY(&desc[*seq_size], ctx_p->hw.key2_slot);
+		} else {
+			HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, 
+					     (key_dma_addr+key_len/2), key_len/2,
+					     NS_BIT);
+		}
+		HW_DESC_SET_XEX_DATA_UNIT_SIZE(&desc[*seq_size], du_size);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], S_DIN_to_AES2);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
+		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_XEX_KEY);
+		(*seq_size)++;
+	
+		/* Set state */
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE1);
+		HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[*seq_size], key_len/2);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+				     iv_dma_addr, CC_AES_BLOCK_SIZE,
+				     NS_BIT);
+		(*seq_size)++;
+		break;
+	default:
+		SSI_LOG_ERR("Unsupported cipher mode (%d)\n", cipher_mode);
+		BUG();
+	}
+}
+
+#if SSI_CC_HAS_MULTI2
+static inline void ssi_blkcipher_create_multi2_setup_desc(
+	struct crypto_tfm *tfm,
+	struct blkcipher_req_ctx *req_ctx,
+	unsigned int ivsize,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	
+	int direction = req_ctx->gen_ctx.op_type;
+	/* Load system key */
+	HW_DESC_INIT(&desc[*seq_size]);
+	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, ctx_p->user.key_dma_addr,
+						CC_MULTI2_SYSTEM_KEY_SIZE,
+						NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
+	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_KEY0);
+	(*seq_size)++;
+
+	/* load data key */
+	HW_DESC_INIT(&desc[*seq_size]);
+	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI, 
+					(ctx_p->user.key_dma_addr + 
+						CC_MULTI2_SYSTEM_KEY_SIZE),
+				CC_MULTI2_DATA_KEY_SIZE, NS_BIT);
+	HW_DESC_SET_MULTI2_NUM_ROUNDS(&desc[*seq_size],
+						ctx_p->key_round_number);
+	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
+	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE0 );
+	(*seq_size)++;
+	
+	
+	/* Set state */
+	HW_DESC_INIT(&desc[*seq_size]);
+	HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+			     req_ctx->gen_ctx.iv_dma_addr,
+			     ivsize, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[*seq_size], direction);
+	HW_DESC_SET_FLOW_MODE(&desc[*seq_size], ctx_p->flow_mode);
+	HW_DESC_SET_CIPHER_MODE(&desc[*seq_size], ctx_p->cipher_mode);
+	HW_DESC_SET_SETUP_MODE(&desc[*seq_size], SETUP_LOAD_STATE1);	
+	(*seq_size)++;
+	
+}
+#endif /*SSI_CC_HAS_MULTI2*/
+
+static inline void
+ssi_blkcipher_create_data_desc(
+	struct crypto_tfm *tfm,
+	struct blkcipher_req_ctx *req_ctx,
+	struct scatterlist *dst, struct scatterlist *src,
+	unsigned int nbytes,
+	void *areq,
+	HwDesc_s desc[],
+	unsigned int *seq_size)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	unsigned int flow_mode = ctx_p->flow_mode;
+
+	switch (ctx_p->flow_mode) {
+	case S_DIN_to_AES:
+		flow_mode = DIN_AES_DOUT;
+		break;
+	case S_DIN_to_DES:
+		flow_mode = DIN_DES_DOUT;
+		break;
+#if SSI_CC_HAS_MULTI2
+	case S_DIN_to_MULTI2:
+		flow_mode = DIN_MULTI2_DOUT;
+		break;
+#endif /*SSI_CC_HAS_MULTI2*/
+	default:
+		SSI_LOG_ERR("invalid flow mode, flow_mode = %d \n", flow_mode);
+		return;
+	}
+	/* Process */
+	if (likely(req_ctx->dma_buf_type == SSI_DMA_BUF_DLLI)){
+		SSI_LOG_DEBUG(" data params addr 0x%llX length 0x%X \n",
+			     (unsigned long long)sg_dma_address(src),
+			     nbytes);
+		SSI_LOG_DEBUG(" data params addr 0x%llX length 0x%X \n",
+			     (unsigned long long)sg_dma_address(dst),
+			     nbytes);
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+				     sg_dma_address(src),
+				     nbytes, NS_BIT);
+		HW_DESC_SET_DOUT_DLLI(&desc[*seq_size],
+				      sg_dma_address(dst),
+				      nbytes,
+				      NS_BIT, (areq == NULL)? 0:1);
+		if (areq != NULL) {
+			HW_DESC_SET_QUEUE_LAST_IND(&desc[*seq_size]);
+		}
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		(*seq_size)++;
+	} else {
+		/* bypass */
+		SSI_LOG_DEBUG(" bypass params addr 0x%llX "
+			     "length 0x%X addr 0x%08X\n",
+			(unsigned long long)req_ctx->mlli_params.mlli_dma_addr,
+			req_ctx->mlli_params.mlli_len,
+			(unsigned int)ctx_p->drvdata->mlli_sram_addr);
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_DLLI,
+				     req_ctx->mlli_params.mlli_dma_addr,
+				     req_ctx->mlli_params.mlli_len,
+				     NS_BIT);
+		HW_DESC_SET_DOUT_SRAM(&desc[*seq_size],
+				      ctx_p->drvdata->mlli_sram_addr,
+				      req_ctx->mlli_params.mlli_len);
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], BYPASS);
+		(*seq_size)++;
+
+		HW_DESC_INIT(&desc[*seq_size]);
+		HW_DESC_SET_DIN_TYPE(&desc[*seq_size], DMA_MLLI,
+			ctx_p->drvdata->mlli_sram_addr,
+				     req_ctx->in_mlli_nents, NS_BIT);
+		if (req_ctx->out_nents == 0) {
+			SSI_LOG_DEBUG(" din/dout params addr 0x%08X "
+				     "addr 0x%08X\n",
+			(unsigned int)ctx_p->drvdata->mlli_sram_addr,
+			(unsigned int)ctx_p->drvdata->mlli_sram_addr);
+			HW_DESC_SET_DOUT_MLLI(&desc[*seq_size], 
+			ctx_p->drvdata->mlli_sram_addr,
+					      req_ctx->in_mlli_nents,
+					      NS_BIT,(areq == NULL)? 0:1);
+		} else {
+			SSI_LOG_DEBUG(" din/dout params "
+				     "addr 0x%08X addr 0x%08X\n",
+				(unsigned int)ctx_p->drvdata->mlli_sram_addr,
+				(unsigned int)ctx_p->drvdata->mlli_sram_addr + 
+				(uint32_t)LLI_ENTRY_BYTE_SIZE * 
+							req_ctx->in_nents);
+			HW_DESC_SET_DOUT_MLLI(&desc[*seq_size], 
+				(ctx_p->drvdata->mlli_sram_addr +
+				LLI_ENTRY_BYTE_SIZE * 
+						req_ctx->in_mlli_nents), 
+				req_ctx->out_mlli_nents, NS_BIT,(areq == NULL)? 0:1);
+		}
+		if (areq != NULL) {
+			HW_DESC_SET_QUEUE_LAST_IND(&desc[*seq_size]);
+		}
+		HW_DESC_SET_FLOW_MODE(&desc[*seq_size], flow_mode);
+		(*seq_size)++;
+	}
+}
+
+static int ssi_blkcipher_complete(struct device *dev,
+                                  struct ssi_ablkcipher_ctx *ctx_p, 
+                                  struct blkcipher_req_ctx *req_ctx,
+                                  struct scatterlist *dst, struct scatterlist *src,
+                                  void *info, //req info
+                                  unsigned int ivsize,
+                                  void *areq,
+                                  void __iomem *cc_base)
+{
+	int completion_error = 0;
+	uint32_t inflight_counter;
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	START_CYCLE_COUNT();
+	ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
+	info = req_ctx->backup_info;
+	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_4);
+
+
+	/*Set the inflight couter value to local variable*/
+	inflight_counter =  ctx_p->drvdata->inflight_counter;
+	/*Decrease the inflight counter*/
+	if(ctx_p->flow_mode == BYPASS && ctx_p->drvdata->inflight_counter > 0)
+		ctx_p->drvdata->inflight_counter--;
+
+	if(areq){
+		ablkcipher_request_complete(areq, completion_error);
+		return 0;
+	}
+	return completion_error;
+}
+
+static int ssi_blkcipher_process(
+	struct crypto_tfm *tfm,
+	struct blkcipher_req_ctx *req_ctx,
+	struct scatterlist *dst, struct scatterlist *src,
+	unsigned int nbytes,
+	void *info, //req info
+	unsigned int ivsize,
+	void *areq, 
+	enum drv_crypto_direction direction)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct device *dev = &ctx_p->drvdata->plat_dev->dev;
+	HwDesc_s desc[MAX_ABLKCIPHER_SEQ_LEN];
+	struct ssi_crypto_req ssi_req = {};
+	int rc, seq_len = 0,cts_restore_flag = 0;
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	SSI_LOG_DEBUG("%s areq=%p info=%p nbytes=%d\n",
+		((direction==DRV_CRYPTO_DIRECTION_ENCRYPT)?"Encrypt":"Decrypt"),
+		     areq, info, nbytes);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	/* STAT_PHASE_0: Init and sanity checks */
+	START_CYCLE_COUNT();
+	
+	/* TODO: check data length according to mode */
+	if (unlikely(validate_data_size(ctx_p, nbytes))) {
+		SSI_LOG_ERR("Unsupported data size %d.\n", nbytes);
+		crypto_tfm_set_flags(tfm, CRYPTO_TFM_RES_BAD_BLOCK_LEN);
+		return -EINVAL;
+	}
+	if (nbytes == 0) {
+		/* No data to process is valid */
+		return 0;
+	}
+        /*For CTS in case of data size aligned to 16 use CBC mode*/
+	if (((nbytes % AES_BLOCK_SIZE) == 0) && (ctx_p->cipher_mode == DRV_CIPHER_CBC_CTS)){
+
+		ctx_p->cipher_mode = DRV_CIPHER_CBC;
+		cts_restore_flag = 1;
+	}
+
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_ablkcipher_complete;
+	ssi_req.user_arg = (void *)areq;
+
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = (direction == DRV_CRYPTO_DIRECTION_DECRYPT) ?
+		STAT_OP_TYPE_DECODE : STAT_OP_TYPE_ENCODE;
+
+#endif
+
+	/* Setup request context */
+	req_ctx->gen_ctx.op_type = direction;
+	
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_0);
+
+	/* STAT_PHASE_1: Map buffers */
+	START_CYCLE_COUNT();
+	
+	rc = ssi_buffer_mgr_map_blkcipher_request(ctx_p->drvdata, req_ctx, ivsize, nbytes, info, src, dst);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("map_request() failed\n");
+		goto exit_process;
+	}
+
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_1);
+
+	/* STAT_PHASE_2: Create sequence */
+	START_CYCLE_COUNT();
+
+	/* Setup processing */
+#if SSI_CC_HAS_MULTI2
+	if (ctx_p->flow_mode == S_DIN_to_MULTI2) {
+		ssi_blkcipher_create_multi2_setup_desc(tfm,
+						       req_ctx,
+						       ivsize,
+						       desc,
+						       &seq_len);
+	} else
+#endif /*SSI_CC_HAS_MULTI2*/
+	{
+		ssi_blkcipher_create_setup_desc(tfm,
+						req_ctx,
+						ivsize,
+						nbytes,
+						desc,
+						&seq_len);
+	}
+	/* Data processing */
+	ssi_blkcipher_create_data_desc(tfm,
+			      req_ctx, 
+			      dst, src,
+			      nbytes,
+			      areq,
+			      desc, &seq_len);
+
+	/* do we need to generate IV? */
+	if (req_ctx->is_giv == true) {
+		ssi_req.ivgen_dma_addr[0] = req_ctx->gen_ctx.iv_dma_addr;
+		ssi_req.ivgen_dma_addr_len = 1;
+		/* set the IV size (8/16 B long)*/
+		ssi_req.ivgen_size = ivsize;
+	}
+	END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_2);
+
+	/* STAT_PHASE_3: Lock HW and push sequence */
+	START_CYCLE_COUNT();
+	
+	rc = send_request(ctx_p->drvdata, &ssi_req, desc, seq_len, (areq == NULL)? 0:1);
+	if(areq != NULL) {
+		if (unlikely(rc != -EINPROGRESS)) {
+			/* Failed to send the request or request completed synchronously */
+			ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
+		}
+
+		END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
+	} else {
+		if (rc != 0) {
+			ssi_buffer_mgr_unmap_blkcipher_request(dev, req_ctx, ivsize, src, dst);
+			END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);            
+		} else {
+			END_CYCLE_COUNT(ssi_req.op_type, STAT_PHASE_3);
+			rc = ssi_blkcipher_complete(dev, ctx_p, req_ctx, dst, src, info, ivsize, NULL, ctx_p->drvdata->cc_base);
+		} 
+	}
+
+exit_process:
+	if (cts_restore_flag != 0)
+		ctx_p->cipher_mode = DRV_CIPHER_CBC_CTS;
+	
+	return rc;
+}
+
+static void ssi_ablkcipher_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
+{
+	struct ablkcipher_request *areq = (struct ablkcipher_request *)ssi_req;
+	struct blkcipher_req_ctx *req_ctx = ablkcipher_request_ctx(areq);
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(areq);
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_ablkcipher_ctx(tfm);
+	unsigned int ivsize = crypto_ablkcipher_ivsize(tfm);
+
+	CHECK_AND_RETURN_VOID_UPON_FIPS_ERROR();
+
+	ssi_blkcipher_complete(dev, ctx_p, req_ctx, areq->dst, areq->src, areq->info, ivsize, areq, cc_base);
+}
+
+
+
+static int ssi_sblkcipher_init(struct crypto_tfm *tfm)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+
+	/* Allocate sync ctx buffer */
+	ctx_p->sync_ctx = kmalloc(sizeof(struct blkcipher_req_ctx), GFP_KERNEL|GFP_DMA);
+	if (!ctx_p->sync_ctx) {
+		SSI_LOG_ERR("Allocating sync ctx buffer in context failed\n");
+		return -ENOMEM;
+	}
+	SSI_LOG_DEBUG("Allocated sync ctx buffer in context ctx_p->sync_ctx=@%p\n",
+								ctx_p->sync_ctx);
+
+	return ssi_blkcipher_init(tfm);
+}
+
+
+static void ssi_sblkcipher_exit(struct crypto_tfm *tfm)
+{
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	
+	kfree(ctx_p->sync_ctx);
+	SSI_LOG_DEBUG("Free sync ctx buffer in context ctx_p->sync_ctx=@%p\n", ctx_p->sync_ctx);
+
+	ssi_blkcipher_exit(tfm);
+}
+
+#ifdef SYNC_ALGS
+static int ssi_sblkcipher_encrypt(struct blkcipher_desc *desc,
+                        struct scatterlist *dst, struct scatterlist *src,
+                        unsigned int nbytes)
+{
+	struct crypto_blkcipher *blk_tfm = desc->tfm;
+	struct crypto_tfm *tfm = crypto_blkcipher_tfm(blk_tfm);
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct blkcipher_req_ctx *req_ctx = ctx_p->sync_ctx;
+	unsigned int ivsize = crypto_blkcipher_ivsize(blk_tfm);
+
+	req_ctx->backup_info = desc->info;
+	req_ctx->is_giv = false;
+
+	return ssi_blkcipher_process(tfm, req_ctx, dst, src, nbytes, desc->info, ivsize, NULL, DRV_CRYPTO_DIRECTION_ENCRYPT);
+}
+
+static int ssi_sblkcipher_decrypt(struct blkcipher_desc *desc,
+                        struct scatterlist *dst, struct scatterlist *src,
+                        unsigned int nbytes)
+{
+	struct crypto_blkcipher *blk_tfm = desc->tfm;
+	struct crypto_tfm *tfm = crypto_blkcipher_tfm(blk_tfm);
+	struct ssi_ablkcipher_ctx *ctx_p = crypto_tfm_ctx(tfm);
+	struct blkcipher_req_ctx *req_ctx = ctx_p->sync_ctx;
+	unsigned int ivsize = crypto_blkcipher_ivsize(blk_tfm);
+
+	req_ctx->backup_info = desc->info;
+	req_ctx->is_giv = false;
+
+	return ssi_blkcipher_process(tfm, req_ctx, dst, src, nbytes, desc->info, ivsize, NULL, DRV_CRYPTO_DIRECTION_DECRYPT);
+}
+#endif
+
+/* Async wrap functions */
+
+static int ssi_ablkcipher_init(struct crypto_tfm *tfm)
+{
+	struct ablkcipher_tfm *ablktfm = &tfm->crt_ablkcipher;
+	
+	ablktfm->reqsize = sizeof(struct blkcipher_req_ctx);
+
+	return ssi_blkcipher_init(tfm);
+}
+
+
+static int ssi_ablkcipher_setkey(struct crypto_ablkcipher *tfm, 
+				const u8 *key, 
+				unsigned int keylen)
+{
+	return ssi_blkcipher_setkey(crypto_ablkcipher_tfm(tfm), key, keylen);
+}
+
+static int ssi_ablkcipher_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *ablk_tfm = crypto_ablkcipher_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(ablk_tfm);
+	struct blkcipher_req_ctx *req_ctx = ablkcipher_request_ctx(req);
+	unsigned int ivsize = crypto_ablkcipher_ivsize(ablk_tfm);
+
+	req_ctx->backup_info = req->info;
+	req_ctx->is_giv = false;
+
+	return ssi_blkcipher_process(tfm, req_ctx, req->dst, req->src, req->nbytes, req->info, ivsize, (void *)req, DRV_CRYPTO_DIRECTION_ENCRYPT);
+}
+
+static int ssi_ablkcipher_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *ablk_tfm = crypto_ablkcipher_reqtfm(req);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(ablk_tfm);
+	struct blkcipher_req_ctx *req_ctx = ablkcipher_request_ctx(req);
+	unsigned int ivsize = crypto_ablkcipher_ivsize(ablk_tfm);
+
+	req_ctx->backup_info = req->info;
+	req_ctx->is_giv = false;
+	return ssi_blkcipher_process(tfm, req_ctx, req->dst, req->src, req->nbytes, req->info, ivsize, (void *)req, DRV_CRYPTO_DIRECTION_DECRYPT);
+}
+
+
+/* DX Block cipher alg */
+static struct ssi_alg_template blkcipher_algs[] = {
+/* Async template */
+#if SSI_CC_HAS_AES_XTS
+	{
+		.name = "xts(aes)",
+		.driver_name = "xts-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			.geniv = "eseqiv",
+			},
+		.cipher_mode = DRV_CIPHER_XTS,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+	{
+		.name = "xts(aes)",
+		.driver_name = "xts-aes-du512-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_512,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_XTS,
+		.flow_mode = S_DIN_to_AES,
+	.synchronous = false,
+	},
+	{
+		.name = "xts(aes)",
+		.driver_name = "xts-aes-du4096-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_4096,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_XTS,
+		.flow_mode = S_DIN_to_AES,
+	.synchronous = false,
+	},
+#endif /*SSI_CC_HAS_AES_XTS*/
+#if SSI_CC_HAS_AES_ESSIV
+	{
+		.name = "essiv(aes)",
+		.driver_name = "essiv-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_ESSIV,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+	{
+		.name = "essiv(aes)",
+		.driver_name = "essiv-aes-du512-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_512,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_ESSIV,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+	{
+		.name = "essiv(aes)",
+		.driver_name = "essiv-aes-du4096-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_4096,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_ESSIV,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+#endif /*SSI_CC_HAS_AES_ESSIV*/
+#if SSI_CC_HAS_AES_BITLOCKER
+	{
+		.name = "bitlocker(aes)",
+		.driver_name = "bitlocker-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_BITLOCKER,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+	{
+		.name = "bitlocker(aes)",
+		.driver_name = "bitlocker-aes-du512-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_512,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_BITLOCKER,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+	{
+		.name = "bitlocker(aes)",
+		.driver_name = "bitlocker-aes-du4096-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_BULK_DU_4096,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE * 2,
+			.max_keysize = AES_MAX_KEY_SIZE * 2,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_BITLOCKER,
+		.flow_mode = S_DIN_to_AES,
+		.synchronous = false,
+	},
+#endif /*SSI_CC_HAS_AES_BITLOCKER*/
+	{
+		.name = "ecb(aes)",
+		.driver_name = "ecb-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = 0,
+			},
+		.cipher_mode = DRV_CIPHER_ECB,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+	{
+		.name = "cbc(aes)",
+		.driver_name = "cbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+	{
+		.name = "ofb(aes)",
+		.driver_name = "ofb-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_OFB,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+#if SSI_CC_HAS_AES_CTS
+	{
+		.name = "cts1(cbc(aes))",
+		.driver_name = "cts1-cbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_CBC_CTS,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+#endif
+	{
+		.name = "ctr(aes)",
+		.driver_name = "ctr-aes-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_CTR,
+		.flow_mode = S_DIN_to_AES,
+        .synchronous = false,
+	},
+	{
+		.name = "cbc(des3_ede)",
+		.driver_name = "cbc-3des-dx",
+		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize = DES3_EDE_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_DES,
+        .synchronous = false,
+	},
+	{
+		.name = "ecb(des3_ede)",
+		.driver_name = "ecb-3des-dx",
+		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize = 0,
+			},
+		.cipher_mode = DRV_CIPHER_ECB,
+		.flow_mode = S_DIN_to_DES,
+        .synchronous = false,
+	},
+	{
+		.name = "cbc(des)",
+		.driver_name = "cbc-des-dx",
+		.blocksize = DES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = DES_KEY_SIZE,
+			.max_keysize = DES_KEY_SIZE,
+			.ivsize = DES_BLOCK_SIZE,
+			},
+		.cipher_mode = DRV_CIPHER_CBC,
+		.flow_mode = S_DIN_to_DES,
+        .synchronous = false,
+	},
+	{
+		.name = "ecb(des)",
+		.driver_name = "ecb-des-dx",
+		.blocksize = DES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = DES_KEY_SIZE,
+			.max_keysize = DES_KEY_SIZE,
+			.ivsize = 0,
+			},
+		.cipher_mode = DRV_CIPHER_ECB,
+		.flow_mode = S_DIN_to_DES,
+        .synchronous = false,
+	},
+#if SSI_CC_HAS_MULTI2
+	{
+		.name = "cbc(multi2)",
+		.driver_name = "cbc-multi2-dx",
+		.blocksize = CC_MULTI2_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_decrypt,
+			.min_keysize = CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE + 1,
+			.max_keysize = CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE + 1,
+			.ivsize = CC_MULTI2_IV_SIZE,
+			},
+		.cipher_mode = DRV_MULTI2_CBC,
+		.flow_mode = S_DIN_to_MULTI2,
+        .synchronous = false,
+	},
+	{
+		.name = "ofb(multi2)",
+		.driver_name = "ofb-multi2-dx",
+		.blocksize = 1,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ssi_ablkcipher_setkey,
+			.encrypt = ssi_ablkcipher_encrypt,
+			.decrypt = ssi_ablkcipher_encrypt,
+			.min_keysize = CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE + 1,
+			.max_keysize = CC_MULTI2_SYSTEM_N_DATA_KEY_SIZE + 1,
+			.ivsize = CC_MULTI2_IV_SIZE,
+			},
+		.cipher_mode = DRV_MULTI2_OFB,
+		.flow_mode = S_DIN_to_MULTI2,
+        .synchronous = false,
+	},
+#endif /*SSI_CC_HAS_MULTI2*/
+};
+
+static 
+struct ssi_crypto_alg *ssi_ablkcipher_create_alg(struct ssi_alg_template *template)
+{
+	struct ssi_crypto_alg *t_alg;
+	struct crypto_alg *alg;
+
+	t_alg = kzalloc(sizeof(struct ssi_crypto_alg), GFP_KERNEL);
+	if (!t_alg) {
+		SSI_LOG_ERR("failed to allocate t_alg\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	alg = &t_alg->crypto_alg;
+
+	snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s", template->name);
+	snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+		 template->driver_name);
+	alg->cra_module = THIS_MODULE;
+	alg->cra_priority = SSI_CRA_PRIO;
+	alg->cra_blocksize = template->blocksize;
+	alg->cra_alignmask = 0;
+	alg->cra_ctxsize = sizeof(struct ssi_ablkcipher_ctx);
+	
+	alg->cra_init = template->synchronous? ssi_sblkcipher_init:ssi_ablkcipher_init;
+	alg->cra_exit = template->synchronous? ssi_sblkcipher_exit:ssi_blkcipher_exit;
+	alg->cra_type = template->synchronous? &crypto_blkcipher_type:&crypto_ablkcipher_type;
+	if(template->synchronous) {
+		alg->cra_blkcipher = template->template_sblkcipher;
+		alg->cra_flags = CRYPTO_ALG_KERN_DRIVER_ONLY |
+				template->type;
+	} else {
+		alg->cra_ablkcipher = template->template_ablkcipher;
+		alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_KERN_DRIVER_ONLY |
+				template->type;
+	}
+
+	t_alg->cipher_mode = template->cipher_mode;
+	t_alg->flow_mode = template->flow_mode;
+
+	return t_alg;
+}
+
+int ssi_ablkcipher_free(struct ssi_drvdata *drvdata)
+{
+	struct ssi_crypto_alg *t_alg, *n;
+	struct ssi_blkcipher_handle *blkcipher_handle = 
+						drvdata->blkcipher_handle;
+	struct device *dev;
+	dev = &drvdata->plat_dev->dev;
+
+	if (blkcipher_handle != NULL) {
+		/* Remove registered algs */
+		list_for_each_entry_safe(t_alg, n,
+				&blkcipher_handle->blkcipher_alg_list,
+					 entry) {
+			crypto_unregister_alg(&t_alg->crypto_alg);
+			list_del(&t_alg->entry);
+			kfree(t_alg);
+		}
+		kfree(blkcipher_handle);
+		drvdata->blkcipher_handle = NULL;
+	}
+	return 0;
+}
+
+
+
+int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata)
+{
+	struct ssi_blkcipher_handle *ablkcipher_handle;
+	struct ssi_crypto_alg *t_alg;
+	int rc = -ENOMEM;
+	int alg;
+
+	ablkcipher_handle = kmalloc(sizeof(struct ssi_blkcipher_handle),
+		GFP_KERNEL);
+	if (ablkcipher_handle == NULL)
+		return -ENOMEM;
+
+	drvdata->blkcipher_handle = ablkcipher_handle;
+
+	INIT_LIST_HEAD(&ablkcipher_handle->blkcipher_alg_list);
+
+	/* Linux crypto */
+	SSI_LOG_DEBUG("Number of algorithms = %zu\n", ARRAY_SIZE(blkcipher_algs));
+	for (alg = 0; alg < ARRAY_SIZE(blkcipher_algs); alg++) {
+		SSI_LOG_DEBUG("creating %s\n", blkcipher_algs[alg].driver_name);
+		t_alg = ssi_ablkcipher_create_alg(&blkcipher_algs[alg]);
+		if (IS_ERR(t_alg)) {
+			rc = PTR_ERR(t_alg);
+			SSI_LOG_ERR("%s alg allocation failed\n",
+				 blkcipher_algs[alg].driver_name);
+			goto fail0;
+		}
+		t_alg->drvdata = drvdata;
+
+		SSI_LOG_DEBUG("registering %s\n", blkcipher_algs[alg].driver_name);
+		rc = crypto_register_alg(&t_alg->crypto_alg);
+		SSI_LOG_DEBUG("%s alg registration rc = %x\n",
+			t_alg->crypto_alg.cra_driver_name, rc);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("%s alg registration failed\n",
+				t_alg->crypto_alg.cra_driver_name);
+			kfree(t_alg);
+			goto fail0;
+		} else {
+			list_add_tail(&t_alg->entry, 
+				      &ablkcipher_handle->blkcipher_alg_list);
+			SSI_LOG_DEBUG("Registered %s\n", 
+					t_alg->crypto_alg.cra_driver_name);
+		}
+	}
+	return 0;
+
+fail0:
+	ssi_ablkcipher_free(drvdata);
+	return rc;
+}
diff --git a/drivers/staging/ccree/ssi_cipher.h b/drivers/staging/ccree/ssi_cipher.h
new file mode 100644
index 000000000000..ba4eb7c4893f
--- /dev/null
+++ b/drivers/staging/ccree/ssi_cipher.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_cipher.h
+   ARM CryptoCell Cipher Crypto API
+ */
+
+#ifndef __SSI_CIPHER_H__
+#define __SSI_CIPHER_H__
+
+#include <linux/kernel.h>
+#include <crypto/algapi.h>
+#include "ssi_driver.h"
+#include "ssi_buffer_mgr.h"
+
+
+/* Crypto cipher flags */
+#define CC_CRYPTO_CIPHER_KEY_KFDE0    (1 << 0)
+#define CC_CRYPTO_CIPHER_KEY_KFDE1    (1 << 1)
+#define CC_CRYPTO_CIPHER_KEY_KFDE2    (1 << 2)
+#define CC_CRYPTO_CIPHER_KEY_KFDE3    (1 << 3)
+#define CC_CRYPTO_CIPHER_DU_SIZE_512B (1 << 4)
+
+#define CC_CRYPTO_CIPHER_KEY_KFDE_MASK (CC_CRYPTO_CIPHER_KEY_KFDE0 | CC_CRYPTO_CIPHER_KEY_KFDE1 | CC_CRYPTO_CIPHER_KEY_KFDE2 | CC_CRYPTO_CIPHER_KEY_KFDE3)
+
+
+struct blkcipher_req_ctx {
+	struct async_gen_req_ctx gen_ctx;
+	enum ssi_req_dma_buf_type dma_buf_type;
+	uint32_t in_nents;
+	uint32_t in_mlli_nents;
+	uint32_t out_nents;
+	uint32_t out_mlli_nents;
+	uint8_t *backup_info; /*store iv for generated IV flow*/
+	bool is_giv;
+	struct mlli_params mlli_params;
+};
+
+
+
+int ssi_ablkcipher_alloc(struct ssi_drvdata *drvdata);
+
+int ssi_ablkcipher_free(struct ssi_drvdata *drvdata);
+
+#ifndef CRYPTO_ALG_BULK_MASK
+
+#define CRYPTO_ALG_BULK_DU_512	0x00002000
+#define CRYPTO_ALG_BULK_DU_4096	0x00004000
+#define CRYPTO_ALG_BULK_MASK	(CRYPTO_ALG_BULK_DU_512 |\
+				CRYPTO_ALG_BULK_DU_4096)
+#endif /* CRYPTO_ALG_BULK_MASK */
+
+
+#ifdef CRYPTO_TFM_REQ_HW_KEY
+
+static inline bool ssi_is_hw_key(struct crypto_tfm *tfm)
+{
+	return (crypto_tfm_get_flags(tfm) & CRYPTO_TFM_REQ_HW_KEY);
+}
+
+#else 
+
+struct arm_hw_key_info {
+	int hw_key1;
+	int hw_key2;
+};
+
+static inline bool ssi_is_hw_key(struct crypto_tfm *tfm)
+{
+	return 0;
+}
+
+#endif /* CRYPTO_TFM_REQ_HW_KEY */
+
+
+#endif /*__SSI_CIPHER_H__*/
diff --git a/drivers/staging/ccree/ssi_config.h b/drivers/staging/ccree/ssi_config.h
new file mode 100644
index 000000000000..d96a5436f6d7
--- /dev/null
+++ b/drivers/staging/ccree/ssi_config.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_config.h
+   Definitions for ARM CryptoCell Linux Crypto Driver
+ */
+
+#ifndef __SSI_CONFIG_H__
+#define __SSI_CONFIG_H__
+
+#include <linux/version.h>
+
+#define DISABLE_COHERENT_DMA_OPS
+//#define FLUSH_CACHE_ALL
+//#define COMPLETION_DELAY
+//#define DX_DUMP_DESCS
+// #define DX_DUMP_BYTES
+// #define CC_DEBUG
+#define ENABLE_CC_SYSFS		/* Enable sysfs interface for debugging REE driver */
+//#define ENABLE_CC_CYCLE_COUNT
+//#define DX_IRQ_DELAY 100000
+#define DMA_BIT_MASK_LEN	48	/* was 32 bit, but for juno's sake it was enlarged to 48 bit */
+
+#if defined ENABLE_CC_CYCLE_COUNT && defined ENABLE_CC_SYSFS
+#define CC_CYCLE_COUNT
+#endif
+
+
+#if defined (CONFIG_ARM64)	// TODO currently only this mode was test on Juno (which is ARM64), need to enable coherent also.
+#define DISABLE_COHERENT_DMA_OPS
+#endif
+
+/* Define the CryptoCell DMA cache coherency signals configuration */
+#if defined (DISABLE_COHERENT_DMA_OPS)
+	/* Software Controlled Cache Coherency (SCCC) */ 
+	#define SSI_CACHE_PARAMS (0x000)
+	/* CC attached to NONE-ACP such as HPP/ACE/AMBA4.
+	 * The customer is responsible to enable/disable this feature
+	 * according to his platform type. */
+	#define DX_HAS_ACP 0
+#else
+	#define SSI_CACHE_PARAMS (0xEEE)
+	/* CC attached to ACP */
+	#define DX_HAS_ACP 1
+#endif
+
+#endif /*__DX_CONFIG_H__*/
+
diff --git a/drivers/staging/ccree/ssi_driver.c b/drivers/staging/ccree/ssi_driver.c
new file mode 100644
index 000000000000..bc19adce6dee
--- /dev/null
+++ b/drivers/staging/ccree/ssi_driver.c
@@ -0,0 +1,556 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <linux/crypto.h>
+#include <crypto/algapi.h>
+#include <crypto/aes.h>
+#include <crypto/sha.h>
+#include <crypto/aead.h>
+#include <crypto/authenc.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/internal/skcipher.h>
+
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/random.h>
+#include <linux/ioport.h>
+#include <linux/interrupt.h>
+#include <linux/fcntl.h>
+#include <linux/poll.h>
+#include <linux/proc_fs.h>
+#include <linux/mutex.h>
+#include <linux/sysctl.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/platform_device.h>
+#include <linux/mm.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/pm.h>
+
+/* cache.h required for L1_CACHE_ALIGN() and cache_line_size() */
+#include <linux/cache.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/pagemap.h>
+#include <linux/sched.h>
+#include <linux/random.h>
+#include <linux/of.h>
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "ssi_request_mgr.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_sysfs.h"
+#include "ssi_cipher.h"
+#include "ssi_aead.h"
+#include "ssi_hash.h"
+#include "ssi_ivgen.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_pm.h"
+#include "ssi_fips_local.h"
+
+
+#ifdef DX_DUMP_BYTES
+void dump_byte_array(const char *name, const uint8_t *the_array, unsigned long size)
+{
+	int i , line_offset = 0, ret = 0;
+	const uint8_t *cur_byte;
+	char line_buf[80];
+
+	if (the_array == NULL) {
+		SSI_LOG_ERR("cannot dump_byte_array - NULL pointer\n");
+		return;
+	}
+
+	ret = snprintf(line_buf, sizeof(line_buf), "%s[%lu]: ",
+		name, size);
+	if (ret < 0) {
+		SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n",ret);
+		return;
+	}
+	line_offset = ret;
+	for (i = 0 , cur_byte = the_array;
+	     (i < size) && (line_offset < sizeof(line_buf)); i++, cur_byte++) {
+			ret = snprintf(line_buf + line_offset,
+					sizeof(line_buf) - line_offset,
+					"0x%02X ", *cur_byte);
+		if (ret < 0) {
+			SSI_LOG_ERR("snprintf returned %d . aborting buffer array dump\n",ret);
+			return;
+		}
+		line_offset += ret;
+		if (line_offset > 75) { /* Cut before line end */
+			SSI_LOG_DEBUG("%s\n", line_buf);
+			line_offset = 0;
+		}
+	}
+
+	if (line_offset > 0) /* Dump remaining line */
+		SSI_LOG_DEBUG("%s\n", line_buf);
+}
+#endif
+
+static irqreturn_t cc_isr(int irq, void *dev_id)
+{
+	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)dev_id;
+	void __iomem *cc_base = drvdata->cc_base;
+	uint32_t irr;
+	uint32_t imr;
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	/* STAT_OP_TYPE_GENERIC STAT_PHASE_0: Interrupt */
+	START_CYCLE_COUNT();
+
+	/* read the interrupt status */
+	irr = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRR));
+	SSI_LOG_DEBUG("Got IRR=0x%08X\n", irr);
+	if (unlikely(irr == 0)) { /* Probably shared interrupt line */
+		SSI_LOG_ERR("Got interrupt with empty IRR\n");
+		return IRQ_NONE;
+	}
+	imr = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR));
+
+	/* clear interrupt - must be before processing events */
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), irr);
+
+	drvdata->irq = irr;
+	/* Completion interrupt - most probable */
+	if (likely((irr & SSI_COMP_IRQ_MASK) != 0)) {
+		/* Mask AXI completion interrupt - will be unmasked in Deferred service handler */
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), imr | SSI_COMP_IRQ_MASK);
+		irr &= ~SSI_COMP_IRQ_MASK;
+		complete_request(drvdata);
+	}
+#ifdef CC_SUPPORT_FIPS
+	/* TEE FIPS interrupt */
+	if (likely((irr & SSI_GPR0_IRQ_MASK) != 0)) {
+		/* Mask interrupt - will be unmasked in Deferred service handler */
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), imr | SSI_GPR0_IRQ_MASK);
+		irr &= ~SSI_GPR0_IRQ_MASK;
+		fips_handler(drvdata);
+	}
+#endif
+	/* AXI error interrupt */
+	if (unlikely((irr & SSI_AXI_ERR_IRQ_MASK) != 0)) {
+		uint32_t axi_err;
+		
+		/* Read the AXI error ID */
+		axi_err = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_ERR));
+		SSI_LOG_DEBUG("AXI completion error: axim_mon_err=0x%08X\n", axi_err);
+		
+		irr &= ~SSI_AXI_ERR_IRQ_MASK;
+	}
+
+	if (unlikely(irr != 0)) {
+		SSI_LOG_DEBUG("IRR includes unknown cause bits (0x%08X)\n", irr);
+		/* Just warning */
+	}
+
+	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_0);
+	START_CYCLE_COUNT_AT(drvdata->isr_exit_cycles);
+
+	return IRQ_HANDLED;
+}
+
+int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe)
+{
+	unsigned int val;
+	void __iomem *cc_base = drvdata->cc_base;
+
+	/* Unmask all AXI interrupt sources AXI_CFG1 register */
+	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CFG));
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CFG), val & ~SSI_AXI_IRQ_MASK);
+	SSI_LOG_DEBUG("AXIM_CFG=0x%08X\n", CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CFG)));
+
+	/* Clear all pending interrupts */
+	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRR));
+	SSI_LOG_DEBUG("IRR=0x%08X\n", val);
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), val);
+
+	/* Unmask relevant interrupt cause */
+	val = (~(SSI_COMP_IRQ_MASK | SSI_AXI_ERR_IRQ_MASK | SSI_GPR0_IRQ_MASK));
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), val);
+		
+#ifdef DX_HOST_IRQ_TIMER_INIT_VAL_REG_OFFSET
+#ifdef DX_IRQ_DELAY
+	/* Set CC IRQ delay */
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRQ_TIMER_INIT_VAL),
+		DX_IRQ_DELAY);
+#endif
+	if (CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRQ_TIMER_INIT_VAL)) > 0) {
+		SSI_LOG_DEBUG("irq_delay=%d CC cycles\n",
+			CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRQ_TIMER_INIT_VAL)));
+	}
+#endif
+
+	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS));
+	if (is_probe == true) {
+		SSI_LOG_INFO("Cache params previous: 0x%08X\n", val);
+	}
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS), SSI_CACHE_PARAMS);
+	val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_CACHE_PARAMS));
+	if (is_probe == true) {
+		SSI_LOG_INFO("Cache params current: 0x%08X  (expected: 0x%08X)\n", val, SSI_CACHE_PARAMS);
+	}
+
+	return 0;
+}
+
+static int init_cc_resources(struct platform_device *plat_dev)
+{
+	struct resource *req_mem_cc_regs = NULL;
+	void __iomem *cc_base = NULL;
+	bool irq_registered = false;
+	struct ssi_drvdata *new_drvdata = kzalloc(sizeof(struct ssi_drvdata), GFP_KERNEL);
+	uint32_t signature_val;
+	int rc = 0;
+
+	if (unlikely(new_drvdata == NULL)) {
+		SSI_LOG_ERR("Failed to allocate drvdata");
+		rc = -ENOMEM;
+		goto init_cc_res_err;
+	}
+
+	/*Initialize inflight counter used in dx_ablkcipher_secure_complete used for count of BYSPASS blocks operations*/
+	new_drvdata->inflight_counter = 0;
+
+	dev_set_drvdata(&plat_dev->dev, new_drvdata);
+	/* Get device resources */
+	/* First CC registers space */
+	new_drvdata->res_mem = platform_get_resource(plat_dev, IORESOURCE_MEM, 0);
+	if (unlikely(new_drvdata->res_mem == NULL)) {
+		SSI_LOG_ERR("Failed getting IO memory resource\n");
+		rc = -ENODEV;
+		goto init_cc_res_err;
+	}
+	SSI_LOG_DEBUG("Got MEM resource (%s): start=0x%llX end=0x%llX\n",
+		new_drvdata->res_mem->name,
+		(unsigned long long)new_drvdata->res_mem->start,
+		(unsigned long long)new_drvdata->res_mem->end);
+	/* Map registers space */
+	req_mem_cc_regs = request_mem_region(new_drvdata->res_mem->start, resource_size(new_drvdata->res_mem), "arm_cc7x_regs");
+	if (unlikely(req_mem_cc_regs == NULL)) {
+		SSI_LOG_ERR("Couldn't allocate registers memory region at "
+			     "0x%08X\n", (unsigned int)new_drvdata->res_mem->start);
+		rc = -EBUSY;
+		goto init_cc_res_err;
+	}
+	cc_base = ioremap(new_drvdata->res_mem->start, resource_size(new_drvdata->res_mem));
+	if (unlikely(cc_base == NULL)) {
+		SSI_LOG_ERR("ioremap[CC](0x%08X,0x%08X) failed\n",
+			(unsigned int)new_drvdata->res_mem->start, (unsigned int)resource_size(new_drvdata->res_mem));
+		rc = -ENOMEM;
+		goto init_cc_res_err;
+	}
+	SSI_LOG_DEBUG("CC registers mapped from %pa to 0x%p\n", &new_drvdata->res_mem->start, cc_base);
+	new_drvdata->cc_base = cc_base;
+	
+
+	/* Then IRQ */
+	new_drvdata->res_irq = platform_get_resource(plat_dev, IORESOURCE_IRQ, 0);
+	if (unlikely(new_drvdata->res_irq == NULL)) {
+		SSI_LOG_ERR("Failed getting IRQ resource\n");
+		rc = -ENODEV;
+		goto init_cc_res_err;
+	}
+	rc = request_irq(new_drvdata->res_irq->start, cc_isr,
+			 IRQF_SHARED, "arm_cc7x", new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("Could not register to interrupt %llu\n",
+			(unsigned long long)new_drvdata->res_irq->start);
+		goto init_cc_res_err;
+	}
+	init_completion(&new_drvdata->icache_setup_completion);
+
+	irq_registered = true;
+	SSI_LOG_DEBUG("Registered to IRQ (%s) %llu\n",
+		new_drvdata->res_irq->name,
+		(unsigned long long)new_drvdata->res_irq->start);
+
+	new_drvdata->plat_dev = plat_dev;
+
+	if(new_drvdata->plat_dev->dev.dma_mask == NULL)
+	{
+		new_drvdata->plat_dev->dev.dma_mask = & new_drvdata->plat_dev->dev.coherent_dma_mask;
+	}
+	if (!new_drvdata->plat_dev->dev.coherent_dma_mask)
+	{
+		new_drvdata->plat_dev->dev.coherent_dma_mask = DMA_BIT_MASK(DMA_BIT_MASK_LEN);
+	}
+
+	/* Verify correct mapping */
+	signature_val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_SIGNATURE));
+	if (signature_val != DX_DEV_SIGNATURE) {
+		SSI_LOG_ERR("Invalid CC signature: SIGNATURE=0x%08X != expected=0x%08X\n",
+			signature_val, (uint32_t)DX_DEV_SIGNATURE);
+		rc = -EINVAL;
+		goto init_cc_res_err;
+	}
+	SSI_LOG_DEBUG("CC SIGNATURE=0x%08X\n", signature_val);
+
+	/* Display HW versions */
+	SSI_LOG(KERN_INFO, "ARM CryptoCell %s Driver: HW version 0x%08X, Driver version %s\n", SSI_DEV_NAME_STR,
+		CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_VERSION)), DRV_MODULE_VERSION);
+
+	rc = init_cc_regs(new_drvdata, true);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("init_cc_regs failed\n");
+		goto init_cc_res_err;
+	}
+
+#ifdef ENABLE_CC_SYSFS
+	rc = ssi_sysfs_init(&(plat_dev->dev.kobj), new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("init_stat_db failed\n");
+		goto init_cc_res_err;
+	}
+#endif
+
+	rc = ssi_sram_mgr_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_sram_mgr_init failed\n");
+		goto init_cc_res_err;
+	}
+
+	new_drvdata->mlli_sram_addr =
+		ssi_sram_mgr_alloc(new_drvdata, MAX_MLLI_BUFF_SIZE);
+	if (unlikely(new_drvdata->mlli_sram_addr == NULL_SRAM_ADDR)) {
+		SSI_LOG_ERR("Failed to alloc MLLI Sram buffer\n");
+		rc = -ENOMEM;
+		goto init_cc_res_err;
+	}
+
+	rc = request_mgr_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("request_mgr_init failed\n");
+		goto init_cc_res_err;
+	}
+
+	rc = ssi_buffer_mgr_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("buffer_mgr_init failed\n");
+		goto init_cc_res_err;
+	}
+
+	rc = ssi_power_mgr_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_power_mgr_init failed\n");
+		goto init_cc_res_err;
+	}
+
+	rc = ssi_fips_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("SSI_FIPS_INIT failed 0x%x\n", rc);
+		goto init_cc_res_err;
+	}
+
+	rc = ssi_ivgen_init(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_ivgen_init failed\n");
+		goto init_cc_res_err;
+	}
+
+	/* Allocate crypto algs */
+	rc = ssi_ablkcipher_alloc(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_ablkcipher_alloc failed\n");
+		goto init_cc_res_err;
+	}
+
+	/* hash must be allocated before aead since hash exports APIs */
+	rc = ssi_hash_alloc(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_hash_alloc failed\n");
+		goto init_cc_res_err;
+	}
+
+	rc = ssi_aead_alloc(new_drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("ssi_aead_alloc failed\n");
+		goto init_cc_res_err;
+	}
+
+	return 0;
+
+init_cc_res_err:
+	SSI_LOG_ERR("Freeing CC HW resources!\n");
+	
+	if (new_drvdata != NULL) {
+		ssi_aead_free(new_drvdata);
+		ssi_hash_free(new_drvdata);
+		ssi_ablkcipher_free(new_drvdata);
+		ssi_ivgen_fini(new_drvdata);
+		ssi_power_mgr_fini(new_drvdata);
+		ssi_buffer_mgr_fini(new_drvdata);
+		request_mgr_fini(new_drvdata);
+		ssi_sram_mgr_fini(new_drvdata);
+		ssi_fips_fini(new_drvdata);
+#ifdef ENABLE_CC_SYSFS
+		ssi_sysfs_fini();
+#endif
+	
+		if (req_mem_cc_regs != NULL) {
+			if (irq_registered) {
+				free_irq(new_drvdata->res_irq->start, new_drvdata);
+				new_drvdata->res_irq = NULL;
+				iounmap(cc_base);
+				new_drvdata->cc_base = NULL;
+			}
+			release_mem_region(new_drvdata->res_mem->start,
+				resource_size(new_drvdata->res_mem));
+			new_drvdata->res_mem = NULL;
+		}
+		kfree(new_drvdata);
+		dev_set_drvdata(&plat_dev->dev, NULL);
+	}
+
+	return rc;
+}
+
+void fini_cc_regs(struct ssi_drvdata *drvdata)
+{
+	/* Mask all interrupts */
+	WRITE_REGISTER(drvdata->cc_base + 
+		       CC_REG_OFFSET(HOST_RGF, HOST_IMR), 0xFFFFFFFF);
+
+}
+
+static void cleanup_cc_resources(struct platform_device *plat_dev)
+{
+	struct ssi_drvdata *drvdata =
+		(struct ssi_drvdata *)dev_get_drvdata(&plat_dev->dev);
+
+        ssi_aead_free(drvdata);
+        ssi_hash_free(drvdata);
+        ssi_ablkcipher_free(drvdata);
+	ssi_ivgen_fini(drvdata);
+	ssi_power_mgr_fini(drvdata);
+	ssi_buffer_mgr_fini(drvdata);
+	request_mgr_fini(drvdata);
+	ssi_sram_mgr_fini(drvdata);
+	ssi_fips_fini(drvdata);
+#ifdef ENABLE_CC_SYSFS
+	ssi_sysfs_fini();
+#endif
+
+	/* Mask all interrupts */
+	WRITE_REGISTER(drvdata->cc_base + CC_REG_OFFSET(HOST_RGF, HOST_IMR),
+		0xFFFFFFFF);
+	free_irq(drvdata->res_irq->start, drvdata);
+	drvdata->res_irq = NULL;
+
+	fini_cc_regs(drvdata);
+
+	if (drvdata->cc_base != NULL) {
+		iounmap(drvdata->cc_base);
+		release_mem_region(drvdata->res_mem->start,
+			resource_size(drvdata->res_mem));
+		drvdata->cc_base = NULL;
+		drvdata->res_mem = NULL;
+	}
+
+	kfree(drvdata);
+	dev_set_drvdata(&plat_dev->dev, NULL);
+}
+
+static int cc7x_probe(struct platform_device *plat_dev)
+{
+	int rc;
+#if defined(CONFIG_ARM) && defined(CC_DEBUG)
+	uint32_t ctr, cacheline_size;
+
+	asm volatile("mrc p15, 0, %0, c0, c0, 1" : "=r" (ctr));
+	cacheline_size =  4 << ((ctr >> 16) & 0xf);
+	SSI_LOG_DEBUG("CP15(L1_CACHE_BYTES) = %u , Kconfig(L1_CACHE_BYTES) = %u\n",
+		cacheline_size, L1_CACHE_BYTES);
+
+	asm volatile("mrc p15, 0, %0, c0, c0, 0" : "=r" (ctr));
+	SSI_LOG_DEBUG("Main ID register (MIDR): Implementer 0x%02X, Arch 0x%01X,"
+		     " Part 0x%03X, Rev r%dp%d\n",
+		(ctr>>24), (ctr>>16)&0xF, (ctr>>4)&0xFFF, (ctr>>20)&0xF, ctr&0xF);
+#endif
+
+	/* Map registers space */
+	rc = init_cc_resources(plat_dev);
+	if (rc != 0)
+		return rc;
+
+	SSI_LOG(KERN_INFO, "ARM cc7x_ree device initialized\n");
+
+	return 0;
+}
+
+static int cc7x_remove(struct platform_device *plat_dev)
+{
+	SSI_LOG_DEBUG("Releasing cc7x resources...\n");
+	
+	cleanup_cc_resources(plat_dev);
+
+	SSI_LOG(KERN_INFO, "ARM cc7x_ree device terminated\n");
+#ifdef ENABLE_CYCLE_COUNT
+	display_all_stat_db();
+#endif
+	
+	return 0;
+}
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+static struct dev_pm_ops arm_cc7x_driver_pm = {
+	SET_RUNTIME_PM_OPS(ssi_power_mgr_runtime_suspend, ssi_power_mgr_runtime_resume, NULL)
+};
+#endif
+
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+#define	DX_DRIVER_RUNTIME_PM	(&arm_cc7x_driver_pm)
+#else
+#define	DX_DRIVER_RUNTIME_PM	NULL
+#endif
+
+
+#ifdef CONFIG_OF
+static const struct of_device_id arm_cc7x_dev_of_match[] = {
+	{.compatible = "arm,cryptocell-712-ree"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, arm_cc7x_dev_of_match);
+#endif
+
+static struct platform_driver cc7x_driver = {
+	.driver = {
+		   .name = "cc7xree",
+#ifdef CONFIG_OF
+		   .of_match_table = arm_cc7x_dev_of_match,
+#endif
+		   .pm = DX_DRIVER_RUNTIME_PM,
+	},
+	.probe = cc7x_probe,
+	.remove = cc7x_remove,
+};
+module_platform_driver(cc7x_driver);
+
+/* Module description */
+MODULE_DESCRIPTION("ARM TrustZone CryptoCell REE Driver");
+MODULE_VERSION(DRV_MODULE_VERSION);
+MODULE_AUTHOR("ARM");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/ccree/ssi_driver.h b/drivers/staging/ccree/ssi_driver.h
new file mode 100644
index 000000000000..891958b99634
--- /dev/null
+++ b/drivers/staging/ccree/ssi_driver.h
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_driver.h
+   ARM CryptoCell Linux Crypto Driver
+ */
+
+#ifndef __SSI_DRIVER_H__
+#define __SSI_DRIVER_H__
+
+#include "ssi_config.h"
+#ifdef COMP_IN_WQ
+#include <linux/workqueue.h>
+#else
+#include <linux/interrupt.h>
+#endif
+#include <linux/dma-mapping.h>
+#include <crypto/algapi.h>
+#include <crypto/internal/skcipher.h>
+#include <crypto/aes.h>
+#include <crypto/sha.h>
+#include <crypto/aead.h>
+#include <crypto/authenc.h>
+#include <crypto/hash.h>
+#include <linux/version.h>
+
+#ifndef INT32_MAX /* Missing in Linux kernel */
+#define INT32_MAX 0x7FFFFFFFL
+#endif
+
+/* Registers definitions from shared/hw/ree_include */
+#include "dx_reg_base_host.h"
+#include "dx_host.h"
+#define DX_CC_HOST_VIRT /* must be defined before including dx_cc_regs.h */
+#include "cc_hw_queue_defs.h"
+#include "cc_regs.h"
+#include "dx_reg_common.h"
+#include "cc_hal.h"
+#include "ssi_sram_mgr.h"
+#define CC_SUPPORT_SHA DX_DEV_SHA_MAX
+#include "cc_crypto_ctx.h"
+#include "ssi_sysfs.h"
+#include "hash_defs.h"
+#include "ssi_fips_local.h"
+
+#define DRV_MODULE_VERSION "3.0"
+
+#define SSI_DEV_NAME_STR "cc715ree"
+#define SSI_CC_HAS_AES_CCM 1
+#define SSI_CC_HAS_AES_GCM 1
+#define SSI_CC_HAS_AES_XTS 1
+#define SSI_CC_HAS_AES_ESSIV 1
+#define SSI_CC_HAS_AES_BITLOCKER 1
+#define SSI_CC_HAS_AES_CTS 1
+#define SSI_CC_HAS_MULTI2 0
+#define SSI_CC_HAS_CMAC 1
+
+#define SSI_AXI_IRQ_MASK ((1 << DX_AXIM_CFG_BRESPMASK_BIT_SHIFT) | (1 << DX_AXIM_CFG_RRESPMASK_BIT_SHIFT) |	\
+			(1 << DX_AXIM_CFG_INFLTMASK_BIT_SHIFT) | (1 << DX_AXIM_CFG_COMPMASK_BIT_SHIFT))
+
+#define SSI_AXI_ERR_IRQ_MASK (1 << DX_HOST_IRR_AXI_ERR_INT_BIT_SHIFT)
+
+#define SSI_COMP_IRQ_MASK (1 << DX_HOST_IRR_AXIM_COMP_INT_BIT_SHIFT)
+
+/* TEE FIPS status interrupt */
+#define SSI_GPR0_IRQ_MASK (1 << DX_HOST_IRR_GPR0_BIT_SHIFT)
+
+#define SSI_CRA_PRIO 3000
+
+#define MIN_HW_QUEUE_SIZE 50 /* Minimum size required for proper function */
+
+#define MAX_REQUEST_QUEUE_SIZE 4096
+#define MAX_MLLI_BUFF_SIZE 2080
+#define MAX_ICV_NENTS_SUPPORTED 2
+
+/* Definitions for HW descriptors DIN/DOUT fields */
+#define NS_BIT 1
+#define AXI_ID 0
+/* AXI_ID is not actually the AXI ID of the transaction but the value of AXI_ID 
+   field in the HW descriptor. The DMA engine +8 that value. */
+
+/* Logging macros */
+#define SSI_LOG(level, format, ...) \
+	printk(level "cc715ree::%s: " format , __func__, ##__VA_ARGS__)
+#define SSI_LOG_ERR(format, ...) SSI_LOG(KERN_ERR, format, ##__VA_ARGS__)
+#define SSI_LOG_WARNING(format, ...) SSI_LOG(KERN_WARNING, format, ##__VA_ARGS__)
+#define SSI_LOG_NOTICE(format, ...) SSI_LOG(KERN_NOTICE, format, ##__VA_ARGS__)
+#define SSI_LOG_INFO(format, ...) SSI_LOG(KERN_INFO, format, ##__VA_ARGS__)
+#ifdef CC_DEBUG
+#define SSI_LOG_DEBUG(format, ...) SSI_LOG(KERN_DEBUG, format, ##__VA_ARGS__)
+#else /* Debug log messages are removed at compile time for non-DEBUG config. */
+#define SSI_LOG_DEBUG(format, ...) do {} while (0)
+#endif
+
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
+
+#define SSI_MAX_IVGEN_DMA_ADDRESSES 	3
+struct ssi_crypto_req {
+	void (*user_cb)(struct device *dev, void *req, void __iomem *cc_base);
+	void *user_arg;
+	dma_addr_t ivgen_dma_addr[SSI_MAX_IVGEN_DMA_ADDRESSES]; /* For the first 'ivgen_dma_addr_len' addresses of this array,
+					 generated IV would be placed in it by send_request().
+					 Same generated IV for all addresses! */
+	unsigned int ivgen_dma_addr_len; /* Amount of 'ivgen_dma_addr' elements to be filled. */
+	unsigned int ivgen_size; /* The generated IV size required, 8/16 B allowed. */
+	struct completion seq_compl; /* request completion */
+#ifdef ENABLE_CYCLE_COUNT
+	enum stat_op op_type;
+	cycles_t submit_cycle;
+	bool is_monitored_p;
+#endif
+};
+
+/**
+ * struct ssi_drvdata - driver private data context
+ * @cc_base:	virt address of the CC registers
+ * @irq:	device IRQ number
+ * @irq_mask:	Interrupt mask shadow (1 for masked interrupts)
+ * @fw_ver:	SeP loaded firmware version
+ */
+struct ssi_drvdata {
+	struct resource *res_mem;
+	struct resource *res_irq;
+	void __iomem *cc_base;
+#ifdef DX_BASE_ENV_REGS
+	void __iomem *env_base; /* ARM CryptoCell development FPGAs only */
+#endif
+	unsigned int irq;
+	uint32_t irq_mask;
+	uint32_t fw_ver;
+	/* Calibration time of start/stop
+	*  monitor descriptors */
+	uint32_t monitor_null_cycles;
+	struct platform_device *plat_dev;
+	ssi_sram_addr_t mlli_sram_addr;
+	struct completion icache_setup_completion;
+	void *buff_mgr_handle;
+	void *hash_handle;
+	void *aead_handle;
+	void *blkcipher_handle;
+	void *request_mgr_handle;
+	void *fips_handle;
+	void *ivgen_handle;
+	void *sram_mgr_handle;
+
+#ifdef ENABLE_CYCLE_COUNT
+	cycles_t isr_exit_cycles; /* Save for isr-to-tasklet latency */
+#endif
+	uint32_t inflight_counter;
+
+};
+
+struct ssi_crypto_alg {
+	struct list_head entry;
+	int cipher_mode;
+	int flow_mode; /* Note: currently, refers to the cipher mode only. */
+	int auth_mode;
+	struct ssi_drvdata *drvdata;
+	struct crypto_alg crypto_alg;
+	struct aead_alg aead_alg;
+};
+
+struct ssi_alg_template {
+	char name[CRYPTO_MAX_ALG_NAME];
+	char driver_name[CRYPTO_MAX_ALG_NAME];
+	unsigned int blocksize;
+	u32 type;
+	union {
+		struct ablkcipher_alg ablkcipher;
+		struct aead_alg aead;
+		struct blkcipher_alg blkcipher;
+		struct cipher_alg cipher;
+		struct compress_alg compress;
+	} template_u;
+	int cipher_mode;
+	int flow_mode; /* Note: currently, refers to the cipher mode only. */
+	int auth_mode;
+	bool synchronous;
+	struct ssi_drvdata *drvdata;
+};
+
+struct async_gen_req_ctx {
+	dma_addr_t iv_dma_addr;
+	enum drv_crypto_direction op_type;
+};
+
+#ifdef DX_DUMP_BYTES
+void dump_byte_array(const char *name, const uint8_t *the_array, unsigned long size);
+#else
+#define dump_byte_array(name, array, size) do {	\
+} while (0);
+#endif
+
+#ifdef ENABLE_CYCLE_COUNT
+#define DECL_CYCLE_COUNT_RESOURCES cycles_t _last_cycles_read
+#define START_CYCLE_COUNT() do { _last_cycles_read = get_cycles(); } while (0)
+#define END_CYCLE_COUNT(_stat_op_type, _stat_phase) update_host_stat(_stat_op_type, _stat_phase, get_cycles() - _last_cycles_read)
+#define GET_START_CYCLE_COUNT() _last_cycles_read
+#define START_CYCLE_COUNT_AT(_var) do { _var = get_cycles(); } while(0)
+#define END_CYCLE_COUNT_AT(_var, _stat_op_type, _stat_phase) update_host_stat(_stat_op_type, _stat_phase, get_cycles() - _var)
+#else
+#define DECL_CYCLE_COUNT_RESOURCES 
+#define START_CYCLE_COUNT() do { } while (0)
+#define END_CYCLE_COUNT(_stat_op_type, _stat_phase) do { } while (0)
+#define GET_START_CYCLE_COUNT() 0
+#define START_CYCLE_COUNT_AT(_var) do { } while (0)
+#define END_CYCLE_COUNT_AT(_var, _stat_op_type, _stat_phase) do { } while (0)
+#endif /*ENABLE_CYCLE_COUNT*/
+
+int init_cc_regs(struct ssi_drvdata *drvdata, bool is_probe);
+void fini_cc_regs(struct ssi_drvdata *drvdata);
+
+#endif /*__SSI_DRIVER_H__*/
+
diff --git a/drivers/staging/ccree/ssi_fips.c b/drivers/staging/ccree/ssi_fips.c
new file mode 100644
index 000000000000..50f748511979
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+/**************************************************************
+This file defines the driver FIPS APIs                                                             *
+***************************************************************/
+
+#include <linux/module.h>
+#include "ssi_fips.h"
+
+
+extern int ssi_fips_ext_get_state(ssi_fips_state_t *p_state);
+extern int ssi_fips_ext_get_error(ssi_fips_error_t *p_err);
+
+/*
+This function returns the REE FIPS state.  
+It should be called by kernel module. 
+*/
+int ssi_fips_get_state(ssi_fips_state_t *p_state)
+{
+        int rc = 0;
+
+	if (p_state == NULL) {
+		return -EINVAL;
+	}
+
+	rc = ssi_fips_ext_get_state(p_state);
+
+	return rc;
+}
+
+EXPORT_SYMBOL(ssi_fips_get_state);
+
+/*
+This function returns the REE FIPS error.  
+It should be called by kernel module. 
+*/
+int ssi_fips_get_error(ssi_fips_error_t *p_err)
+{
+        int rc = 0;
+
+	if (p_err == NULL) {
+		return -EINVAL;
+	}
+
+	rc = ssi_fips_ext_get_error(p_err);
+
+	return rc;
+}
+
+EXPORT_SYMBOL(ssi_fips_get_error);
diff --git a/drivers/staging/ccree/ssi_fips.h b/drivers/staging/ccree/ssi_fips.h
new file mode 100644
index 000000000000..19bcdeb34308
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __SSI_FIPS_H__
+#define __SSI_FIPS_H__
+
+
+#ifndef INT32_MAX /* Missing in Linux kernel */
+#define INT32_MAX 0x7FFFFFFFL
+#endif
+
+
+/*! 
+@file
+@brief This file contains FIPS related defintions and APIs.
+*/
+
+typedef enum ssi_fips_state {
+        CC_FIPS_STATE_NOT_SUPPORTED = 0,
+        CC_FIPS_STATE_SUPPORTED,
+        CC_FIPS_STATE_ERROR,
+        CC_FIPS_STATE_RESERVE32B = INT32_MAX
+} ssi_fips_state_t;
+
+
+typedef enum ssi_fips_error {
+	CC_REE_FIPS_ERROR_OK = 0,
+	CC_REE_FIPS_ERROR_GENERAL,
+	CC_REE_FIPS_ERROR_FROM_TEE,
+	CC_REE_FIPS_ERROR_AES_ECB_PUT,
+	CC_REE_FIPS_ERROR_AES_CBC_PUT,
+	CC_REE_FIPS_ERROR_AES_OFB_PUT,
+	CC_REE_FIPS_ERROR_AES_CTR_PUT,
+	CC_REE_FIPS_ERROR_AES_CBC_CTS_PUT,
+	CC_REE_FIPS_ERROR_AES_XTS_PUT,
+	CC_REE_FIPS_ERROR_AES_CMAC_PUT,
+	CC_REE_FIPS_ERROR_AESCCM_PUT,
+	CC_REE_FIPS_ERROR_AESGCM_PUT,
+	CC_REE_FIPS_ERROR_DES_ECB_PUT,
+	CC_REE_FIPS_ERROR_DES_CBC_PUT,
+	CC_REE_FIPS_ERROR_SHA1_PUT,
+	CC_REE_FIPS_ERROR_SHA256_PUT,
+	CC_REE_FIPS_ERROR_SHA512_PUT,
+	CC_REE_FIPS_ERROR_HMAC_SHA1_PUT,
+	CC_REE_FIPS_ERROR_HMAC_SHA256_PUT,
+	CC_REE_FIPS_ERROR_HMAC_SHA512_PUT,
+	CC_REE_FIPS_ERROR_ROM_CHECKSUM,
+	CC_REE_FIPS_ERROR_RESERVE32B = INT32_MAX
+} ssi_fips_error_t;
+
+
+
+int ssi_fips_get_state(ssi_fips_state_t *p_state);
+int ssi_fips_get_error(ssi_fips_error_t *p_err);
+
+#endif  /*__SSI_FIPS_H__*/
+
diff --git a/drivers/staging/ccree/ssi_fips_data.h b/drivers/staging/ccree/ssi_fips_data.h
new file mode 100644
index 000000000000..3fddd8f74e07
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips_data.h
@@ -0,0 +1,315 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+The test vectors were taken from:
+
+* AES
+NIST Special Publication 800-38A 2001 Edition
+Recommendation for Block Cipher Modes of Operation
+http://csrc.nist.gov/publications/nistpubs/800-38a/sp800-38a.pdf
+Appendix F: Example Vectors for Modes of Operation of the AES
+
+* AES CTS
+Advanced Encryption Standard (AES) Encryption for Kerberos 5
+February 2005
+https://tools.ietf.org/html/rfc3962#appendix-B
+B.  Sample Test Vectors
+
+* AES XTS
+http://csrc.nist.gov/groups/STM/cavp/#08
+http://csrc.nist.gov/groups/STM/cavp/documents/aes/XTSTestVectors.zip
+
+* AES CMAC
+http://csrc.nist.gov/groups/STM/cavp/index.html#07
+http://csrc.nist.gov/groups/STM/cavp/documents/mac/cmactestvectors.zip
+ 
+* AES-CCM
+http://csrc.nist.gov/groups/STM/cavp/#07
+http://csrc.nist.gov/groups/STM/cavp/documents/mac/ccmtestvectors.zip
+
+* AES-GCM
+http://csrc.nist.gov/groups/STM/cavp/documents/mac/gcmtestvectors.zip
+
+* Triple-DES
+NIST Special Publication 800-67 January 2012
+Recommendation for the Triple Data Encryption Algorithm (TDEA) Block Cipher
+http://csrc.nist.gov/publications/nistpubs/800-67-Rev1/SP-800-67-Rev1.pdf
+APPENDIX B: EXAMPLE OF TDEA FORWARD AND INVERSE CIPHER OPERATIONS
+and
+http://csrc.nist.gov/groups/STM/cavp/#01
+http://csrc.nist.gov/groups/STM/cavp/documents/des/tdesmct_intermediate.zip
+
+* HASH
+http://csrc.nist.gov/groups/STM/cavp/#03
+http://csrc.nist.gov/groups/STM/cavp/documents/shs/shabytetestvectors.zip 
+ 
+* HMAC 
+http://csrc.nist.gov/groups/STM/cavp/#07
+http://csrc.nist.gov/groups/STM/cavp/documents/mac/hmactestvectors.zip 
+ 
+*/
+
+/* NIST AES */
+#define AES_128_BIT_KEY_SIZE    16
+#define AES_192_BIT_KEY_SIZE    24
+#define AES_256_BIT_KEY_SIZE    32
+#define AES_512_BIT_KEY_SIZE    64
+
+#define NIST_AES_IV_SIZE        16
+
+#define NIST_AES_128_KEY        { 0x2b, 0x7e, 0x15, 0x16, 0x28, 0xae, 0xd2, 0xa6, 0xab, 0xf7, 0x15, 0x88, 0x09, 0xcf, 0x4f, 0x3c }
+#define NIST_AES_192_KEY        { 0x8e, 0x73, 0xb0, 0xf7, 0xda, 0x0e, 0x64, 0x52, 0xc8, 0x10, 0xf3, 0x2b, 0x80, 0x90, 0x79, 0xe5, \
+				  0x62, 0xf8, 0xea, 0xd2, 0x52, 0x2c, 0x6b, 0x7b }
+#define NIST_AES_256_KEY        { 0x60, 0x3d, 0xeb, 0x10, 0x15, 0xca, 0x71, 0xbe, 0x2b, 0x73, 0xae, 0xf0, 0x85, 0x7d, 0x77, 0x81, \
+				  0x1f, 0x35, 0x2c, 0x07, 0x3b, 0x61, 0x08, 0xd7, 0x2d, 0x98, 0x10, 0xa3, 0x09, 0x14, 0xdf, 0xf4 }
+#define NIST_AES_VECTOR_SIZE    16
+#define NIST_AES_PLAIN_DATA     { 0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96, 0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a }
+
+#define NIST_AES_ECB_IV         { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
+#define NIST_AES_128_ECB_CIPHER { 0x3a, 0xd7, 0x7b, 0xb4, 0x0d, 0x7a, 0x36, 0x60, 0xa8, 0x9e, 0xca, 0xf3, 0x24, 0x66, 0xef, 0x97 }
+#define NIST_AES_192_ECB_CIPHER { 0xbd, 0x33, 0x4f, 0x1d, 0x6e, 0x45, 0xf2, 0x5f, 0xf7, 0x12, 0xa2, 0x14, 0x57, 0x1f, 0xa5, 0xcc }
+#define NIST_AES_256_ECB_CIPHER { 0xf3, 0xee, 0xd1, 0xbd, 0xb5, 0xd2, 0xa0, 0x3c, 0x06, 0x4b, 0x5a, 0x7e, 0x3d, 0xb1, 0x81, 0xf8 }
+
+#define NIST_AES_CBC_IV         { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f }
+#define NIST_AES_128_CBC_CIPHER { 0x76, 0x49, 0xab, 0xac, 0x81, 0x19, 0xb2, 0x46, 0xce, 0xe9, 0x8e, 0x9b, 0x12, 0xe9, 0x19, 0x7d }
+#define NIST_AES_192_CBC_CIPHER { 0x4f, 0x02, 0x1d, 0xb2, 0x43, 0xbc, 0x63, 0x3d, 0x71, 0x78, 0x18, 0x3a, 0x9f, 0xa0, 0x71, 0xe8 } 
+#define NIST_AES_256_CBC_CIPHER { 0xf5, 0x8c, 0x4c, 0x04, 0xd6, 0xe5, 0xf1, 0xba, 0x77, 0x9e, 0xab, 0xfb, 0x5f, 0x7b, 0xfb, 0xd6 } 
+
+#define NIST_AES_OFB_IV         { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f }
+#define NIST_AES_128_OFB_CIPHER { 0x3b, 0x3f, 0xd9, 0x2e, 0xb7, 0x2d, 0xad, 0x20, 0x33, 0x34, 0x49, 0xf8, 0xe8, 0x3c, 0xfb, 0x4a }
+#define NIST_AES_192_OFB_CIPHER { 0xcd, 0xc8, 0x0d, 0x6f, 0xdd, 0xf1, 0x8c, 0xab, 0x34, 0xc2, 0x59, 0x09, 0xc9, 0x9a, 0x41, 0x74 } 
+#define NIST_AES_256_OFB_CIPHER { 0xdc, 0x7e, 0x84, 0xbf, 0xda, 0x79, 0x16, 0x4b, 0x7e, 0xcd, 0x84, 0x86, 0x98, 0x5d, 0x38, 0x60 }
+
+#define NIST_AES_CTR_IV         { 0xf0, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa, 0xfb, 0xfc, 0xfd, 0xfe, 0xff }
+#define NIST_AES_128_CTR_CIPHER { 0x87, 0x4d, 0x61, 0x91, 0xb6, 0x20, 0xe3, 0x26, 0x1b, 0xef, 0x68, 0x64, 0x99, 0x0d, 0xb6, 0xce }
+#define NIST_AES_192_CTR_CIPHER { 0x1a, 0xbc, 0x93, 0x24, 0x17, 0x52, 0x1c, 0xa2, 0x4f, 0x2b, 0x04, 0x59, 0xfe, 0x7e, 0x6e, 0x0b } 
+#define NIST_AES_256_CTR_CIPHER { 0x60, 0x1e, 0xc3, 0x13, 0x77, 0x57, 0x89, 0xa5, 0xb7, 0xa7, 0xf5, 0x04, 0xbb, 0xf3, 0xd2, 0x28 } 
+
+
+#define RFC3962_AES_128_KEY            { 0x63, 0x68, 0x69, 0x63, 0x6b, 0x65, 0x6e, 0x20, 0x74, 0x65, 0x72, 0x69, 0x79, 0x61, 0x6b, 0x69 }
+#define RFC3962_AES_VECTOR_SIZE        17
+#define RFC3962_AES_PLAIN_DATA         { 0x49, 0x20, 0x77, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6c, 0x69, 0x6b, 0x65, 0x20, 0x74, 0x68, 0x65, 0x20 }
+#define RFC3962_AES_CBC_CTS_IV         { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
+#define RFC3962_AES_128_CBC_CTS_CIPHER { 0xc6, 0x35, 0x35, 0x68, 0xf2, 0xbf, 0x8c, 0xb4, 0xd8, 0xa5, 0x80, 0x36, 0x2d, 0xa7, 0xff, 0x7f, 0x97 }
+
+
+#define NIST_AES_256_XTS_KEY            { 0xa1, 0xb9, 0x0c, 0xba, 0x3f, 0x06, 0xac, 0x35, 0x3b, 0x2c, 0x34, 0x38, 0x76, 0x08, 0x17, 0x62, \
+					  0x09, 0x09, 0x23, 0x02, 0x6e, 0x91, 0x77, 0x18, 0x15, 0xf2, 0x9d, 0xab, 0x01, 0x93, 0x2f, 0x2f }
+#define NIST_AES_256_XTS_IV             { 0x4f, 0xae, 0xf7, 0x11, 0x7c, 0xda, 0x59, 0xc6, 0x6e, 0x4b, 0x92, 0x01, 0x3e, 0x76, 0x8a, 0xd5 }
+#define NIST_AES_256_XTS_VECTOR_SIZE    16
+#define NIST_AES_256_XTS_PLAIN          { 0xeb, 0xab, 0xce, 0x95, 0xb1, 0x4d, 0x3c, 0x8d, 0x6f, 0xb3, 0x50, 0x39, 0x07, 0x90, 0x31, 0x1c } 
+#define NIST_AES_256_XTS_CIPHER         { 0x77, 0x8a, 0xe8, 0xb4, 0x3c, 0xb9, 0x8d, 0x5a, 0x82, 0x50, 0x81, 0xd5, 0xbe, 0x47, 0x1c, 0x63 } 
+
+#define NIST_AES_512_XTS_KEY            { 0x1e, 0xa6, 0x61, 0xc5, 0x8d, 0x94, 0x3a, 0x0e, 0x48, 0x01, 0xe4, 0x2f, 0x4b, 0x09, 0x47, 0x14, \
+					  0x9e, 0x7f, 0x9f, 0x8e, 0x3e, 0x68, 0xd0, 0xc7, 0x50, 0x52, 0x10, 0xbd, 0x31, 0x1a, 0x0e, 0x7c, \
+					  0xd6, 0xe1, 0x3f, 0xfd, 0xf2, 0x41, 0x8d, 0x8d, 0x19, 0x11, 0xc0, 0x04, 0xcd, 0xa5, 0x8d, 0xa3, \
+					  0xd6, 0x19, 0xb7, 0xe2, 0xb9, 0x14, 0x1e, 0x58, 0x31, 0x8e, 0xea, 0x39, 0x2c, 0xf4, 0x1b, 0x08 }
+#define NIST_AES_512_XTS_IV             { 0xad, 0xf8, 0xd9, 0x26, 0x27, 0x46, 0x4a, 0xd2, 0xf0, 0x42, 0x8e, 0x84, 0xa9, 0xf8, 0x75, 0x64,  }
+#define NIST_AES_512_XTS_VECTOR_SIZE    32
+#define NIST_AES_512_XTS_PLAIN          { 0x2e, 0xed, 0xea, 0x52, 0xcd, 0x82, 0x15, 0xe1, 0xac, 0xc6, 0x47, 0xe8, 0x10, 0xbb, 0xc3, 0x64, \
+					  0x2e, 0x87, 0x28, 0x7f, 0x8d, 0x2e, 0x57, 0xe3, 0x6c, 0x0a, 0x24, 0xfb, 0xc1, 0x2a, 0x20, 0x2e } 
+#define NIST_AES_512_XTS_CIPHER         { 0xcb, 0xaa, 0xd0, 0xe2, 0xf6, 0xce, 0xa3, 0xf5, 0x0b, 0x37, 0xf9, 0x34, 0xd4, 0x6a, 0x9b, 0x13, \
+					  0x0b, 0x9d, 0x54, 0xf0, 0x7e, 0x34, 0xf3, 0x6a, 0xf7, 0x93, 0xe8, 0x6f, 0x73, 0xc6, 0xd7, 0xdb } 
+
+
+/* NIST AES-CMAC */
+#define NIST_AES_128_CMAC_KEY           { 0x67, 0x08, 0xc9, 0x88, 0x7b, 0x84, 0x70, 0x84, 0xf1, 0x23, 0xd3, 0xdd, 0x9c, 0x3a, 0x81, 0x36 }
+#define NIST_AES_128_CMAC_PLAIN_DATA    { 0xa8, 0xde, 0x55, 0x17, 0x0c, 0x6d, 0xc0, 0xd8, 0x0d, 0xe3, 0x2f, 0x50, 0x8b, 0xf4, 0x9b, 0x70 }
+#define NIST_AES_128_CMAC_MAC           { 0xcf, 0xef, 0x9b, 0x78, 0x39, 0x84, 0x1f, 0xdb, 0xcc, 0xbb, 0x6c, 0x2c, 0xf2, 0x38, 0xf7 }
+#define NIST_AES_128_CMAC_VECTOR_SIZE   16
+#define NIST_AES_128_CMAC_OUTPUT_SIZE   15
+
+#define NIST_AES_192_CMAC_KEY           { 0x20, 0x51, 0xaf, 0x34, 0x76, 0x2e, 0xbe, 0x55, 0x6f, 0x72, 0xa5, 0xc6, 0xed, 0xc7, 0x77, 0x1e, \
+					  0xb9, 0x24, 0x5f, 0xad, 0x76, 0xf0, 0x34, 0xbe }
+#define NIST_AES_192_CMAC_PLAIN_DATA    { 0xae, 0x8e, 0x93, 0xc9, 0xc9, 0x91, 0xcf, 0x89, 0x6a, 0x49, 0x1a, 0x89, 0x07, 0xdf, 0x4e, 0x4b, \
+					  0xe5, 0x18, 0x6a, 0xe4, 0x96, 0xcd, 0x34, 0x0d, 0xc1, 0x9b, 0x23, 0x78, 0x21, 0xdb, 0x7b, 0x60 }
+#define NIST_AES_192_CMAC_MAC           { 0x74, 0xf7, 0x46, 0x08, 0xc0, 0x4f, 0x0f, 0x4e, 0x47, 0xfa, 0x64, 0x04, 0x33, 0xb6, 0xe6, 0xfb }
+#define NIST_AES_192_CMAC_VECTOR_SIZE   32
+#define NIST_AES_192_CMAC_OUTPUT_SIZE   16
+
+#define NIST_AES_256_CMAC_KEY           { 0x3a, 0x75, 0xa9, 0xd2, 0xbd, 0xb8, 0xc8, 0x04, 0xba, 0x4a, 0xb4, 0x98, 0x35, 0x73, 0xa6, 0xb2, \
+					  0x53, 0x16, 0x0d, 0xd9, 0x0f, 0x8e, 0xdd, 0xfb, 0x2f, 0xdc, 0x2a, 0xb1, 0x76, 0x04, 0xf5, 0xc5 }
+#define NIST_AES_256_CMAC_PLAIN_DATA    { 0x42, 0xf3, 0x5d, 0x5a, 0xa5, 0x33, 0xa7, 0xa0, 0xa5, 0xf7, 0x4e, 0x14, 0x4f, 0x2a, 0x5f, 0x20 }
+#define NIST_AES_256_CMAC_MAC           { 0xf1, 0x53, 0x2f, 0x87, 0x32, 0xd9, 0xf5, 0x90, 0x30, 0x07 }
+#define NIST_AES_256_CMAC_VECTOR_SIZE   16
+#define NIST_AES_256_CMAC_OUTPUT_SIZE   10
+
+
+/* NIST TDES */
+#define TDES_NUM_OF_KEYS                3
+#define NIST_TDES_VECTOR_SIZE           8
+#define NIST_TDES_IV_SIZE               8
+
+#define NIST_TDES_ECB_IV             	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
+
+#define NIST_TDES_ECB3_KEY		{ 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, \
+					  0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x01, \
+					  0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, 0x01, 0x23 }
+#define NIST_TDES_ECB3_PLAIN_DATA    	{ 0x54, 0x68, 0x65, 0x20, 0x71, 0x75, 0x66, 0x63 }
+#define NIST_TDES_ECB3_CIPHER        	{ 0xa8, 0x26, 0xfd, 0x8c, 0xe5, 0x3b, 0x85, 0x5f }
+
+#define NIST_TDES_CBC3_IV            	{ 0xf8, 0xee, 0xe1, 0x35, 0x9c, 0x6e, 0x54, 0x40 }
+#define NIST_TDES_CBC3_KEY		{ 0xe9, 0xda, 0x37, 0xf8, 0xdc, 0x97, 0x6d, 0x5b, \
+					  0xb6, 0x8c, 0x04, 0xe3, 0xec, 0x98, 0x20, 0x15, \
+					  0xf4, 0x0e, 0x08, 0xb5, 0x97, 0x29, 0xf2, 0x8f }
+#define NIST_TDES_CBC3_PLAIN_DATA    	{ 0x3b, 0xb7, 0xa7, 0xdb, 0xa3, 0xd5, 0x92, 0x91 }
+#define NIST_TDES_CBC3_CIPHER        	{ 0x5b, 0x84, 0x24, 0xd2, 0x39, 0x3e, 0x55, 0xa2 }
+
+
+/* NIST AES-CCM */
+#define NIST_AESCCM_128_BIT_KEY_SIZE    16
+#define NIST_AESCCM_192_BIT_KEY_SIZE    24
+#define NIST_AESCCM_256_BIT_KEY_SIZE    32
+
+#define NIST_AESCCM_B0_VAL              0x79  /* L'[0:2]=1 , M'[3-5]=7 , Adata[6]=1, reserved[7]=0 */
+#define NIST_AESCCM_NONCE_SIZE          13
+#define NIST_AESCCM_IV_SIZE             16
+#define NIST_AESCCM_ADATA_SIZE          32
+#define NIST_AESCCM_TEXT_SIZE           16
+#define NIST_AESCCM_TAG_SIZE            16
+
+#define NIST_AESCCM_128_KEY             { 0x70, 0x01, 0x0e, 0xd9, 0x0e, 0x61, 0x86, 0xec, 0xad, 0x41, 0xf0, 0xd3, 0xc7, 0xc4, 0x2f, 0xf8 }
+#define NIST_AESCCM_128_NONCE           { 0xa5, 0xf4, 0xf4, 0x98, 0x6e, 0x98, 0x47, 0x29, 0x65, 0xf5, 0xab, 0xcc, 0x4b }
+#define NIST_AESCCM_128_ADATA           { 0x3f, 0xec, 0x0e, 0x5c, 0xc2, 0x4d, 0x67, 0x13, 0x94, 0x37, 0xcb, 0xc8, 0x11, 0x24, 0x14, 0xfc, \
+					  0x8d, 0xac, 0xcd, 0x1a, 0x94, 0xb4, 0x9a, 0x4c, 0x76, 0xe2, 0xd3, 0x93, 0x03, 0x54, 0x73, 0x17 }
+#define NIST_AESCCM_128_PLAIN_TEXT      { 0xbe, 0x32, 0x2f, 0x58, 0xef, 0xa7, 0xf8, 0xc6, 0x8a, 0x63, 0x5e, 0x0b, 0x9c, 0xce, 0x77, 0xf2 }
+#define NIST_AESCCM_128_CIPHER          { 0x8e, 0x44, 0x25, 0xae, 0x57, 0x39, 0x74, 0xf0, 0xf0, 0x69, 0x3a, 0x18, 0x8b, 0x52, 0x58, 0x12 }
+#define NIST_AESCCM_128_MAC             { 0xee, 0xf0, 0x8e, 0x3f, 0xb1, 0x5f, 0x42, 0x27, 0xe0, 0xd9, 0x89, 0xa4, 0xd5, 0x87, 0xa8, 0xcf }
+
+#define NIST_AESCCM_192_KEY             { 0x68, 0x73, 0xf1, 0xc6, 0xc3, 0x09, 0x75, 0xaf, 0xf6, 0xf0, 0x84, 0x70, 0x26, 0x43, 0x21, 0x13, \
+					  0x0a, 0x6e, 0x59, 0x84, 0xad, 0xe3, 0x24, 0xe9 }
+#define NIST_AESCCM_192_NONCE           { 0x7c, 0x4d, 0x2f, 0x7c, 0xec, 0x04, 0x36, 0x1f, 0x18, 0x7f, 0x07, 0x26, 0xd5 }
+#define NIST_AESCCM_192_ADATA           { 0x77, 0x74, 0x3b, 0x5d, 0x83, 0xa0, 0x0d, 0x2c, 0x8d, 0x5f, 0x7e, 0x10, 0x78, 0x15, 0x31, 0xb4, \
+					  0x96, 0xe0, 0x9f, 0x3b, 0xc9, 0x29, 0x5d, 0x7a, 0xe9, 0x79, 0x9e, 0x64, 0x66, 0x8e, 0xf8, 0xc5 }
+#define NIST_AESCCM_192_PLAIN_TEXT      { 0x50, 0x51, 0xa0, 0xb0, 0xb6, 0x76, 0x6c, 0xd6, 0xea, 0x29, 0xa6, 0x72, 0x76, 0x9d, 0x40, 0xfe }
+#define NIST_AESCCM_192_CIPHER          { 0x0c, 0xe5, 0xac, 0x8d, 0x6b, 0x25, 0x6f, 0xb7, 0x58, 0x0b, 0xf6, 0xac, 0xc7, 0x64, 0x26, 0xaf }
+#define NIST_AESCCM_192_MAC             { 0x40, 0xbc, 0xe5, 0x8f, 0xd4, 0xcd, 0x65, 0x48, 0xdf, 0x90, 0xa0, 0x33, 0x7c, 0x84, 0x20, 0x04 }
+
+#define NIST_AESCCM_256_KEY             { 0xee, 0x8c, 0xe1, 0x87, 0x16, 0x97, 0x79, 0xd1, 0x3e, 0x44, 0x3d, 0x64, 0x28, 0xe3, 0x8b, 0x38, \
+					  0xb5, 0x5d, 0xfb, 0x90, 0xf0, 0x22, 0x8a, 0x8a, 0x4e, 0x62, 0xf8, 0xf5, 0x35, 0x80, 0x6e, 0x62 }
+#define NIST_AESCCM_256_NONCE           { 0x12, 0x16, 0x42, 0xc4, 0x21, 0x8b, 0x39, 0x1c, 0x98, 0xe6, 0x26, 0x9c, 0x8a }
+#define NIST_AESCCM_256_ADATA           { 0x71, 0x8d, 0x13, 0xe4, 0x75, 0x22, 0xac, 0x4c, 0xdf, 0x3f, 0x82, 0x80, 0x63, 0x98, 0x0b, 0x6d, \
+					  0x45, 0x2f, 0xcd, 0xcd, 0x6e, 0x1a, 0x19, 0x04, 0xbf, 0x87, 0xf5, 0x48, 0xa5, 0xfd, 0x5a, 0x05 }
+#define NIST_AESCCM_256_PLAIN_TEXT      { 0xd1, 0x5f, 0x98, 0xf2, 0xc6, 0xd6, 0x70, 0xf5, 0x5c, 0x78, 0xa0, 0x66, 0x48, 0x33, 0x2b, 0xc9 }
+#define NIST_AESCCM_256_CIPHER          { 0xcc, 0x17, 0xbf, 0x87, 0x94, 0xc8, 0x43, 0x45, 0x7d, 0x89, 0x93, 0x91, 0x89, 0x8e, 0xd2, 0x2a }
+#define NIST_AESCCM_256_MAC             { 0x6f, 0x9d, 0x28, 0xfc, 0xb6, 0x42, 0x34, 0xe1, 0xcd, 0x79, 0x3c, 0x41, 0x44, 0xf1, 0xda, 0x50 }
+
+
+/* NIST AES-GCM */
+#define NIST_AESGCM_128_BIT_KEY_SIZE    16
+#define NIST_AESGCM_192_BIT_KEY_SIZE    24
+#define NIST_AESGCM_256_BIT_KEY_SIZE    32
+
+#define NIST_AESGCM_IV_SIZE             12
+#define NIST_AESGCM_ADATA_SIZE          16
+#define NIST_AESGCM_TEXT_SIZE           16
+#define NIST_AESGCM_TAG_SIZE            16
+
+#define NIST_AESGCM_128_KEY             { 0x81, 0x6e, 0x39, 0x07, 0x04, 0x10, 0xcf, 0x21, 0x84, 0x90, 0x4d, 0xa0, 0x3e, 0xa5, 0x07, 0x5a }
+#define NIST_AESGCM_128_IV              { 0x32, 0xc3, 0x67, 0xa3, 0x36, 0x26, 0x13, 0xb2, 0x7f, 0xc3, 0xe6, 0x7e }
+#define NIST_AESGCM_128_ADATA           { 0xf2, 0xa3, 0x07, 0x28, 0xed, 0x87, 0x4e, 0xe0, 0x29, 0x83, 0xc2, 0x94, 0x43, 0x5d, 0x3c, 0x16 }
+#define NIST_AESGCM_128_PLAIN_TEXT      { 0xec, 0xaf, 0xe9, 0x6c, 0x67, 0xa1, 0x64, 0x67, 0x44, 0xf1, 0xc8, 0x91, 0xf5, 0xe6, 0x94, 0x27 }
+#define NIST_AESGCM_128_CIPHER          { 0x55, 0x2e, 0xbe, 0x01, 0x2e, 0x7b, 0xcf, 0x90, 0xfc, 0xef, 0x71, 0x2f, 0x83, 0x44, 0xe8, 0xf1 }
+#define NIST_AESGCM_128_MAC             { 0xec, 0xaa, 0xe9, 0xfc, 0x68, 0x27, 0x6a, 0x45, 0xab, 0x0c, 0xa3, 0xcb, 0x9d, 0xd9, 0x53, 0x9f }
+
+#define NIST_AESGCM_192_KEY             { 0x0c, 0x44, 0xd6, 0xc9, 0x28, 0xee, 0x11, 0x2c, 0xe6, 0x65, 0xfe, 0x54, 0x7e, 0xbd, 0x38, 0x72, \
+					  0x98, 0xa9, 0x54, 0xb4, 0x62, 0xf6, 0x95, 0xd8 }
+#define NIST_AESGCM_192_IV              { 0x18, 0xb8, 0xf3, 0x20, 0xfe, 0xf4, 0xae, 0x8c, 0xcb, 0xe8, 0xf9, 0x52 }
+#define NIST_AESGCM_192_ADATA           { 0x73, 0x41, 0xd4, 0x3f, 0x98, 0xcf, 0x38, 0x82, 0x21, 0x18, 0x09, 0x41, 0x97, 0x03, 0x76, 0xe8 }
+#define NIST_AESGCM_192_PLAIN_TEXT      { 0x96, 0xad, 0x07, 0xf9, 0xb6, 0x28, 0xb6, 0x52, 0xcf, 0x86, 0xcb, 0x73, 0x17, 0x88, 0x6f, 0x51 }
+#define NIST_AESGCM_192_CIPHER          { 0xa6, 0x64, 0x07, 0x81, 0x33, 0x40, 0x5e, 0xb9, 0x09, 0x4d, 0x36, 0xf7, 0xe0, 0x70, 0x19, 0x1f }
+#define NIST_AESGCM_192_MAC             { 0xe8, 0xf9, 0xc3, 0x17, 0x84, 0x7c, 0xe3, 0xf3, 0xc2, 0x39, 0x94, 0xa4, 0x02, 0xf0, 0x65, 0x81 }
+
+#define NIST_AESGCM_256_KEY             { 0x54, 0xe3, 0x52, 0xea, 0x1d, 0x84, 0xbf, 0xe6, 0x4a, 0x10, 0x11, 0x09, 0x61, 0x11, 0xfb, 0xe7, \
+					  0x66, 0x8a, 0xd2, 0x20, 0x3d, 0x90, 0x2a, 0x01, 0x45, 0x8c, 0x3b, 0xbd, 0x85, 0xbf, 0xce, 0x14 }
+#define NIST_AESGCM_256_IV              { 0xdf, 0x7c, 0x3b, 0xca, 0x00, 0x39, 0x6d, 0x0c, 0x01, 0x84, 0x95, 0xd9 }
+#define NIST_AESGCM_256_ADATA           { 0x7e, 0x96, 0x8d, 0x71, 0xb5, 0x0c, 0x1f, 0x11, 0xfd, 0x00, 0x1f, 0x3f, 0xef, 0x49, 0xd0, 0x45 }
+#define NIST_AESGCM_256_PLAIN_TEXT      { 0x85, 0xfc, 0x3d, 0xfa, 0xd9, 0xb5, 0xa8, 0xd3, 0x25, 0x8e, 0x4f, 0xc4, 0x45, 0x71, 0xbd, 0x3b }
+#define NIST_AESGCM_256_CIPHER          { 0x42, 0x6e, 0x0e, 0xfc, 0x69, 0x3b, 0x7b, 0xe1, 0xf3, 0x01, 0x8d, 0xb7, 0xdd, 0xbb, 0x7e, 0x4d }
+#define NIST_AESGCM_256_MAC             { 0xee, 0x82, 0x57, 0x79, 0x5b, 0xe6, 0xa1, 0x16, 0x4d, 0x7e, 0x1d, 0x2d, 0x6c, 0xac, 0x77, 0xa7 }
+
+
+/* NIST HASH */
+#define NIST_SHA_MSG_SIZE               16
+
+#define NIST_SHA_1_MSG                  { 0x35, 0x52, 0x69, 0x4c, 0xdf, 0x66, 0x3f, 0xd9, 0x4b, 0x22, 0x47, 0x47, 0xac, 0x40, 0x6a, 0xaf }
+#define NIST_SHA_1_MD                   { 0xa1, 0x50, 0xde, 0x92, 0x74, 0x54, 0x20, 0x2d, 0x94, 0xe6, 0x56, 0xde, 0x4c, 0x7c, 0x0c, 0xa6, \
+					  0x91, 0xde, 0x95, 0x5d }
+
+#define NIST_SHA_256_MSG                { 0x0a, 0x27, 0x84, 0x7c, 0xdc, 0x98, 0xbd, 0x6f, 0x62, 0x22, 0x0b, 0x04, 0x6e, 0xdd, 0x76, 0x2b }
+#define NIST_SHA_256_MD                 { 0x80, 0xc2, 0x5e, 0xc1, 0x60, 0x05, 0x87, 0xe7, 0xf2, 0x8b, 0x18, 0xb1, 0xb1, 0x8e, 0x3c, 0xdc, \
+					  0x89, 0x92, 0x8e, 0x39, 0xca, 0xb3, 0xbc, 0x25, 0xe4, 0xd4, 0xa4, 0xc1, 0x39, 0xbc, 0xed, 0xc4 }
+
+#define NIST_SHA_512_MSG                { 0xcd, 0x67, 0xbd, 0x40, 0x54, 0xaa, 0xa3, 0xba, 0xa0, 0xdb, 0x17, 0x8c, 0xe2, 0x32, 0xfd, 0x5a }
+#define NIST_SHA_512_MD                 { 0x0d, 0x85, 0x21, 0xf8, 0xf2, 0xf3, 0x90, 0x03, 0x32, 0xd1, 0xa1, 0xa5, 0x5c, 0x60, 0xba, 0x81, \
+					  0xd0, 0x4d, 0x28, 0xdf, 0xe8, 0xc5, 0x04, 0xb6, 0x32, 0x8a, 0xe7, 0x87, 0x92, 0x5f, 0xe0, 0x18, \
+					  0x8f, 0x2b, 0xa9, 0x1c, 0x3a, 0x9f, 0x0c, 0x16, 0x53, 0xc4, 0xbf, 0x0a, 0xda, 0x35, 0x64, 0x55, \
+					  0xea, 0x36, 0xfd, 0x31, 0xf8, 0xe7, 0x3e, 0x39, 0x51, 0xca, 0xd4, 0xeb, 0xba, 0x8c, 0x6e, 0x04 }
+
+
+/* NIST HMAC */
+#define NIST_HMAC_MSG_SIZE              128
+
+#define NIST_HMAC_SHA1_KEY_SIZE         10
+#define NIST_HMAC_SHA1_KEY		{ 0x59, 0x78, 0x59, 0x28, 0xd7, 0x25, 0x16, 0xe3, 0x12, 0x72 }
+#define NIST_HMAC_SHA1_MSG		{ 0xa3, 0xce, 0x88, 0x99, 0xdf, 0x10, 0x22, 0xe8, 0xd2, 0xd5, 0x39, 0xb4, 0x7b, 0xf0, 0xe3, 0x09, \
+					  0xc6, 0x6f, 0x84, 0x09, 0x5e, 0x21, 0x43, 0x8e, 0xc3, 0x55, 0xbf, 0x11, 0x9c, 0xe5, 0xfd, 0xcb, \
+					  0x4e, 0x73, 0xa6, 0x19, 0xcd, 0xf3, 0x6f, 0x25, 0xb3, 0x69, 0xd8, 0xc3, 0x8f, 0xf4, 0x19, 0x99, \
+					  0x7f, 0x0c, 0x59, 0x83, 0x01, 0x08, 0x22, 0x36, 0x06, 0xe3, 0x12, 0x23, 0x48, 0x3f, 0xd3, 0x9e, \
+					  0xde, 0xaa, 0x4d, 0x3f, 0x0d, 0x21, 0x19, 0x88, 0x62, 0xd2, 0x39, 0xc9, 0xfd, 0x26, 0x07, 0x41, \
+					  0x30, 0xff, 0x6c, 0x86, 0x49, 0x3f, 0x52, 0x27, 0xab, 0x89, 0x5c, 0x8f, 0x24, 0x4b, 0xd4, 0x2c, \
+					  0x7a, 0xfc, 0xe5, 0xd1, 0x47, 0xa2, 0x0a, 0x59, 0x07, 0x98, 0xc6, 0x8e, 0x70, 0x8e, 0x96, 0x49, \
+					  0x02, 0xd1, 0x24, 0xda, 0xde, 0xcd, 0xbd, 0xa9, 0xdb, 0xd0, 0x05, 0x1e, 0xd7, 0x10, 0xe9, 0xbf }
+#define NIST_HMAC_SHA1_MD               { 0x3c, 0x81, 0x62, 0x58, 0x9a, 0xaf, 0xae, 0xe0, 0x24, 0xfc, 0x9a, 0x5c, 0xa5, 0x0d, 0xd2, 0x33, \
+					  0x6f, 0xe3, 0xeb, 0x28 }
+
+#define NIST_HMAC_SHA256_KEY_SIZE       40
+#define NIST_HMAC_SHA256_KEY		{ 0x97, 0x79, 0xd9, 0x12, 0x06, 0x42, 0x79, 0x7f, 0x17, 0x47, 0x02, 0x5d, 0x5b, 0x22, 0xb7, 0xac, \
+					  0x60, 0x7c, 0xab, 0x08, 0xe1, 0x75, 0x8f, 0x2f, 0x3a, 0x46, 0xc8, 0xbe, 0x1e, 0x25, 0xc5, 0x3b, \
+					  0x8c, 0x6a, 0x8f, 0x58, 0xff, 0xef, 0xa1, 0x76 }
+#define NIST_HMAC_SHA256_MSG		{ 0xb1, 0x68, 0x9c, 0x25, 0x91, 0xea, 0xf3, 0xc9, 0xe6, 0x60, 0x70, 0xf8, 0xa7, 0x79, 0x54, 0xff, \
+					  0xb8, 0x17, 0x49, 0xf1, 0xb0, 0x03, 0x46, 0xf9, 0xdf, 0xe0, 0xb2, 0xee, 0x90, 0x5d, 0xcc, 0x28, \
+					  0x8b, 0xaf, 0x4a, 0x92, 0xde, 0x3f, 0x40, 0x01, 0xdd, 0x9f, 0x44, 0xc4, 0x68, 0xc3, 0xd0, 0x7d, \
+					  0x6c, 0x6e, 0xe8, 0x2f, 0xac, 0xea, 0xfc, 0x97, 0xc2, 0xfc, 0x0f, 0xc0, 0x60, 0x17, 0x19, 0xd2, \
+					  0xdc, 0xd0, 0xaa, 0x2a, 0xec, 0x92, 0xd1, 0xb0, 0xae, 0x93, 0x3c, 0x65, 0xeb, 0x06, 0xa0, 0x3c, \
+					  0x9c, 0x93, 0x5c, 0x2b, 0xad, 0x04, 0x59, 0x81, 0x02, 0x41, 0x34, 0x7a, 0xb8, 0x7e, 0x9f, 0x11, \
+					  0xad, 0xb3, 0x04, 0x15, 0x42, 0x4c, 0x6c, 0x7f, 0x5f, 0x22, 0xa0, 0x03, 0xb8, 0xab, 0x8d, 0xe5, \
+					  0x4f, 0x6d, 0xed, 0x0e, 0x3a, 0xb9, 0x24, 0x5f, 0xa7, 0x95, 0x68, 0x45, 0x1d, 0xfa, 0x25, 0x8e }
+#define NIST_HMAC_SHA256_MD             { 0x76, 0x9f, 0x00, 0xd3, 0xe6, 0xa6, 0xcc, 0x1f, 0xb4, 0x26, 0xa1, 0x4a, 0x4f, 0x76, 0xc6, 0x46, \
+					  0x2e, 0x61, 0x49, 0x72, 0x6e, 0x0d, 0xee, 0x0e, 0xc0, 0xcf, 0x97, 0xa1, 0x66, 0x05, 0xac, 0x8b }
+
+#define NIST_HMAC_SHA512_KEY_SIZE       100
+#define NIST_HMAC_SHA512_KEY		{ 0x57, 0xc2, 0xeb, 0x67, 0x7b, 0x50, 0x93, 0xb9, 0xe8, 0x29, 0xea, 0x4b, 0xab, 0xb5, 0x0b, 0xde, \
+					  0x55, 0xd0, 0xad, 0x59, 0xfe, 0xc3, 0x4a, 0x61, 0x89, 0x73, 0x80, 0x2b, 0x2a, 0xd9, 0xb7, 0x8e, \
+					  0x26, 0xb2, 0x04, 0x5d, 0xda, 0x78, 0x4d, 0xf3, 0xff, 0x90, 0xae, 0x0f, 0x2c, 0xc5, 0x1c, 0xe3, \
+					  0x9c, 0xf5, 0x48, 0x67, 0x32, 0x0a, 0xc6, 0xf3, 0xba, 0x2c, 0x6f, 0x0d, 0x72, 0x36, 0x04, 0x80, \
+					  0xc9, 0x66, 0x14, 0xae, 0x66, 0x58, 0x1f, 0x26, 0x6c, 0x35, 0xfb, 0x79, 0xfd, 0x28, 0x77, 0x4a, \
+					  0xfd, 0x11, 0x3f, 0xa5, 0x18, 0x7e, 0xff, 0x92, 0x06, 0xd7, 0xcb, 0xe9, 0x0d, 0xd8, 0xbf, 0x67, \
+					  0xc8, 0x44, 0xe2, 0x02 }
+#define NIST_HMAC_SHA512_MSG		{ 0x24, 0x23, 0xdf, 0xf4, 0x8b, 0x31, 0x2b, 0xe8, 0x64, 0xcb, 0x34, 0x90, 0x64, 0x1f, 0x79, 0x3d, \
+					  0x2b, 0x9f, 0xb6, 0x8a, 0x77, 0x63, 0xb8, 0xe2, 0x98, 0xc8, 0x6f, 0x42, 0x24, 0x5e, 0x45, 0x40, \
+					  0xeb, 0x01, 0xae, 0x4d, 0x2d, 0x45, 0x00, 0x37, 0x0b, 0x18, 0x86, 0xf2, 0x3c, 0xa2, 0xcf, 0x97, \
+					  0x01, 0x70, 0x4c, 0xad, 0x5b, 0xd2, 0x1b, 0xa8, 0x7b, 0x81, 0x1d, 0xaf, 0x7a, 0x85, 0x4e, 0xa2, \
+					  0x4a, 0x56, 0x56, 0x5c, 0xed, 0x42, 0x5b, 0x35, 0xe4, 0x0e, 0x1a, 0xcb, 0xeb, 0xe0, 0x36, 0x03, \
+					  0xe3, 0x5d, 0xcf, 0x4a, 0x10, 0x0e, 0x57, 0x21, 0x84, 0x08, 0xa1, 0xd8, 0xdb, 0xcc, 0x3b, 0x99, \
+					  0x29, 0x6c, 0xfe, 0xa9, 0x31, 0xef, 0xe3, 0xeb, 0xd8, 0xf7, 0x19, 0xa6, 0xd9, 0xa1, 0x54, 0x87, \
+					  0xb9, 0xad, 0x67, 0xea, 0xfe, 0xdf, 0x15, 0x55, 0x9c, 0xa4, 0x24, 0x45, 0xb0, 0xf9, 0xb4, 0x2e }
+#define NIST_HMAC_SHA512_MD             { 0x33, 0xc5, 0x11, 0xe9, 0xbc, 0x23, 0x07, 0xc6, 0x27, 0x58, 0xdf, 0x61, 0x12, 0x5a, 0x98, 0x0e, \
+					  0xe6, 0x4c, 0xef, 0xeb, 0xd9, 0x09, 0x31, 0xcb, 0x91, 0xc1, 0x37, 0x42, 0xd4, 0x71, 0x4c, 0x06, \
+					  0xde, 0x40, 0x03, 0xfa, 0xf3, 0xc4, 0x1c, 0x06, 0xae, 0xfc, 0x63, 0x8a, 0xd4, 0x7b, 0x21, 0x90, \
+					  0x6e, 0x6b, 0x10, 0x48, 0x16, 0xb7, 0x2d, 0xe6, 0x26, 0x9e, 0x04, 0x5a, 0x1f, 0x44, 0x29, 0xd4 }
+
diff --git a/drivers/staging/ccree/ssi_fips_ext.c b/drivers/staging/ccree/ssi_fips_ext.c
new file mode 100644
index 000000000000..2ac432fe1233
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips_ext.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**************************************************************
+This file defines the driver FIPS functions that should be 
+implemented by the driver user. Current implementation is sample code only.
+***************************************************************/
+
+#include <linux/module.h>
+#include "ssi_fips_local.h"
+#include "ssi_driver.h"
+
+
+static bool tee_error;
+module_param(tee_error, bool, 0644);
+MODULE_PARM_DESC(tee_error, "Simulate TEE library failure flag: 0 - no error (default), 1 - TEE error occured ");
+
+static ssi_fips_state_t fips_state = CC_FIPS_STATE_NOT_SUPPORTED;
+static ssi_fips_error_t fips_error = CC_REE_FIPS_ERROR_OK;
+
+/*
+This function returns the FIPS REE state. 
+The function should be implemented by the driver user, depends on where                          .
+the state value is stored. 
+The reference code uses global variable. 
+*/
+int ssi_fips_ext_get_state(ssi_fips_state_t *p_state)
+{
+        int rc = 0;
+
+	if (p_state == NULL) {
+		return -EINVAL;
+	}
+
+	*p_state = fips_state;
+
+	return rc;
+}
+
+/*
+This function returns the FIPS REE error. 
+The function should be implemented by the driver user, depends on where                          .
+the error value is stored. 
+The reference code uses global variable. 
+*/
+int ssi_fips_ext_get_error(ssi_fips_error_t *p_err)
+{
+        int rc = 0;
+
+	if (p_err == NULL) {
+		return -EINVAL;
+	}
+
+	*p_err = fips_error;
+
+	return rc;
+}
+
+/*
+This function sets the FIPS REE state. 
+The function should be implemented by the driver user, depends on where                          .
+the state value is stored. 
+The reference code uses global variable. 
+*/
+int ssi_fips_ext_set_state(ssi_fips_state_t state)
+{
+	fips_state = state;
+	return 0;
+}
+
+/*
+This function sets the FIPS REE error. 
+The function should be implemented by the driver user, depends on where                          .
+the error value is stored. 
+The reference code uses global variable. 
+*/
+int ssi_fips_ext_set_error(ssi_fips_error_t err)
+{
+	fips_error = err;
+	return 0;
+}
+
+
diff --git a/drivers/staging/ccree/ssi_fips_ll.c b/drivers/staging/ccree/ssi_fips_ll.c
new file mode 100644
index 000000000000..d573574bbb98
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips_ll.c
@@ -0,0 +1,1681 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**************************************************************
+This file defines the driver FIPS Low Level implmentaion functions,
+that executes the KAT.
+***************************************************************/
+#include <linux/kernel.h>
+
+#include "ssi_driver.h"
+#include "ssi_fips_local.h"
+#include "ssi_fips_data.h"
+#include "cc_crypto_ctx.h"
+#include "ssi_hash.h"
+#include "ssi_request_mgr.h"
+
+
+static const uint32_t digest_len_init[] = {
+	0x00000040, 0x00000000, 0x00000000, 0x00000000 };
+static const uint32_t sha1_init[] = { 
+	SHA1_H4, SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
+static const uint32_t sha256_init[] = {
+	SHA256_H7, SHA256_H6, SHA256_H5, SHA256_H4,
+	SHA256_H3, SHA256_H2, SHA256_H1, SHA256_H0 };
+#if (CC_SUPPORT_SHA > 256)
+static const uint32_t digest_len_sha512_init[] = { 
+	0x00000080, 0x00000000, 0x00000000, 0x00000000 };
+static const uint64_t sha512_init[] = {
+	SHA512_H7, SHA512_H6, SHA512_H5, SHA512_H4,
+	SHA512_H3, SHA512_H2, SHA512_H1, SHA512_H0 };
+#endif
+
+
+#define NIST_CIPHER_AES_MAX_VECTOR_SIZE      32
+
+struct fips_cipher_ctx {
+	uint8_t iv[CC_AES_IV_SIZE];
+	uint8_t key[AES_512_BIT_KEY_SIZE];
+	uint8_t din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	uint8_t dout[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+};
+
+typedef struct _FipsCipherData {
+	uint8_t                   isAes;
+	uint8_t                   key[AES_512_BIT_KEY_SIZE];
+	size_t                    keySize;
+	uint8_t                   iv[CC_AES_IV_SIZE];
+	enum drv_crypto_direction direction;
+	enum drv_cipher_mode      oprMode;
+	uint8_t                   dataIn[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	uint8_t                   dataOut[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	size_t                    dataInSize;
+} FipsCipherData;
+
+
+struct fips_cmac_ctx {
+	uint8_t key[AES_256_BIT_KEY_SIZE];
+	uint8_t din[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+};
+
+typedef struct _FipsCmacData {
+	enum drv_crypto_direction direction;
+	uint8_t                   key[AES_256_BIT_KEY_SIZE];
+	size_t                    key_size;
+	uint8_t                   data_in[NIST_CIPHER_AES_MAX_VECTOR_SIZE];
+	size_t                    data_in_size;
+	uint8_t                   mac_res[CC_DIGEST_SIZE_MAX];
+	size_t                    mac_res_size;
+} FipsCmacData;
+
+
+struct fips_hash_ctx {
+	uint8_t initial_digest[CC_DIGEST_SIZE_MAX];
+	uint8_t din[NIST_SHA_MSG_SIZE];
+	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+};
+
+typedef struct _FipsHashData {
+	enum drv_hash_mode    hash_mode;
+	uint8_t               data_in[NIST_SHA_MSG_SIZE];
+	size_t                data_in_size;
+	uint8_t               mac_res[CC_DIGEST_SIZE_MAX];
+} FipsHashData;
+
+
+/* note that the hmac key length must be equal or less than block size (block size is 64 up to sha256 and 128 for sha384/512) */
+struct fips_hmac_ctx {
+	uint8_t initial_digest[CC_DIGEST_SIZE_MAX];
+	uint8_t key[CC_HMAC_BLOCK_SIZE_MAX];
+	uint8_t k0[CC_HMAC_BLOCK_SIZE_MAX];
+	uint8_t digest_bytes_len[HASH_LEN_SIZE];
+	uint8_t tmp_digest[CC_DIGEST_SIZE_MAX];
+	uint8_t din[NIST_HMAC_MSG_SIZE];
+	uint8_t mac_res[CC_DIGEST_SIZE_MAX];
+};
+
+typedef struct _FipsHmacData {
+	enum drv_hash_mode    hash_mode;
+	uint8_t               key[CC_HMAC_BLOCK_SIZE_MAX];
+	size_t                key_size;
+	uint8_t               data_in[NIST_HMAC_MSG_SIZE];
+	size_t                data_in_size;
+	uint8_t               mac_res[CC_DIGEST_SIZE_MAX];
+} FipsHmacData;
+
+
+#define FIPS_CCM_B0_A0_ADATA_SIZE   (NIST_AESCCM_IV_SIZE + NIST_AESCCM_IV_SIZE + NIST_AESCCM_ADATA_SIZE)
+
+struct fips_ccm_ctx {
+	uint8_t b0_a0_adata[FIPS_CCM_B0_A0_ADATA_SIZE];
+	uint8_t iv[NIST_AESCCM_IV_SIZE];
+	uint8_t ctr_cnt_0[NIST_AESCCM_IV_SIZE];
+	uint8_t key[CC_AES_KEY_SIZE_MAX];
+	uint8_t din[NIST_AESCCM_TEXT_SIZE];
+	uint8_t dout[NIST_AESCCM_TEXT_SIZE];
+	uint8_t mac_res[NIST_AESCCM_TAG_SIZE];
+};
+
+typedef struct _FipsCcmData {
+	enum drv_crypto_direction direction;
+	uint8_t                   key[CC_AES_KEY_SIZE_MAX];
+	size_t                    keySize;
+	uint8_t                   nonce[NIST_AESCCM_NONCE_SIZE];
+	uint8_t                   adata[NIST_AESCCM_ADATA_SIZE];
+	size_t                    adataSize;
+	uint8_t                   dataIn[NIST_AESCCM_TEXT_SIZE];
+	size_t                    dataInSize;
+	uint8_t                   dataOut[NIST_AESCCM_TEXT_SIZE];
+	uint8_t                   tagSize;
+	uint8_t                   macResOut[NIST_AESCCM_TAG_SIZE];
+} FipsCcmData;
+
+
+struct fips_gcm_ctx {
+	uint8_t adata[NIST_AESGCM_ADATA_SIZE];
+	uint8_t key[CC_AES_KEY_SIZE_MAX];
+	uint8_t hkey[CC_AES_KEY_SIZE_MAX];
+	uint8_t din[NIST_AESGCM_TEXT_SIZE];
+	uint8_t dout[NIST_AESGCM_TEXT_SIZE];
+	uint8_t mac_res[NIST_AESGCM_TAG_SIZE];
+	uint8_t len_block[AES_BLOCK_SIZE];
+	uint8_t iv_inc1[AES_BLOCK_SIZE];
+	uint8_t iv_inc2[AES_BLOCK_SIZE];
+};
+
+typedef struct _FipsGcmData {
+	enum drv_crypto_direction direction;
+	uint8_t                   key[CC_AES_KEY_SIZE_MAX];
+	size_t                    keySize;
+	uint8_t                   iv[NIST_AESGCM_IV_SIZE];
+	uint8_t                   adata[NIST_AESGCM_ADATA_SIZE];
+	size_t                    adataSize;
+	uint8_t                   dataIn[NIST_AESGCM_TEXT_SIZE];
+	size_t                    dataInSize;
+	uint8_t                   dataOut[NIST_AESGCM_TEXT_SIZE];
+	uint8_t                   tagSize;
+	uint8_t                   macResOut[NIST_AESGCM_TAG_SIZE];
+} FipsGcmData;
+
+
+typedef union _fips_ctx {
+	struct fips_cipher_ctx cipher;
+	struct fips_cmac_ctx cmac;
+	struct fips_hash_ctx hash;
+	struct fips_hmac_ctx hmac;
+	struct fips_ccm_ctx ccm;
+	struct fips_gcm_ctx gcm;
+} fips_ctx;
+
+
+/* test data tables */
+static const FipsCipherData FipsCipherDataTable[] = {
+	/* AES */
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_ECB, NIST_AES_PLAIN_DATA, NIST_AES_128_ECB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_ECB, NIST_AES_128_ECB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_ECB, NIST_AES_PLAIN_DATA, NIST_AES_192_ECB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_ECB, NIST_AES_192_ECB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_ECB, NIST_AES_PLAIN_DATA, NIST_AES_256_ECB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_ECB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_ECB, NIST_AES_256_ECB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC, NIST_AES_PLAIN_DATA, NIST_AES_128_CBC_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC, NIST_AES_128_CBC_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC, NIST_AES_PLAIN_DATA, NIST_AES_192_CBC_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC, NIST_AES_192_CBC_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC, NIST_AES_PLAIN_DATA, NIST_AES_256_CBC_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_CBC_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC, NIST_AES_256_CBC_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_OFB, NIST_AES_PLAIN_DATA, NIST_AES_128_OFB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_OFB, NIST_AES_128_OFB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_OFB, NIST_AES_PLAIN_DATA, NIST_AES_192_OFB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_OFB, NIST_AES_192_OFB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_OFB, NIST_AES_PLAIN_DATA, NIST_AES_256_OFB_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_OFB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_OFB, NIST_AES_256_OFB_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CTR, NIST_AES_PLAIN_DATA, NIST_AES_128_CTR_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_128_KEY, CC_AES_128_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CTR, NIST_AES_128_CTR_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CTR, NIST_AES_PLAIN_DATA, NIST_AES_192_CTR_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_192_KEY, CC_AES_192_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CTR, NIST_AES_192_CTR_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CTR, NIST_AES_PLAIN_DATA, NIST_AES_256_CTR_CIPHER, NIST_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_KEY, CC_AES_256_BIT_KEY_SIZE, NIST_AES_CTR_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CTR, NIST_AES_256_CTR_CIPHER, NIST_AES_PLAIN_DATA, NIST_AES_VECTOR_SIZE },
+	{ 1, RFC3962_AES_128_KEY,  CC_AES_128_BIT_KEY_SIZE, RFC3962_AES_CBC_CTS_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC_CTS, RFC3962_AES_PLAIN_DATA, RFC3962_AES_128_CBC_CTS_CIPHER, RFC3962_AES_VECTOR_SIZE },
+	{ 1, RFC3962_AES_128_KEY,  CC_AES_128_BIT_KEY_SIZE, RFC3962_AES_CBC_CTS_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC_CTS, RFC3962_AES_128_CBC_CTS_CIPHER, RFC3962_AES_PLAIN_DATA, RFC3962_AES_VECTOR_SIZE },
+	{ 1, NIST_AES_256_XTS_KEY, CC_AES_256_BIT_KEY_SIZE,   NIST_AES_256_XTS_IV,  DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_XTS,     NIST_AES_256_XTS_PLAIN, NIST_AES_256_XTS_CIPHER, NIST_AES_256_XTS_VECTOR_SIZE },
+	{ 1, NIST_AES_256_XTS_KEY, CC_AES_256_BIT_KEY_SIZE,   NIST_AES_256_XTS_IV,  DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_XTS,     NIST_AES_256_XTS_CIPHER, NIST_AES_256_XTS_PLAIN, NIST_AES_256_XTS_VECTOR_SIZE },
+#if (CC_SUPPORT_SHA > 256)
+	{ 1, NIST_AES_512_XTS_KEY, 2*CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_VECTOR_SIZE },
+	{ 1, NIST_AES_512_XTS_KEY, 2*CC_AES_256_BIT_KEY_SIZE, NIST_AES_512_XTS_IV,  DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_XTS,     NIST_AES_512_XTS_CIPHER, NIST_AES_512_XTS_PLAIN, NIST_AES_512_XTS_VECTOR_SIZE },
+#endif
+	/* DES */
+	{ 0, NIST_TDES_ECB3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_ECB_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_ECB, NIST_TDES_ECB3_PLAIN_DATA, NIST_TDES_ECB3_CIPHER, NIST_TDES_VECTOR_SIZE },
+	{ 0, NIST_TDES_ECB3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_ECB_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_ECB, NIST_TDES_ECB3_CIPHER, NIST_TDES_ECB3_PLAIN_DATA, NIST_TDES_VECTOR_SIZE },
+	{ 0, NIST_TDES_CBC3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_CBC3_IV, DRV_CRYPTO_DIRECTION_ENCRYPT, DRV_CIPHER_CBC, NIST_TDES_CBC3_PLAIN_DATA, NIST_TDES_CBC3_CIPHER, NIST_TDES_VECTOR_SIZE },
+	{ 0, NIST_TDES_CBC3_KEY, CC_DRV_DES_TRIPLE_KEY_SIZE, NIST_TDES_CBC3_IV, DRV_CRYPTO_DIRECTION_DECRYPT, DRV_CIPHER_CBC, NIST_TDES_CBC3_CIPHER, NIST_TDES_CBC3_PLAIN_DATA, NIST_TDES_VECTOR_SIZE },
+};
+#define FIPS_CIPHER_NUM_OF_TESTS        (sizeof(FipsCipherDataTable) / sizeof(FipsCipherData))
+
+static const FipsCmacData FipsCmacDataTable[] = {
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AES_128_CMAC_KEY, AES_128_BIT_KEY_SIZE, NIST_AES_128_CMAC_PLAIN_DATA, NIST_AES_128_CMAC_VECTOR_SIZE, NIST_AES_128_CMAC_MAC, NIST_AES_128_CMAC_OUTPUT_SIZE },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AES_192_CMAC_KEY, AES_192_BIT_KEY_SIZE, NIST_AES_192_CMAC_PLAIN_DATA, NIST_AES_192_CMAC_VECTOR_SIZE, NIST_AES_192_CMAC_MAC, NIST_AES_192_CMAC_OUTPUT_SIZE },
+	{ DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AES_256_CMAC_KEY, AES_256_BIT_KEY_SIZE, NIST_AES_256_CMAC_PLAIN_DATA, NIST_AES_256_CMAC_VECTOR_SIZE, NIST_AES_256_CMAC_MAC, NIST_AES_256_CMAC_OUTPUT_SIZE },
+};
+#define FIPS_CMAC_NUM_OF_TESTS        (sizeof(FipsCmacDataTable) / sizeof(FipsCmacData))
+
+static const FipsHashData FipsHashDataTable[] = {
+        { DRV_HASH_SHA1,   NIST_SHA_1_MSG,   NIST_SHA_MSG_SIZE, NIST_SHA_1_MD },
+        { DRV_HASH_SHA256, NIST_SHA_256_MSG, NIST_SHA_MSG_SIZE, NIST_SHA_256_MD },
+#if (CC_SUPPORT_SHA > 256)
+//        { DRV_HASH_SHA512, NIST_SHA_512_MSG, NIST_SHA_MSG_SIZE, NIST_SHA_512_MD },
+#endif
+};
+#define FIPS_HASH_NUM_OF_TESTS        (sizeof(FipsHashDataTable) / sizeof(FipsHashData))
+
+static const FipsHmacData FipsHmacDataTable[] = {
+        { DRV_HASH_SHA1,   NIST_HMAC_SHA1_KEY,   NIST_HMAC_SHA1_KEY_SIZE,   NIST_HMAC_SHA1_MSG,   NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA1_MD },
+        { DRV_HASH_SHA256, NIST_HMAC_SHA256_KEY, NIST_HMAC_SHA256_KEY_SIZE, NIST_HMAC_SHA256_MSG, NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA256_MD },
+#if (CC_SUPPORT_SHA > 256)
+//        { DRV_HASH_SHA512, NIST_HMAC_SHA512_KEY, NIST_HMAC_SHA512_KEY_SIZE, NIST_HMAC_SHA512_MSG, NIST_HMAC_MSG_SIZE, NIST_HMAC_SHA512_MD },
+#endif
+};
+#define FIPS_HMAC_NUM_OF_TESTS        (sizeof(FipsHmacDataTable) / sizeof(FipsHmacData))
+
+static const FipsCcmData FipsCcmDataTable[] = {
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_128_KEY, NIST_AESCCM_128_BIT_KEY_SIZE, NIST_AESCCM_128_NONCE, NIST_AESCCM_128_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_128_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_128_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_128_MAC },
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_192_KEY, NIST_AESCCM_192_BIT_KEY_SIZE, NIST_AESCCM_192_NONCE, NIST_AESCCM_192_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_192_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_192_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_192_MAC },
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESCCM_256_KEY, NIST_AESCCM_256_BIT_KEY_SIZE, NIST_AESCCM_256_NONCE, NIST_AESCCM_256_ADATA, NIST_AESCCM_ADATA_SIZE, NIST_AESCCM_256_CIPHER, NIST_AESCCM_TEXT_SIZE, NIST_AESCCM_256_PLAIN_TEXT, NIST_AESCCM_TAG_SIZE, NIST_AESCCM_256_MAC },
+};
+#define FIPS_CCM_NUM_OF_TESTS        (sizeof(FipsCcmDataTable) / sizeof(FipsCcmData))
+
+static const FipsGcmData FipsGcmDataTable[] = {
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_128_KEY, NIST_AESGCM_128_BIT_KEY_SIZE, NIST_AESGCM_128_IV, NIST_AESGCM_128_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_128_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_128_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_128_MAC },
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_192_KEY, NIST_AESGCM_192_BIT_KEY_SIZE, NIST_AESGCM_192_IV, NIST_AESGCM_192_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_192_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_192_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_192_MAC },
+        { DRV_CRYPTO_DIRECTION_ENCRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
+        { DRV_CRYPTO_DIRECTION_DECRYPT, NIST_AESGCM_256_KEY, NIST_AESGCM_256_BIT_KEY_SIZE, NIST_AESGCM_256_IV, NIST_AESGCM_256_ADATA, NIST_AESGCM_ADATA_SIZE, NIST_AESGCM_256_CIPHER, NIST_AESGCM_TEXT_SIZE, NIST_AESGCM_256_PLAIN_TEXT, NIST_AESGCM_TAG_SIZE, NIST_AESGCM_256_MAC },
+};
+#define FIPS_GCM_NUM_OF_TESTS        (sizeof(FipsGcmDataTable) / sizeof(FipsGcmData))
+
+
+static inline ssi_fips_error_t 
+FIPS_CipherToFipsError(enum drv_cipher_mode mode, bool is_aes)
+{
+	switch (mode)
+	{
+	case DRV_CIPHER_ECB:
+		return is_aes ? CC_REE_FIPS_ERROR_AES_ECB_PUT : CC_REE_FIPS_ERROR_DES_ECB_PUT ;
+	case DRV_CIPHER_CBC:
+		return is_aes ? CC_REE_FIPS_ERROR_AES_CBC_PUT : CC_REE_FIPS_ERROR_DES_CBC_PUT ;
+	case DRV_CIPHER_OFB:
+		return CC_REE_FIPS_ERROR_AES_OFB_PUT;
+	case DRV_CIPHER_CTR:
+		return CC_REE_FIPS_ERROR_AES_CTR_PUT;
+	case DRV_CIPHER_CBC_CTS:
+		return CC_REE_FIPS_ERROR_AES_CBC_CTS_PUT;
+	case DRV_CIPHER_XTS:
+		return CC_REE_FIPS_ERROR_AES_XTS_PUT;
+	default:
+		return CC_REE_FIPS_ERROR_GENERAL;
+	}
+
+	return CC_REE_FIPS_ERROR_GENERAL;
+}
+
+
+static inline int 
+ssi_cipher_fips_run_test(struct ssi_drvdata *drvdata,
+			 bool is_aes,
+			 int cipher_mode,
+			 int direction,
+			 dma_addr_t key_dma_addr,
+			 size_t key_len,
+			 dma_addr_t iv_dma_addr,
+			 size_t iv_len,
+			 dma_addr_t din_dma_addr,
+			 dma_addr_t dout_dma_addr,
+			 size_t data_size)
+{
+	/* max number of descriptors used for the flow */
+	#define FIPS_CIPHER_MAX_SEQ_LEN 6
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_CIPHER_MAX_SEQ_LEN];
+	int idx = 0;
+	int s_flow_mode = is_aes ? S_DIN_to_AES : S_DIN_to_DES;
+
+	/* create setup descriptors */
+	switch (cipher_mode) {
+	case DRV_CIPHER_CBC:
+	case DRV_CIPHER_CBC_CTS:
+	case DRV_CIPHER_CTR:
+	case DRV_CIPHER_OFB:
+		/* Load cipher state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				     iv_dma_addr, iv_len, NS_BIT);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
+		if ((cipher_mode == DRV_CIPHER_CTR) || 
+		    (cipher_mode == DRV_CIPHER_OFB) ) {
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		} else {
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		}
+		idx++;
+		/*FALLTHROUGH*/
+	case DRV_CIPHER_ECB:
+		/* Load key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		if (is_aes) {
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+					     key_dma_addr, 
+					     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len),
+					     NS_BIT);
+			HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
+		} else {/*des*/
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+					     key_dma_addr, key_len,
+					     NS_BIT);
+			HW_DESC_SET_KEY_SIZE_DES(&desc[idx], key_len);
+		}
+		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+		break;
+	case DRV_CIPHER_XTS:
+		/* Load AES key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				     key_dma_addr, key_len/2, NS_BIT);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* load XEX key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+				     (key_dma_addr+key_len/2), key_len/2, NS_BIT);
+		HW_DESC_SET_XEX_DATA_UNIT_SIZE(&desc[idx], data_size);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_XEX_KEY);
+		idx++;
+
+		/* Set state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], cipher_mode);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], direction);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len/2);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], s_flow_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				     iv_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+		idx++;
+		break;
+	default:
+		FIPS_LOG("Unsupported cipher mode (%d)\n", cipher_mode);
+		BUG();
+	}
+
+	/* create data descriptor */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, data_size, NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], dout_dma_addr, data_size, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], is_aes ? DIN_AES_DOUT : DIN_DES_DOUT);
+	idx++;
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_CIPHER_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	// send_request returns error just in some corner cases which should not appear in this flow.
+	return rc;
+}
+
+
+ssi_fips_error_t
+ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_cipher_ctx *virt_ctx = (struct fips_cipher_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers for iv, key, din, dout */
+	dma_addr_t iv_dma_addr = dma_coherent_buffer + offsetof(struct fips_cipher_ctx, iv);
+	dma_addr_t key_dma_addr = dma_coherent_buffer + offsetof(struct fips_cipher_ctx, key);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_cipher_ctx, din);
+	dma_addr_t dout_dma_addr = dma_coherent_buffer + offsetof(struct fips_cipher_ctx, dout);
+
+	for (i = 0; i < FIPS_CIPHER_NUM_OF_TESTS; ++i)
+	{
+		FipsCipherData *cipherData = (FipsCipherData*)&FipsCipherDataTable[i];
+		int rc = 0;
+		size_t iv_size = cipherData->isAes ? NIST_AES_IV_SIZE : NIST_TDES_IV_SIZE ;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_cipher_ctx));
+
+		/* copy into the allocated buffer */
+		memcpy(virt_ctx->iv, cipherData->iv, iv_size);
+		memcpy(virt_ctx->key, cipherData->key, cipherData->keySize);
+		memcpy(virt_ctx->din, cipherData->dataIn, cipherData->dataInSize);
+
+		FIPS_DBG("ssi_cipher_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_cipher_fips_run_test(drvdata,
+					      cipherData->isAes,
+					      cipherData->oprMode,
+					      cipherData->direction,
+					      key_dma_addr,
+					      cipherData->keySize,
+					      iv_dma_addr,
+					      iv_size,
+					      din_dma_addr,
+					      dout_dma_addr,
+					      cipherData->dataInSize);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_cipher_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = FIPS_CipherToFipsError(cipherData->oprMode, cipherData->isAes);
+			break;
+		}
+
+		/* compare actual dout to expected */
+		if (memcmp(virt_ctx->dout, cipherData->dataOut, cipherData->dataInSize) != 0)
+		{
+			FIPS_LOG("dout comparison error %d - oprMode=%d, isAes=%d\n", i, cipherData->oprMode, cipherData->isAes);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x  (size=%d) \n", (size_t)cipherData->dataOut, (size_t)virt_ctx->dout, cipherData->dataInSize);
+			for (i = 0; i < cipherData->dataInSize; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, cipherData->dataOut[i], virt_ctx->dout[i]);
+			}
+
+			error = FIPS_CipherToFipsError(cipherData->oprMode, cipherData->isAes);
+			break;
+		}
+	}
+
+	return error;
+}
+
+
+static inline int 
+ssi_cmac_fips_run_test(struct ssi_drvdata *drvdata,
+		       dma_addr_t key_dma_addr,
+		       size_t key_len,
+		       dma_addr_t din_dma_addr,
+		       size_t din_len,
+		       dma_addr_t digest_dma_addr,
+		       size_t digest_len)
+{
+	/* max number of descriptors used for the flow */
+	#define FIPS_CMAC_MAX_SEQ_LEN 4
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_CMAC_MAX_SEQ_LEN];
+	int idx = 0;
+
+	/* Setup CMAC Key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
+			     ((key_len == 24) ? AES_MAX_KEY_SIZE : key_len), NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Load MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, digest_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+
+	//ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+			     din_dma_addr, 
+			     din_len, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;
+	
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], digest_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC); 
+	idx++;
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_CMAC_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	// send_request returns error just in some corner cases which should not appear in this flow.
+	return rc;
+}
+
+ssi_fips_error_t
+ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_cmac_ctx *virt_ctx = (struct fips_cmac_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers for key, din, dout */
+	dma_addr_t key_dma_addr = dma_coherent_buffer + offsetof(struct fips_cmac_ctx, key);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_cmac_ctx, din);
+	dma_addr_t mac_res_dma_addr = dma_coherent_buffer + offsetof(struct fips_cmac_ctx, mac_res);
+
+	for (i = 0; i < FIPS_CMAC_NUM_OF_TESTS; ++i)
+	{
+		FipsCmacData *cmac_data = (FipsCmacData*)&FipsCmacDataTable[i];
+		int rc = 0;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_cmac_ctx));
+
+		/* copy into the allocated buffer */
+		memcpy(virt_ctx->key, cmac_data->key, cmac_data->key_size);
+		memcpy(virt_ctx->din, cmac_data->data_in, cmac_data->data_in_size);
+
+		BUG_ON(cmac_data->direction != DRV_CRYPTO_DIRECTION_ENCRYPT);
+
+		FIPS_DBG("ssi_cmac_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_cmac_fips_run_test(drvdata,
+					    key_dma_addr,
+					    cmac_data->key_size,
+					    din_dma_addr,
+					    cmac_data->data_in_size,
+					    mac_res_dma_addr,
+					    cmac_data->mac_res_size);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_cmac_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = CC_REE_FIPS_ERROR_AES_CMAC_PUT;
+			break;
+		}
+
+		/* compare actual mac result to expected */
+		if (memcmp(virt_ctx->mac_res, cmac_data->mac_res, cmac_data->mac_res_size) != 0)
+		{
+			FIPS_LOG("comparison error %d - digest_size=%d \n", i, cmac_data->mac_res_size);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)cmac_data->mac_res, (size_t)virt_ctx->mac_res);
+			for (i = 0; i < cmac_data->mac_res_size; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, cmac_data->mac_res[i], virt_ctx->mac_res[i]);
+			}
+
+			error = CC_REE_FIPS_ERROR_AES_CMAC_PUT;
+			break;
+		}
+	}
+
+	return error;
+}
+
+
+static inline ssi_fips_error_t 
+FIPS_HashToFipsError(enum drv_hash_mode hash_mode)
+{
+	switch (hash_mode) {
+	case DRV_HASH_SHA1:
+		return CC_REE_FIPS_ERROR_SHA1_PUT;
+	case DRV_HASH_SHA256:
+		return CC_REE_FIPS_ERROR_SHA256_PUT;
+#if (CC_SUPPORT_SHA > 256)
+	case DRV_HASH_SHA512:
+		return CC_REE_FIPS_ERROR_SHA512_PUT;
+#endif
+	default:
+		return CC_REE_FIPS_ERROR_GENERAL;
+	}
+
+	return CC_REE_FIPS_ERROR_GENERAL;
+}
+
+static inline int 
+ssi_hash_fips_run_test(struct ssi_drvdata *drvdata,
+		       dma_addr_t initial_digest_dma_addr,
+		       dma_addr_t din_dma_addr,
+		       size_t data_in_size,
+		       dma_addr_t mac_res_dma_addr,
+		       enum drv_hash_mode hash_mode,
+		       enum drv_hash_hw_mode hw_mode,
+		       int digest_size,
+		       int inter_digestsize)
+{
+	/* max number of descriptors used for the flow */
+	#define FIPS_HASH_MAX_SEQ_LEN 4
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_HASH_MAX_SEQ_LEN];
+	int idx = 0;
+
+	/* Load initial digest */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, initial_digest_dma_addr, inter_digestsize, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Load the hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* data descriptor */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, data_in_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	if (unlikely((hash_mode == DRV_HASH_MD5) ||
+		     (hash_mode == DRV_HASH_SHA384) ||
+		     (hash_mode == DRV_HASH_SHA512))) {
+		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+	} else {
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	}
+	idx++;
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_HASH_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	return rc;
+}
+
+ssi_fips_error_t
+ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_hash_ctx *virt_ctx = (struct fips_hash_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers for initial_digest, din, mac_res */
+	dma_addr_t initial_digest_dma_addr = dma_coherent_buffer + offsetof(struct fips_hash_ctx, initial_digest);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_hash_ctx, din);
+	dma_addr_t mac_res_dma_addr = dma_coherent_buffer + offsetof(struct fips_hash_ctx, mac_res);
+
+	for (i = 0; i < FIPS_HASH_NUM_OF_TESTS; ++i)
+	{
+		FipsHashData *hash_data = (FipsHashData*)&FipsHashDataTable[i];
+		int rc = 0;
+		enum drv_hash_hw_mode hw_mode = 0;
+		int digest_size = 0;
+		int inter_digestsize = 0;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_hash_ctx));
+
+		switch (hash_data->hash_mode) {
+		case DRV_HASH_SHA1:
+			hw_mode = DRV_HASH_HW_SHA1;
+			digest_size = CC_SHA1_DIGEST_SIZE;
+			inter_digestsize = CC_SHA1_DIGEST_SIZE;
+			/* copy the initial digest into the allocated cache coherent buffer */
+			memcpy(virt_ctx->initial_digest, (void*)sha1_init, CC_SHA1_DIGEST_SIZE);
+			break;
+		case DRV_HASH_SHA256:
+			hw_mode = DRV_HASH_HW_SHA256;
+			digest_size = CC_SHA256_DIGEST_SIZE;
+			inter_digestsize = CC_SHA256_DIGEST_SIZE;
+			memcpy(virt_ctx->initial_digest, (void*)sha256_init, CC_SHA256_DIGEST_SIZE);
+			break;
+#if (CC_SUPPORT_SHA > 256)
+		case DRV_HASH_SHA512:
+			hw_mode = DRV_HASH_HW_SHA512;
+			digest_size = CC_SHA512_DIGEST_SIZE;
+			inter_digestsize = CC_SHA512_DIGEST_SIZE;
+			memcpy(virt_ctx->initial_digest, (void*)sha512_init, CC_SHA512_DIGEST_SIZE);
+			break;
+#endif
+		default:
+			error = FIPS_HashToFipsError(hash_data->hash_mode);
+			break;
+		}
+
+		/* copy the din data into the allocated buffer */
+		memcpy(virt_ctx->din, hash_data->data_in, hash_data->data_in_size);
+
+		/* run the test on HW */
+		FIPS_DBG("ssi_hash_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_hash_fips_run_test(drvdata,
+					    initial_digest_dma_addr,
+					    din_dma_addr,
+					    hash_data->data_in_size,
+					    mac_res_dma_addr,
+					    hash_data->hash_mode,
+					    hw_mode,
+					    digest_size,
+					    inter_digestsize);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_hash_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = FIPS_HashToFipsError(hash_data->hash_mode);
+			break;
+                }
+
+		/* compare actual mac result to expected */
+		if (memcmp(virt_ctx->mac_res, hash_data->mac_res, digest_size) != 0)
+		{
+			FIPS_LOG("comparison error %d - hash_mode=%d digest_size=%d \n", i, hash_data->hash_mode, digest_size);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)hash_data->mac_res, (size_t)virt_ctx->mac_res);
+			for (i = 0; i < digest_size; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, hash_data->mac_res[i], virt_ctx->mac_res[i]);
+			}
+
+			error = FIPS_HashToFipsError(hash_data->hash_mode);
+			break;
+                }
+	}
+
+	return error;
+}
+
+
+static inline ssi_fips_error_t 
+FIPS_HmacToFipsError(enum drv_hash_mode hash_mode)
+{
+	switch (hash_mode) {
+	case DRV_HASH_SHA1:
+		return CC_REE_FIPS_ERROR_HMAC_SHA1_PUT;
+	case DRV_HASH_SHA256:
+		return CC_REE_FIPS_ERROR_HMAC_SHA256_PUT;
+#if (CC_SUPPORT_SHA > 256)
+	case DRV_HASH_SHA512:
+		return CC_REE_FIPS_ERROR_HMAC_SHA512_PUT;
+#endif
+	default:
+		return CC_REE_FIPS_ERROR_GENERAL;
+	}
+
+	return CC_REE_FIPS_ERROR_GENERAL;
+}
+
+static inline int 
+ssi_hmac_fips_run_test(struct ssi_drvdata *drvdata,
+		       dma_addr_t initial_digest_dma_addr,
+		       dma_addr_t key_dma_addr,
+		       size_t key_size,
+		       dma_addr_t din_dma_addr,
+		       size_t data_in_size,
+		       dma_addr_t mac_res_dma_addr,
+		       enum drv_hash_mode hash_mode,
+		       enum drv_hash_hw_mode hw_mode,
+		       size_t digest_size,
+		       size_t inter_digestsize,
+		       size_t block_size,
+		       dma_addr_t k0_dma_addr,
+		       dma_addr_t tmp_digest_dma_addr,
+		       dma_addr_t digest_bytes_len_dma_addr)
+{
+	/* The implemented flow is not the same as the one implemented in ssi_hash.c (setkey + digest flows).
+	   In this flow, there is no need to store and reload some of the intermidiate results. */
+
+	/* max number of descriptors used for the flow */
+	#define FIPS_HMAC_MAX_SEQ_LEN 12
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_HMAC_MAX_SEQ_LEN];
+	int idx = 0;
+	int i;
+	/* calc the hash opad first and ipad only afterwards (unlike the flow in ssi_hash.c) */
+	unsigned int hmacPadConst[2] = { HMAC_OPAD_CONST, HMAC_IPAD_CONST };
+
+	// assume (key_size <= block_size)
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr, key_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, key_size, NS_BIT, 0);
+	idx++;
+
+	// if needed, append Key with zeros to create K0
+	if ((block_size - key_size) != 0) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, (block_size - key_size));
+		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+				      (k0_dma_addr + key_size), (block_size - key_size),
+				      NS_BIT, 0);
+		idx++;
+	}
+
+	BUG_ON(idx > FIPS_HMAC_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, 0);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		return rc;
+	}
+	idx = 0;
+
+	/* calc derived HMAC key */
+	for (i = 0; i < 2; i++) {
+		/* Load hash initial state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, initial_digest_dma_addr, inter_digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+
+		/* Load the hash current length*/
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Prepare opad/ipad key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		idx++;
+
+		/* Perform HASH update */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				     k0_dma_addr,
+				     block_size, NS_BIT);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx],hw_mode);
+		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+
+		if (i == 0) {
+			/* First iteration - calc H(K0^opad) into tmp_digest_dma_addr */
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+			HW_DESC_SET_DOUT_DLLI(&desc[idx],
+					      tmp_digest_dma_addr,
+					      inter_digestsize,
+					      NS_BIT, 0);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+			idx++;
+
+			// is this needed?? or continue with current descriptors??
+			BUG_ON(idx > FIPS_HMAC_MAX_SEQ_LEN);
+			rc = send_request(drvdata, &ssi_req, desc, idx, 0);
+			if (unlikely(rc != 0)) {
+				SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+				return rc;
+			}
+			idx = 0;
+		}
+	}
+
+	/* data descriptor */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+			     din_dma_addr, data_in_size,
+			     NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	/* HW last hash block padding (aka. "DO_PAD") */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+	idx++;
+
+	/* store the hash digest result in the context */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], k0_dma_addr, digest_size, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	if (unlikely((hash_mode == DRV_HASH_MD5) ||
+		     (hash_mode == DRV_HASH_SHA384) ||
+		     (hash_mode == DRV_HASH_SHA512))) {
+		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+	} else {
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	}
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	idx++;
+
+	/* at this point:
+	   tmp_digest = H(o_key_pad)
+	   k0 = H(i_key_pad || m)
+	   */
+
+	/* Loading hash opad xor key state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, tmp_digest_dma_addr, inter_digestsize, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Load the hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* Perform HASH update */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, k0_dma_addr, digest_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, digest_size, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	if (unlikely((hash_mode == DRV_HASH_MD5) ||
+		     (hash_mode == DRV_HASH_SHA384) ||
+		     (hash_mode == DRV_HASH_SHA512))) {
+		HW_DESC_SET_BYTES_SWAP(&desc[idx], 1);
+	} else {
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	}
+	idx++;
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_HMAC_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	return rc;
+}
+
+ssi_fips_error_t
+ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_hmac_ctx *virt_ctx = (struct fips_hmac_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers */
+	dma_addr_t initial_digest_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, initial_digest);
+	dma_addr_t key_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, key);
+	dma_addr_t k0_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, k0);
+	dma_addr_t tmp_digest_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, tmp_digest);
+	dma_addr_t digest_bytes_len_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, digest_bytes_len);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, din);
+	dma_addr_t mac_res_dma_addr = dma_coherent_buffer + offsetof(struct fips_hmac_ctx, mac_res);
+
+	for (i = 0; i < FIPS_HMAC_NUM_OF_TESTS; ++i)
+	{
+		FipsHmacData *hmac_data = (FipsHmacData*)&FipsHmacDataTable[i];
+		int rc = 0;
+		enum drv_hash_hw_mode hw_mode = 0;
+		int digest_size = 0;
+		int block_size = 0;
+		int inter_digestsize = 0;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_hmac_ctx));
+
+		switch (hmac_data->hash_mode) {
+		case DRV_HASH_SHA1:
+			hw_mode = DRV_HASH_HW_SHA1;
+			digest_size = CC_SHA1_DIGEST_SIZE;
+			block_size = CC_SHA1_BLOCK_SIZE;
+			inter_digestsize = CC_SHA1_DIGEST_SIZE;
+			memcpy(virt_ctx->initial_digest, (void*)sha1_init, CC_SHA1_DIGEST_SIZE);
+			memcpy(virt_ctx->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
+			break;
+		case DRV_HASH_SHA256:
+			hw_mode = DRV_HASH_HW_SHA256;
+			digest_size = CC_SHA256_DIGEST_SIZE;
+			block_size = CC_SHA256_BLOCK_SIZE;
+			inter_digestsize = CC_SHA256_DIGEST_SIZE;
+			memcpy(virt_ctx->initial_digest, (void*)sha256_init, CC_SHA256_DIGEST_SIZE);
+			memcpy(virt_ctx->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
+			break;
+#if (CC_SUPPORT_SHA > 256)
+		case DRV_HASH_SHA512:
+			hw_mode = DRV_HASH_HW_SHA512;
+			digest_size = CC_SHA512_DIGEST_SIZE;
+			block_size = CC_SHA512_BLOCK_SIZE;
+			inter_digestsize = CC_SHA512_DIGEST_SIZE;
+			memcpy(virt_ctx->initial_digest, (void*)sha512_init, CC_SHA512_DIGEST_SIZE);
+			memcpy(virt_ctx->digest_bytes_len, digest_len_sha512_init, HASH_LEN_SIZE);
+			break;
+#endif
+		default:
+			error = FIPS_HmacToFipsError(hmac_data->hash_mode);
+			break;
+		}
+
+		/* copy into the allocated buffer */
+		memcpy(virt_ctx->key, hmac_data->key, hmac_data->key_size);
+		memcpy(virt_ctx->din, hmac_data->data_in, hmac_data->data_in_size);
+
+		/* run the test on HW */
+		FIPS_DBG("ssi_hmac_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_hmac_fips_run_test(drvdata,
+					    initial_digest_dma_addr,
+					    key_dma_addr,
+					    hmac_data->key_size,
+					    din_dma_addr,
+					    hmac_data->data_in_size,
+					    mac_res_dma_addr,
+					    hmac_data->hash_mode,
+					    hw_mode,
+					    digest_size,
+					    inter_digestsize,
+					    block_size,
+					    k0_dma_addr,
+					    tmp_digest_dma_addr,
+					    digest_bytes_len_dma_addr);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_hmac_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = FIPS_HmacToFipsError(hmac_data->hash_mode);
+			break;
+		}
+
+		/* compare actual mac result to expected */
+		if (memcmp(virt_ctx->mac_res, hmac_data->mac_res, digest_size) != 0)
+		{
+			FIPS_LOG("comparison error %d - hash_mode=%d digest_size=%d \n", i, hmac_data->hash_mode, digest_size);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)hmac_data->mac_res, (size_t)virt_ctx->mac_res);
+			for (i = 0; i < digest_size; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, hmac_data->mac_res[i], virt_ctx->mac_res[i]);
+			}
+
+			error = FIPS_HmacToFipsError(hmac_data->hash_mode);
+			break;
+		}
+	}
+
+	return error;
+}
+
+
+static inline int 
+ssi_ccm_fips_run_test(struct ssi_drvdata *drvdata,
+		      enum drv_crypto_direction direction,
+		      dma_addr_t key_dma_addr,
+		      size_t key_size,
+		      dma_addr_t iv_dma_addr,
+		      dma_addr_t ctr_cnt_0_dma_addr,
+		      dma_addr_t b0_a0_adata_dma_addr,
+		      size_t b0_a0_adata_size,
+		      dma_addr_t din_dma_addr,
+		      size_t din_size,
+		      dma_addr_t dout_dma_addr,
+		      dma_addr_t mac_res_dma_addr)
+{
+	/* max number of descriptors used for the flow */
+	#define FIPS_CCM_MAX_SEQ_LEN 10
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_CCM_MAX_SEQ_LEN];
+	unsigned int idx = 0;
+	unsigned int cipher_flow_mode;
+
+	if (direction == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		cipher_flow_mode = AES_to_HASH_and_DOUT;
+	} else { /* Encrypt */
+		cipher_flow_mode = AES_and_HASH;
+	}
+
+	/* load key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
+			     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ? CC_AES_KEY_SIZE_MAX : key_size),
+			     NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* load ctr state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     iv_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* load MAC key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, key_dma_addr,
+			     ((key_size == NIST_AESCCM_192_BIT_KEY_SIZE) ? CC_AES_KEY_SIZE_MAX : key_size),
+			     NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* load MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* prcess assoc data */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, b0_a0_adata_dma_addr, b0_a0_adata_size, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	/* process the cipher */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, din_dma_addr, din_size, NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], dout_dma_addr, din_size, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], cipher_flow_mode);
+	idx++;
+
+	/* Read temporal MAC */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CBC_MAC);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT, 0);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++;
+
+	/* load AES-CTR state (for last MAC calculation)*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CTR);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     ctr_cnt_0_dma_addr,
+			     AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Memory Barrier */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* encrypt the "T" value and store MAC inplace */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], mac_res_dma_addr, NIST_AESCCM_TAG_SIZE, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;	
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_CCM_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	return rc;
+}
+
+ssi_fips_error_t
+ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_ccm_ctx *virt_ctx = (struct fips_ccm_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers */
+	dma_addr_t b0_a0_adata_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, b0_a0_adata);
+	dma_addr_t iv_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, iv);
+	dma_addr_t ctr_cnt_0_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, ctr_cnt_0);
+	dma_addr_t key_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, key);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, din);
+	dma_addr_t dout_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, dout);
+	dma_addr_t mac_res_dma_addr = dma_coherent_buffer + offsetof(struct fips_ccm_ctx, mac_res);
+
+	for (i = 0; i < FIPS_CCM_NUM_OF_TESTS; ++i)
+	{
+		FipsCcmData *ccmData = (FipsCcmData*)&FipsCcmDataTable[i];
+		int rc = 0;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_ccm_ctx));
+
+		/* copy the nonce, key, adata, din data into the allocated buffer */
+		memcpy(virt_ctx->key, ccmData->key, ccmData->keySize);
+		memcpy(virt_ctx->din, ccmData->dataIn, ccmData->dataInSize);
+		{
+			/* build B0 -- B0, nonce, l(m) */
+			__be16 data = cpu_to_be16(NIST_AESCCM_TEXT_SIZE);
+			virt_ctx->b0_a0_adata[0] = NIST_AESCCM_B0_VAL;
+			memcpy(virt_ctx->b0_a0_adata + 1, ccmData->nonce, NIST_AESCCM_NONCE_SIZE);
+			memcpy(virt_ctx->b0_a0_adata + 14, (u8 *)&data, sizeof(__be16));
+			/* build A0+ADATA */
+			virt_ctx->b0_a0_adata[NIST_AESCCM_IV_SIZE + 0] = (ccmData->adataSize >> 8) & 0xFF;
+			virt_ctx->b0_a0_adata[NIST_AESCCM_IV_SIZE + 1] = ccmData->adataSize & 0xFF;
+			memcpy(virt_ctx->b0_a0_adata + NIST_AESCCM_IV_SIZE + 2, ccmData->adata, ccmData->adataSize);
+			/* iv */
+			virt_ctx->iv[0] = 1; /* L' */
+			memcpy(virt_ctx->iv + 1, ccmData->nonce, NIST_AESCCM_NONCE_SIZE);
+			virt_ctx->iv[15] = 1;
+			/* ctr_count_0 */
+			memcpy(virt_ctx->ctr_cnt_0, virt_ctx->iv, NIST_AESCCM_IV_SIZE);
+			virt_ctx->ctr_cnt_0[15] = 0;
+		}
+
+		FIPS_DBG("ssi_ccm_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_ccm_fips_run_test(drvdata,
+					   ccmData->direction,
+					   key_dma_addr,
+					   ccmData->keySize,
+					   iv_dma_addr,
+					   ctr_cnt_0_dma_addr,
+					   b0_a0_adata_dma_addr,
+					   FIPS_CCM_B0_A0_ADATA_SIZE,
+					   din_dma_addr,
+					   ccmData->dataInSize,
+					   dout_dma_addr,
+					   mac_res_dma_addr);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_ccm_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = CC_REE_FIPS_ERROR_AESCCM_PUT;
+			break;
+		}
+
+		/* compare actual dout to expected */
+		if (memcmp(virt_ctx->dout, ccmData->dataOut, ccmData->dataInSize) != 0)
+		{
+			FIPS_LOG("dout comparison error %d - size=%d \n", i, ccmData->dataInSize);
+                        error = CC_REE_FIPS_ERROR_AESCCM_PUT;
+			break;
+                }
+
+		/* compare actual mac result to expected */
+		if (memcmp(virt_ctx->mac_res, ccmData->macResOut, ccmData->tagSize) != 0)
+		{
+			FIPS_LOG("mac_res comparison error %d - mac_size=%d \n", i, ccmData->tagSize);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)ccmData->macResOut, (size_t)virt_ctx->mac_res);
+			for (i = 0; i < ccmData->tagSize; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, ccmData->macResOut[i], virt_ctx->mac_res[i]);
+			}
+
+			error = CC_REE_FIPS_ERROR_AESCCM_PUT;
+			break;
+		}
+	}
+
+	return error;
+}
+
+
+static inline int
+ssi_gcm_fips_run_test(struct ssi_drvdata *drvdata,
+		      enum drv_crypto_direction direction,
+		      dma_addr_t key_dma_addr,
+		      size_t key_size,
+		      dma_addr_t hkey_dma_addr,
+		      dma_addr_t block_len_dma_addr,
+		      dma_addr_t iv_inc1_dma_addr,
+		      dma_addr_t iv_inc2_dma_addr,
+		      dma_addr_t adata_dma_addr,
+		      size_t adata_size,
+		      dma_addr_t din_dma_addr,
+		      size_t din_size,
+		      dma_addr_t dout_dma_addr,
+		      dma_addr_t mac_res_dma_addr)
+{
+	/* max number of descriptors used for the flow */
+	#define FIPS_GCM_MAX_SEQ_LEN 15
+
+	int rc;
+	struct ssi_crypto_req ssi_req = {0};
+	HwDesc_s desc[FIPS_GCM_MAX_SEQ_LEN];
+	unsigned int idx = 0;
+	unsigned int cipher_flow_mode;
+
+	if (direction == DRV_CRYPTO_DIRECTION_DECRYPT) {
+		cipher_flow_mode = AES_and_HASH;
+	} else { /* Encrypt */
+		cipher_flow_mode = AES_to_HASH_and_DOUT;
+	}
+
+/////////////////////////////////   1   ////////////////////////////////////
+//	ssi_aead_gcm_setup_ghash_desc(req, desc, seq_size);
+/////////////////////////////////   1   ////////////////////////////////////
+
+	/* load key to AES*/
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);	
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx],
+			     DMA_DLLI, key_dma_addr, key_size,
+			     NS_BIT); 
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* process one zero block to generate hkey */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx],
+			      hkey_dma_addr, AES_BLOCK_SIZE,
+			      NS_BIT, 0); 
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;
+
+	/* Memory Barrier */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* Load GHASH subkey */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     hkey_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Configure Hash Engine to work with GHASH.
+	   Since it was not possible to extend HASH submodes to add GHASH,
+	   The following command is necessary in order to select GHASH (according to HW designers)*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);	
+	HW_DESC_SET_CIPHER_DO(&desc[idx], 1); //1=AES_SK RKEK
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	/* Load GHASH initial STATE (which is 0). (for any hash there is an initial state) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x0, AES_BLOCK_SIZE);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED); 
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+
+
+/////////////////////////////////   2   ////////////////////////////////////
+	/* prcess(ghash) assoc data */
+//	if (req->assoclen > 0)
+//		ssi_aead_create_assoc_desc(req, DIN_HASH, desc, seq_size);
+/////////////////////////////////   2   ////////////////////////////////////
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+			     adata_dma_addr, adata_size,
+			     NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+
+/////////////////////////////////   3   ////////////////////////////////////
+//	ssi_aead_gcm_setup_gctr_desc(req, desc, seq_size);
+/////////////////////////////////   3   ////////////////////////////////////
+
+	/* load key to AES*/
+	HW_DESC_INIT(&desc[idx]);	
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);	
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     key_dma_addr, key_size,
+			     NS_BIT); 
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* load AES/CTR initial CTR value inc by 2*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     iv_inc2_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+
+/////////////////////////////////   4   ////////////////////////////////////
+	/* process(gctr+ghash) */
+//	if (req_ctx->cryptlen != 0)
+//		ssi_aead_process_cipher_data_desc(req, cipher_flow_mode, desc, seq_size); 
+/////////////////////////////////   4   ////////////////////////////////////
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     din_dma_addr, din_size,
+			     NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx],
+			      dout_dma_addr, din_size,
+			      NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], cipher_flow_mode);
+	idx++;
+
+
+/////////////////////////////////   5   ////////////////////////////////////
+//	ssi_aead_process_gcm_result_desc(req, desc, seq_size);
+/////////////////////////////////   5   ////////////////////////////////////
+
+	/* prcess(ghash) gcm_block_len */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+			     block_len_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+	idx++;
+
+	/* Store GHASH state after GHASH(Associated Data + Cipher +LenBlock) */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_HASH_HW_GHASH);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx],
+			      mac_res_dma_addr, AES_BLOCK_SIZE,
+			      NS_BIT, 0);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_AES_NOT_HASH_MODE(&desc[idx]);
+	idx++; 
+
+	/* load AES/CTR initial CTR value inc by 1*/
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_size);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     iv_inc1_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);	
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Memory Barrier */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+	idx++;
+
+	/* process GCTR on stored GHASH and store MAC inplace */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_GCTR);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+			     mac_res_dma_addr, AES_BLOCK_SIZE,
+			     NS_BIT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx],
+			      mac_res_dma_addr, AES_BLOCK_SIZE,
+			      NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	idx++;
+
+	/* perform the operation - Lock HW and push sequence */
+	BUG_ON(idx > FIPS_GCM_MAX_SEQ_LEN);
+	rc = send_request(drvdata, &ssi_req, desc, idx, false);
+
+	return rc;
+}
+
+ssi_fips_error_t
+ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer)
+{
+	ssi_fips_error_t error = CC_REE_FIPS_ERROR_OK;
+	size_t i;
+	struct fips_gcm_ctx *virt_ctx = (struct fips_gcm_ctx *)cpu_addr_buffer;
+
+	/* set the phisical pointers */
+	dma_addr_t adata_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, adata);
+	dma_addr_t key_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, key);
+	dma_addr_t hkey_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, hkey);
+	dma_addr_t din_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, din);
+	dma_addr_t dout_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, dout);
+	dma_addr_t mac_res_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, mac_res);
+	dma_addr_t len_block_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, len_block);
+	dma_addr_t iv_inc1_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, iv_inc1);
+	dma_addr_t iv_inc2_dma_addr = dma_coherent_buffer + offsetof(struct fips_gcm_ctx, iv_inc2);
+
+	for (i = 0; i < FIPS_GCM_NUM_OF_TESTS; ++i)
+	{
+		FipsGcmData *gcmData = (FipsGcmData*)&FipsGcmDataTable[i];
+		int rc = 0;
+
+		memset(cpu_addr_buffer, 0, sizeof(struct fips_gcm_ctx));
+
+		/* copy the key, adata, din data - into the allocated buffer */
+		memcpy(virt_ctx->key, gcmData->key, gcmData->keySize);
+		memcpy(virt_ctx->adata, gcmData->adata, gcmData->adataSize);
+		memcpy(virt_ctx->din, gcmData->dataIn, gcmData->dataInSize);
+
+		/* len_block */
+		{
+			__be64 len_bits;
+			len_bits = cpu_to_be64(gcmData->adataSize * 8);
+			memcpy(virt_ctx->len_block, &len_bits, sizeof(len_bits));
+			len_bits = cpu_to_be64(gcmData->dataInSize * 8);
+			memcpy(virt_ctx->len_block + 8, &len_bits, sizeof(len_bits));
+		}
+		/* iv_inc1, iv_inc2 */
+		{
+			__be32 counter = cpu_to_be32(1);
+			memcpy(virt_ctx->iv_inc1, gcmData->iv, NIST_AESGCM_IV_SIZE);
+			memcpy(virt_ctx->iv_inc1 + NIST_AESGCM_IV_SIZE, &counter, sizeof(counter));
+			counter = cpu_to_be32(2);
+			memcpy(virt_ctx->iv_inc2, gcmData->iv, NIST_AESGCM_IV_SIZE);
+			memcpy(virt_ctx->iv_inc2 + NIST_AESGCM_IV_SIZE, &counter, sizeof(counter));
+		}
+
+		FIPS_DBG("ssi_gcm_fips_run_test -  (i = %d) \n", i);
+		rc = ssi_gcm_fips_run_test(drvdata,
+					   gcmData->direction,
+					   key_dma_addr,
+					   gcmData->keySize,
+					   hkey_dma_addr,
+					   len_block_dma_addr,
+					   iv_inc1_dma_addr,
+					   iv_inc2_dma_addr,
+					   adata_dma_addr,
+					   gcmData->adataSize,
+					   din_dma_addr,
+					   gcmData->dataInSize,
+					   dout_dma_addr,
+					   mac_res_dma_addr);
+		if (rc != 0)
+		{
+			FIPS_LOG("ssi_gcm_fips_run_test %d returned error - rc = %d \n", i, rc);
+			error = CC_REE_FIPS_ERROR_AESGCM_PUT;
+			break;
+		}
+
+		if (gcmData->direction == DRV_CRYPTO_DIRECTION_ENCRYPT) {
+			/* compare actual dout to expected */
+			if (memcmp(virt_ctx->dout, gcmData->dataOut, gcmData->dataInSize) != 0)
+			{
+				FIPS_LOG("dout comparison error %d - size=%d \n", i, gcmData->dataInSize);
+				FIPS_LOG("  i  expected   received \n");
+				FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)gcmData->dataOut, (size_t)virt_ctx->dout);
+				for (i = 0; i < gcmData->dataInSize; ++i)
+				{
+					FIPS_LOG("  %d    0x%02x     0x%02x \n", i, gcmData->dataOut[i], virt_ctx->dout[i]);
+				}
+
+				error = CC_REE_FIPS_ERROR_AESGCM_PUT;
+				break;
+			}
+		}
+
+		/* compare actual mac result to expected */
+		if (memcmp(virt_ctx->mac_res, gcmData->macResOut, gcmData->tagSize) != 0)
+		{
+			FIPS_LOG("mac_res comparison error %d - mac_size=%d \n", i, gcmData->tagSize);
+			FIPS_LOG("  i  expected   received \n");
+			FIPS_LOG("  i  0x%08x 0x%08x \n", (size_t)gcmData->macResOut, (size_t)virt_ctx->mac_res);
+			for (i = 0; i < gcmData->tagSize; ++i)
+			{
+				FIPS_LOG("  %d    0x%02x     0x%02x \n", i, gcmData->macResOut[i], virt_ctx->mac_res[i]);
+			}
+
+			error = CC_REE_FIPS_ERROR_AESGCM_PUT;
+			break;
+		}
+	}
+	return error;
+}
+
+
+size_t ssi_fips_max_mem_alloc_size(void)
+{
+	FIPS_DBG("sizeof(struct fips_cipher_ctx) %d \n", sizeof(struct fips_cipher_ctx));
+	FIPS_DBG("sizeof(struct fips_cmac_ctx) %d \n", sizeof(struct fips_cmac_ctx));
+	FIPS_DBG("sizeof(struct fips_hash_ctx) %d \n", sizeof(struct fips_hash_ctx));
+	FIPS_DBG("sizeof(struct fips_hmac_ctx) %d \n", sizeof(struct fips_hmac_ctx));
+	FIPS_DBG("sizeof(struct fips_ccm_ctx) %d \n", sizeof(struct fips_ccm_ctx));
+	FIPS_DBG("sizeof(struct fips_gcm_ctx) %d \n", sizeof(struct fips_gcm_ctx));
+
+	return sizeof(fips_ctx);
+}
+
diff --git a/drivers/staging/ccree/ssi_fips_local.c b/drivers/staging/ccree/ssi_fips_local.c
new file mode 100644
index 000000000000..51b535a2a09a
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips_local.c
@@ -0,0 +1,369 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/**************************************************************
+This file defines the driver FIPS internal function, used by the driver itself.
+***************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <crypto/des.h>
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "cc_hal.h"
+
+
+#define FIPS_POWER_UP_TEST_CIPHER	1
+#define FIPS_POWER_UP_TEST_CMAC		1
+#define FIPS_POWER_UP_TEST_HASH		1
+#define FIPS_POWER_UP_TEST_HMAC		1
+#define FIPS_POWER_UP_TEST_CCM		1
+#define FIPS_POWER_UP_TEST_GCM		1
+
+static bool ssi_fips_support = 1;
+module_param(ssi_fips_support, bool, 0644);
+MODULE_PARM_DESC(ssi_fips_support, "FIPS supported flag: 0 - off , 1 - on (default)");
+
+static void fips_dsr(unsigned long devarg);
+
+struct ssi_fips_handle {
+#ifdef COMP_IN_WQ
+	struct workqueue_struct *workq;
+	struct delayed_work fipswork;
+#else
+	struct tasklet_struct fipstask;
+#endif
+};
+
+
+extern int ssi_fips_get_state(ssi_fips_state_t *p_state);
+extern int ssi_fips_get_error(ssi_fips_error_t *p_err);
+extern int ssi_fips_ext_set_state(ssi_fips_state_t state);
+extern int ssi_fips_ext_set_error(ssi_fips_error_t err);
+
+/* FIPS power-up tests */
+extern ssi_fips_error_t ssi_cipher_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern ssi_fips_error_t ssi_cmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern ssi_fips_error_t ssi_hash_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern ssi_fips_error_t ssi_hmac_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern ssi_fips_error_t ssi_ccm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern ssi_fips_error_t ssi_gcm_fips_power_up_tests(struct ssi_drvdata *drvdata, void *cpu_addr_buffer, dma_addr_t dma_coherent_buffer);
+extern size_t ssi_fips_max_mem_alloc_size(void);
+
+
+/* The function called once at driver entry point to check whether TEE FIPS error occured.*/
+static enum ssi_fips_error ssi_fips_get_tee_error(struct ssi_drvdata *drvdata)
+{
+	uint32_t regVal;
+	void __iomem *cc_base = drvdata->cc_base;
+
+	regVal = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, GPR_HOST));
+	if (regVal == (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK)) {
+		return CC_REE_FIPS_ERROR_OK;
+	} 
+	return CC_REE_FIPS_ERROR_FROM_TEE;
+}
+
+
+/* 
+ This function should push the FIPS REE library status towards the TEE library.
+ By writing the error state to HOST_GPR0 register. The function is called from  						.
+ driver entry point so no need to protect by mutex.
+*/
+static void ssi_fips_update_tee_upon_ree_status(struct ssi_drvdata *drvdata, ssi_fips_error_t err)
+{
+	void __iomem *cc_base = drvdata->cc_base;
+	if (err == CC_REE_FIPS_ERROR_OK) {
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS|CC_FIPS_SYNC_MODULE_OK));
+	} else {
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_GPR0), (CC_FIPS_SYNC_REE_STATUS|CC_FIPS_SYNC_MODULE_ERROR));
+	}
+}
+
+
+
+void ssi_fips_fini(struct ssi_drvdata *drvdata)
+{
+	struct ssi_fips_handle *fips_h = drvdata->fips_handle;
+
+	if (fips_h == NULL)
+		return; /* Not allocated */
+
+#ifdef COMP_IN_WQ
+	if (fips_h->workq != NULL) {
+		flush_workqueue(fips_h->workq);
+		destroy_workqueue(fips_h->workq);
+	}
+#else
+	/* Kill tasklet */
+	tasklet_kill(&fips_h->fipstask);
+#endif
+	memset(fips_h, 0, sizeof(struct ssi_fips_handle));
+	kfree(fips_h);
+	drvdata->fips_handle = NULL;
+}
+
+void fips_handler(struct ssi_drvdata *drvdata)
+{
+	struct ssi_fips_handle *fips_handle_ptr = 
+						drvdata->fips_handle;
+#ifdef COMP_IN_WQ
+	queue_delayed_work(fips_handle_ptr->workq, &fips_handle_ptr->fipswork, 0);
+#else
+	tasklet_schedule(&fips_handle_ptr->fipstask);
+#endif
+}
+
+
+
+#ifdef COMP_IN_WQ
+static void fips_wq_handler(struct work_struct *work)
+{
+	struct ssi_drvdata *drvdata =
+		container_of(work, struct ssi_drvdata, fipswork.work);
+
+	fips_dsr((unsigned long)drvdata);
+}
+#endif
+
+/* Deferred service handler, run as interrupt-fired tasklet */
+static void fips_dsr(unsigned long devarg)
+{
+	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)devarg;
+	void __iomem *cc_base = drvdata->cc_base;
+	uint32_t irq;
+	uint32_t teeFipsError = 0;
+
+	irq = (drvdata->irq & (SSI_GPR0_IRQ_MASK));
+
+	if (irq & SSI_GPR0_IRQ_MASK) {
+		teeFipsError = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, GPR_HOST));
+		if (teeFipsError != (CC_FIPS_SYNC_TEE_STATUS | CC_FIPS_SYNC_MODULE_OK)) {
+			ssi_fips_set_error(drvdata, CC_REE_FIPS_ERROR_FROM_TEE);
+		} 
+	}
+
+	/* after verifing that there is nothing to do, Unmask AXI completion interrupt */
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), 
+		CC_HAL_READ_REGISTER(
+		CC_REG_OFFSET(HOST_RGF, HOST_IMR)) & ~irq);
+}
+
+
+ssi_fips_error_t cc_fips_run_power_up_tests(struct ssi_drvdata *drvdata)
+{
+	ssi_fips_error_t fips_error = CC_REE_FIPS_ERROR_OK;
+	void * cpu_addr_buffer = NULL;
+	dma_addr_t dma_handle;
+	size_t alloc_buff_size = ssi_fips_max_mem_alloc_size();
+	struct device *dev = &drvdata->plat_dev->dev;
+
+	// allocate memory using dma_alloc_coherent - for phisical, consecutive and cache coherent buffer (memory map is not needed)
+	// the return value is the virtual address - use it to copy data into the buffer
+	// the dma_handle is the returned phy address - use it in the HW descriptor
+	FIPS_DBG("dma_alloc_coherent \n");
+	cpu_addr_buffer = dma_alloc_coherent(dev, alloc_buff_size, &dma_handle, GFP_KERNEL);
+	if (cpu_addr_buffer == NULL) {
+		return CC_REE_FIPS_ERROR_GENERAL;
+	}
+	FIPS_DBG("allocated coherent buffer - addr 0x%08X , size = %d \n", (size_t)cpu_addr_buffer, alloc_buff_size);
+
+#if FIPS_POWER_UP_TEST_CIPHER
+	FIPS_DBG("ssi_cipher_fips_power_up_tests ...\n");
+	fips_error = ssi_cipher_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+	FIPS_DBG("ssi_cipher_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+#endif
+#if FIPS_POWER_UP_TEST_CMAC
+	if (likely(fips_error == CC_REE_FIPS_ERROR_OK)) {
+		FIPS_DBG("ssi_cmac_fips_power_up_tests ...\n");
+		fips_error = ssi_cmac_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+		FIPS_DBG("ssi_cmac_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+	}
+#endif
+#if FIPS_POWER_UP_TEST_HASH
+	if (likely(fips_error == CC_REE_FIPS_ERROR_OK)) {
+		FIPS_DBG("ssi_hash_fips_power_up_tests ...\n");
+		fips_error = ssi_hash_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+		FIPS_DBG("ssi_hash_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+	}
+#endif
+#if FIPS_POWER_UP_TEST_HMAC
+	if (likely(fips_error == CC_REE_FIPS_ERROR_OK)) {
+		FIPS_DBG("ssi_hmac_fips_power_up_tests ...\n");
+		fips_error = ssi_hmac_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+		FIPS_DBG("ssi_hmac_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+	}
+#endif
+#if FIPS_POWER_UP_TEST_CCM
+	if (likely(fips_error == CC_REE_FIPS_ERROR_OK)) {
+		FIPS_DBG("ssi_ccm_fips_power_up_tests ...\n");
+		fips_error = ssi_ccm_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+		FIPS_DBG("ssi_ccm_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+	}
+#endif
+#if FIPS_POWER_UP_TEST_GCM
+	if (likely(fips_error == CC_REE_FIPS_ERROR_OK)) {
+		FIPS_DBG("ssi_gcm_fips_power_up_tests ...\n");
+		fips_error = ssi_gcm_fips_power_up_tests(drvdata, cpu_addr_buffer, dma_handle);
+		FIPS_DBG("ssi_gcm_fips_power_up_tests - done. (fips_error = %d) \n", fips_error);
+	}
+#endif
+	/* deallocate the buffer when all tests are done... */
+	FIPS_DBG("dma_free_coherent \n");
+	dma_free_coherent(dev, alloc_buff_size, cpu_addr_buffer, dma_handle);
+
+	return fips_error;
+}
+
+
+
+/* The function checks if FIPS supported and FIPS error exists.* 
+*  It should be used in every driver API.*/
+int ssi_fips_check_fips_error(void)
+{
+	ssi_fips_state_t fips_state; 
+
+	if (ssi_fips_get_state(&fips_state) != 0) {
+		FIPS_LOG("ssi_fips_get_state FAILED, returning.. \n");
+		return -ENOEXEC;
+	}
+	if (fips_state == CC_FIPS_STATE_ERROR) {
+		FIPS_LOG("ssi_fips_get_state: fips_state is %d, returning.. \n", fips_state);
+		return -ENOEXEC;
+	}
+	return 0;
+}
+
+
+/* The function sets the REE FIPS state.* 
+*  It should be used while driver is being loaded .*/
+int ssi_fips_set_state(ssi_fips_state_t state)
+{
+	return ssi_fips_ext_set_state(state);
+}
+
+/* The function sets the REE FIPS error, and pushes the error to TEE library. * 
+*  It should be used when any of the KAT tests fails .*/
+int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, ssi_fips_error_t err)
+{
+	int rc = 0;
+        ssi_fips_error_t current_err;
+
+        FIPS_LOG("ssi_fips_set_error - fips_error = %d \n", err);
+
+	// setting no error is not allowed
+	if (err == CC_REE_FIPS_ERROR_OK) {
+                return -ENOEXEC;
+	} 
+        // If error exists, do not set new error
+        if (ssi_fips_get_error(&current_err) != 0) {
+                return -ENOEXEC;
+        }
+        if (current_err != CC_REE_FIPS_ERROR_OK) {
+                return -ENOEXEC;
+        }
+        // set REE internal error and state
+	rc = ssi_fips_ext_set_error(err);
+	if (rc != 0) {
+                return -ENOEXEC;
+	}
+	rc = ssi_fips_ext_set_state(CC_FIPS_STATE_ERROR);
+	if (rc != 0) {
+                return -ENOEXEC;
+	}
+
+        // push error towards TEE libraray, if it's not TEE error
+	if (err != CC_REE_FIPS_ERROR_FROM_TEE) {
+		ssi_fips_update_tee_upon_ree_status(p_drvdata, err);
+	}
+	return rc;
+}
+
+
+/* The function called once at driver entry point .*/
+int ssi_fips_init(struct ssi_drvdata *p_drvdata)
+{
+	ssi_fips_error_t rc = CC_REE_FIPS_ERROR_OK;
+	struct ssi_fips_handle *fips_h;
+
+	FIPS_DBG("CC FIPS code ..  (fips=%d) \n", ssi_fips_support);
+
+	fips_h = kzalloc(sizeof(struct ssi_fips_handle),GFP_KERNEL);
+	if (fips_h == NULL) {
+		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
+		return -ENOMEM;
+	}
+
+	p_drvdata->fips_handle = fips_h;
+
+#ifdef COMP_IN_WQ
+	SSI_LOG_DEBUG("Initializing fips workqueue\n");
+	fips_h->workq = create_singlethread_workqueue("arm_cc7x_fips_wq");
+	if (unlikely(fips_h->workq == NULL)) {
+		SSI_LOG_ERR("Failed creating fips work queue\n");
+		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
+		rc = -ENOMEM;
+		goto ssi_fips_init_err;
+	}
+	INIT_DELAYED_WORK(&fips_h->fipswork, fips_wq_handler);
+#else
+	SSI_LOG_DEBUG("Initializing fips tasklet\n");
+	tasklet_init(&fips_h->fipstask, fips_dsr, (unsigned long)p_drvdata);
+#endif
+
+	/* init fips driver data */
+	rc = ssi_fips_set_state((ssi_fips_support == 0)? CC_FIPS_STATE_NOT_SUPPORTED : CC_FIPS_STATE_SUPPORTED);
+	if (unlikely(rc != 0)) {
+		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
+		rc = -EAGAIN;
+		goto ssi_fips_init_err;
+	}
+
+	/* Run power up tests (before registration and operating the HW engines) */
+	FIPS_DBG("ssi_fips_get_tee_error \n");
+	rc = ssi_fips_get_tee_error(p_drvdata);
+	if (unlikely(rc != CC_REE_FIPS_ERROR_OK)) {
+		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_FROM_TEE);
+		rc = -EAGAIN;
+		goto ssi_fips_init_err;
+	}
+
+	FIPS_DBG("cc_fips_run_power_up_tests \n");
+	rc = cc_fips_run_power_up_tests(p_drvdata);
+	if (unlikely(rc != CC_REE_FIPS_ERROR_OK)) {
+		ssi_fips_set_error(p_drvdata, rc);
+		rc = -EAGAIN;
+		goto ssi_fips_init_err;
+	}
+	FIPS_LOG("cc_fips_run_power_up_tests - done  ...  fips_error = %d \n", rc);
+
+	/* when all tests passed, update TEE with fips OK status after power up tests */
+	ssi_fips_update_tee_upon_ree_status(p_drvdata, CC_REE_FIPS_ERROR_OK);
+
+	if (unlikely(rc != 0)) {
+		rc = -EAGAIN;
+		ssi_fips_set_error(p_drvdata, CC_REE_FIPS_ERROR_GENERAL);
+		goto ssi_fips_init_err;
+	}
+
+	return 0;
+
+ssi_fips_init_err:
+	ssi_fips_fini(p_drvdata);
+	return rc;
+}
+
diff --git a/drivers/staging/ccree/ssi_fips_local.h b/drivers/staging/ccree/ssi_fips_local.h
new file mode 100644
index 000000000000..65997c15a20e
--- /dev/null
+++ b/drivers/staging/ccree/ssi_fips_local.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __SSI_FIPS_LOCAL_H__
+#define __SSI_FIPS_LOCAL_H__
+
+
+#ifdef CONFIG_CCX7REE_FIPS_SUPPORT
+
+#include "ssi_fips.h"
+struct ssi_drvdata;
+
+// IG - how to make 1 file for TEE and REE
+typedef enum CC_FipsSyncStatus{
+	CC_FIPS_SYNC_MODULE_OK 		= 0x0,
+	CC_FIPS_SYNC_MODULE_ERROR 	= 0x1,
+	CC_FIPS_SYNC_REE_STATUS 	= 0x4,
+	CC_FIPS_SYNC_TEE_STATUS 	= 0x8,
+	CC_FIPS_SYNC_STATUS_RESERVE32B 	= INT32_MAX
+}CCFipsSyncStatus_t;
+
+
+#define CHECK_AND_RETURN_UPON_FIPS_ERROR() {\
+        if (ssi_fips_check_fips_error() != 0) {\
+                return -ENOEXEC;\
+        }\
+}
+#define CHECK_AND_RETURN_VOID_UPON_FIPS_ERROR() {\
+        if (ssi_fips_check_fips_error() != 0) {\
+                return;\
+        }\
+}
+#define SSI_FIPS_INIT(p_drvData)  (ssi_fips_init(p_drvData))
+#define SSI_FIPS_FINI(p_drvData)  (ssi_fips_fini(p_drvData))
+
+#define FIPS_LOG(...)	SSI_LOG(KERN_INFO, __VA_ARGS__)
+#define FIPS_DBG(...)	//SSI_LOG(KERN_INFO, __VA_ARGS__)
+
+/* FIPS functions */
+int ssi_fips_init(struct ssi_drvdata *p_drvdata);
+void ssi_fips_fini(struct ssi_drvdata *drvdata);
+int ssi_fips_check_fips_error(void);
+int ssi_fips_set_error(struct ssi_drvdata *p_drvdata, ssi_fips_error_t err);
+void fips_handler(struct ssi_drvdata *drvdata);
+
+#else  /* CONFIG_CC7XXREE_FIPS_SUPPORT */
+
+#define CHECK_AND_RETURN_UPON_FIPS_ERROR()
+#define CHECK_AND_RETURN_VOID_UPON_FIPS_ERROR()
+
+static inline int ssi_fips_init(struct ssi_drvdata *p_drvdata)
+{
+	return 0;
+}
+
+static inline void ssi_fips_fini(struct ssi_drvdata *drvdata) {}
+
+void fips_handler(struct ssi_drvdata *drvdata);
+
+#endif  /* CONFIG_CC7XXREE_FIPS_SUPPORT */
+
+
+#endif  /*__SSI_FIPS_LOCAL_H__*/
+
diff --git a/drivers/staging/ccree/ssi_hash.c b/drivers/staging/ccree/ssi_hash.c
new file mode 100644
index 000000000000..8ff5d4ec9e5c
--- /dev/null
+++ b/drivers/staging/ccree/ssi_hash.c
@@ -0,0 +1,2742 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <crypto/algapi.h>
+#include <crypto/hash.h>
+#include <crypto/sha.h>
+#include <crypto/md5.h>
+#include <crypto/internal/hash.h>
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "ssi_request_mgr.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_sysfs.h"
+#include "ssi_hash.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_fips_local.h"
+
+#define SSI_MAX_AHASH_SEQ_LEN 12
+#define SSI_MAX_HASH_OPAD_TMP_KEYS_SIZE MAX(SSI_MAX_HASH_BLCK_SIZE, 3 * AES_BLOCK_SIZE)
+
+struct ssi_hash_handle {
+	ssi_sram_addr_t digest_len_sram_addr; /* const value in SRAM*/
+	ssi_sram_addr_t larval_digest_sram_addr;   /* const value in SRAM */
+	struct list_head hash_list;
+	struct completion init_comp;
+};
+
+static const uint32_t digest_len_init[] = {
+	0x00000040, 0x00000000, 0x00000000, 0x00000000 };
+static const uint32_t md5_init[] = { 
+	SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
+static const uint32_t sha1_init[] = { 
+	SHA1_H4, SHA1_H3, SHA1_H2, SHA1_H1, SHA1_H0 };
+static const uint32_t sha224_init[] = { 
+	SHA224_H7, SHA224_H6, SHA224_H5, SHA224_H4,
+	SHA224_H3, SHA224_H2, SHA224_H1, SHA224_H0 };
+static const uint32_t sha256_init[] = {
+	SHA256_H7, SHA256_H6, SHA256_H5, SHA256_H4,
+	SHA256_H3, SHA256_H2, SHA256_H1, SHA256_H0 };
+#if (DX_DEV_SHA_MAX > 256)
+static const uint32_t digest_len_sha512_init[] = { 
+	0x00000080, 0x00000000, 0x00000000, 0x00000000 };
+static const uint64_t sha384_init[] = {
+	SHA384_H7, SHA384_H6, SHA384_H5, SHA384_H4,
+	SHA384_H3, SHA384_H2, SHA384_H1, SHA384_H0 };
+static const uint64_t sha512_init[] = {
+	SHA512_H7, SHA512_H6, SHA512_H5, SHA512_H4,
+	SHA512_H3, SHA512_H2, SHA512_H1, SHA512_H0 };
+#endif
+
+static void ssi_hash_create_xcbc_setup(
+	struct ahash_request *areq, 
+	HwDesc_s desc[],
+	unsigned int *seq_size);
+
+static void ssi_hash_create_cmac_setup(struct ahash_request *areq, 
+				  HwDesc_s desc[],
+				  unsigned int *seq_size);
+
+struct ssi_hash_alg {
+	struct list_head entry;
+	bool synchronize;
+	int hash_mode;
+	int hw_mode;
+	int inter_digestsize;
+	struct ssi_drvdata *drvdata;
+	union {
+		struct ahash_alg ahash_alg;
+		struct shash_alg shash_alg;
+	};
+};
+
+
+struct hash_key_req_ctx {
+	uint32_t keylen;
+	dma_addr_t key_dma_addr;
+};
+
+/* hash per-session context */
+struct ssi_hash_ctx {
+	struct ssi_drvdata *drvdata;
+	/* holds the origin digest; the digest after "setkey" if HMAC,* 
+	   the initial digest if HASH. */
+	uint8_t digest_buff[SSI_MAX_HASH_DIGEST_SIZE]  ____cacheline_aligned;
+	uint8_t opad_tmp_keys_buff[SSI_MAX_HASH_OPAD_TMP_KEYS_SIZE]  ____cacheline_aligned;
+	dma_addr_t opad_tmp_keys_dma_addr  ____cacheline_aligned;
+	dma_addr_t digest_buff_dma_addr;
+	/* use for hmac with key large then mode block size */
+	struct hash_key_req_ctx key_params;
+	int hash_mode;
+	int hw_mode;
+	int inter_digestsize;
+	struct completion setkey_comp;
+	bool is_hmac;
+};
+
+static const struct crypto_type crypto_shash_type;
+
+static void ssi_hash_create_data_desc(
+	struct ahash_req_ctx *areq_ctx,
+	struct ssi_hash_ctx *ctx, 
+	unsigned int flow_mode,HwDesc_s desc[],
+	bool is_not_last_data,
+	unsigned int *seq_size);
+
+static inline void ssi_set_hash_endianity(uint32_t mode, HwDesc_s *desc)
+{
+	if (unlikely((mode == DRV_HASH_MD5) ||
+		(mode == DRV_HASH_SHA384) ||
+		(mode == DRV_HASH_SHA512))) {
+		HW_DESC_SET_BYTES_SWAP(desc, 1);
+	} else {
+		HW_DESC_SET_CIPHER_CONFIG0(desc, HASH_DIGEST_RESULT_LITTLE_ENDIAN);
+	}
+}
+
+static int ssi_hash_map_result(struct device *dev, 
+			       struct ahash_req_ctx *state, 
+			       unsigned int digestsize)
+{
+	state->digest_result_dma_addr = 
+		dma_map_single(dev, (void *)state->digest_result_buff,
+			       digestsize,
+			       DMA_BIDIRECTIONAL);
+	if (unlikely(dma_mapping_error(dev, state->digest_result_dma_addr))) {
+		SSI_LOG_ERR("Mapping digest result buffer %u B for DMA failed\n",
+			digestsize);
+		return -ENOMEM;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_result_dma_addr,
+						digestsize);
+	SSI_LOG_DEBUG("Mapped digest result buffer %u B "
+		     "at va=%pK to dma=0x%llX\n",
+		digestsize, state->digest_result_buff,
+		(unsigned long long)state->digest_result_dma_addr);
+
+	return 0;
+}
+
+static int ssi_hash_map_request(struct device *dev, 
+				struct ahash_req_ctx *state, 
+				struct ssi_hash_ctx *ctx)
+{
+	bool is_hmac = ctx->is_hmac;
+	ssi_sram_addr_t larval_digest_addr = ssi_ahash_get_larval_digest_sram_addr(
+					ctx->drvdata, ctx->hash_mode);
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc;
+	int rc = -ENOMEM;
+
+	state->buff0 = kzalloc(SSI_MAX_HASH_BLCK_SIZE ,GFP_KERNEL|GFP_DMA);
+	if (!state->buff0) {
+		SSI_LOG_ERR("Allocating buff0 in context failed\n");
+		goto fail0;
+	}
+	state->buff1 = kzalloc(SSI_MAX_HASH_BLCK_SIZE ,GFP_KERNEL|GFP_DMA);
+	if (!state->buff1) {
+		SSI_LOG_ERR("Allocating buff1 in context failed\n");
+		goto fail_buff0;
+	}
+	state->digest_result_buff = kzalloc(SSI_MAX_HASH_DIGEST_SIZE ,GFP_KERNEL|GFP_DMA);
+	if (!state->digest_result_buff) {
+		SSI_LOG_ERR("Allocating digest_result_buff in context failed\n");
+		goto fail_buff1;
+	}
+	state->digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL|GFP_DMA);
+	if (!state->digest_buff) {
+		SSI_LOG_ERR("Allocating digest-buffer in context failed\n");
+		goto fail_digest_result_buff;
+	}
+
+	SSI_LOG_DEBUG("Allocated digest-buffer in context ctx->digest_buff=@%p\n", state->digest_buff);
+	if (ctx->hw_mode != DRV_CIPHER_XCBC_MAC) {
+		state->digest_bytes_len = kzalloc(HASH_LEN_SIZE, GFP_KERNEL|GFP_DMA);
+		if (!state->digest_bytes_len) {
+			SSI_LOG_ERR("Allocating digest-bytes-len in context failed\n");
+			goto fail1;
+		}
+		SSI_LOG_DEBUG("Allocated digest-bytes-len in context state->>digest_bytes_len=@%p\n", state->digest_bytes_len);
+	} else {
+		state->digest_bytes_len = NULL;
+	}
+
+	state->opad_digest_buff = kzalloc(ctx->inter_digestsize, GFP_KERNEL|GFP_DMA);
+	if (!state->opad_digest_buff) {
+		SSI_LOG_ERR("Allocating opad-digest-buffer in context failed\n");
+		goto fail2;
+	}
+	SSI_LOG_DEBUG("Allocated opad-digest-buffer in context state->digest_bytes_len=@%p\n", state->opad_digest_buff);
+
+	state->digest_buff_dma_addr = dma_map_single(dev, (void *)state->digest_buff, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, state->digest_buff_dma_addr)) {
+		SSI_LOG_ERR("Mapping digest len %d B at va=%pK for DMA failed\n",
+		ctx->inter_digestsize, state->digest_buff);
+		goto fail3;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr, 
+							ctx->inter_digestsize);
+	SSI_LOG_DEBUG("Mapped digest %d B at va=%pK to dma=0x%llX\n",
+		ctx->inter_digestsize, state->digest_buff,
+		(unsigned long long)state->digest_buff_dma_addr);
+
+	if (is_hmac) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr);
+		dma_sync_single_for_cpu(dev, ctx->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr, 
+							ctx->inter_digestsize);
+		if ((ctx->hw_mode == DRV_CIPHER_XCBC_MAC) || (ctx->hw_mode == DRV_CIPHER_CMAC)) {
+			memset(state->digest_buff, 0, ctx->inter_digestsize);
+		} else { /*sha*/
+			memcpy(state->digest_buff, ctx->digest_buff, ctx->inter_digestsize);
+#if (DX_DEV_SHA_MAX > 256)
+			if (unlikely((ctx->hash_mode == DRV_HASH_SHA512) || (ctx->hash_mode == DRV_HASH_SHA384))) {
+				memcpy(state->digest_bytes_len, digest_len_sha512_init, HASH_LEN_SIZE);
+			} else {
+				memcpy(state->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
+			}
+#else
+			memcpy(state->digest_bytes_len, digest_len_init, HASH_LEN_SIZE);
+#endif
+		}
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
+		dma_sync_single_for_device(dev, state->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr, 
+							ctx->inter_digestsize);
+
+		if (ctx->hash_mode != DRV_HASH_NULL) {
+			SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
+			dma_sync_single_for_cpu(dev, ctx->opad_tmp_keys_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+			memcpy(state->opad_digest_buff, ctx->opad_tmp_keys_buff, ctx->inter_digestsize);
+			SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr, 
+							ctx->inter_digestsize);
+		} 
+	} else { /*hash*/
+		/* Copy the initial digests if hash flow. The SRAM contains the
+		initial digests in the expected order for all SHA* */
+		HW_DESC_INIT(&desc);
+		HW_DESC_SET_DIN_SRAM(&desc, larval_digest_addr, ctx->inter_digestsize);
+		HW_DESC_SET_DOUT_DLLI(&desc, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc, BYPASS);
+
+		rc = send_request(ctx->drvdata, &ssi_req, &desc, 1, 0);
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			goto fail4;
+		}
+	}
+
+	if (ctx->hw_mode != DRV_CIPHER_XCBC_MAC) {
+		state->digest_bytes_len_dma_addr = dma_map_single(dev, (void *)state->digest_bytes_len, HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(dev, state->digest_bytes_len_dma_addr)) {
+			SSI_LOG_ERR("Mapping digest len %u B at va=%pK for DMA failed\n",
+			HASH_LEN_SIZE, state->digest_bytes_len);
+			goto fail4;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr,
+								HASH_LEN_SIZE);
+		SSI_LOG_DEBUG("Mapped digest len %u B at va=%pK to dma=0x%llX\n",
+			HASH_LEN_SIZE, state->digest_bytes_len,
+			(unsigned long long)state->digest_bytes_len_dma_addr);
+	} else {
+		state->digest_bytes_len_dma_addr = 0;
+	}
+
+	if (is_hmac && ctx->hash_mode != DRV_HASH_NULL) {
+		state->opad_digest_dma_addr = dma_map_single(dev, (void *)state->opad_digest_buff, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(dev, state->opad_digest_dma_addr)) {
+			SSI_LOG_ERR("Mapping opad digest %d B at va=%pK for DMA failed\n",
+			ctx->inter_digestsize, state->opad_digest_buff);
+			goto fail5;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(state->opad_digest_dma_addr,
+							ctx->inter_digestsize);
+		SSI_LOG_DEBUG("Mapped opad digest %d B at va=%pK to dma=0x%llX\n",
+			ctx->inter_digestsize, state->opad_digest_buff,
+			(unsigned long long)state->opad_digest_dma_addr);
+	} else {
+		state->opad_digest_dma_addr = 0;
+	}
+	state->buff0_cnt = 0;
+	state->buff1_cnt = 0;
+	state->buff_index = 0;
+	state->mlli_params.curr_pool = NULL;
+
+	return 0;
+
+fail5:
+	if (state->digest_bytes_len_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr);
+		dma_unmap_single(dev, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+		state->digest_bytes_len_dma_addr = 0;
+	}
+fail4:
+	if (state->digest_buff_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
+		dma_unmap_single(dev, state->digest_buff_dma_addr, ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		state->digest_buff_dma_addr = 0;
+	}
+fail3:
+	kfree(state->opad_digest_buff);
+fail2:
+	kfree(state->digest_bytes_len);
+fail1:
+	 kfree(state->digest_buff);
+fail_digest_result_buff:
+	 if (state->digest_result_buff != NULL) {
+		 kfree(state->digest_result_buff);
+	     state->digest_result_buff = NULL;
+	 }
+fail_buff1:
+	 if (state->buff1 != NULL) {
+		 kfree(state->buff1);
+	     state->buff1 = NULL;
+	 }
+fail_buff0:
+	 if (state->buff0 != NULL) {
+		 kfree(state->buff0);
+	     state->buff0 = NULL;
+	 }
+fail0:
+	return rc;
+}
+
+static void ssi_hash_unmap_request(struct device *dev, 
+				   struct ahash_req_ctx *state, 
+				   struct ssi_hash_ctx *ctx)
+{
+	if (state->digest_buff_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_buff_dma_addr);
+		dma_unmap_single(dev, state->digest_buff_dma_addr,
+				 ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped digest-buffer: digest_buff_dma_addr=0x%llX\n",
+			(unsigned long long)state->digest_buff_dma_addr);
+		state->digest_buff_dma_addr = 0;
+	}
+	if (state->digest_bytes_len_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_bytes_len_dma_addr);
+		dma_unmap_single(dev, state->digest_bytes_len_dma_addr,
+				 HASH_LEN_SIZE, DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped digest-bytes-len buffer: digest_bytes_len_dma_addr=0x%llX\n",
+			(unsigned long long)state->digest_bytes_len_dma_addr);
+		state->digest_bytes_len_dma_addr = 0;
+	}
+	if (state->opad_digest_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->opad_digest_dma_addr);
+		dma_unmap_single(dev, state->opad_digest_dma_addr,
+				 ctx->inter_digestsize, DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped opad-digest: opad_digest_dma_addr=0x%llX\n",
+			(unsigned long long)state->opad_digest_dma_addr);
+		state->opad_digest_dma_addr = 0;
+	}
+
+	kfree(state->opad_digest_buff);
+	kfree(state->digest_bytes_len);
+	kfree(state->digest_buff);
+	kfree(state->digest_result_buff);
+	kfree(state->buff1);
+	kfree(state->buff0);
+}
+
+static void ssi_hash_unmap_result(struct device *dev, 
+				  struct ahash_req_ctx *state, 
+				  unsigned int digestsize, u8 *result)
+{
+	if (state->digest_result_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(state->digest_result_dma_addr);
+		dma_unmap_single(dev,
+				 state->digest_result_dma_addr,
+				 digestsize,
+				  DMA_BIDIRECTIONAL);	
+		SSI_LOG_DEBUG("unmpa digest result buffer "
+			     "va (%pK) pa (%llx) len %u\n",
+			     state->digest_result_buff, 
+			     (unsigned long long)state->digest_result_dma_addr,
+			     digestsize);
+		memcpy(result,
+		       state->digest_result_buff,
+		       digestsize);
+	}
+	state->digest_result_dma_addr = 0;
+}
+
+static void ssi_hash_update_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
+{
+	struct ahash_request *req = (struct ahash_request *)ssi_req;
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+
+	SSI_LOG_DEBUG("req=%pK\n", req);
+
+	ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, false);
+	req->base.complete(&req->base, 0);
+}
+
+static void ssi_hash_digest_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
+{
+	struct ahash_request *req = (struct ahash_request *)ssi_req;
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	
+	SSI_LOG_DEBUG("req=%pK\n", req);
+
+	ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, false);
+	ssi_hash_unmap_result(dev, state, digestsize, req->result);
+	ssi_hash_unmap_request(dev, state, ctx);
+	req->base.complete(&req->base, 0);
+}
+
+static void ssi_hash_complete(struct device *dev, void *ssi_req, void __iomem *cc_base)
+{
+	struct ahash_request *req = (struct ahash_request *)ssi_req;
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	
+	SSI_LOG_DEBUG("req=%pK\n", req);
+
+	ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, false);
+	ssi_hash_unmap_result(dev, state, digestsize, req->result);
+	ssi_hash_unmap_request(dev, state, ctx);
+	req->base.complete(&req->base, 0);
+}
+
+static int ssi_hash_digest(struct ahash_req_ctx *state, 
+			   struct ssi_hash_ctx *ctx, 
+			   unsigned int digestsize, 
+			   struct scatterlist *src, 
+			   unsigned int nbytes, u8 *result, 
+			   void *async_req)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	bool is_hmac = ctx->is_hmac;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	ssi_sram_addr_t larval_digest_addr = ssi_ahash_get_larval_digest_sram_addr(
+					ctx->drvdata, ctx->hash_mode);
+	int idx = 0;
+	int rc = 0;
+
+
+	SSI_LOG_DEBUG("===== %s-digest (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	if (unlikely(ssi_hash_map_request(dev, state, ctx) != 0)) {
+		SSI_LOG_ERR("map_ahash_source() failed\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, src, nbytes, 1) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+
+	if (async_req) {
+		/* Setup DX request structure */
+		ssi_req.user_cb = (void *)ssi_hash_digest_complete;
+		ssi_req.user_arg = (void *)async_req;
+#ifdef ENABLE_CYCLE_COUNT
+		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+	}
+
+	/* If HMAC then load hash IPAD xor key, if HASH then load initial digest */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	if (is_hmac) {
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
+	} else {
+		HW_DESC_SET_DIN_SRAM(&desc[idx], larval_digest_addr, ctx->inter_digestsize);
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Load the hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+
+	if (is_hmac) {
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+	} else {
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+		if (likely(nbytes != 0)) {
+			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+		} else {
+			HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+		}
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
+
+	if (is_hmac) {
+		/* HW last hash block padding (aka. "DO_PAD") */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+		HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+		idx++;
+
+		/* store the hash digest result in the context */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		idx++;
+
+		/* Loading hash opad xor key state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+		/* Load the hash current length */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		idx++;
+
+		/* Perform HASH update */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+	}
+
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0);   /*TODO*/
+	if (async_req) {
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	ssi_set_hash_endianity(ctx->hash_mode, &desc[idx]);
+	idx++;
+
+	if (async_req) {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+		if (unlikely(rc != -EINPROGRESS)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+			ssi_hash_unmap_request(dev, state, ctx);
+		}
+	} else {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+		if (rc != 0) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+		} else {
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);			
+		}
+		ssi_hash_unmap_result(dev, state, digestsize, result);
+		ssi_hash_unmap_request(dev, state, ctx);
+	}
+	return rc;
+}
+
+static int ssi_hash_update(struct ahash_req_ctx *state, 
+			   struct ssi_hash_ctx *ctx, 
+			   unsigned int block_size, 
+			   struct scatterlist *src, 
+			   unsigned int nbytes, 
+			   void *async_req)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	uint32_t idx = 0;
+	int rc;
+
+	SSI_LOG_DEBUG("===== %s-update (%d) ====\n", ctx->is_hmac ?
+					"hmac":"hash", nbytes);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	if (nbytes == 0) {
+		/* no real updates required */
+		return 0;
+	}
+
+	if (unlikely(rc = ssi_buffer_mgr_map_hash_request_update(ctx->drvdata, state, src, nbytes, block_size))) {
+		if (rc == 1) {
+			SSI_LOG_DEBUG(" data size not require HW update %x\n",
+				     nbytes);
+			/* No hardware updates are required */
+			return 0;
+		}
+		SSI_LOG_ERR("map_ahash_request_update() failed\n");
+		return -ENOMEM;
+	}
+
+	if (async_req) {
+		/* Setup DX request structure */
+		ssi_req.user_cb = (void *)ssi_hash_update_complete;
+		ssi_req.user_arg = async_req;
+#ifdef ENABLE_CYCLE_COUNT
+		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+	}
+
+	/* Restore hash digest */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+	/* Restore hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
+
+	/* store the hash digest result in context */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 0);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	idx++;
+
+	/* store current hash length in context */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT, async_req? 1:0);
+	if (async_req) {
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+	idx++;
+
+	if (async_req) {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+		if (unlikely(rc != -EINPROGRESS)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+		}
+	} else {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+		if (rc != 0) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+		} else {
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);
+		}
+	}
+	return rc;
+}
+
+static int ssi_hash_finup(struct ahash_req_ctx *state, 
+			  struct ssi_hash_ctx *ctx, 
+			  unsigned int digestsize, 
+			  struct scatterlist *src, 
+			  unsigned int nbytes, 
+			  u8 *result, 
+			  void *async_req)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	bool is_hmac = ctx->is_hmac;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	int idx = 0;
+	int rc;
+
+	SSI_LOG_DEBUG("===== %s-finup (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, src , nbytes, 1) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	if (async_req) {
+		/* Setup DX request structure */
+		ssi_req.user_cb = (void *)ssi_hash_complete;
+		ssi_req.user_arg = async_req;
+#ifdef ENABLE_CYCLE_COUNT
+		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+	}
+
+	/* Restore hash digest */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Restore hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
+
+	if (is_hmac) {
+		/* Store the hash digest result in the context */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
+		ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		idx++;
+
+		/* Loading hash OPAD xor key state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+		/* Load the hash current length */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		idx++;
+
+		/* Perform HASH update on last digest */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+	}
+
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0); /*TODO*/
+	if (async_req) {
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	idx++;
+
+	if (async_req) {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+		if (unlikely(rc != -EINPROGRESS)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+		}
+	} else {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+		if (rc != 0) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+		} else {
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+			ssi_hash_unmap_request(dev, state, ctx);
+		}
+	}
+	return rc;
+}
+
+static int ssi_hash_final(struct ahash_req_ctx *state, 
+			  struct ssi_hash_ctx *ctx, 
+			  unsigned int digestsize, 
+			  struct scatterlist *src, 
+			  unsigned int nbytes, 
+			  u8 *result, 
+			  void *async_req)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	bool is_hmac = ctx->is_hmac;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	int idx = 0;
+	int rc;
+
+	SSI_LOG_DEBUG("===== %s-final (%d) ====\n", is_hmac?"hmac":"hash", nbytes);
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, src, nbytes, 0) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	if (async_req) {
+		/* Setup DX request structure */
+		ssi_req.user_cb = (void *)ssi_hash_complete;
+		ssi_req.user_arg = async_req;
+#ifdef ENABLE_CYCLE_COUNT
+		ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+	}
+
+	/* Restore hash digest */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	idx++;
+
+	/* Restore hash current length */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	ssi_hash_create_data_desc(state, ctx, DIN_HASH, desc, false, &idx);
+
+	/* "DO-PAD" must be enabled only when writing current length to HW */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_DO(&desc[idx], DO_PAD);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_bytes_len_dma_addr, HASH_LEN_SIZE, NS_BIT, 0);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE1);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	idx++;
+
+	if (is_hmac) {
+		/* Store the hash digest result in the context */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, digestsize, NS_BIT, 0);
+		ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		idx++;
+
+		/* Loading hash OPAD xor key state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->opad_digest_dma_addr, ctx->inter_digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+		/* Load the hash current length */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_SRAM(&desc[idx], ssi_ahash_get_initial_digest_len_sram_addr(ctx->drvdata, ctx->hash_mode), HASH_LEN_SIZE);
+		HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Memory Barrier: wait for IPAD/OPAD axi write to complete */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		idx++;
+
+		/* Perform HASH update on last digest */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, digestsize, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+	}
+
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, async_req? 1:0);
+	if (async_req) {
+		HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	}
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+	HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	idx++;
+
+	if (async_req) {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+		if (unlikely(rc != -EINPROGRESS)) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+		}
+	} else {
+		rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+		if (rc != 0) {
+			SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, true);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+		} else {
+			ssi_buffer_mgr_unmap_hash_request(dev, state, src, false);
+			ssi_hash_unmap_result(dev, state, digestsize, result);
+			ssi_hash_unmap_request(dev, state, ctx);
+		}
+	}
+	return rc;
+}
+
+static int ssi_hash_init(struct ahash_req_ctx *state, struct ssi_hash_ctx *ctx)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	state->xcbc_count = 0;	
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	ssi_hash_map_request(dev, state, ctx);
+
+	return 0;
+}
+
+#ifdef EXPORT_FIXED
+static int ssi_hash_export(struct ssi_hash_ctx *ctx, void *out)
+{
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	memcpy(out, ctx, sizeof(struct ssi_hash_ctx));
+	return 0;
+}
+
+static int ssi_hash_import(struct ssi_hash_ctx *ctx, const void *in)
+{
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	memcpy(ctx, in, sizeof(struct ssi_hash_ctx));
+	return 0;
+}
+#endif
+
+static int ssi_hash_setkey(void *hash,
+			   const u8 *key, 
+			   unsigned int keylen, 
+			   bool synchronize)
+{
+	unsigned int hmacPadConst[2] = { HMAC_IPAD_CONST, HMAC_OPAD_CONST };
+	struct ssi_crypto_req ssi_req = {};
+	struct ssi_hash_ctx *ctx = NULL;
+	int blocksize = 0;
+	int digestsize = 0;
+	int i, idx = 0, rc = 0;
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	ssi_sram_addr_t larval_addr;
+
+	 SSI_LOG_DEBUG("ssi_hash_setkey: start keylen: %d", keylen);
+	
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	if (synchronize) {
+		ctx = crypto_shash_ctx(((struct crypto_shash *)hash));
+		blocksize = crypto_tfm_alg_blocksize(&((struct crypto_shash *)hash)->base);
+		digestsize = crypto_shash_digestsize(((struct crypto_shash *)hash));
+	} else {
+		ctx = crypto_ahash_ctx(((struct crypto_ahash *)hash));
+		blocksize = crypto_tfm_alg_blocksize(&((struct crypto_ahash *)hash)->base);
+		digestsize = crypto_ahash_digestsize(((struct crypto_ahash *)hash));
+	}
+	
+	larval_addr = ssi_ahash_get_larval_digest_sram_addr(
+					ctx->drvdata, ctx->hash_mode);
+
+	/* The keylen value distinguishes HASH in case keylen is ZERO bytes,
+	   any NON-ZERO value utilizes HMAC flow */
+	ctx->key_params.keylen = keylen;
+	ctx->key_params.key_dma_addr = 0;
+	ctx->is_hmac = true;
+
+	if (keylen != 0) {
+		ctx->key_params.key_dma_addr = dma_map_single(
+						&ctx->drvdata->plat_dev->dev,
+						(void *)key,
+						keylen, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(&ctx->drvdata->plat_dev->dev,
+					       ctx->key_params.key_dma_addr))) {
+			SSI_LOG_ERR("Mapping key va=0x%p len=%u for"
+				   " DMA failed\n", key, keylen);
+			return -ENOMEM;
+		}
+		SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr, keylen);
+		SSI_LOG_DEBUG("mapping key-buffer: key_dma_addr=0x%llX "
+			     "keylen=%u\n",
+			     (unsigned long long)ctx->key_params.key_dma_addr,
+			     ctx->key_params.keylen);
+
+		if (keylen > blocksize) {
+			/* Load hash initial state */
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+			HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr,
+					ctx->inter_digestsize);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+			idx++;
+	
+			/* Load the hash current length*/
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+			HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_ENABLED);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+			idx++;
+	
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+					     ctx->key_params.key_dma_addr, 
+					     keylen, NS_BIT);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+			idx++;
+	
+			/* Get hashed key */
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+			HW_DESC_SET_DOUT_DLLI(&desc[idx], ctx->opad_tmp_keys_dma_addr,
+					      digestsize, NS_BIT, 0);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+			HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+			HW_DESC_SET_CIPHER_CONFIG1(&desc[idx], HASH_PADDING_DISABLED);
+			ssi_set_hash_endianity(ctx->hash_mode,&desc[idx]);
+			idx++;
+	
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - digestsize));
+			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+			HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+					      (ctx->opad_tmp_keys_dma_addr + digestsize),
+					      (blocksize - digestsize),
+					      NS_BIT, 0);
+			idx++;
+		} else {
+			HW_DESC_INIT(&desc[idx]);
+			HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+					     ctx->key_params.key_dma_addr, 
+					     keylen, NS_BIT);
+			HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+			HW_DESC_SET_DOUT_DLLI(&desc[idx],
+					(ctx->opad_tmp_keys_dma_addr),
+					keylen, NS_BIT, 0);
+			idx++;
+
+			if ((blocksize - keylen) != 0) {
+				HW_DESC_INIT(&desc[idx]);
+				HW_DESC_SET_DIN_CONST(&desc[idx], 0, (blocksize - keylen));
+				HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+				HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+						      (ctx->opad_tmp_keys_dma_addr + keylen),
+						      (blocksize - keylen),
+						      NS_BIT, 0);
+				idx++;
+			}
+		}
+	} else {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, blocksize);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], 
+				      (ctx->opad_tmp_keys_dma_addr),
+				      blocksize,
+				      NS_BIT, 0);
+		idx++;
+	}
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		goto out;
+	}
+
+	/* calc derived HMAC key */
+	for (idx = 0, i = 0; i < 2; i++) {
+		/* Load hash initial state */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_SRAM(&desc[idx], larval_addr,
+				ctx->inter_digestsize);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+		idx++;
+
+		/* Load the hash current length*/
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0, HASH_LEN_SIZE);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+		/* Prepare ipad key */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_XOR_VAL(&desc[idx], hmacPadConst[i]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_HASH);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+		idx++;
+
+		/* Perform HASH update */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI,
+				     ctx->opad_tmp_keys_dma_addr,
+				     blocksize, NS_BIT);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx],ctx->hw_mode);
+		HW_DESC_SET_XOR_ACTIVE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_HASH);
+		idx++;
+
+		/* Get the IPAD/OPAD xor key (Note, IPAD is the initial digest of the first HASH "update" state) */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		if (i > 0) /* Not first iteration */
+			HW_DESC_SET_DOUT_DLLI(&desc[idx],
+					      ctx->opad_tmp_keys_dma_addr,
+					      ctx->inter_digestsize,
+					      NS_BIT, 0);
+		else /* First iteration */
+			HW_DESC_SET_DOUT_DLLI(&desc[idx],
+					      ctx->digest_buff_dma_addr,
+					      ctx->inter_digestsize,
+					      NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_HASH_to_DOUT);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+		idx++;
+	}
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+
+out:
+	if (rc != 0) {
+		if (synchronize) {
+			crypto_shash_set_flags((struct crypto_shash *)hash, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		} else {
+			crypto_ahash_set_flags((struct crypto_ahash *)hash, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		}
+	}
+
+	if (ctx->key_params.key_dma_addr) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr);
+		dma_unmap_single(&ctx->drvdata->plat_dev->dev,
+				ctx->key_params.key_dma_addr,
+				ctx->key_params.keylen, DMA_TO_DEVICE);
+		SSI_LOG_DEBUG("Unmapped key-buffer: key_dma_addr=0x%llX keylen=%u\n",
+				(unsigned long long)ctx->key_params.key_dma_addr,
+				ctx->key_params.keylen);
+	}
+	return rc;
+}
+
+
+static int ssi_xcbc_setkey(struct crypto_ahash *ahash,
+			const u8 *key, unsigned int keylen)
+{
+	struct ssi_crypto_req ssi_req = {};
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	int idx = 0, rc = 0;
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+
+	SSI_LOG_DEBUG("===== setkey (%d) ====\n", keylen);
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	switch (keylen) {
+		case AES_KEYSIZE_128:
+		case AES_KEYSIZE_192:
+		case AES_KEYSIZE_256:
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	ctx->key_params.keylen = keylen;
+
+	ctx->key_params.key_dma_addr = dma_map_single(
+					&ctx->drvdata->plat_dev->dev,
+					(void *)key,
+					keylen, DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(&ctx->drvdata->plat_dev->dev,
+				       ctx->key_params.key_dma_addr))) {
+		SSI_LOG_ERR("Mapping key va=0x%p len=%u for"
+			   " DMA failed\n", key, keylen);
+		return -ENOMEM;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr, keylen);
+	SSI_LOG_DEBUG("mapping key-buffer: key_dma_addr=0x%llX "
+		     "keylen=%u\n",
+		     (unsigned long long)ctx->key_params.key_dma_addr,
+		     ctx->key_params.keylen);
+	
+	ctx->is_hmac = true;
+	/* 1. Load the AES key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->key_params.key_dma_addr, keylen, NS_BIT);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keylen);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	idx++;
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x01010101, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
+					   XCBC_MAC_K1_OFFSET), 
+			      CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
+	idx++;
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x02020202, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
+					   XCBC_MAC_K2_OFFSET), 
+			      CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
+	idx++;
+
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_CONST(&desc[idx], 0x03030303, CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], (ctx->opad_tmp_keys_dma_addr + 
+					   XCBC_MAC_K3_OFFSET),
+			       CC_AES_128_BIT_KEY_SIZE, NS_BIT, 0);
+	idx++;
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 0);
+
+	if (rc != 0)
+		crypto_ahash_set_flags(ahash, CRYPTO_TFM_RES_BAD_KEY_LEN);
+
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->key_params.key_dma_addr);
+	dma_unmap_single(&ctx->drvdata->plat_dev->dev,
+			ctx->key_params.key_dma_addr,
+			ctx->key_params.keylen, DMA_TO_DEVICE);
+	SSI_LOG_DEBUG("Unmapped key-buffer: key_dma_addr=0x%llX keylen=%u\n",
+			(unsigned long long)ctx->key_params.key_dma_addr,
+			ctx->key_params.keylen);
+
+	return rc;
+}
+#if SSI_CC_HAS_CMAC
+static int ssi_cmac_setkey(struct crypto_ahash *ahash,
+			const u8 *key, unsigned int keylen)
+{
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	DECL_CYCLE_COUNT_RESOURCES;
+	SSI_LOG_DEBUG("===== setkey (%d) ====\n", keylen);
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+
+	ctx->is_hmac = true;
+
+	switch (keylen) {
+		case AES_KEYSIZE_128:
+		case AES_KEYSIZE_192:
+		case AES_KEYSIZE_256:
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	ctx->key_params.keylen = keylen;
+
+	/* STAT_PHASE_1: Copy key to ctx */
+	START_CYCLE_COUNT();
+	
+	SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
+	dma_sync_single_for_cpu(&ctx->drvdata->plat_dev->dev,
+				ctx->opad_tmp_keys_dma_addr, 
+				keylen, DMA_TO_DEVICE);
+
+	memcpy(ctx->opad_tmp_keys_buff, key, keylen);
+	if (keylen == 24)
+		memset(ctx->opad_tmp_keys_buff + 24, 0, CC_AES_KEY_SIZE_MAX - 24);
+	
+	dma_sync_single_for_device(&ctx->drvdata->plat_dev->dev,
+				   ctx->opad_tmp_keys_dma_addr, 
+				   keylen, DMA_TO_DEVICE);
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr, keylen);
+		
+	ctx->key_params.keylen = keylen;
+	
+	END_CYCLE_COUNT(STAT_OP_TYPE_SETKEY, STAT_PHASE_1);
+
+	return 0;
+}
+#endif
+
+static void ssi_hash_free_ctx(struct ssi_hash_ctx *ctx)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+
+	if (ctx->digest_buff_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr);
+		dma_unmap_single(dev, ctx->digest_buff_dma_addr,
+				 sizeof(ctx->digest_buff), DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped digest-buffer: "
+			     "digest_buff_dma_addr=0x%llX\n",
+			(unsigned long long)ctx->digest_buff_dma_addr);
+		ctx->digest_buff_dma_addr = 0;
+	}
+	if (ctx->opad_tmp_keys_dma_addr != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr);
+		dma_unmap_single(dev, ctx->opad_tmp_keys_dma_addr,
+				 sizeof(ctx->opad_tmp_keys_buff),
+				 DMA_BIDIRECTIONAL);
+		SSI_LOG_DEBUG("Unmapped opad-digest: "
+			     "opad_tmp_keys_dma_addr=0x%llX\n",
+			(unsigned long long)ctx->opad_tmp_keys_dma_addr);
+		ctx->opad_tmp_keys_dma_addr = 0;
+	}
+
+	ctx->key_params.keylen = 0;
+
+}
+
+
+static int ssi_hash_alloc_ctx(struct ssi_hash_ctx *ctx)
+{
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+
+	ctx->key_params.keylen = 0;
+
+	ctx->digest_buff_dma_addr = dma_map_single(dev, (void *)ctx->digest_buff, sizeof(ctx->digest_buff), DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, ctx->digest_buff_dma_addr)) {
+		SSI_LOG_ERR("Mapping digest len %zu B at va=%pK for DMA failed\n",
+			sizeof(ctx->digest_buff), ctx->digest_buff);
+		goto fail;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->digest_buff_dma_addr,
+						sizeof(ctx->digest_buff));
+	SSI_LOG_DEBUG("Mapped digest %zu B at va=%pK to dma=0x%llX\n",
+		sizeof(ctx->digest_buff), ctx->digest_buff,
+		(unsigned long long)ctx->digest_buff_dma_addr);
+
+	ctx->opad_tmp_keys_dma_addr = dma_map_single(dev, (void *)ctx->opad_tmp_keys_buff, sizeof(ctx->opad_tmp_keys_buff), DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, ctx->opad_tmp_keys_dma_addr)) {
+		SSI_LOG_ERR("Mapping opad digest %zu B at va=%pK for DMA failed\n",
+			sizeof(ctx->opad_tmp_keys_buff),
+			ctx->opad_tmp_keys_buff);
+		goto fail;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ctx->opad_tmp_keys_dma_addr,
+					sizeof(ctx->opad_tmp_keys_buff));
+	SSI_LOG_DEBUG("Mapped opad_tmp_keys %zu B at va=%pK to dma=0x%llX\n",
+		sizeof(ctx->opad_tmp_keys_buff), ctx->opad_tmp_keys_buff,
+		(unsigned long long)ctx->opad_tmp_keys_dma_addr);
+
+	ctx->is_hmac = false;
+	return 0;
+
+fail:
+	ssi_hash_free_ctx(ctx);
+	return -ENOMEM;
+}
+
+static int ssi_shash_cra_init(struct crypto_tfm *tfm)
+{		
+	struct ssi_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct shash_alg * shash_alg = 
+		container_of(tfm->__crt_alg, struct shash_alg, base);
+	struct ssi_hash_alg *ssi_alg =
+			container_of(shash_alg, struct ssi_hash_alg, shash_alg);
+        	
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	ctx->hash_mode = ssi_alg->hash_mode;
+	ctx->hw_mode = ssi_alg->hw_mode;
+	ctx->inter_digestsize = ssi_alg->inter_digestsize;
+	ctx->drvdata = ssi_alg->drvdata;
+
+	return ssi_hash_alloc_ctx(ctx);
+}
+
+static int ssi_ahash_cra_init(struct crypto_tfm *tfm)
+{
+	struct ssi_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct hash_alg_common * hash_alg_common = 
+		container_of(tfm->__crt_alg, struct hash_alg_common, base);
+	struct ahash_alg *ahash_alg = 
+		container_of(hash_alg_common, struct ahash_alg, halg);
+	struct ssi_hash_alg *ssi_alg =
+			container_of(ahash_alg, struct ssi_hash_alg, ahash_alg);
+
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	crypto_ahash_set_reqsize(__crypto_ahash_cast(tfm),
+				sizeof(struct ahash_req_ctx));
+
+	ctx->hash_mode = ssi_alg->hash_mode;
+	ctx->hw_mode = ssi_alg->hw_mode;
+	ctx->inter_digestsize = ssi_alg->inter_digestsize;
+	ctx->drvdata = ssi_alg->drvdata;
+
+	return ssi_hash_alloc_ctx(ctx);
+}
+
+static void ssi_hash_cra_exit(struct crypto_tfm *tfm)
+{
+	struct ssi_hash_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	SSI_LOG_DEBUG("ssi_hash_cra_exit");
+	ssi_hash_free_ctx(ctx);
+}
+
+static int ssi_mac_update(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	unsigned int block_size = crypto_tfm_alg_blocksize(&tfm->base);
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	int rc;
+	uint32_t idx = 0;
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	if (req->nbytes == 0) {
+		/* no real updates required */
+		return 0;
+	}
+
+	state->xcbc_count++;
+
+	if (unlikely(rc = ssi_buffer_mgr_map_hash_request_update(ctx->drvdata, state, req->src, req->nbytes, block_size))) {
+		if (rc == 1) {
+			SSI_LOG_DEBUG(" data size not require HW update %x\n",
+				     req->nbytes);
+			/* No hardware updates are required */
+			return 0;
+		}
+		SSI_LOG_ERR("map_ahash_request_update() failed\n");
+		return -ENOMEM;
+	}
+
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+		ssi_hash_create_xcbc_setup(req, desc, &idx);
+	} else {
+		ssi_hash_create_cmac_setup(req, desc, &idx);
+	}
+	
+	ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, true, &idx);
+
+	/* store the hash digest result in context */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, ctx->inter_digestsize, NS_BIT, 1);
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	idx++;
+
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_hash_update_complete;
+	ssi_req.user_arg = (void *)req;
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+	if (unlikely(rc != -EINPROGRESS)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, true);
+	}
+	return rc;
+}
+
+static int ssi_mac_final(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	int idx = 0;
+	int rc = 0;
+	uint32_t keySize, keyLen;
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+
+	uint32_t rem_cnt = state->buff_index ? state->buff1_cnt :
+			state->buff0_cnt;
+	
+
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+		keySize = CC_AES_128_BIT_KEY_SIZE;
+		keyLen  = CC_AES_128_BIT_KEY_SIZE;
+	} else {
+		keySize = (ctx->key_params.keylen == 24) ? AES_MAX_KEY_SIZE : ctx->key_params.keylen;
+		keyLen =  ctx->key_params.keylen;
+	}
+
+	SSI_LOG_DEBUG("===== final  xcbc reminder (%d) ====\n", rem_cnt);
+
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, req->src, req->nbytes, 0) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_hash_complete;
+	ssi_req.user_arg = (void *)req;
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+
+	if (state->xcbc_count && (rem_cnt == 0)) {
+		/* Load key for ECB decryption */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_ECB);
+		HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DRV_CRYPTO_DIRECTION_DECRYPT);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+				     (ctx->opad_tmp_keys_dma_addr + 
+				      XCBC_MAC_K1_OFFSET),
+				    keySize, NS_BIT);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+		idx++;
+
+
+		/* Initiate decryption of block state to previous block_state-XOR-M[n] */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+		HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT,0);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+		idx++;
+
+		/* Memory Barrier: wait for axi write to complete */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_NO_DMA(&desc[idx], 0, 0xfffff0);
+		HW_DESC_SET_DOUT_NO_DMA(&desc[idx], 0, 0, 1);
+		idx++;
+	}
+	
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+		ssi_hash_create_xcbc_setup(req, desc, &idx);
+	} else {
+		ssi_hash_create_cmac_setup(req, desc, &idx);
+	}
+
+	if (state->xcbc_count == 0) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
+		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		idx++;
+	} else if (rem_cnt > 0) {
+		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
+	} else {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_CONST(&desc[idx], 0x00, CC_AES_BLOCK_SIZE);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], DIN_AES_DOUT);
+		idx++;
+	}
+	
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, 1); /*TODO*/
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	idx++;
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+	if (unlikely(rc != -EINPROGRESS)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, true);
+		ssi_hash_unmap_result(dev, state, digestsize, req->result);
+	}
+	return rc;
+}
+
+static int ssi_mac_finup(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	int idx = 0;
+	int rc = 0;
+	uint32_t key_len = 0;
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+
+	SSI_LOG_DEBUG("===== finup xcbc(%d) ====\n", req->nbytes);
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	if (state->xcbc_count > 0 && req->nbytes == 0) {
+		SSI_LOG_DEBUG("No data to update. Call to fdx_mac_final \n");
+		return ssi_mac_final(req);
+	}
+	
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, req->src, req->nbytes, 1) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_hash_complete;
+	ssi_req.user_arg = (void *)req;
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+		key_len = CC_AES_128_BIT_KEY_SIZE;
+		ssi_hash_create_xcbc_setup(req, desc, &idx);
+	} else {
+		key_len = ctx->key_params.keylen;
+		ssi_hash_create_cmac_setup(req, desc, &idx);
+	}
+
+	if (req->nbytes == 0) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], key_len);
+		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		idx++;
+	} else {
+		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
+	}
+	
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, digestsize, NS_BIT, 1); /*TODO*/
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	idx++;
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+	if (unlikely(rc != -EINPROGRESS)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, true);
+		ssi_hash_unmap_result(dev, state, digestsize, req->result);
+	}
+	return rc;
+}
+
+static int ssi_mac_digest(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct device *dev = &ctx->drvdata->plat_dev->dev;
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	struct ssi_crypto_req ssi_req = {};
+	HwDesc_s desc[SSI_MAX_AHASH_SEQ_LEN];
+	uint32_t keyLen;
+	int idx = 0;
+	int rc;
+
+	SSI_LOG_DEBUG("===== -digest mac (%d) ====\n",  req->nbytes);
+	CHECK_AND_RETURN_UPON_FIPS_ERROR();
+	
+	if (unlikely(ssi_hash_map_request(dev, state, ctx) != 0)) {
+		SSI_LOG_ERR("map_ahash_source() failed\n");
+		return -ENOMEM;
+	}
+	if (unlikely(ssi_hash_map_result(dev, state, digestsize) != 0)) {
+		SSI_LOG_ERR("map_ahash_digest() failed\n");
+		return -ENOMEM;
+	}
+
+	if (unlikely(ssi_buffer_mgr_map_hash_request_final(ctx->drvdata, state, req->src, req->nbytes, 1) != 0)) {
+		SSI_LOG_ERR("map_ahash_request_final() failed\n");
+		return -ENOMEM;
+	}
+	
+	/* Setup DX request structure */
+	ssi_req.user_cb = (void *)ssi_hash_digest_complete;
+	ssi_req.user_arg = (void *)req;
+#ifdef ENABLE_CYCLE_COUNT
+	ssi_req.op_type = STAT_OP_TYPE_ENCODE; /* Use "Encode" stats */
+#endif
+
+	
+	if (ctx->hw_mode == DRV_CIPHER_XCBC_MAC) {
+		keyLen = CC_AES_128_BIT_KEY_SIZE;
+		ssi_hash_create_xcbc_setup(req, desc, &idx);
+	} else {
+		keyLen = ctx->key_params.keylen;
+		ssi_hash_create_cmac_setup(req, desc, &idx);
+	}
+
+	if (req->nbytes == 0) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode);
+		HW_DESC_SET_KEY_SIZE_AES(&desc[idx], keyLen);
+		HW_DESC_SET_CMAC_SIZE0_MODE(&desc[idx]);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+		idx++;
+	} else {
+		ssi_hash_create_data_desc(state, ctx, DIN_AES_DOUT, desc, false, &idx);
+	}
+	
+	/* Get final MAC result */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DOUT_DLLI(&desc[idx], state->digest_result_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT,1);
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[idx]);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_AES_to_DOUT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_WRITE_STATE0);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx],DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], ctx->hw_mode); 
+	idx++;
+
+	rc = send_request(ctx->drvdata, &ssi_req, desc, idx, 1);
+	if (unlikely(rc != -EINPROGRESS)) {
+		SSI_LOG_ERR("send_request() failed (rc=%d)\n", rc);
+		ssi_buffer_mgr_unmap_hash_request(dev, state, req->src, true);
+		ssi_hash_unmap_result(dev, state, digestsize, req->result);
+		ssi_hash_unmap_request(dev, state, ctx);
+	}
+	return rc;
+}
+
+//shash wrap functions
+#ifdef SYNC_ALGS
+static int ssi_shash_digest(struct shash_desc *desc, 
+			    const u8 *data, unsigned int len, u8 *out)
+{
+	struct ahash_req_ctx *state = shash_desc_ctx(desc);
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+	uint32_t digestsize = crypto_shash_digestsize(tfm);
+	struct scatterlist src;
+
+	if (len == 0) {
+		return ssi_hash_digest(state, ctx, digestsize, NULL, 0, out, NULL);
+	}
+	
+	/* sg_init_one may crash when len is 0 (depends on kernel configuration) */
+	sg_init_one(&src, (const void *)data, len);
+		
+	return ssi_hash_digest(state, ctx, digestsize, &src, len, out, NULL);
+}
+
+static int ssi_shash_update(struct shash_desc *desc, 
+						const u8 *data, unsigned int len)
+{
+	struct ahash_req_ctx *state = shash_desc_ctx(desc);
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+	uint32_t blocksize = crypto_tfm_alg_blocksize(&tfm->base);
+	struct scatterlist src;
+
+	sg_init_one(&src, (const void *)data, len);
+	
+	return ssi_hash_update(state, ctx, blocksize, &src, len, NULL);
+}
+
+static int ssi_shash_finup(struct shash_desc *desc, 
+			   const u8 *data, unsigned int len, u8 *out)
+{
+	struct ahash_req_ctx *state = shash_desc_ctx(desc);
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+	uint32_t digestsize = crypto_shash_digestsize(tfm);
+	struct scatterlist src;
+	
+	sg_init_one(&src, (const void *)data, len);
+	
+	return ssi_hash_finup(state, ctx, digestsize, &src, len, out, NULL);
+}
+
+static int ssi_shash_final(struct shash_desc *desc, u8 *out)
+{
+	struct ahash_req_ctx *state = shash_desc_ctx(desc);
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+	uint32_t digestsize = crypto_shash_digestsize(tfm);
+		
+	return ssi_hash_final(state, ctx, digestsize, NULL, 0, out, NULL);
+}
+
+static int ssi_shash_init(struct shash_desc *desc)
+{
+	struct ahash_req_ctx *state = shash_desc_ctx(desc);
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+
+	return ssi_hash_init(state, ctx);
+}
+
+#ifdef EXPORT_FIXED
+static int ssi_shash_export(struct shash_desc *desc, void *out)
+{
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+
+	return ssi_hash_export(ctx, out);
+}
+
+static int ssi_shash_import(struct shash_desc *desc, const void *in)
+{
+	struct crypto_shash *tfm = desc->tfm;
+	struct ssi_hash_ctx *ctx = crypto_shash_ctx(tfm);
+	
+	return ssi_hash_import(ctx, in);
+}
+#endif
+
+static int ssi_shash_setkey(struct crypto_shash *tfm, 
+			    const u8 *key, unsigned int keylen)
+{
+	return ssi_hash_setkey((void *) tfm, key, keylen, true);
+}
+
+#endif /* SYNC_ALGS */
+
+//ahash wrap functions
+static int ssi_ahash_digest(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	
+	return ssi_hash_digest(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
+}
+
+static int ssi_ahash_update(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	unsigned int block_size = crypto_tfm_alg_blocksize(&tfm->base);
+	
+	return ssi_hash_update(state, ctx, block_size, req->src, req->nbytes, (void *)req);
+}
+
+static int ssi_ahash_finup(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	
+	return ssi_hash_finup(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
+}
+
+static int ssi_ahash_final(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	uint32_t digestsize = crypto_ahash_digestsize(tfm);
+	
+	return ssi_hash_final(state, ctx, digestsize, req->src, req->nbytes, req->result, (void *)req);
+}
+
+static int ssi_ahash_init(struct ahash_request *req)
+{
+	struct ahash_req_ctx *state = ahash_request_ctx(req);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);	
+
+	SSI_LOG_DEBUG("===== init (%d) ====\n", req->nbytes);
+
+	return ssi_hash_init(state, ctx);
+}
+
+#ifdef EXPORT_FIXED
+static int ssi_ahash_export(struct ahash_request *req, void *out)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	
+	return ssi_hash_export(ctx, out);
+}
+
+static int ssi_ahash_import(struct ahash_request *req, const void *in)
+{
+	struct crypto_ahash *ahash = crypto_ahash_reqtfm(req);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(ahash);
+	
+	return ssi_hash_import(ctx, in);
+}
+#endif
+
+static int ssi_ahash_setkey(struct crypto_ahash *ahash,
+			const u8 *key, unsigned int keylen)
+{	
+	return ssi_hash_setkey((void *) ahash, key, keylen, false);
+}
+
+struct ssi_hash_template {
+	char name[CRYPTO_MAX_ALG_NAME];
+	char driver_name[CRYPTO_MAX_ALG_NAME];
+	char hmac_name[CRYPTO_MAX_ALG_NAME];
+	char hmac_driver_name[CRYPTO_MAX_ALG_NAME];
+	unsigned int blocksize;
+	bool synchronize;
+	union {
+		struct ahash_alg template_ahash;
+		struct shash_alg template_shash;
+	};	
+	int hash_mode;
+	int hw_mode;
+	int inter_digestsize;
+	struct ssi_drvdata *drvdata;
+};
+
+/* hash descriptors */
+static struct ssi_hash_template driver_hash[] = {
+	//Asynchronize hash template
+	{
+		.name = "sha1",
+		.driver_name = "sha1-dx",
+		.hmac_name = "hmac(sha1)",
+		.hmac_driver_name = "hmac-sha1-dx",
+		.blocksize = SHA1_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA1_DIGEST_SIZE,
+				.statesize = sizeof(struct sha1_state),
+				},
+			},
+		.hash_mode = DRV_HASH_SHA1,
+		.hw_mode = DRV_HASH_HW_SHA1,
+		.inter_digestsize = SHA1_DIGEST_SIZE,
+	},
+	{
+		.name = "sha256",
+		.driver_name = "sha256-dx",
+		.hmac_name = "hmac(sha256)",
+		.hmac_driver_name = "hmac-sha256-dx",
+		.blocksize = SHA256_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA256_DIGEST_SIZE,
+				.statesize = sizeof(struct sha256_state),
+				},
+			},
+		.hash_mode = DRV_HASH_SHA256,
+		.hw_mode = DRV_HASH_HW_SHA256,
+		.inter_digestsize = SHA256_DIGEST_SIZE,
+	},
+	{
+		.name = "sha224",
+		.driver_name = "sha224-dx",
+		.hmac_name = "hmac(sha224)",
+		.hmac_driver_name = "hmac-sha224-dx",
+		.blocksize = SHA224_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA224_DIGEST_SIZE,
+				.statesize = sizeof(struct sha256_state),
+				},
+			},
+		.hash_mode = DRV_HASH_SHA224,
+		.hw_mode = DRV_HASH_HW_SHA256,
+		.inter_digestsize = SHA256_DIGEST_SIZE,
+	},
+#if (DX_DEV_SHA_MAX > 256)
+	{
+		.name = "sha384",
+		.driver_name = "sha384-dx",
+		.hmac_name = "hmac(sha384)",
+		.hmac_driver_name = "hmac-sha384-dx",
+		.blocksize = SHA384_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA384_DIGEST_SIZE,
+				.statesize = sizeof(struct sha512_state),
+				},
+			},
+		.hash_mode = DRV_HASH_SHA384,
+		.hw_mode = DRV_HASH_HW_SHA512,
+		.inter_digestsize = SHA512_DIGEST_SIZE,
+	},
+	{
+		.name = "sha512",
+		.driver_name = "sha512-dx",
+		.hmac_name = "hmac(sha512)",
+		.hmac_driver_name = "hmac-sha512-dx",
+		.blocksize = SHA512_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = SHA512_DIGEST_SIZE,
+				.statesize = sizeof(struct sha512_state),
+				},
+			},
+		.hash_mode = DRV_HASH_SHA512,
+		.hw_mode = DRV_HASH_HW_SHA512,
+		.inter_digestsize = SHA512_DIGEST_SIZE,
+	},
+#endif
+	{
+		.name = "md5",
+		.driver_name = "md5-dx",
+		.hmac_name = "hmac(md5)",
+		.hmac_driver_name = "hmac-md5-dx",
+		.blocksize = MD5_HMAC_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_ahash_update,
+			.final = ssi_ahash_final,
+			.finup = ssi_ahash_finup,
+			.digest = ssi_ahash_digest,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.setkey = ssi_ahash_setkey,
+			.halg = {
+				.digestsize = MD5_DIGEST_SIZE,
+				.statesize = sizeof(struct md5_state),
+				},
+			},
+		.hash_mode = DRV_HASH_MD5,
+		.hw_mode = DRV_HASH_HW_MD5,
+		.inter_digestsize = MD5_DIGEST_SIZE,
+	},
+	{
+		.name = "xcbc(aes)",
+		.driver_name = "xcbc-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_mac_update,
+			.final = ssi_mac_final,
+			.finup = ssi_mac_finup,
+			.digest = ssi_mac_digest,
+			.setkey = ssi_xcbc_setkey,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.halg = {
+				.digestsize = AES_BLOCK_SIZE,
+				.statesize = sizeof(struct aeshash_state),
+				},
+			},
+			.hash_mode = DRV_HASH_NULL,
+			.hw_mode = DRV_CIPHER_XCBC_MAC,
+			.inter_digestsize = AES_BLOCK_SIZE,
+		},
+#if SSI_CC_HAS_CMAC
+	{
+		.name = "cmac(aes)",
+		.driver_name = "cmac-aes-dx",
+		.blocksize = AES_BLOCK_SIZE,
+		.synchronize = false,
+		.template_ahash = {
+			.init = ssi_ahash_init,
+			.update = ssi_mac_update,
+			.final = ssi_mac_final,
+			.finup = ssi_mac_finup,
+			.digest = ssi_mac_digest,
+			.setkey = ssi_cmac_setkey,
+#ifdef EXPORT_FIXED
+			.export = ssi_ahash_export,
+			.import = ssi_ahash_import,
+#endif
+			.halg = {
+				.digestsize = AES_BLOCK_SIZE,
+				.statesize = sizeof(struct aeshash_state),
+				},
+			},
+			.hash_mode = DRV_HASH_NULL,
+			.hw_mode = DRV_CIPHER_CMAC,
+			.inter_digestsize = AES_BLOCK_SIZE,
+		},
+#endif
+	
+};
+
+static struct ssi_hash_alg *
+ssi_hash_create_alg(struct ssi_hash_template *template, bool keyed)
+{
+	struct ssi_hash_alg *t_crypto_alg;
+	struct crypto_alg *alg;
+
+	t_crypto_alg = kzalloc(sizeof(struct ssi_hash_alg), GFP_KERNEL);
+	if (!t_crypto_alg) {
+		SSI_LOG_ERR("failed to allocate t_alg\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	t_crypto_alg->synchronize = template->synchronize;
+	if (template->synchronize) {
+		struct shash_alg *halg;
+		t_crypto_alg->shash_alg = template->template_shash;
+		halg = &t_crypto_alg->shash_alg;
+		alg = &halg->base;
+		if (!keyed) halg->setkey = NULL;
+	} else {
+		struct ahash_alg *halg;
+		t_crypto_alg->ahash_alg = template->template_ahash;
+		halg = &t_crypto_alg->ahash_alg;
+		alg = &halg->halg.base;
+		if (!keyed) halg->setkey = NULL;
+	}
+
+	if (keyed) {
+		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->hmac_name);
+		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->hmac_driver_name);
+	} else {
+		snprintf(alg->cra_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->name);
+		snprintf(alg->cra_driver_name, CRYPTO_MAX_ALG_NAME, "%s",
+			 template->driver_name);
+	}
+	alg->cra_module = THIS_MODULE;
+	alg->cra_ctxsize = sizeof(struct ssi_hash_ctx);
+	alg->cra_priority = SSI_CRA_PRIO;
+	alg->cra_blocksize = template->blocksize;
+	alg->cra_alignmask = 0;
+	alg->cra_exit = ssi_hash_cra_exit;
+	
+	if (template->synchronize) {
+		alg->cra_init = ssi_shash_cra_init;		
+		alg->cra_flags = CRYPTO_ALG_TYPE_SHASH |
+			CRYPTO_ALG_KERN_DRIVER_ONLY;
+		alg->cra_type = &crypto_shash_type;
+	} else {
+		alg->cra_init = ssi_ahash_cra_init;
+		alg->cra_flags = CRYPTO_ALG_ASYNC | CRYPTO_ALG_TYPE_AHASH |
+			CRYPTO_ALG_KERN_DRIVER_ONLY;
+		alg->cra_type = &crypto_ahash_type;
+	}
+
+	t_crypto_alg->hash_mode = template->hash_mode;
+	t_crypto_alg->hw_mode = template->hw_mode;
+	t_crypto_alg->inter_digestsize = template->inter_digestsize;
+
+	return t_crypto_alg;
+}
+
+int ssi_hash_init_sram_digest_consts(struct ssi_drvdata *drvdata)
+{
+	struct ssi_hash_handle *hash_handle = drvdata->hash_handle;
+	ssi_sram_addr_t sram_buff_ofs = hash_handle->digest_len_sram_addr;
+	unsigned int larval_seq_len = 0;
+	HwDesc_s larval_seq[CC_DIGEST_SIZE_MAX/sizeof(uint32_t)];
+	int rc = 0;
+#if (DX_DEV_SHA_MAX > 256)
+	int i;
+#endif
+
+	/* Copy-to-sram digest-len */
+	ssi_sram_mgr_const2sram_desc(digest_len_init, sram_buff_ofs,
+		ARRAY_SIZE(digest_len_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+
+	sram_buff_ofs += sizeof(digest_len_init);
+	larval_seq_len = 0;
+
+#if (DX_DEV_SHA_MAX > 256)
+	/* Copy-to-sram digest-len for sha384/512 */
+	ssi_sram_mgr_const2sram_desc(digest_len_sha512_init, sram_buff_ofs,
+		ARRAY_SIZE(digest_len_sha512_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+
+	sram_buff_ofs += sizeof(digest_len_sha512_init);
+	larval_seq_len = 0;
+#endif
+
+	/* The initial digests offset */
+	hash_handle->larval_digest_sram_addr = sram_buff_ofs;
+
+	/* Copy-to-sram initial SHA* digests */
+	ssi_sram_mgr_const2sram_desc(md5_init, sram_buff_ofs,
+		ARRAY_SIZE(md5_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+	sram_buff_ofs += sizeof(md5_init);
+	larval_seq_len = 0;
+
+	ssi_sram_mgr_const2sram_desc(sha1_init, sram_buff_ofs,
+		ARRAY_SIZE(sha1_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+	sram_buff_ofs += sizeof(sha1_init);
+	larval_seq_len = 0;
+
+	ssi_sram_mgr_const2sram_desc(sha224_init, sram_buff_ofs,
+		ARRAY_SIZE(sha224_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+	sram_buff_ofs += sizeof(sha224_init);
+	larval_seq_len = 0;
+
+	ssi_sram_mgr_const2sram_desc(sha256_init, sram_buff_ofs,
+		ARRAY_SIZE(sha256_init), larval_seq, &larval_seq_len);
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0))
+		goto init_digest_const_err;
+	sram_buff_ofs += sizeof(sha256_init);
+	larval_seq_len = 0;
+
+#if (DX_DEV_SHA_MAX > 256)
+	/* We are forced to swap each double-word larval before copying to sram */
+	for (i = 0; i < ARRAY_SIZE(sha384_init); i++) {
+		const uint32_t const0 = ((uint32_t *)((uint64_t *)&sha384_init[i]))[1];
+		const uint32_t const1 = ((uint32_t *)((uint64_t *)&sha384_init[i]))[0];
+
+		ssi_sram_mgr_const2sram_desc(&const0, sram_buff_ofs, 1,
+			larval_seq, &larval_seq_len);
+		sram_buff_ofs += sizeof(uint32_t);
+		ssi_sram_mgr_const2sram_desc(&const1, sram_buff_ofs, 1,
+			larval_seq, &larval_seq_len);
+		sram_buff_ofs += sizeof(uint32_t);
+	}
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("send_request() failed (rc = %d)\n", rc);
+		goto init_digest_const_err;
+	}
+	larval_seq_len = 0;
+
+	for (i = 0; i < ARRAY_SIZE(sha512_init); i++) {
+		const uint32_t const0 = ((uint32_t *)((uint64_t *)&sha512_init[i]))[1];
+		const uint32_t const1 = ((uint32_t *)((uint64_t *)&sha512_init[i]))[0];
+
+		ssi_sram_mgr_const2sram_desc(&const0, sram_buff_ofs, 1,
+			larval_seq, &larval_seq_len);
+		sram_buff_ofs += sizeof(uint32_t);
+		ssi_sram_mgr_const2sram_desc(&const1, sram_buff_ofs, 1,
+			larval_seq, &larval_seq_len);
+		sram_buff_ofs += sizeof(uint32_t);
+	}
+	rc = send_request_init(drvdata, larval_seq, larval_seq_len);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("send_request() failed (rc = %d)\n", rc);
+		goto init_digest_const_err;
+	}
+#endif
+
+init_digest_const_err:
+	return rc;
+}
+
+int ssi_hash_alloc(struct ssi_drvdata *drvdata)
+{
+	struct ssi_hash_handle *hash_handle;
+	ssi_sram_addr_t sram_buff;
+	uint32_t sram_size_to_alloc;
+	int rc = 0;
+	int alg;
+
+	hash_handle = kzalloc(sizeof(struct ssi_hash_handle), GFP_KERNEL);
+	if (hash_handle == NULL) {
+		SSI_LOG_ERR("kzalloc failed to allocate %zu B\n",
+			sizeof(struct ssi_hash_handle));
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	drvdata->hash_handle = hash_handle;
+
+	sram_size_to_alloc = sizeof(digest_len_init) +
+#if (DX_DEV_SHA_MAX > 256)
+			sizeof(digest_len_sha512_init) +
+			sizeof(sha384_init) +
+			sizeof(sha512_init) +
+#endif
+			sizeof(md5_init) +
+			sizeof(sha1_init) +
+			sizeof(sha224_init) +
+			sizeof(sha256_init);
+				
+	sram_buff = ssi_sram_mgr_alloc(drvdata, sram_size_to_alloc);
+	if (sram_buff == NULL_SRAM_ADDR) {
+		SSI_LOG_ERR("SRAM pool exhausted\n");
+		rc = -ENOMEM;
+		goto fail;
+	}
+
+	/* The initial digest-len offset */
+	hash_handle->digest_len_sram_addr = sram_buff;
+
+	/*must be set before the alg registration as it is being used there*/
+	rc = ssi_hash_init_sram_digest_consts(drvdata);
+	if (unlikely(rc != 0)) {
+		SSI_LOG_ERR("Init digest CONST failed (rc=%d)\n", rc);
+		goto fail;
+	}
+
+	INIT_LIST_HEAD(&hash_handle->hash_list);
+
+	/* ahash registration */
+	for (alg = 0; alg < ARRAY_SIZE(driver_hash); alg++) {
+		struct ssi_hash_alg *t_alg;
+		
+		/* register hmac version */
+
+		if ((((struct ssi_hash_template)driver_hash[alg]).hw_mode != DRV_CIPHER_XCBC_MAC) &&
+			(((struct ssi_hash_template)driver_hash[alg]).hw_mode != DRV_CIPHER_CMAC)) {
+			t_alg = ssi_hash_create_alg(&driver_hash[alg], true);
+			if (IS_ERR(t_alg)) {
+				rc = PTR_ERR(t_alg);
+				SSI_LOG_ERR("%s alg allocation failed\n",
+					 driver_hash[alg].driver_name);
+				goto fail;
+			}
+			t_alg->drvdata = drvdata;
+	
+			if (t_alg->synchronize) {
+				rc = crypto_register_shash(&t_alg->shash_alg);
+				if (unlikely(rc != 0)) {
+					SSI_LOG_ERR("%s alg registration failed\n",
+						t_alg->shash_alg.base.cra_driver_name);
+					kfree(t_alg);
+					goto fail;
+				} else
+					list_add_tail(&t_alg->entry, &hash_handle->hash_list);
+			} else {
+				rc = crypto_register_ahash(&t_alg->ahash_alg);
+				if (unlikely(rc != 0)) {
+					SSI_LOG_ERR("%s alg registration failed\n",
+						t_alg->ahash_alg.halg.base.cra_driver_name);
+					kfree(t_alg);
+					goto fail;
+				} else
+					list_add_tail(&t_alg->entry, &hash_handle->hash_list);
+			}
+		}
+
+		/* register hash version */
+		t_alg = ssi_hash_create_alg(&driver_hash[alg], false);
+		if (IS_ERR(t_alg)) {
+			rc = PTR_ERR(t_alg);
+			SSI_LOG_ERR("%s alg allocation failed\n",
+				 driver_hash[alg].driver_name);
+			goto fail;
+		}
+		t_alg->drvdata = drvdata;
+		
+		if (t_alg->synchronize) {
+			rc = crypto_register_shash(&t_alg->shash_alg);
+			if (unlikely(rc != 0)) {
+				SSI_LOG_ERR("%s alg registration failed\n",
+					t_alg->shash_alg.base.cra_driver_name);
+				kfree(t_alg);
+				goto fail;
+			} else
+				list_add_tail(&t_alg->entry, &hash_handle->hash_list);	
+				
+		} else {
+			rc = crypto_register_ahash(&t_alg->ahash_alg);
+			if (unlikely(rc != 0)) {
+				SSI_LOG_ERR("%s alg registration failed\n",
+					t_alg->ahash_alg.halg.base.cra_driver_name);
+				kfree(t_alg);
+				goto fail;
+			} else
+				list_add_tail(&t_alg->entry, &hash_handle->hash_list);
+		}
+	}
+
+	return 0;
+
+fail:
+
+	if (drvdata->hash_handle != NULL) {
+		kfree(drvdata->hash_handle);
+		drvdata->hash_handle = NULL;
+	}
+	return rc;
+}
+
+int ssi_hash_free(struct ssi_drvdata *drvdata)
+{
+	struct ssi_hash_alg *t_hash_alg, *hash_n;
+	struct ssi_hash_handle *hash_handle = drvdata->hash_handle;
+
+	if (hash_handle != NULL) {
+
+		list_for_each_entry_safe(t_hash_alg, hash_n, &hash_handle->hash_list, entry) {
+			if (t_hash_alg->synchronize) {
+				crypto_unregister_shash(&t_hash_alg->shash_alg);
+			} else {
+				crypto_unregister_ahash(&t_hash_alg->ahash_alg);
+			}
+			list_del(&t_hash_alg->entry);
+			kfree(t_hash_alg);
+		}
+		
+		kfree(hash_handle);
+		drvdata->hash_handle = NULL;
+	}
+	return 0;
+}
+
+static void ssi_hash_create_xcbc_setup(struct ahash_request *areq, 
+				  HwDesc_s desc[],
+				  unsigned int *seq_size) {
+	unsigned int idx = *seq_size;
+	struct ahash_req_ctx *state = ahash_request_ctx(areq);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	/* Setup XCBC MAC K1 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
+						    + XCBC_MAC_K1_OFFSET),
+			     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Setup XCBC MAC K2 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
+						    + XCBC_MAC_K2_OFFSET),
+			      CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Setup XCBC MAC K3 */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, (ctx->opad_tmp_keys_dma_addr 
+						    + XCBC_MAC_K3_OFFSET),
+			     CC_AES_128_BIT_KEY_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE2);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Loading MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_XCBC_MAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+	*seq_size = idx;
+}
+
+static void ssi_hash_create_cmac_setup(struct ahash_request *areq, 
+				  HwDesc_s desc[],
+				  unsigned int *seq_size)
+{
+	unsigned int idx = *seq_size;
+	struct ahash_req_ctx *state = ahash_request_ctx(areq);
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(areq);
+	struct ssi_hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	/* Setup CMAC Key */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, ctx->opad_tmp_keys_dma_addr,
+		((ctx->key_params.keylen == 24) ? AES_MAX_KEY_SIZE : ctx->key_params.keylen), NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->key_params.keylen);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+
+	/* Load MAC state */
+	HW_DESC_INIT(&desc[idx]);
+	HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, state->digest_buff_dma_addr, CC_AES_BLOCK_SIZE, NS_BIT);
+	HW_DESC_SET_SETUP_MODE(&desc[idx], SETUP_LOAD_STATE0);
+	HW_DESC_SET_CIPHER_MODE(&desc[idx], DRV_CIPHER_CMAC);
+	HW_DESC_SET_CIPHER_CONFIG0(&desc[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_KEY_SIZE_AES(&desc[idx], ctx->key_params.keylen);
+	HW_DESC_SET_FLOW_MODE(&desc[idx], S_DIN_to_AES);
+	idx++;
+	*seq_size = idx;
+}
+
+static void ssi_hash_create_data_desc(struct ahash_req_ctx *areq_ctx,
+				      struct ssi_hash_ctx *ctx,
+				      unsigned int flow_mode,
+				      HwDesc_s desc[],
+				      bool is_not_last_data, 
+				      unsigned int *seq_size)
+{
+	unsigned int idx = *seq_size;
+
+	if (likely(areq_ctx->data_dma_buf_type == SSI_DMA_BUF_DLLI)) {
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+				     sg_dma_address(areq_ctx->curr_sg), 
+				     areq_ctx->curr_sg->length, NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		idx++;
+	} else {
+		if (areq_ctx->data_dma_buf_type == SSI_DMA_BUF_NULL) {
+			SSI_LOG_DEBUG(" NULL mode\n");
+			/* nothing to build */
+			return;
+		}
+		/* bypass */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_DLLI, 
+				     areq_ctx->mlli_params.mlli_dma_addr, 
+				     areq_ctx->mlli_params.mlli_len, 
+				     NS_BIT);
+		HW_DESC_SET_DOUT_SRAM(&desc[idx], 
+				      ctx->drvdata->mlli_sram_addr, 
+				      areq_ctx->mlli_params.mlli_len);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], BYPASS);
+		idx++;
+		/* process */
+		HW_DESC_INIT(&desc[idx]);
+		HW_DESC_SET_DIN_TYPE(&desc[idx], DMA_MLLI, 
+				     ctx->drvdata->mlli_sram_addr, 
+				     areq_ctx->mlli_nents,
+				     NS_BIT);
+		HW_DESC_SET_FLOW_MODE(&desc[idx], flow_mode);
+		idx++;
+	}
+	if (is_not_last_data) {
+		HW_DESC_SET_DIN_NOT_LAST_INDICATION(&desc[idx-1]);
+	}
+	/* return updated desc sequence size */
+	*seq_size = idx;
+}
+
+/*!
+ * Gets the address of the initial digest in SRAM 
+ * according to the given hash mode
+ * 
+ * \param drvdata
+ * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256
+ * 
+ * \return uint32_t The address of the inital digest in SRAM
+ */
+ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, uint32_t mode)
+{
+	struct ssi_drvdata *_drvdata = (struct ssi_drvdata *)drvdata;
+	struct ssi_hash_handle *hash_handle = _drvdata->hash_handle;
+
+	switch (mode) {
+	case DRV_HASH_NULL:
+		break; /*Ignore*/
+	case DRV_HASH_MD5:
+		return (hash_handle->larval_digest_sram_addr);
+	case DRV_HASH_SHA1:
+		return (hash_handle->larval_digest_sram_addr +
+			sizeof(md5_init));
+	case DRV_HASH_SHA224:
+		return (hash_handle->larval_digest_sram_addr +
+			sizeof(md5_init) +
+			sizeof(sha1_init));
+	case DRV_HASH_SHA256:
+		return (hash_handle->larval_digest_sram_addr +
+			sizeof(md5_init) +
+			sizeof(sha1_init) +
+			sizeof(sha224_init));
+#if (DX_DEV_SHA_MAX > 256)
+	case DRV_HASH_SHA384:
+		return (hash_handle->larval_digest_sram_addr +
+			sizeof(md5_init) +
+			sizeof(sha1_init) +
+			sizeof(sha224_init) +
+			sizeof(sha256_init));
+	case DRV_HASH_SHA512:
+		return (hash_handle->larval_digest_sram_addr +
+			sizeof(md5_init) +
+			sizeof(sha1_init) +
+			sizeof(sha224_init) +
+			sizeof(sha256_init) +
+			sizeof(sha384_init));
+#endif
+	default:
+		SSI_LOG_ERR("Invalid hash mode (%d)\n", mode);
+	}
+
+	/*This is valid wrong value to avoid kernel crash*/
+	return hash_handle->larval_digest_sram_addr;
+}
+
+ssi_sram_addr_t
+ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, uint32_t mode)
+{
+	struct ssi_drvdata *_drvdata = (struct ssi_drvdata *)drvdata;
+	struct ssi_hash_handle *hash_handle = _drvdata->hash_handle;
+	ssi_sram_addr_t digest_len_addr = hash_handle->digest_len_sram_addr;
+
+	switch (mode) {
+	case DRV_HASH_SHA1:
+	case DRV_HASH_SHA224:
+	case DRV_HASH_SHA256:
+	case DRV_HASH_MD5:
+		return digest_len_addr;
+#if (DX_DEV_SHA_MAX > 256)
+	case DRV_HASH_SHA384:
+	case DRV_HASH_SHA512:
+		return  digest_len_addr + sizeof(digest_len_init);
+#endif
+	default:
+		return digest_len_addr; /*to avoid kernel crash*/
+	}
+}
+
diff --git a/drivers/staging/ccree/ssi_hash.h b/drivers/staging/ccree/ssi_hash.h
new file mode 100644
index 000000000000..a2b076d3af72
--- /dev/null
+++ b/drivers/staging/ccree/ssi_hash.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_hash.h
+   ARM CryptoCell Hash Crypto API
+ */
+
+#ifndef __SSI_HASH_H__
+#define __SSI_HASH_H__
+
+#include "ssi_buffer_mgr.h"
+
+#define HMAC_IPAD_CONST	0x36363636
+#define HMAC_OPAD_CONST	0x5C5C5C5C
+#if (DX_DEV_SHA_MAX > 256)
+#define HASH_LEN_SIZE 16
+#define SSI_MAX_HASH_DIGEST_SIZE	SHA512_DIGEST_SIZE
+#define SSI_MAX_HASH_BLCK_SIZE SHA512_BLOCK_SIZE
+#else
+#define HASH_LEN_SIZE 8
+#define SSI_MAX_HASH_DIGEST_SIZE	SHA256_DIGEST_SIZE
+#define SSI_MAX_HASH_BLCK_SIZE SHA256_BLOCK_SIZE
+#endif
+
+#define XCBC_MAC_K1_OFFSET 0
+#define XCBC_MAC_K2_OFFSET 16
+#define XCBC_MAC_K3_OFFSET 32
+
+// this struct was taken from drivers/crypto/nx/nx-aes-xcbc.c and it is used for xcbc/cmac statesize
+struct aeshash_state {
+	u8 state[AES_BLOCK_SIZE];
+	unsigned int count;
+	u8 buffer[AES_BLOCK_SIZE];
+};
+
+/* ahash state */
+struct ahash_req_ctx {
+	uint8_t* buff0;
+	uint8_t* buff1;
+	uint8_t* digest_result_buff;
+	struct async_gen_req_ctx gen_ctx;
+	enum ssi_req_dma_buf_type data_dma_buf_type;
+	uint8_t *digest_buff;
+	uint8_t *opad_digest_buff;
+	uint8_t *digest_bytes_len;
+	dma_addr_t opad_digest_dma_addr;
+	dma_addr_t digest_buff_dma_addr;
+	dma_addr_t digest_bytes_len_dma_addr;
+	dma_addr_t digest_result_dma_addr;
+	uint32_t buff0_cnt;
+	uint32_t buff1_cnt;
+	uint32_t buff_index;
+	uint32_t xcbc_count; /* count xcbc update operatations */
+	struct scatterlist buff_sg[2];
+	struct scatterlist *curr_sg;
+	uint32_t in_nents;
+	uint32_t mlli_nents;
+	struct mlli_params mlli_params;	
+};
+
+int ssi_hash_alloc(struct ssi_drvdata *drvdata);
+int ssi_hash_init_sram_digest_consts(struct ssi_drvdata *drvdata);
+int ssi_hash_free(struct ssi_drvdata *drvdata);
+
+/*!
+ * Gets the initial digest length
+ * 
+ * \param drvdata 
+ * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256/SHA384/SHA512
+ * 
+ * \return uint32_t returns the address of the initial digest length in SRAM
+ */
+ssi_sram_addr_t
+ssi_ahash_get_initial_digest_len_sram_addr(void *drvdata, uint32_t mode);
+
+/*!
+ * Gets the address of the initial digest in SRAM 
+ * according to the given hash mode
+ * 
+ * \param drvdata 
+ * \param mode The Hash mode. Supported modes: MD5/SHA1/SHA224/SHA256/SHA384/SHA512
+ * 
+ * \return uint32_t The address of the inital digest in SRAM
+ */
+ssi_sram_addr_t ssi_ahash_get_larval_digest_sram_addr(void *drvdata, uint32_t mode);
+
+#endif /*__SSI_HASH_H__*/
+
diff --git a/drivers/staging/ccree/ssi_ivgen.c b/drivers/staging/ccree/ssi_ivgen.c
new file mode 100644
index 000000000000..f16f4692f404
--- /dev/null
+++ b/drivers/staging/ccree/ssi_ivgen.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/platform_device.h>
+#include <crypto/ctr.h>
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "ssi_ivgen.h"
+#include "ssi_request_mgr.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_buffer_mgr.h"
+
+/* The max. size of pool *MUST* be <= SRAM total size */
+#define SSI_IVPOOL_SIZE 1024
+/* The first 32B fraction of pool are dedicated to the
+   next encryption "key" & "IV" for pool regeneration */
+#define SSI_IVPOOL_META_SIZE (CC_AES_IV_SIZE + AES_KEYSIZE_128)
+#define SSI_IVPOOL_GEN_SEQ_LEN	4
+
+/**
+ * struct ssi_ivgen_ctx -IV pool generation context 
+ * @pool:          the start address of the iv-pool resides in internal RAM 
+ * @ctr_key_dma:   address of pool's encryption key material in internal RAM
+ * @ctr_iv_dma:    address of pool's counter iv in internal RAM
+ * @next_iv_ofs:   the offset to the next available IV in pool
+ * @pool_meta:     virt. address of the initial enc. key/IV
+ * @pool_meta_dma: phys. address of the initial enc. key/IV
+ */
+struct ssi_ivgen_ctx {
+	ssi_sram_addr_t pool;
+	ssi_sram_addr_t ctr_key;
+	ssi_sram_addr_t ctr_iv;
+	uint32_t next_iv_ofs;
+	uint8_t *pool_meta;
+	dma_addr_t pool_meta_dma;
+};
+
+/*!
+ * Generates SSI_IVPOOL_SIZE of random bytes by 
+ * encrypting 0's using AES128-CTR.
+ * 
+ * \param ivgen iv-pool context
+ * \param iv_seq IN/OUT array to the descriptors sequence
+ * \param iv_seq_len IN/OUT pointer to the sequence length 
+ */
+static int ssi_ivgen_generate_pool(
+	struct ssi_ivgen_ctx *ivgen_ctx,
+	HwDesc_s iv_seq[],
+	unsigned int *iv_seq_len)
+{
+	unsigned int idx = *iv_seq_len;
+
+	if ( (*iv_seq_len + SSI_IVPOOL_GEN_SEQ_LEN) > SSI_IVPOOL_SEQ_LEN) {
+		/* The sequence will be longer than allowed */
+		return -EINVAL;
+	}
+	/* Setup key */
+	HW_DESC_INIT(&iv_seq[idx]);
+	HW_DESC_SET_DIN_SRAM(&iv_seq[idx], ivgen_ctx->ctr_key, AES_KEYSIZE_128);
+	HW_DESC_SET_SETUP_MODE(&iv_seq[idx], SETUP_LOAD_KEY0);
+	HW_DESC_SET_CIPHER_CONFIG0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], S_DIN_to_AES);
+	HW_DESC_SET_KEY_SIZE_AES(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_CIPHER_MODE(&iv_seq[idx], DRV_CIPHER_CTR);
+	idx++;
+
+	/* Setup cipher state */
+	HW_DESC_INIT(&iv_seq[idx]);
+	HW_DESC_SET_DIN_SRAM(&iv_seq[idx], ivgen_ctx->ctr_iv, CC_AES_IV_SIZE);
+	HW_DESC_SET_CIPHER_CONFIG0(&iv_seq[idx], DESC_DIRECTION_ENCRYPT_ENCRYPT);
+	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], S_DIN_to_AES);
+	HW_DESC_SET_SETUP_MODE(&iv_seq[idx], SETUP_LOAD_STATE1);
+	HW_DESC_SET_KEY_SIZE_AES(&iv_seq[idx], CC_AES_128_BIT_KEY_SIZE);
+	HW_DESC_SET_CIPHER_MODE(&iv_seq[idx], DRV_CIPHER_CTR);
+	idx++;
+
+	/* Perform dummy encrypt to skip first block */
+	HW_DESC_INIT(&iv_seq[idx]);
+	HW_DESC_SET_DIN_CONST(&iv_seq[idx], 0, CC_AES_IV_SIZE);
+	HW_DESC_SET_DOUT_SRAM(&iv_seq[idx], ivgen_ctx->pool, CC_AES_IV_SIZE);
+	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], DIN_AES_DOUT);
+	idx++;
+
+	/* Generate IV pool */
+	HW_DESC_INIT(&iv_seq[idx]);
+	HW_DESC_SET_DIN_CONST(&iv_seq[idx], 0, SSI_IVPOOL_SIZE);
+	HW_DESC_SET_DOUT_SRAM(&iv_seq[idx], ivgen_ctx->pool, SSI_IVPOOL_SIZE);
+	HW_DESC_SET_FLOW_MODE(&iv_seq[idx], DIN_AES_DOUT);
+	idx++;
+
+	*iv_seq_len = idx; /* Update sequence length */
+
+	/* queue ordering assures pool readiness */
+	ivgen_ctx->next_iv_ofs = SSI_IVPOOL_META_SIZE;
+
+	return 0;
+}
+
+/*!
+ * Generates the initial pool in SRAM. 
+ * This function should be invoked when resuming DX driver. 
+ * 
+ * \param drvdata 
+ *  
+ * \return int Zero for success, negative value otherwise.
+ */
+int ssi_ivgen_init_sram_pool(struct ssi_drvdata *drvdata)
+{
+	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
+	HwDesc_s iv_seq[SSI_IVPOOL_SEQ_LEN];
+	unsigned int iv_seq_len = 0;
+	int rc;
+
+	/* Generate initial enc. key/iv */
+	get_random_bytes(ivgen_ctx->pool_meta, SSI_IVPOOL_META_SIZE);
+
+	/* The first 32B reserved for the enc. Key/IV */
+	ivgen_ctx->ctr_key = ivgen_ctx->pool;
+	ivgen_ctx->ctr_iv = ivgen_ctx->pool + AES_KEYSIZE_128;
+
+	/* Copy initial enc. key and IV to SRAM at a single descriptor */
+	HW_DESC_INIT(&iv_seq[iv_seq_len]);
+	HW_DESC_SET_DIN_TYPE(&iv_seq[iv_seq_len], DMA_DLLI,
+		ivgen_ctx->pool_meta_dma, SSI_IVPOOL_META_SIZE,
+		NS_BIT);
+	HW_DESC_SET_DOUT_SRAM(&iv_seq[iv_seq_len], ivgen_ctx->pool,
+		SSI_IVPOOL_META_SIZE);
+	HW_DESC_SET_FLOW_MODE(&iv_seq[iv_seq_len], BYPASS);
+	iv_seq_len++;
+
+	/* Generate initial pool */
+	rc = ssi_ivgen_generate_pool(ivgen_ctx, iv_seq, &iv_seq_len);
+	if (unlikely(rc != 0)) {
+		return rc;
+	}
+	/* Fire-and-forget */
+	return send_request_init(drvdata, iv_seq, iv_seq_len);
+}
+
+/*!
+ * Free iv-pool and ivgen context.
+ *  
+ * \param drvdata 
+ */
+void ssi_ivgen_fini(struct ssi_drvdata *drvdata)
+{
+	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
+	struct device *device = &(drvdata->plat_dev->dev);
+
+	if (ivgen_ctx == NULL)
+		return;
+
+	if (ivgen_ctx->pool_meta != NULL) {
+		memset(ivgen_ctx->pool_meta, 0, SSI_IVPOOL_META_SIZE);
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(ivgen_ctx->pool_meta_dma);
+		dma_free_coherent(device, SSI_IVPOOL_META_SIZE,
+			ivgen_ctx->pool_meta, ivgen_ctx->pool_meta_dma);
+	}
+
+	ivgen_ctx->pool = NULL_SRAM_ADDR;
+
+	/* release "this" context */
+	kfree(ivgen_ctx);
+}
+
+/*!
+ * Allocates iv-pool and maps resources. 
+ * This function generates the first IV pool.  
+ * 
+ * \param drvdata Driver's private context
+ * 
+ * \return int Zero for success, negative value otherwise.
+ */
+int ssi_ivgen_init(struct ssi_drvdata *drvdata)
+{
+	struct ssi_ivgen_ctx *ivgen_ctx;
+	struct device *device = &drvdata->plat_dev->dev;
+	int rc;
+
+	/* Allocate "this" context */
+	drvdata->ivgen_handle = kzalloc(sizeof(struct ssi_ivgen_ctx), GFP_KERNEL);
+	if (!drvdata->ivgen_handle) {
+		SSI_LOG_ERR("Not enough memory to allocate IVGEN context "
+			   "(%zu B)\n", sizeof(struct ssi_ivgen_ctx));
+		rc = -ENOMEM;
+		goto out;
+	}
+	ivgen_ctx = drvdata->ivgen_handle;
+
+	/* Allocate pool's header for intial enc. key/IV */
+	ivgen_ctx->pool_meta = dma_alloc_coherent(device, SSI_IVPOOL_META_SIZE,
+			&ivgen_ctx->pool_meta_dma, GFP_KERNEL);
+	if (!ivgen_ctx->pool_meta) {
+		SSI_LOG_ERR("Not enough memory to allocate DMA of pool_meta "
+			   "(%u B)\n", SSI_IVPOOL_META_SIZE);
+		rc = -ENOMEM;
+		goto out;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(ivgen_ctx->pool_meta_dma,
+							SSI_IVPOOL_META_SIZE);
+	/* Allocate IV pool in SRAM */
+	ivgen_ctx->pool = ssi_sram_mgr_alloc(drvdata, SSI_IVPOOL_SIZE);
+	if (ivgen_ctx->pool == NULL_SRAM_ADDR) {
+		SSI_LOG_ERR("SRAM pool exhausted\n");
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	return ssi_ivgen_init_sram_pool(drvdata);
+
+out:
+	ssi_ivgen_fini(drvdata);
+	return rc;
+}
+
+/*!
+ * Acquires 16 Bytes IV from the iv-pool
+ * 
+ * \param drvdata Driver private context
+ * \param iv_out_dma Array of physical IV out addresses
+ * \param iv_out_dma_len Length of iv_out_dma array (additional elements of iv_out_dma array are ignore)
+ * \param iv_out_size May be 8 or 16 bytes long 
+ * \param iv_seq IN/OUT array to the descriptors sequence
+ * \param iv_seq_len IN/OUT pointer to the sequence length 
+ *  
+ * \return int Zero for success, negative value otherwise. 
+ */
+int ssi_ivgen_getiv(
+	struct ssi_drvdata *drvdata,
+	dma_addr_t iv_out_dma[],
+	unsigned int iv_out_dma_len,
+	unsigned int iv_out_size,
+	HwDesc_s iv_seq[],
+	unsigned int *iv_seq_len)
+{
+	struct ssi_ivgen_ctx *ivgen_ctx = drvdata->ivgen_handle;
+	unsigned int idx = *iv_seq_len;
+	unsigned int t;
+
+	if ((iv_out_size != CC_AES_IV_SIZE) &&
+	    (iv_out_size != CTR_RFC3686_IV_SIZE)) {
+		return -EINVAL;
+	}
+	if ( (iv_out_dma_len + 1) > SSI_IVPOOL_SEQ_LEN) {
+		/* The sequence will be longer than allowed */
+		return -EINVAL;
+	}
+
+	//check that number of generated IV is limited to max dma address iv buffer size
+	if ( iv_out_dma_len > SSI_MAX_IVGEN_DMA_ADDRESSES) {
+		/* The sequence will be longer than allowed */
+		return -EINVAL;
+	}
+
+	for (t = 0; t < iv_out_dma_len; t++) {
+		/* Acquire IV from pool */
+		HW_DESC_INIT(&iv_seq[idx]);
+		HW_DESC_SET_DIN_SRAM(&iv_seq[idx],
+			ivgen_ctx->pool + ivgen_ctx->next_iv_ofs,
+			iv_out_size);
+		HW_DESC_SET_DOUT_DLLI(&iv_seq[idx], iv_out_dma[t],
+			iv_out_size, NS_BIT, 0);
+		HW_DESC_SET_FLOW_MODE(&iv_seq[idx], BYPASS);
+		idx++;
+	}
+
+	/* Bypass operation is proceeded by crypto sequence, hence must
+	*  assure bypass-write-transaction by a memory barrier */
+	HW_DESC_INIT(&iv_seq[idx]);
+	HW_DESC_SET_DIN_NO_DMA(&iv_seq[idx], 0, 0xfffff0);
+	HW_DESC_SET_DOUT_NO_DMA(&iv_seq[idx], 0, 0, 1);
+	idx++;
+
+	*iv_seq_len = idx; /* update seq length */
+
+	/* Update iv index */
+	ivgen_ctx->next_iv_ofs += iv_out_size;
+
+	if ((SSI_IVPOOL_SIZE - ivgen_ctx->next_iv_ofs) < CC_AES_IV_SIZE) {
+		SSI_LOG_DEBUG("Pool exhausted, regenerating iv-pool\n");
+		/* pool is drained -regenerate it! */
+		return ssi_ivgen_generate_pool(ivgen_ctx, iv_seq, iv_seq_len);
+	}
+
+	return 0;
+}
+
+
diff --git a/drivers/staging/ccree/ssi_ivgen.h b/drivers/staging/ccree/ssi_ivgen.h
new file mode 100644
index 000000000000..bc69cd8ca418
--- /dev/null
+++ b/drivers/staging/ccree/ssi_ivgen.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __SSI_IVGEN_H__
+#define __SSI_IVGEN_H__
+
+#include "cc_hw_queue_defs.h"
+
+
+#define SSI_IVPOOL_SEQ_LEN 8
+
+/*!
+ * Allocates iv-pool and maps resources. 
+ * This function generates the first IV pool.  
+ * 
+ * \param drvdata Driver's private context
+ * 
+ * \return int Zero for success, negative value otherwise.
+ */
+int ssi_ivgen_init(struct ssi_drvdata *drvdata);
+
+/*!
+ * Free iv-pool and ivgen context.
+ *  
+ * \param drvdata 
+ */
+void ssi_ivgen_fini(struct ssi_drvdata *drvdata);
+
+/*!
+ * Generates the initial pool in SRAM. 
+ * This function should be invoked when resuming DX driver. 
+ * 
+ * \param drvdata 
+ *  
+ * \return int Zero for success, negative value otherwise.
+ */
+int ssi_ivgen_init_sram_pool(struct ssi_drvdata *drvdata);
+
+/*!
+ * Acquires 16 Bytes IV from the iv-pool
+ * 
+ * \param drvdata Driver private context
+ * \param iv_out_dma Array of physical IV out addresses
+ * \param iv_out_dma_len Length of iv_out_dma array (additional elements of iv_out_dma array are ignore)
+ * \param iv_out_size May be 8 or 16 bytes long 
+ * \param iv_seq IN/OUT array to the descriptors sequence
+ * \param iv_seq_len IN/OUT pointer to the sequence length 
+ *  
+ * \return int Zero for success, negative value otherwise. 
+ */
+int ssi_ivgen_getiv(
+	struct ssi_drvdata *drvdata,
+	dma_addr_t iv_out_dma[],
+	unsigned int iv_out_dma_len,
+	unsigned int iv_out_size,
+	HwDesc_s iv_seq[],
+	unsigned int *iv_seq_len);
+
+#endif /*__SSI_IVGEN_H__*/
diff --git a/drivers/staging/ccree/ssi_pm.c b/drivers/staging/ccree/ssi_pm.c
new file mode 100644
index 000000000000..dd399f28a68c
--- /dev/null
+++ b/drivers/staging/ccree/ssi_pm.c
@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "ssi_config.h"
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <crypto/ctr.h>
+#include <linux/pm_runtime.h>
+#include "ssi_driver.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_request_mgr.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_sysfs.h"
+#include "ssi_ivgen.h"
+#include "ssi_hash.h"
+#include "ssi_pm.h"
+#include "ssi_pm_ext.h"
+
+
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+
+#define POWER_DOWN_ENABLE 0x01
+#define POWER_DOWN_DISABLE 0x00
+
+
+int ssi_power_mgr_runtime_suspend(struct device *dev)
+{
+	struct ssi_drvdata *drvdata =
+		(struct ssi_drvdata *)dev_get_drvdata(dev);
+	int rc;
+
+	SSI_LOG_DEBUG("ssi_power_mgr_runtime_suspend: set HOST_POWER_DOWN_EN\n");
+	WRITE_REGISTER(drvdata->cc_base + CC_REG_OFFSET(HOST_RGF, HOST_POWER_DOWN_EN), POWER_DOWN_ENABLE);
+	rc = ssi_request_mgr_runtime_suspend_queue(drvdata);
+	if (rc != 0) {
+		SSI_LOG_ERR("ssi_request_mgr_runtime_suspend_queue (%x)\n", rc);
+		return rc;
+	}
+	fini_cc_regs(drvdata);
+
+	/* Specific HW suspend code */
+	ssi_pm_ext_hw_suspend(dev);
+	return 0;
+}
+
+int ssi_power_mgr_runtime_resume(struct device *dev)
+{
+	int rc;
+	struct ssi_drvdata *drvdata =
+		(struct ssi_drvdata *)dev_get_drvdata(dev);
+
+	SSI_LOG_DEBUG("ssi_power_mgr_runtime_resume , unset HOST_POWER_DOWN_EN\n");
+	WRITE_REGISTER(drvdata->cc_base + CC_REG_OFFSET(HOST_RGF, HOST_POWER_DOWN_EN), POWER_DOWN_DISABLE);
+	/* Specific HW resume code */
+	ssi_pm_ext_hw_resume(dev);
+
+	rc = init_cc_regs(drvdata, false);
+	if (rc !=0) {
+		SSI_LOG_ERR("init_cc_regs (%x)\n",rc);
+		return rc;
+	}
+
+	rc = ssi_request_mgr_runtime_resume_queue(drvdata);
+	if (rc !=0) {
+		SSI_LOG_ERR("ssi_request_mgr_runtime_resume_queue (%x)\n",rc);
+		return rc;
+	}
+
+	/* must be after the queue resuming as it uses the HW queue*/
+	ssi_hash_init_sram_digest_consts(drvdata);
+	
+	ssi_ivgen_init_sram_pool(drvdata);
+	return 0;
+}
+
+int ssi_power_mgr_runtime_get(struct device *dev)
+{
+	int rc = 0;
+
+	if (ssi_request_mgr_is_queue_runtime_suspend(
+				(struct ssi_drvdata *)dev_get_drvdata(dev))) {
+		rc = pm_runtime_get_sync(dev);
+	} else {
+		pm_runtime_get_noresume(dev);
+	}
+	return rc;
+}
+
+int ssi_power_mgr_runtime_put_suspend(struct device *dev)
+{
+	int rc = 0;
+
+	if (!ssi_request_mgr_is_queue_runtime_suspend(
+				(struct ssi_drvdata *)dev_get_drvdata(dev))) {
+		pm_runtime_mark_last_busy(dev);
+		rc = pm_runtime_put_autosuspend(dev);
+	}
+	else {
+		/* Something wrong happens*/
+		BUG();
+	}
+	return rc;
+
+}
+
+#endif
+
+
+
+int ssi_power_mgr_init(struct ssi_drvdata *drvdata)
+{
+	int rc = 0;
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+	struct platform_device *plat_dev = drvdata->plat_dev;
+	/* must be before the enabling to avoid resdundent suspending */
+	pm_runtime_set_autosuspend_delay(&plat_dev->dev,SSI_SUSPEND_TIMEOUT);
+	pm_runtime_use_autosuspend(&plat_dev->dev);
+	/* activate the PM module */
+	rc = pm_runtime_set_active(&plat_dev->dev);
+	if (rc != 0)
+		return rc;
+	/* enable the PM module*/
+	pm_runtime_enable(&plat_dev->dev);
+#endif
+	return rc;
+}
+
+void ssi_power_mgr_fini(struct ssi_drvdata *drvdata)
+{
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+	struct platform_device *plat_dev = drvdata->plat_dev;
+
+	pm_runtime_disable(&plat_dev->dev);
+#endif
+}
diff --git a/drivers/staging/ccree/ssi_pm.h b/drivers/staging/ccree/ssi_pm.h
new file mode 100644
index 000000000000..516fc3f445a8
--- /dev/null
+++ b/drivers/staging/ccree/ssi_pm.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_pm.h
+    */
+
+#ifndef __SSI_POWER_MGR_H__
+#define __SSI_POWER_MGR_H__
+
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+
+
+#define SSI_SUSPEND_TIMEOUT 3000
+
+
+int ssi_power_mgr_init(struct ssi_drvdata *drvdata);
+
+void ssi_power_mgr_fini(struct ssi_drvdata *drvdata);
+
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+int ssi_power_mgr_runtime_suspend(struct device *dev);
+
+int ssi_power_mgr_runtime_resume(struct device *dev);
+
+int ssi_power_mgr_runtime_get(struct device *dev);
+
+int ssi_power_mgr_runtime_put_suspend(struct device *dev);
+#endif
+
+#endif /*__POWER_MGR_H__*/
+
diff --git a/drivers/staging/ccree/ssi_pm_ext.c b/drivers/staging/ccree/ssi_pm_ext.c
new file mode 100644
index 000000000000..f86bbab22073
--- /dev/null
+++ b/drivers/staging/ccree/ssi_pm_ext.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "ssi_config.h"
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <crypto/ctr.h>
+#include <linux/pm_runtime.h>
+#include "ssi_driver.h"
+#include "ssi_sram_mgr.h"
+#include "ssi_pm_ext.h"
+
+/*
+This function should suspend the HW (if possiable), It should be implemented by 
+the driver user. 
+The reference code clears the internal SRAM to imitate lose of state. 
+*/
+void ssi_pm_ext_hw_suspend(struct device *dev)
+{
+	struct ssi_drvdata *drvdata =
+		(struct ssi_drvdata *)dev_get_drvdata(dev);
+	unsigned int val;
+	void __iomem *cc_base = drvdata->cc_base;
+	unsigned int  sram_addr = 0;
+
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_ADDR), sram_addr);
+
+	for (;sram_addr < SSI_CC_SRAM_SIZE ; sram_addr+=4) {
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_DATA), 0x0);
+
+		do {
+			val = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, SRAM_DATA_READY));
+		} while (!(val &0x1));
+	}
+}
+
+/*
+This function should resume the HW (if possiable).It should be implemented by 
+the driver user. 
+*/
+void ssi_pm_ext_hw_resume(struct device *dev)
+{
+	return;
+}
+
diff --git a/drivers/staging/ccree/ssi_pm_ext.h b/drivers/staging/ccree/ssi_pm_ext.h
new file mode 100644
index 000000000000..b4e2795de29e
--- /dev/null
+++ b/drivers/staging/ccree/ssi_pm_ext.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_pm_ext.h
+    */
+
+#ifndef __PM_EXT_H__
+#define __PM_EXT_H__
+
+
+#include "ssi_config.h"
+#include "ssi_driver.h"
+
+void ssi_pm_ext_hw_suspend(struct device *dev);
+
+void ssi_pm_ext_hw_resume(struct device *dev);
+
+
+#endif /*__POWER_MGR_H__*/
+
diff --git a/drivers/staging/ccree/ssi_request_mgr.c b/drivers/staging/ccree/ssi_request_mgr.c
new file mode 100644
index 000000000000..522bd62c102e
--- /dev/null
+++ b/drivers/staging/ccree/ssi_request_mgr.c
@@ -0,0 +1,713 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ssi_config.h"
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <crypto/ctr.h>
+#ifdef FLUSH_CACHE_ALL
+#include <asm/cacheflush.h>
+#endif
+#include <linux/pm_runtime.h>
+#include "ssi_driver.h"
+#include "ssi_buffer_mgr.h"
+#include "ssi_request_mgr.h"
+#include "ssi_sysfs.h"
+#include "ssi_ivgen.h"
+#include "ssi_pm.h"
+#include "ssi_fips.h"
+#include "ssi_fips_local.h"
+
+#define SSI_MAX_POLL_ITER	10
+
+#define AXIM_MON_BASE_OFFSET CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_COMP)
+
+#ifdef CC_CYCLE_COUNT
+
+#define MONITOR_CNTR_BIT 0
+
+/**
+ * Monitor descriptor. 
+ * Used to measure CC performance. 
+ */
+#define INIT_CC_MONITOR_DESC(desc_p) \
+do { \
+	HW_DESC_INIT(desc_p); \
+	HW_DESC_SET_DIN_MONITOR_CNTR(desc_p); \
+} while (0)
+
+/** 
+ * Try adding monitor descriptor BEFORE enqueuing sequence.
+ */
+#define CC_CYCLE_DESC_HEAD(cc_base_addr, desc_p, lock_p, is_monitored_p) \
+do { \
+	if (!test_and_set_bit(MONITOR_CNTR_BIT, (lock_p))) { \
+		enqueue_seq((cc_base_addr), (desc_p), 1); \
+		*(is_monitored_p) = true; \
+	} else { \
+		*(is_monitored_p) = false; \
+	} \
+} while (0)
+
+/**
+ * If CC_CYCLE_DESC_HEAD was successfully added: 
+ * 1. Add memory barrier descriptor to ensure last AXI transaction.  
+ * 2. Add monitor descriptor to sequence tail AFTER enqueuing sequence.
+ */
+#define CC_CYCLE_DESC_TAIL(cc_base_addr, desc_p, is_monitored) \
+do { \
+	if ((is_monitored) == true) { \
+		HwDesc_s barrier_desc; \
+		HW_DESC_INIT(&barrier_desc); \
+		HW_DESC_SET_DIN_NO_DMA(&barrier_desc, 0, 0xfffff0); \
+		HW_DESC_SET_DOUT_NO_DMA(&barrier_desc, 0, 0, 1); \
+		enqueue_seq((cc_base_addr), &barrier_desc, 1); \
+		enqueue_seq((cc_base_addr), (desc_p), 1); \
+	} \
+} while (0)
+
+/**
+ * Try reading CC monitor counter value upon sequence complete. 
+ * Can only succeed if the lock_p is taken by the owner of the given request.
+ */
+#define END_CC_MONITOR_COUNT(cc_base_addr, stat_op_type, stat_phase, monitor_null_cycles, lock_p, is_monitored) \
+do { \
+	uint32_t elapsed_cycles; \
+	if ((is_monitored) == true) { \
+		elapsed_cycles = READ_REGISTER((cc_base_addr) + CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR)); \
+		clear_bit(MONITOR_CNTR_BIT, (lock_p)); \
+		if (elapsed_cycles > 0) \
+			update_cc_stat(stat_op_type, stat_phase, (elapsed_cycles - monitor_null_cycles)); \
+	} \
+} while (0)
+
+#else /*CC_CYCLE_COUNT*/
+
+#define INIT_CC_MONITOR_DESC(desc_p) do { } while (0)
+#define CC_CYCLE_DESC_HEAD(cc_base_addr, desc_p, lock_p, is_monitored_p) do { } while (0)
+#define CC_CYCLE_DESC_TAIL(cc_base_addr, desc_p, is_monitored) do { } while (0)
+#define END_CC_MONITOR_COUNT(cc_base_addr, stat_op_type, stat_phase, monitor_null_cycles, lock_p, is_monitored) do { } while (0)
+#endif /*CC_CYCLE_COUNT*/
+
+
+struct ssi_request_mgr_handle {
+	/* Request manager resources */
+	unsigned int hw_queue_size; /* HW capability */
+	unsigned int min_free_hw_slots;
+	unsigned int max_used_sw_slots;
+	struct ssi_crypto_req req_queue[MAX_REQUEST_QUEUE_SIZE];
+	uint32_t req_queue_head;
+	uint32_t req_queue_tail;
+	uint32_t axi_completed;
+	uint32_t q_free_slots;
+	spinlock_t hw_lock;
+	HwDesc_s compl_desc;
+	uint8_t *dummy_comp_buff;
+	dma_addr_t dummy_comp_buff_dma;
+	HwDesc_s monitor_desc;
+	volatile unsigned long monitor_lock;
+#ifdef COMP_IN_WQ
+	struct workqueue_struct *workq;
+	struct delayed_work compwork;
+#else
+	struct tasklet_struct comptask;
+#endif
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+	bool is_runtime_suspended;
+#endif
+};
+
+static void comp_handler(unsigned long devarg);
+#ifdef COMP_IN_WQ
+static void comp_work_handler(struct work_struct *work);
+#endif
+
+void request_mgr_fini(struct ssi_drvdata *drvdata)
+{
+	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
+
+	if (req_mgr_h == NULL)
+		return; /* Not allocated */
+
+	if (req_mgr_h->dummy_comp_buff_dma != 0) {
+		SSI_RESTORE_DMA_ADDR_TO_48BIT(req_mgr_h->dummy_comp_buff_dma);
+		dma_free_coherent(&drvdata->plat_dev->dev,
+				  sizeof(uint32_t), req_mgr_h->dummy_comp_buff,
+				  req_mgr_h->dummy_comp_buff_dma);
+	}
+
+	SSI_LOG_DEBUG("max_used_hw_slots=%d\n", (req_mgr_h->hw_queue_size -
+						req_mgr_h->min_free_hw_slots) );
+	SSI_LOG_DEBUG("max_used_sw_slots=%d\n", req_mgr_h->max_used_sw_slots);
+
+#ifdef COMP_IN_WQ
+	flush_workqueue(req_mgr_h->workq);
+	destroy_workqueue(req_mgr_h->workq);
+#else
+	/* Kill tasklet */
+	tasklet_kill(&req_mgr_h->comptask);
+#endif
+	memset(req_mgr_h, 0, sizeof(struct ssi_request_mgr_handle));
+	kfree(req_mgr_h);
+	drvdata->request_mgr_handle = NULL;
+}
+
+int request_mgr_init(struct ssi_drvdata *drvdata)
+{
+#ifdef CC_CYCLE_COUNT
+	HwDesc_s monitor_desc[2];
+	struct ssi_crypto_req monitor_req = {0};
+#endif
+	struct ssi_request_mgr_handle *req_mgr_h;
+	int rc = 0;
+
+	req_mgr_h = kzalloc(sizeof(struct ssi_request_mgr_handle),GFP_KERNEL);
+	if (req_mgr_h == NULL) {
+		rc = -ENOMEM;
+		goto req_mgr_init_err;
+	}
+
+	drvdata->request_mgr_handle = req_mgr_h;
+
+	spin_lock_init(&req_mgr_h->hw_lock);
+#ifdef COMP_IN_WQ
+	SSI_LOG_DEBUG("Initializing completion workqueue\n");
+	req_mgr_h->workq = create_singlethread_workqueue("arm_cc7x_wq");
+	if (unlikely(req_mgr_h->workq == NULL)) {
+		SSI_LOG_ERR("Failed creating work queue\n");
+		rc = -ENOMEM;
+		goto req_mgr_init_err;
+	}
+	INIT_DELAYED_WORK(&req_mgr_h->compwork, comp_work_handler);
+#else
+	SSI_LOG_DEBUG("Initializing completion tasklet\n");
+	tasklet_init(&req_mgr_h->comptask, comp_handler, (unsigned long)drvdata);
+#endif
+	req_mgr_h->hw_queue_size = READ_REGISTER(drvdata->cc_base +
+		CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_SRAM_SIZE));
+	SSI_LOG_DEBUG("hw_queue_size=0x%08X\n", req_mgr_h->hw_queue_size);
+	if (req_mgr_h->hw_queue_size < MIN_HW_QUEUE_SIZE) {
+		SSI_LOG_ERR("Invalid HW queue size = %u (Min. required is %u)\n",
+			req_mgr_h->hw_queue_size, MIN_HW_QUEUE_SIZE);
+		rc = -ENOMEM;
+		goto req_mgr_init_err;
+	}
+	req_mgr_h->min_free_hw_slots = req_mgr_h->hw_queue_size;
+	req_mgr_h->max_used_sw_slots = 0;
+
+
+	/* Allocate DMA word for "dummy" completion descriptor use */
+	req_mgr_h->dummy_comp_buff = dma_alloc_coherent(&drvdata->plat_dev->dev,
+		sizeof(uint32_t), &req_mgr_h->dummy_comp_buff_dma, GFP_KERNEL);
+	if (!req_mgr_h->dummy_comp_buff) {
+		SSI_LOG_ERR("Not enough memory to allocate DMA (%zu) dropped "
+			   "buffer\n", sizeof(uint32_t));
+		rc = -ENOMEM;
+		goto req_mgr_init_err;
+	}
+	SSI_UPDATE_DMA_ADDR_TO_48BIT(req_mgr_h->dummy_comp_buff_dma,
+							     sizeof(uint32_t));
+
+	/* Init. "dummy" completion descriptor */
+	HW_DESC_INIT(&req_mgr_h->compl_desc);
+	HW_DESC_SET_DIN_CONST(&req_mgr_h->compl_desc, 0, sizeof(uint32_t));
+	HW_DESC_SET_DOUT_DLLI(&req_mgr_h->compl_desc,
+		req_mgr_h->dummy_comp_buff_dma,
+		sizeof(uint32_t), NS_BIT, 1);
+	HW_DESC_SET_FLOW_MODE(&req_mgr_h->compl_desc, BYPASS);
+	HW_DESC_SET_QUEUE_LAST_IND(&req_mgr_h->compl_desc);
+
+#ifdef CC_CYCLE_COUNT
+	/* For CC-HW cycle performance trace */
+	INIT_CC_MONITOR_DESC(&req_mgr_h->monitor_desc);
+	set_bit(MONITOR_CNTR_BIT, &req_mgr_h->monitor_lock);
+	monitor_desc[0] = req_mgr_h->monitor_desc;
+	monitor_desc[1] = req_mgr_h->monitor_desc;
+
+	rc = send_request(drvdata, &monitor_req, monitor_desc, 2, 0);
+	if (unlikely(rc != 0))
+		goto req_mgr_init_err;
+
+	drvdata->monitor_null_cycles = READ_REGISTER(drvdata->cc_base +
+		CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_MEASURE_CNTR));
+	SSI_LOG_ERR("Calibration time=0x%08x\n", drvdata->monitor_null_cycles);
+
+	clear_bit(MONITOR_CNTR_BIT, &req_mgr_h->monitor_lock);
+#endif
+
+	return 0;
+
+req_mgr_init_err:
+	request_mgr_fini(drvdata);
+	return rc;
+}
+
+static inline void enqueue_seq(
+	void __iomem *cc_base,
+	HwDesc_s seq[], unsigned int seq_len)
+{
+	int i;
+
+	for (i = 0; i < seq_len; i++) {
+		writel_relaxed(seq[i].word[0], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[1], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[2], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[3], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		writel_relaxed(seq[i].word[4], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+		wmb();
+		writel_relaxed(seq[i].word[5], (volatile void __iomem *)(cc_base+CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_WORD0)));
+#ifdef DX_DUMP_DESCS
+		SSI_LOG_DEBUG("desc[%02d]: 0x%08X 0x%08X 0x%08X 0x%08X 0x%08X 0x%08X\n", i,
+			seq[i].word[0], seq[i].word[1], seq[i].word[2], seq[i].word[3], seq[i].word[4], seq[i].word[5]);
+#endif
+	}
+}
+
+/*!
+ * Completion will take place if and only if user requested completion 
+ * by setting "is_dout = 0" in send_request().  
+ * 
+ * \param dev 
+ * \param dx_compl_h The completion event to signal
+ */
+static void request_mgr_complete(struct device *dev, void *dx_compl_h, void __iomem *cc_base)
+{
+	struct completion *this_compl = dx_compl_h;
+	complete(this_compl);
+}
+
+
+static inline int request_mgr_queues_status_check(
+		struct ssi_request_mgr_handle *req_mgr_h,
+		void __iomem *cc_base,
+		unsigned int total_seq_len)
+{
+	unsigned long poll_queue;
+	
+	/* SW queue is checked only once as it will not 
+	   be chaned during the poll becasue the spinlock_bh 
+	   is held by the thread */
+	if (unlikely(((req_mgr_h->req_queue_head + 1) &
+		      (MAX_REQUEST_QUEUE_SIZE - 1)) == 
+		     req_mgr_h->req_queue_tail)) {
+		SSI_LOG_ERR("SW FIFO is full. req_queue_head=%d sw_fifo_len=%d\n", 
+			   req_mgr_h->req_queue_head, MAX_REQUEST_QUEUE_SIZE);
+		return -EBUSY;
+	}
+
+	if ((likely(req_mgr_h->q_free_slots >= total_seq_len)) ) {
+		return 0;
+	}
+	/* Wait for space in HW queue. Poll constant num of iterations. */
+	for (poll_queue =0; poll_queue < SSI_MAX_POLL_ITER ; poll_queue ++) {
+		req_mgr_h->q_free_slots = 
+			CC_HAL_READ_REGISTER(
+				CC_REG_OFFSET(CRY_KERNEL,
+						 DSCRPTR_QUEUE_CONTENT));
+		if (unlikely(req_mgr_h->q_free_slots < 
+						req_mgr_h->min_free_hw_slots)) {
+			req_mgr_h->min_free_hw_slots = req_mgr_h->q_free_slots;
+		}
+
+		if (likely (req_mgr_h->q_free_slots >= total_seq_len)) {
+			/* If there is enough place return */
+			return 0;
+		}
+
+		SSI_LOG_DEBUG("HW FIFO is full. q_free_slots=%d total_seq_len=%d\n", 
+			req_mgr_h->q_free_slots, total_seq_len);
+	}
+	/* No room in the HW queue try again later */
+	SSI_LOG_DEBUG("HW FIFO full, timeout. req_queue_head=%d "
+		   "sw_fifo_len=%d q_free_slots=%d total_seq_len=%d\n", 
+		     req_mgr_h->req_queue_head,
+		   MAX_REQUEST_QUEUE_SIZE,
+		   req_mgr_h->q_free_slots,
+		   total_seq_len);
+	return -EAGAIN;
+}
+
+/*!
+ * Enqueue caller request to crypto hardware.
+ * 
+ * \param drvdata 
+ * \param ssi_req The request to enqueue
+ * \param desc The crypto sequence
+ * \param len The crypto sequence length
+ * \param is_dout If "true": completion is handled by the caller 
+ *      	  If "false": this function adds a dummy descriptor completion
+ *      	  and waits upon completion signal.
+ * 
+ * \return int Returns -EINPROGRESS if "is_dout=true"; "0" if "is_dout=false"
+ */
+int send_request(
+	struct ssi_drvdata *drvdata, struct ssi_crypto_req *ssi_req,
+	HwDesc_s *desc, unsigned int len, bool is_dout)
+{
+	void __iomem *cc_base = drvdata->cc_base;
+	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
+	unsigned int used_sw_slots;
+	unsigned int iv_seq_len = 0;
+	unsigned int total_seq_len = len; /*initial sequence length*/
+	HwDesc_s iv_seq[SSI_IVPOOL_SEQ_LEN];
+	int rc;
+	unsigned int max_required_seq_len = (total_seq_len +
+					((ssi_req->ivgen_dma_addr_len == 0) ? 0 :
+					SSI_IVPOOL_SEQ_LEN ) +
+					((is_dout == 0 )? 1 : 0));
+	DECL_CYCLE_COUNT_RESOURCES;
+
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+	rc = ssi_power_mgr_runtime_get(&drvdata->plat_dev->dev);
+	if (rc != 0) {
+		SSI_LOG_ERR("ssi_power_mgr_runtime_get returned %x\n",rc);
+		spin_unlock_bh(&req_mgr_h->hw_lock);
+		return rc;
+	}
+#endif
+
+	do {
+		spin_lock_bh(&req_mgr_h->hw_lock);
+
+		/* Check if there is enough place in the SW/HW queues
+		in case iv gen add the max size and in case of no dout add 1 
+		for the internal completion descriptor */
+		rc = request_mgr_queues_status_check(req_mgr_h,
+					       cc_base,
+					       max_required_seq_len);
+		if (likely(rc == 0 ))
+			/* There is enough place in the queue */
+			break;
+		/* something wrong release the spinlock*/
+		spin_unlock_bh(&req_mgr_h->hw_lock);
+
+		if (rc != -EAGAIN) {
+			/* Any error other than HW queue full 
+			   (SW queue is full) */
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+			ssi_power_mgr_runtime_put_suspend(&drvdata->plat_dev->dev);
+#endif
+			return rc;
+		}
+
+		/* HW queue is full - short sleep */
+		msleep(1);
+	} while (1);
+
+	/* Additional completion descriptor is needed incase caller did not
+	   enabled any DLLI/MLLI DOUT bit in the given sequence */
+	if (!is_dout) {
+		init_completion(&ssi_req->seq_compl);
+		ssi_req->user_cb = request_mgr_complete;
+		ssi_req->user_arg = &(ssi_req->seq_compl);
+		total_seq_len++;
+	}
+
+	if (ssi_req->ivgen_dma_addr_len > 0) {
+		SSI_LOG_DEBUG("Acquire IV from pool into %d DMA addresses 0x%llX, 0x%llX, 0x%llX, IV-size=%u\n",
+			ssi_req->ivgen_dma_addr_len,
+			(unsigned long long)ssi_req->ivgen_dma_addr[0],
+			(unsigned long long)ssi_req->ivgen_dma_addr[1],
+			(unsigned long long)ssi_req->ivgen_dma_addr[2],
+			ssi_req->ivgen_size);
+
+		/* Acquire IV from pool */
+		rc = ssi_ivgen_getiv(drvdata, ssi_req->ivgen_dma_addr, ssi_req->ivgen_dma_addr_len,
+			ssi_req->ivgen_size, iv_seq, &iv_seq_len);
+
+		if (unlikely(rc != 0)) {
+			SSI_LOG_ERR("Failed to generate IV (rc=%d)\n", rc);
+			spin_unlock_bh(&req_mgr_h->hw_lock);
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+			ssi_power_mgr_runtime_put_suspend(&drvdata->plat_dev->dev);
+#endif
+			return rc;
+		}
+
+		total_seq_len += iv_seq_len;
+	}
+	
+	used_sw_slots = ((req_mgr_h->req_queue_head - req_mgr_h->req_queue_tail) & (MAX_REQUEST_QUEUE_SIZE-1));
+	if (unlikely(used_sw_slots > req_mgr_h->max_used_sw_slots)) {
+		req_mgr_h->max_used_sw_slots = used_sw_slots;
+	}
+	
+	CC_CYCLE_DESC_HEAD(cc_base, &req_mgr_h->monitor_desc,
+			&req_mgr_h->monitor_lock, &ssi_req->is_monitored_p);
+
+	/* Enqueue request - must be locked with HW lock*/
+	req_mgr_h->req_queue[req_mgr_h->req_queue_head] = *ssi_req;
+	START_CYCLE_COUNT_AT(req_mgr_h->req_queue[req_mgr_h->req_queue_head].submit_cycle);
+	req_mgr_h->req_queue_head = (req_mgr_h->req_queue_head + 1) & (MAX_REQUEST_QUEUE_SIZE - 1);
+	/* TODO: Use circ_buf.h ? */
+
+	SSI_LOG_DEBUG("Enqueue request head=%u\n", req_mgr_h->req_queue_head);
+
+#ifdef FLUSH_CACHE_ALL
+	flush_cache_all();
+#endif
+
+	/* STAT_PHASE_4: Push sequence */
+	START_CYCLE_COUNT();
+	enqueue_seq(cc_base, iv_seq, iv_seq_len);
+	enqueue_seq(cc_base, desc, len);
+	enqueue_seq(cc_base, &req_mgr_h->compl_desc, (is_dout ? 0 : 1));
+	END_CYCLE_COUNT(ssi_req->op_type, STAT_PHASE_4);
+
+	CC_CYCLE_DESC_TAIL(cc_base, &req_mgr_h->monitor_desc, ssi_req->is_monitored_p);
+
+	if (unlikely(req_mgr_h->q_free_slots < total_seq_len)) {
+		/*This means that there was a problem with the resume*/
+		BUG();
+	}
+	/* Update the free slots in HW queue */
+	req_mgr_h->q_free_slots -= total_seq_len;
+
+	spin_unlock_bh(&req_mgr_h->hw_lock);
+
+	if (!is_dout) {
+		/* Wait upon sequence completion.
+		*  Return "0" -Operation done successfully. */
+		return wait_for_completion_interruptible(&ssi_req->seq_compl);
+	} else {
+		/* Operation still in process */
+		return -EINPROGRESS;
+	}
+}
+
+
+/*!
+ * Enqueue caller request to crypto hardware during init process.
+ * assume this function is not called in middle of a flow,
+ * since we set QUEUE_LAST_IND flag in the last descriptor.
+ * 
+ * \param drvdata 
+ * \param desc The crypto sequence
+ * \param len The crypto sequence length
+ * 
+ * \return int Returns "0" upon success
+ */
+int send_request_init(
+	struct ssi_drvdata *drvdata, HwDesc_s *desc, unsigned int len)
+{
+	void __iomem *cc_base = drvdata->cc_base;
+	struct ssi_request_mgr_handle *req_mgr_h = drvdata->request_mgr_handle;
+	unsigned int total_seq_len = len; /*initial sequence length*/
+	int rc = 0;
+
+	/* Wait for space in HW and SW FIFO. Poll for as much as FIFO_TIMEOUT. */
+	rc = request_mgr_queues_status_check(req_mgr_h, cc_base, total_seq_len);
+	if (unlikely(rc != 0 )) {
+		return rc;
+	}
+	HW_DESC_SET_QUEUE_LAST_IND(&desc[len-1]);
+
+	enqueue_seq(cc_base, desc, len);
+
+	/* Update the free slots in HW queue */
+	req_mgr_h->q_free_slots = CC_HAL_READ_REGISTER(
+					CC_REG_OFFSET(CRY_KERNEL,
+					 DSCRPTR_QUEUE_CONTENT));
+
+	return 0;
+}
+
+
+void complete_request(struct ssi_drvdata *drvdata)
+{
+	struct ssi_request_mgr_handle *request_mgr_handle = 
+						drvdata->request_mgr_handle;
+#ifdef COMP_IN_WQ
+	queue_delayed_work(request_mgr_handle->workq, &request_mgr_handle->compwork, 0);
+#else
+	tasklet_schedule(&request_mgr_handle->comptask);
+#endif
+}
+
+#ifdef COMP_IN_WQ
+static void comp_work_handler(struct work_struct *work)
+{
+	struct ssi_drvdata *drvdata =
+		container_of(work, struct ssi_drvdata, compwork.work);
+
+	comp_handler((unsigned long)drvdata);
+}
+#endif
+
+static void proc_completions(struct ssi_drvdata *drvdata)
+{
+	struct ssi_crypto_req *ssi_req;
+	struct platform_device *plat_dev = drvdata->plat_dev;
+	struct ssi_request_mgr_handle * request_mgr_handle = 
+						drvdata->request_mgr_handle;
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+	int rc = 0;
+#endif
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	while(request_mgr_handle->axi_completed) {
+		request_mgr_handle->axi_completed--;
+
+		/* Dequeue request */
+		if (unlikely(request_mgr_handle->req_queue_head == request_mgr_handle->req_queue_tail)) {
+			SSI_LOG_ERR("Request queue is empty req_queue_head==req_queue_tail==%u\n", request_mgr_handle->req_queue_head);
+			BUG();
+		}
+
+		ssi_req = &request_mgr_handle->req_queue[request_mgr_handle->req_queue_tail];
+		END_CYCLE_COUNT_AT(ssi_req->submit_cycle, ssi_req->op_type, STAT_PHASE_5); /* Seq. Comp. */
+		END_CC_MONITOR_COUNT(drvdata->cc_base, ssi_req->op_type, STAT_PHASE_6,
+			drvdata->monitor_null_cycles, &request_mgr_handle->monitor_lock, ssi_req->is_monitored_p);
+
+#ifdef FLUSH_CACHE_ALL
+		flush_cache_all();
+#endif
+
+#ifdef COMPLETION_DELAY
+		/* Delay */
+		{
+			uint32_t axi_err;
+			int i;
+			SSI_LOG_INFO("Delay\n");
+			for (i=0;i<1000000;i++) {
+				axi_err = READ_REGISTER(drvdata->cc_base + CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_ERR));
+			}
+		}
+#endif /* COMPLETION_DELAY */
+
+		if (likely(ssi_req->user_cb != NULL)) {
+			START_CYCLE_COUNT();
+			ssi_req->user_cb(&plat_dev->dev, ssi_req->user_arg, drvdata->cc_base);
+			END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_3);
+		}
+		request_mgr_handle->req_queue_tail = (request_mgr_handle->req_queue_tail + 1) & (MAX_REQUEST_QUEUE_SIZE - 1);
+		SSI_LOG_DEBUG("Dequeue request tail=%u\n", request_mgr_handle->req_queue_tail);
+		SSI_LOG_DEBUG("Request completed. axi_completed=%d\n", request_mgr_handle->axi_completed);
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+		rc = ssi_power_mgr_runtime_put_suspend(&plat_dev->dev);
+		if (rc != 0) {
+			SSI_LOG_ERR("Failed to set runtime suspension %d\n",rc);
+		}
+#endif
+	}
+}
+
+/* Deferred service handler, run as interrupt-fired tasklet */
+static void comp_handler(unsigned long devarg)
+{
+	struct ssi_drvdata *drvdata = (struct ssi_drvdata *)devarg;
+	void __iomem *cc_base = drvdata->cc_base;
+	struct ssi_request_mgr_handle * request_mgr_handle = 
+						drvdata->request_mgr_handle;
+
+	uint32_t irq;
+
+	DECL_CYCLE_COUNT_RESOURCES;
+
+	START_CYCLE_COUNT();
+
+	irq = (drvdata->irq & SSI_COMP_IRQ_MASK);
+
+	if (irq & SSI_COMP_IRQ_MASK) {
+		/* To avoid the interrupt from firing as we unmask it, we clear it now */
+		CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), SSI_COMP_IRQ_MASK);
+	
+		/* Avoid race with above clear: Test completion counter once more */
+		request_mgr_handle->axi_completed += CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
+			CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+	
+		/* ISR-to-Tasklet latency */
+		if (request_mgr_handle->axi_completed) {
+			/* Only if actually reflects ISR-to-completion-handling latency, i.e.,
+			   not duplicate as a result of interrupt after AXIM_MON_ERR clear, before end of loop */
+			END_CYCLE_COUNT_AT(drvdata->isr_exit_cycles, STAT_OP_TYPE_GENERIC, STAT_PHASE_1);
+		}
+	
+		while (request_mgr_handle->axi_completed) {
+			do {
+				proc_completions(drvdata);
+				/* At this point (after proc_completions()), request_mgr_handle->axi_completed is always 0.
+				   The following assignment was changed to = (previously was +=) to conform KW restrictions. */
+				request_mgr_handle->axi_completed = CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
+					CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+			} while (request_mgr_handle->axi_completed > 0);
+	
+			/* To avoid the interrupt from firing as we unmask it, we clear it now */
+			CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_ICR), SSI_COMP_IRQ_MASK);
+			
+			/* Avoid race with above clear: Test completion counter once more */
+			request_mgr_handle->axi_completed += CC_REG_FLD_GET(CRY_KERNEL, AXIM_MON_COMP, VALUE, 
+				CC_HAL_READ_REGISTER(AXIM_MON_BASE_OFFSET));
+		}
+	
+	}
+	/* after verifing that there is nothing to do, Unmask AXI completion interrupt */
+	CC_HAL_WRITE_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IMR), 
+		CC_HAL_READ_REGISTER(
+		CC_REG_OFFSET(HOST_RGF, HOST_IMR)) & ~irq);
+	END_CYCLE_COUNT(STAT_OP_TYPE_GENERIC, STAT_PHASE_2);
+}
+
+/*
+resume the queue configuration - no need to take the lock as this happens inside
+the spin lock protection
+*/
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+int ssi_request_mgr_runtime_resume_queue(struct ssi_drvdata *drvdata)
+{
+	struct ssi_request_mgr_handle * request_mgr_handle = drvdata->request_mgr_handle;
+
+	spin_lock_bh(&request_mgr_handle->hw_lock);
+	request_mgr_handle->is_runtime_suspended = false;
+	spin_unlock_bh(&request_mgr_handle->hw_lock);
+
+	return 0 ;
+}
+
+/*
+suspend the queue configuration. Since it is used for the runtime suspend
+only verify that the queue can be suspended.
+*/
+int ssi_request_mgr_runtime_suspend_queue(struct ssi_drvdata *drvdata)
+{
+	struct ssi_request_mgr_handle * request_mgr_handle = 
+						drvdata->request_mgr_handle;
+	
+	/* lock the send_request */
+	spin_lock_bh(&request_mgr_handle->hw_lock);
+	if (request_mgr_handle->req_queue_head != 
+	    request_mgr_handle->req_queue_tail) {
+		spin_unlock_bh(&request_mgr_handle->hw_lock);
+		return -EBUSY;
+	}
+	request_mgr_handle->is_runtime_suspended = true;
+	spin_unlock_bh(&request_mgr_handle->hw_lock);
+
+	return 0;
+}
+
+bool ssi_request_mgr_is_queue_runtime_suspend(struct ssi_drvdata *drvdata)
+{
+	struct ssi_request_mgr_handle * request_mgr_handle = 
+						drvdata->request_mgr_handle;
+
+	return 	request_mgr_handle->is_runtime_suspended;
+}
+
+#endif
+
diff --git a/drivers/staging/ccree/ssi_request_mgr.h b/drivers/staging/ccree/ssi_request_mgr.h
new file mode 100644
index 000000000000..c09339b566d0
--- /dev/null
+++ b/drivers/staging/ccree/ssi_request_mgr.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file request_mgr.h
+   Request Manager
+ */
+
+#ifndef __REQUEST_MGR_H__
+#define __REQUEST_MGR_H__
+
+#include "cc_hw_queue_defs.h"
+
+int request_mgr_init(struct ssi_drvdata *drvdata);
+
+/*!
+ * Enqueue caller request to crypto hardware.
+ * 
+ * \param drvdata 
+ * \param ssi_req The request to enqueue
+ * \param desc The crypto sequence
+ * \param len The crypto sequence length
+ * \param is_dout If "true": completion is handled by the caller 
+ *      	  If "false": this function adds a dummy descriptor completion
+ *      	  and waits upon completion signal.
+ * 
+ * \return int Returns -EINPROGRESS if "is_dout=ture"; "0" if "is_dout=false"
+ */
+int send_request(
+	struct ssi_drvdata *drvdata, struct ssi_crypto_req *ssi_req,
+	HwDesc_s *desc, unsigned int len, bool is_dout);
+
+int send_request_init(
+	struct ssi_drvdata *drvdata, HwDesc_s *desc, unsigned int len);
+
+void complete_request(struct ssi_drvdata *drvdata);
+
+void request_mgr_fini(struct ssi_drvdata *drvdata);
+
+#if defined (CONFIG_PM_RUNTIME) || defined (CONFIG_PM_SLEEP)
+int ssi_request_mgr_runtime_resume_queue(struct ssi_drvdata *drvdata);
+
+int ssi_request_mgr_runtime_suspend_queue(struct ssi_drvdata *drvdata);
+
+bool ssi_request_mgr_is_queue_runtime_suspend(struct ssi_drvdata *drvdata);
+#endif
+
+#endif /*__REQUEST_MGR_H__*/
diff --git a/drivers/staging/ccree/ssi_sram_mgr.c b/drivers/staging/ccree/ssi_sram_mgr.c
new file mode 100644
index 000000000000..50066e17d1d3
--- /dev/null
+++ b/drivers/staging/ccree/ssi_sram_mgr.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ssi_driver.h"
+#include "ssi_sram_mgr.h"
+
+
+/**
+ * struct ssi_sram_mgr_ctx -Internal RAM context manager
+ * @sram_free_offset:   the offset to the non-allocated area
+ */
+struct ssi_sram_mgr_ctx {
+	ssi_sram_addr_t sram_free_offset;
+};
+
+
+/**
+ * ssi_sram_mgr_fini() - Cleanup SRAM pool.
+ * 
+ * @drvdata: Associated device driver context
+ */
+void ssi_sram_mgr_fini(struct ssi_drvdata *drvdata)
+{
+	struct ssi_sram_mgr_ctx *smgr_ctx = drvdata->sram_mgr_handle;
+
+	/* Free "this" context */
+	if (smgr_ctx != NULL) {
+		memset(smgr_ctx, 0, sizeof(struct ssi_sram_mgr_ctx));
+		kfree(smgr_ctx);
+	}
+}
+
+/**
+ * ssi_sram_mgr_init() - Initializes SRAM pool. 
+ *      The pool starts right at the beginning of SRAM.
+ *      Returns zero for success, negative value otherwise.
+ * 
+ * @drvdata: Associated device driver context
+ */
+int ssi_sram_mgr_init(struct ssi_drvdata *drvdata)
+{
+	struct ssi_sram_mgr_ctx *smgr_ctx;
+	int rc;
+
+	/* Allocate "this" context */
+	drvdata->sram_mgr_handle = kzalloc(
+			sizeof(struct ssi_sram_mgr_ctx), GFP_KERNEL);
+	if (!drvdata->sram_mgr_handle) {
+		SSI_LOG_ERR("Not enough memory to allocate SRAM_MGR ctx (%zu)\n",
+			sizeof(struct ssi_sram_mgr_ctx));
+		rc = -ENOMEM;
+		goto out;
+	}
+	smgr_ctx = drvdata->sram_mgr_handle;
+
+	/* Pool starts at start of SRAM */
+	smgr_ctx->sram_free_offset = 0;
+
+	return 0;
+
+out:
+	ssi_sram_mgr_fini(drvdata);
+	return rc;
+}
+
+/*!
+ * Allocated buffer from SRAM pool. 
+ * Note: Caller is responsible to free the LAST allocated buffer. 
+ * This function does not taking care of any fragmentation may occur 
+ * by the order of calls to alloc/free. 
+ * 
+ * \param drvdata 
+ * \param size The requested bytes to allocate
+ */
+ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size)
+{
+	struct ssi_sram_mgr_ctx *smgr_ctx = drvdata->sram_mgr_handle;
+	ssi_sram_addr_t p;
+
+	if (unlikely((size & 0x3) != 0)) {
+		SSI_LOG_ERR("Requested buffer size (%u) is not multiple of 4",
+			size);
+		return NULL_SRAM_ADDR;
+	}
+	if (unlikely(size > (SSI_CC_SRAM_SIZE - smgr_ctx->sram_free_offset))) {
+		SSI_LOG_ERR("Not enough space to allocate %u B (at offset %llu)\n",
+			size, smgr_ctx->sram_free_offset);
+		return NULL_SRAM_ADDR;
+	}
+	
+	p = smgr_ctx->sram_free_offset;
+	smgr_ctx->sram_free_offset += size;
+	SSI_LOG_DEBUG("Allocated %u B @ %u\n", size, (unsigned int)p);
+	return p;
+}
+
+/**
+ * ssi_sram_mgr_const2sram_desc() - Create const descriptors sequence to
+ *	set values in given array into SRAM. 
+ * Note: each const value can't exceed word size.
+ * 
+ * @src:	  A pointer to array of words to set as consts.
+ * @dst:	  The target SRAM buffer to set into
+ * @nelements:	  The number of words in "src" array
+ * @seq:	  A pointer to the given IN/OUT descriptor sequence
+ * @seq_len:	  A pointer to the given IN/OUT sequence length
+ */
+void ssi_sram_mgr_const2sram_desc(
+	const uint32_t *src, ssi_sram_addr_t dst,
+	unsigned int nelement,
+	HwDesc_s *seq, unsigned int *seq_len)
+{
+	uint32_t i;
+	unsigned int idx = *seq_len;
+
+	for (i = 0; i < nelement; i++, idx++) {
+		HW_DESC_INIT(&seq[idx]);
+		HW_DESC_SET_DIN_CONST(&seq[idx], src[i], sizeof(uint32_t));
+		HW_DESC_SET_DOUT_SRAM(&seq[idx], dst + (i * sizeof(uint32_t)), sizeof(uint32_t));
+		HW_DESC_SET_FLOW_MODE(&seq[idx], BYPASS);
+	}
+
+	*seq_len = idx;
+}
+
diff --git a/drivers/staging/ccree/ssi_sram_mgr.h b/drivers/staging/ccree/ssi_sram_mgr.h
new file mode 100644
index 000000000000..d71fbaf9ac44
--- /dev/null
+++ b/drivers/staging/ccree/ssi_sram_mgr.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __SSI_SRAM_MGR_H__
+#define __SSI_SRAM_MGR_H__
+
+
+#ifndef SSI_CC_SRAM_SIZE
+#define SSI_CC_SRAM_SIZE 4096
+#endif
+
+struct ssi_drvdata;
+
+/**
+ * Address (offset) within CC internal SRAM
+ */
+
+typedef uint64_t ssi_sram_addr_t;
+
+#define NULL_SRAM_ADDR ((ssi_sram_addr_t)-1)
+
+/*!
+ * Initializes SRAM pool. 
+ * The first X bytes of SRAM are reserved for ROM usage, hence, pool 
+ * starts right after X bytes. 
+ *  
+ * \param drvdata 
+ *  
+ * \return int Zero for success, negative value otherwise.
+ */
+int ssi_sram_mgr_init(struct ssi_drvdata *drvdata);
+
+/*!
+ * Uninits SRAM pool.
+ * 
+ * \param drvdata 
+ */
+void ssi_sram_mgr_fini(struct ssi_drvdata *drvdata);
+
+/*!
+ * Allocated buffer from SRAM pool. 
+ * Note: Caller is responsible to free the LAST allocated buffer. 
+ * This function does not taking care of any fragmentation may occur 
+ * by the order of calls to alloc/free. 
+ * 
+ * \param drvdata 
+ * \param size The requested bytes to allocate
+ */
+ssi_sram_addr_t ssi_sram_mgr_alloc(struct ssi_drvdata *drvdata, uint32_t size);
+
+/**
+ * ssi_sram_mgr_const2sram_desc() - Create const descriptors sequence to
+ *	set values in given array into SRAM. 
+ * Note: each const value can't exceed word size.
+ * 
+ * @src:	  A pointer to array of words to set as consts.
+ * @dst:	  The target SRAM buffer to set into
+ * @nelements:	  The number of words in "src" array
+ * @seq:	  A pointer to the given IN/OUT descriptor sequence
+ * @seq_len:	  A pointer to the given IN/OUT sequence length
+ */
+void ssi_sram_mgr_const2sram_desc(
+	const uint32_t *src, ssi_sram_addr_t dst,
+	unsigned int nelement,
+	HwDesc_s *seq, unsigned int *seq_len);
+
+#endif /*__SSI_SRAM_MGR_H__*/
diff --git a/drivers/staging/ccree/ssi_sysfs.c b/drivers/staging/ccree/ssi_sysfs.c
new file mode 100644
index 000000000000..7c514c1072a9
--- /dev/null
+++ b/drivers/staging/ccree/ssi_sysfs.c
@@ -0,0 +1,439 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include "ssi_config.h"
+#include "ssi_driver.h"
+#include "cc_crypto_ctx.h"
+#include "ssi_sysfs.h"
+
+#ifdef ENABLE_CC_SYSFS
+
+static struct ssi_drvdata *sys_get_drvdata(void);
+
+#ifdef CC_CYCLE_COUNT
+
+#include <asm/timex.h>
+
+struct stat_item {
+	unsigned int min;
+	unsigned int max;
+	cycles_t sum;
+	unsigned int count;
+};
+
+struct stat_name {
+	const char *op_type_name;
+	const char *stat_phase_name[MAX_STAT_PHASES];
+};
+
+static struct stat_name stat_name_db[MAX_STAT_OP_TYPES] = 
+{
+	{
+		/* STAT_OP_TYPE_NULL */
+		.op_type_name = "NULL",
+		.stat_phase_name = {NULL},
+	},
+	{
+		.op_type_name = "Encode",
+		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
+		.stat_phase_name[STAT_PHASE_1] = "Map buffers", 
+		.stat_phase_name[STAT_PHASE_2] = "Create sequence", 
+		.stat_phase_name[STAT_PHASE_3] = "Send Request",
+		.stat_phase_name[STAT_PHASE_4] = "HW-Q push",
+		.stat_phase_name[STAT_PHASE_5] = "Sequence completion",
+		.stat_phase_name[STAT_PHASE_6] = "HW cycles",
+	},
+	{	.op_type_name = "Decode",
+		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
+		.stat_phase_name[STAT_PHASE_1] = "Map buffers", 
+		.stat_phase_name[STAT_PHASE_2] = "Create sequence", 
+		.stat_phase_name[STAT_PHASE_3] = "Send Request",
+		.stat_phase_name[STAT_PHASE_4] = "HW-Q push",
+		.stat_phase_name[STAT_PHASE_5] = "Sequence completion",
+		.stat_phase_name[STAT_PHASE_6] = "HW cycles",
+	},
+	{ 	.op_type_name = "Setkey",
+		.stat_phase_name[STAT_PHASE_0] = "Init and sanity checks",
+		.stat_phase_name[STAT_PHASE_1] = "Copy key to ctx",
+		.stat_phase_name[STAT_PHASE_2] = "Create sequence",
+		.stat_phase_name[STAT_PHASE_3] = "Send Request",
+		.stat_phase_name[STAT_PHASE_4] = "HW-Q push",
+		.stat_phase_name[STAT_PHASE_5] = "Sequence completion",
+		.stat_phase_name[STAT_PHASE_6] = "HW cycles",
+	},
+	{
+		.op_type_name = "Generic",
+		.stat_phase_name[STAT_PHASE_0] = "Interrupt",
+		.stat_phase_name[STAT_PHASE_1] = "ISR-to-Tasklet",
+		.stat_phase_name[STAT_PHASE_2] = "Tasklet start-to-end",
+		.stat_phase_name[STAT_PHASE_3] = "Tasklet:user_cb()",
+		.stat_phase_name[STAT_PHASE_4] = "Tasklet:dx_X_complete() - w/o X_complete()",
+		.stat_phase_name[STAT_PHASE_5] = "",
+		.stat_phase_name[STAT_PHASE_6] = "HW cycles",
+	}
+};
+
+/*
+ * Structure used to create a directory 
+ * and its attributes in sysfs.
+ */
+struct sys_dir {
+	struct kobject *sys_dir_kobj;
+	struct attribute_group sys_dir_attr_group;
+	struct attribute **sys_dir_attr_list;
+	uint32_t num_of_attrs;
+	struct ssi_drvdata *drvdata; /* Associated driver context */
+};
+
+/* top level directory structures */
+struct sys_dir sys_top_dir;
+
+static DEFINE_SPINLOCK(stat_lock);
+
+/* List of DBs */
+static struct stat_item stat_host_db[MAX_STAT_OP_TYPES][MAX_STAT_PHASES];
+static struct stat_item stat_cc_db[MAX_STAT_OP_TYPES][MAX_STAT_PHASES];
+
+
+static void init_db(struct stat_item item[MAX_STAT_OP_TYPES][MAX_STAT_PHASES])
+{
+	unsigned int i, j;
+
+	/* Clear db */
+	for (i=0; i<MAX_STAT_OP_TYPES; i++) {
+		for (j=0; j<MAX_STAT_PHASES; j++) {
+			item[i][j].min = 0xFFFFFFFF;
+			item[i][j].max = 0;
+			item[i][j].sum = 0;
+			item[i][j].count = 0;
+		}
+	}
+}
+
+static void update_db(struct stat_item *item, unsigned int result)
+{
+	item->count++;
+	item->sum += result;
+	if (result < item->min)
+		item->min = result;
+	if (result > item->max )
+		item->max = result;
+}
+
+static void display_db(struct stat_item item[MAX_STAT_OP_TYPES][MAX_STAT_PHASES])
+{
+	unsigned int i, j;
+	uint64_t avg;
+
+	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
+		for (j=0; j<MAX_STAT_PHASES; j++) {	
+			if (item[i][j].count > 0) {
+				avg = (uint64_t)item[i][j].sum;
+				do_div(avg, item[i][j].count);
+				SSI_LOG_ERR("%s, %s: min=%d avg=%d max=%d sum=%lld count=%d\n", 
+					stat_name_db[i].op_type_name, stat_name_db[i].stat_phase_name[j], 
+					item[i][j].min, (int)avg, item[i][j].max, (long long)item[i][j].sum, item[i][j].count);
+			}
+		}
+	}
+}
+
+
+/**************************************
+ * Attributes show functions section  *
+ **************************************/
+
+static ssize_t ssi_sys_stats_host_db_clear(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	init_db(stat_host_db);
+	return count;
+}
+
+static ssize_t ssi_sys_stats_cc_db_clear(struct kobject *kobj,
+	struct kobj_attribute *attr, const char *buf, size_t count)
+{
+	init_db(stat_cc_db);
+	return count;
+}
+
+static ssize_t ssi_sys_stat_host_db_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	int i, j ;
+	char line[512];
+	uint32_t min_cyc, max_cyc;
+	uint64_t avg;
+	ssize_t buf_len, tmp_len=0;
+
+	buf_len = scnprintf(buf,PAGE_SIZE,
+		"phase\t\t\t\t\t\t\tmin[cy]\tavg[cy]\tmax[cy]\t#samples\n");
+	if ( buf_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+		return buf_len;
+	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
+		for (j=0; j<MAX_STAT_PHASES-1; j++) {
+			if (stat_host_db[i][j].count > 0) {
+				avg = (uint64_t)stat_host_db[i][j].sum;
+				do_div(avg, stat_host_db[i][j].count);
+				min_cyc = stat_host_db[i][j].min;
+				max_cyc = stat_host_db[i][j].max;
+			} else {
+				avg = min_cyc = max_cyc = 0;
+			}
+			tmp_len = scnprintf(line,512,
+				"%s::%s\t\t\t\t\t%6u\t%6u\t%6u\t%7u\n",
+				stat_name_db[i].op_type_name,
+				stat_name_db[i].stat_phase_name[j],
+				min_cyc, (unsigned int)avg, max_cyc,
+				stat_host_db[i][j].count);
+			if ( tmp_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+				return buf_len;
+			if ( buf_len + tmp_len >= PAGE_SIZE)
+				return buf_len;
+			buf_len += tmp_len;
+			strncat(buf, line,512);
+		}
+	}
+	return buf_len;
+}
+
+static ssize_t ssi_sys_stat_cc_db_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	int i;
+	char line[256];
+	uint32_t min_cyc, max_cyc;
+	uint64_t avg;
+	ssize_t buf_len,tmp_len=0;
+
+	buf_len = scnprintf(buf,PAGE_SIZE,
+		"phase\tmin[cy]\tavg[cy]\tmax[cy]\t#samples\n");
+	if ( buf_len <0 )/* scnprintf shouldn't return negative value according to its implementation*/
+		return buf_len;
+	for (i=STAT_OP_TYPE_ENCODE; i<MAX_STAT_OP_TYPES; i++) {
+		if (stat_cc_db[i][STAT_PHASE_6].count > 0) {
+			avg = (uint64_t)stat_cc_db[i][STAT_PHASE_6].sum;
+			do_div(avg, stat_cc_db[i][STAT_PHASE_6].count);
+			min_cyc = stat_cc_db[i][STAT_PHASE_6].min;
+			max_cyc = stat_cc_db[i][STAT_PHASE_6].max;
+		} else {
+			avg = min_cyc = max_cyc = 0;
+		}
+		tmp_len = scnprintf(line,256,
+			"%s\t%6u\t%6u\t%6u\t%7u\n",
+			stat_name_db[i].op_type_name,
+			min_cyc,
+			(unsigned int)avg,
+			max_cyc,
+			stat_cc_db[i][STAT_PHASE_6].count);
+
+		if ( tmp_len < 0 )/* scnprintf shouldn't return negative value according to its implementation*/
+			return buf_len;
+
+		if ( buf_len + tmp_len >= PAGE_SIZE)
+			return buf_len;
+		buf_len += tmp_len;
+		strncat(buf, line,256);
+	}
+	return buf_len;
+}
+
+void update_host_stat(unsigned int op_type, unsigned int phase, cycles_t result)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&stat_lock, flags);
+	update_db(&(stat_host_db[op_type][phase]), (unsigned int)result);
+	spin_unlock_irqrestore(&stat_lock, flags);
+}
+
+void update_cc_stat(
+	unsigned int op_type,
+	unsigned int phase,
+	unsigned int elapsed_cycles)
+{
+	update_db(&(stat_cc_db[op_type][phase]), elapsed_cycles);
+}
+
+void display_all_stat_db(void)
+{
+	SSI_LOG_ERR("\n=======    CYCLE COUNT STATS    =======\n"); 
+	display_db(stat_host_db);
+	SSI_LOG_ERR("\n======= CC HW CYCLE COUNT STATS =======\n"); 
+	display_db(stat_cc_db);
+}
+#endif /*CC_CYCLE_COUNT*/
+
+
+
+static ssize_t ssi_sys_regdump_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	struct ssi_drvdata *drvdata = sys_get_drvdata();
+	uint32_t register_value;
+	void __iomem* cc_base = drvdata->cc_base;
+	int offset = 0;
+
+	register_value = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_SIGNATURE));
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s \t(0x%lX)\t 0x%08X  \n", "HOST_SIGNATURE       ", DX_HOST_SIGNATURE_REG_OFFSET, register_value);
+	register_value = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_IRR));
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s \t(0x%lX)\t 0x%08X  \n", "HOST_IRR             ", DX_HOST_IRR_REG_OFFSET, register_value);
+	register_value = CC_HAL_READ_REGISTER(CC_REG_OFFSET(HOST_RGF, HOST_POWER_DOWN_EN));
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s \t(0x%lX)\t 0x%08X  \n", "HOST_POWER_DOWN_EN   ", DX_HOST_POWER_DOWN_EN_REG_OFFSET, register_value);
+	register_value =  CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, AXIM_MON_ERR));
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s \t(0x%lX)\t 0x%08X  \n", "AXIM_MON_ERR         ", DX_AXIM_MON_ERR_REG_OFFSET, register_value);
+	register_value = CC_HAL_READ_REGISTER(CC_REG_OFFSET(CRY_KERNEL, DSCRPTR_QUEUE_CONTENT));
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s \t(0x%lX)\t 0x%08X  \n", "DSCRPTR_QUEUE_CONTENT", DX_DSCRPTR_QUEUE_CONTENT_REG_OFFSET, register_value);
+	return offset;
+}
+
+static ssize_t ssi_sys_help_show(struct kobject *kobj,
+		struct kobj_attribute *attr, char *buf)
+{
+	char* help_str[]={
+				"cat reg_dump              ", "Print several of CC register values",
+		#if defined CC_CYCLE_COUNT
+				"cat stats_host            ", "Print host statistics",
+				"echo <number> > stats_host", "Clear host statistics database",
+				"cat stats_cc              ", "Print CC statistics",
+				"echo <number> > stats_cc  ", "Clear CC statistics database",
+		#endif
+				};
+	int i=0, offset = 0;
+
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "Usage:\n");
+	for ( i = 0; i < ARRAY_SIZE(help_str); i+=2) {
+	   offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%s\t\t%s\n", help_str[i], help_str[i+1]);
+	}
+	return offset;
+}
+
+/********************************************************
+ *		SYSFS objects				*
+ ********************************************************/
+/*
+ * Structure used to create a directory
+ * and its attributes in sysfs.
+ */
+struct sys_dir {
+	struct kobject *sys_dir_kobj;
+	struct attribute_group sys_dir_attr_group;
+	struct attribute **sys_dir_attr_list;
+	uint32_t num_of_attrs;
+	struct ssi_drvdata *drvdata; /* Associated driver context */
+};
+
+/* top level directory structures */
+static struct sys_dir sys_top_dir;
+
+/* TOP LEVEL ATTRIBUTES */
+static struct kobj_attribute ssi_sys_top_level_attrs[] = {
+	__ATTR(dump_regs, 0444, ssi_sys_regdump_show, NULL),
+	__ATTR(help, 0444, ssi_sys_help_show, NULL),
+#if defined CC_CYCLE_COUNT
+	__ATTR(stats_host, 0664, ssi_sys_stat_host_db_show, ssi_sys_stats_host_db_clear),
+	__ATTR(stats_cc, 0664, ssi_sys_stat_cc_db_show, ssi_sys_stats_cc_db_clear),
+#endif
+
+};
+
+static struct ssi_drvdata *sys_get_drvdata(void)
+{
+	/* TODO: supporting multiple SeP devices would require avoiding
+	 * global "top_dir" and finding associated "top_dir" by traversing
+	 * up the tree to the kobject which matches one of the top_dir's */
+	return sys_top_dir.drvdata;
+}
+
+static int sys_init_dir(struct sys_dir *sys_dir, struct ssi_drvdata *drvdata,
+		 struct kobject *parent_dir_kobj, const char *dir_name,
+		 struct kobj_attribute *attrs, uint32_t num_of_attrs)
+{
+	int i;
+
+	memset(sys_dir, 0, sizeof(struct sys_dir));
+
+	sys_dir->drvdata = drvdata;
+
+	/* initialize directory kobject */
+	sys_dir->sys_dir_kobj =
+		kobject_create_and_add(dir_name, parent_dir_kobj);
+
+	if (!(sys_dir->sys_dir_kobj))
+		return -ENOMEM;
+	/* allocate memory for directory's attributes list */
+	sys_dir->sys_dir_attr_list =
+		kzalloc(sizeof(struct attribute *) * (num_of_attrs + 1),
+				GFP_KERNEL);
+
+	if (!(sys_dir->sys_dir_attr_list)) {
+		kobject_put(sys_dir->sys_dir_kobj);
+		return -ENOMEM;
+	}
+
+	sys_dir->num_of_attrs = num_of_attrs;
+
+	/* initialize attributes list */
+	for (i = 0; i < num_of_attrs; ++i)
+		sys_dir->sys_dir_attr_list[i] = &(attrs[i].attr);
+
+	/* last list entry should be NULL */
+	sys_dir->sys_dir_attr_list[num_of_attrs] = NULL;
+
+	sys_dir->sys_dir_attr_group.attrs = sys_dir->sys_dir_attr_list;
+
+	return sysfs_create_group(sys_dir->sys_dir_kobj,
+			&(sys_dir->sys_dir_attr_group));
+}
+
+static void sys_free_dir(struct sys_dir *sys_dir)
+{
+	if (!sys_dir)
+		return;
+
+	kfree(sys_dir->sys_dir_attr_list);
+
+	if (sys_dir->sys_dir_kobj != NULL)
+		kobject_put(sys_dir->sys_dir_kobj);
+}
+
+int ssi_sysfs_init(struct kobject *sys_dev_obj, struct ssi_drvdata *drvdata)
+{
+	int retval;
+
+#if defined CC_CYCLE_COUNT
+	/* Init. statistics */
+	init_db(stat_host_db);
+	init_db(stat_cc_db);
+#endif
+
+	SSI_LOG_ERR("setup sysfs under %s\n", sys_dev_obj->name);
+
+	/* Initialize top directory */
+	retval = sys_init_dir(&sys_top_dir, drvdata, sys_dev_obj,
+				"cc_info", ssi_sys_top_level_attrs,
+				ARRAY_SIZE(ssi_sys_top_level_attrs));
+	return retval;
+}
+
+void ssi_sysfs_fini(void)
+{
+	sys_free_dir(&sys_top_dir);
+}
+
+#endif /*ENABLE_CC_SYSFS*/
+
diff --git a/drivers/staging/ccree/ssi_sysfs.h b/drivers/staging/ccree/ssi_sysfs.h
new file mode 100644
index 000000000000..baeac1d99c07
--- /dev/null
+++ b/drivers/staging/ccree/ssi_sysfs.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2012-2017 ARM Limited or its affiliates.
+ * 
+ * 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; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* \file ssi_sysfs.h
+   ARM CryptoCell sysfs APIs
+ */
+
+#ifndef __SSI_SYSFS_H__
+#define __SSI_SYSFS_H__
+
+#include <asm/timex.h>
+
+/* forward declaration */
+struct ssi_drvdata;
+
+enum stat_phase {
+	STAT_PHASE_0 = 0,
+	STAT_PHASE_1,
+	STAT_PHASE_2,
+	STAT_PHASE_3,
+	STAT_PHASE_4,
+	STAT_PHASE_5,
+	STAT_PHASE_6,
+	MAX_STAT_PHASES,
+};
+enum stat_op {
+	STAT_OP_TYPE_NULL = 0,
+	STAT_OP_TYPE_ENCODE,
+	STAT_OP_TYPE_DECODE,
+	STAT_OP_TYPE_SETKEY,
+	STAT_OP_TYPE_GENERIC,
+	MAX_STAT_OP_TYPES,
+};
+
+int ssi_sysfs_init(struct kobject *sys_dev_obj, struct ssi_drvdata *drvdata);
+void ssi_sysfs_fini(void);
+void update_host_stat(unsigned int op_type, unsigned int phase, cycles_t result);
+void update_cc_stat(unsigned int op_type, unsigned int phase, unsigned int elapsed_cycles);
+void display_all_stat_db(void);
+
+#endif /*__SSI_SYSFS_H__*/
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 942507754cab..7a655ed071a3 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -1,6 +1,5 @@
 config COMEDI
 	tristate "Data acquisition support (comedi)"
-	depends on m
 	---help---
 	  Enable support for a wide range of data acquisition devices
 	  for Linux.
@@ -506,7 +505,6 @@ config COMEDI_NI_ATMIO16D
 config COMEDI_NI_LABPC_ISA
 	tristate "NI Lab-PC and compatibles ISA support"
 	select COMEDI_NI_LABPC
-	select COMEDI_NI_LABPC_ISADMA if ISA_DMA_API
 	---help---
 	  Enable support for National Instruments Lab-PC and compatibles
 	  Lab-PC-1200, Lab-PC-1200AI, Lab-PC+.
@@ -1316,6 +1314,9 @@ config COMEDI_NI_LABPC
 
 config COMEDI_NI_LABPC_ISADMA
 	tristate
+	default COMEDI_NI_LABPC
+	depends on COMEDI_NI_LABPC_ISA != n
+	depends on ISA_DMA_API
 	select COMEDI_ISADMA
 
 config COMEDI_NI_TIO
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index 1e1df89b5018..8e9b30b26810 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -161,6 +161,30 @@ int comedi_buf_map_put(struct comedi_buf_map *bm)
 	return 1;
 }
 
+/* helper for "access" vm operation */
+int comedi_buf_map_access(struct comedi_buf_map *bm, unsigned long offset,
+			  void *buf, int len, int write)
+{
+	unsigned int pgoff = offset & ~PAGE_MASK;
+	unsigned long pg = offset >> PAGE_SHIFT;
+	int done = 0;
+
+	while (done < len && pg < bm->n_pages) {
+		int l = min_t(int, len - done, PAGE_SIZE - pgoff);
+		void *b = bm->page_list[pg].virt_addr + pgoff;
+
+		if (write)
+			memcpy(b, buf, l);
+		else
+			memcpy(buf, b, l);
+		buf += l;
+		done += l;
+		pg++;
+		pgoff = 0;
+	}
+	return done;
+}
+
 /* returns s->async->buf_map and increments its kref refcount */
 struct comedi_buf_map *
 comedi_buf_map_from_subdev_get(struct comedi_subdevice *s)
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 8deac8d9225d..f191c2a75732 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -76,8 +76,8 @@ struct comedi_file {
 #define COMEDI_NUM_SUBDEVICE_MINORS	\
 	(COMEDI_NUM_MINORS - COMEDI_NUM_BOARD_MINORS)
 
-static int comedi_num_legacy_minors;
-module_param(comedi_num_legacy_minors, int, 0444);
+static unsigned short comedi_num_legacy_minors;
+module_param(comedi_num_legacy_minors, ushort, 0444);
 MODULE_PARM_DESC(comedi_num_legacy_minors,
 		 "number of comedi minor devices to reserve for non-auto-configured devices (default 0)"
 		);
@@ -2165,9 +2165,24 @@ static void comedi_vm_close(struct vm_area_struct *area)
 	comedi_buf_map_put(bm);
 }
 
+static int comedi_vm_access(struct vm_area_struct *vma, unsigned long addr,
+			    void *buf, int len, int write)
+{
+	struct comedi_buf_map *bm = vma->vm_private_data;
+	unsigned long offset =
+	    addr - vma->vm_start + (vma->vm_pgoff << PAGE_SHIFT);
+
+	if (len < 0)
+		return -EINVAL;
+	if (len > vma->vm_end - addr)
+		len = vma->vm_end - addr;
+	return comedi_buf_map_access(bm, offset, buf, len, write);
+}
+
 static const struct vm_operations_struct comedi_vm_ops = {
 	.open = comedi_vm_open,
 	.close = comedi_vm_close,
+	.access = comedi_vm_access,
 };
 
 static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
@@ -2857,8 +2872,7 @@ static int __init comedi_init(void)
 
 	pr_info("version " COMEDI_RELEASE " - http://www.comedi.org\n");
 
-	if (comedi_num_legacy_minors < 0 ||
-	    comedi_num_legacy_minors > COMEDI_NUM_BOARD_MINORS) {
+	if (comedi_num_legacy_minors > COMEDI_NUM_BOARD_MINORS) {
 		pr_err("invalid value for module parameter \"comedi_num_legacy_minors\".  Valid values are 0 through %i.\n",
 		       COMEDI_NUM_BOARD_MINORS);
 		return -EINVAL;
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index 534415e331b6..6246f4a78ca6 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -29,6 +29,8 @@ void comedi_buf_reset(struct comedi_subdevice *s);
 bool comedi_buf_is_mmapped(struct comedi_subdevice *s);
 void comedi_buf_map_get(struct comedi_buf_map *bm);
 int comedi_buf_map_put(struct comedi_buf_map *bm);
+int comedi_buf_map_access(struct comedi_buf_map *bm, unsigned long offset,
+			  void *buf, int len, int write);
 struct comedi_buf_map *comedi_buf_map_from_subdev_get(
 		struct comedi_subdevice *s);
 unsigned int comedi_buf_write_n_available(struct comedi_subdevice *s);
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index b6af3eba91fd..be1f6133ff1c 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -502,7 +502,7 @@ static int apci3xxx_ai_ns_to_timer(struct comedi_device *dev,
 			timer = *ns / base;
 			break;
 		case CMDF_ROUND_UP:
-			timer = (*ns + base - 1) / base;
+			timer = DIV_ROUND_UP(*ns, base);
 			break;
 		}
 
@@ -787,6 +787,8 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 
 	dev->iobase = pci_resource_start(pcidev, 2);
 	dev->mmio = pci_ioremap_bar(pcidev, 3);
+	if (!dev->mmio)
+		return -ENOMEM;
 
 	if (pcidev->irq > 0) {
 		ret = request_irq(pcidev->irq, apci3xxx_irq_handler,
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 2e6decf1b69d..4e554944bc71 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -206,8 +206,11 @@
 #define CLK_1KHZ	5	/* internal 1 kHz clock */
 #define CLK_OUTNM1	6	/* output of channel-1 modulo total */
 #define CLK_EXT		7	/* external clock */
-/* Macro to construct clock input configuration register value. */
-#define CLK_CONFIG(chan, src)	((((chan) & 3) << 3) | ((src) & 7))
+
+static unsigned int pci224_clk_config(unsigned int chan, unsigned int src)
+{
+	return ((chan & 3) << 3) | (src & 7);
+}
 
 /*
  * Counter/timer gate input configuration sources.
@@ -216,8 +219,11 @@
 #define GAT_GND		1	/* GND (i.e. disabled) */
 #define GAT_EXT		2	/* reserved (external gate input) */
 #define GAT_NOUTNM2	3	/* inverted output of channel-2 modulo total */
-/* Macro to construct gate input configuration register value. */
-#define GAT_CONFIG(chan, src)	((((chan) & 3) << 3) | ((src) & 7))
+
+static unsigned int pci224_gat_config(unsigned int chan, unsigned int src)
+{
+	return ((chan & 3) << 3) | (src & 7);
+}
 
 /*
  * Summary of CLK_OUTNM1 and GAT_NOUTNM2 connections for PCI224 and PCI234:
@@ -817,14 +823,16 @@ static void pci224_ao_start_pacer(struct comedi_device *dev,
 	 * source.
 	 */
 	/* Make sure Z2-0 is gated on.  */
-	outb(GAT_CONFIG(0, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	outb(pci224_gat_config(0, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
 	/* Cascading with Z2-2. */
 	/* Make sure Z2-2 is gated on.  */
-	outb(GAT_CONFIG(2, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
+	outb(pci224_gat_config(2, GAT_VCC), devpriv->iobase1 + PCI224_ZGAT_SCE);
 	/* Z2-2 needs 10 MHz clock. */
-	outb(CLK_CONFIG(2, CLK_10MHZ), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	outb(pci224_clk_config(2, CLK_10MHZ),
+	     devpriv->iobase1 + PCI224_ZCLK_SCE);
 	/* Z2-0 is clocked from Z2-2's output. */
-	outb(CLK_CONFIG(0, CLK_OUTNM1), devpriv->iobase1 + PCI224_ZCLK_SCE);
+	outb(pci224_clk_config(0, CLK_OUTNM1),
+	     devpriv->iobase1 + PCI224_ZCLK_SCE);
 
 	comedi_8254_pacer_enable(dev->pacer, 2, 0, false);
 }
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 42945de31fe2..48c7890c3007 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -376,8 +376,11 @@
 #define CLK_1KHZ	5	/* internal 1 kHz clock */
 #define CLK_OUTNM1	6	/* output of channel-1 modulo total */
 #define CLK_EXT		7	/* external clock */
-/* Macro to construct clock input configuration register value. */
-#define CLK_CONFIG(chan, src)	((((chan) & 3) << 3) | ((src) & 7))
+
+static unsigned int pci230_clk_config(unsigned int chan, unsigned int src)
+{
+	return ((chan & 3) << 3) | (src & 7);
+}
 
 /*
  * Counter/timer gate input configuration sources.
@@ -387,8 +390,7 @@
 #define GAT_EXT		2	/* external gate input (PPCn on PCI230) */
 #define GAT_NOUTNM2	3	/* inverted output of channel-2 modulo total */
 
-static inline unsigned int pci230_gat_config(unsigned int chan,
-					     unsigned int src)
+static unsigned int pci230_gat_config(unsigned int chan, unsigned int src)
 {
 	return ((chan & 3) << 3) | (src & 7);
 }
@@ -698,7 +700,7 @@ static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
 	/* Determine clock source and count. */
 	clk_src = pci230_choose_clk_count(ns, &count, flags);
 	/* Program clock source. */
-	outb(CLK_CONFIG(ct, clk_src), dev->iobase + PCI230_ZCLK_SCE);
+	outb(pci230_clk_config(ct, clk_src), dev->iobase + PCI230_ZCLK_SCE);
 	/* Set initial count. */
 	if (count >= 65536)
 		count = 0;
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index efbf27730d71..b761f000c1dc 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1417,9 +1417,7 @@ static int set_ai_fifo_size(struct comedi_device *dev, unsigned int num_samples)
 	if (retval < 0)
 		return retval;
 
-	num_samples = retval * fifo->num_segments * fifo->sample_packing_ratio;
-
-	return num_samples;
+	return retval * fifo->num_segments * fifo->sample_packing_ratio;
 }
 
 /* query length of fifo */
@@ -2007,7 +2005,7 @@ static unsigned int get_divisor(unsigned int ns, unsigned int flags)
 
 	switch (flags & CMDF_ROUND_MASK) {
 	case CMDF_ROUND_UP:
-		divisor = (ns + TIMER_BASE - 1) / TIMER_BASE;
+		divisor = DIV_ROUND_UP(ns, TIMER_BASE);
 		break;
 	case CMDF_ROUND_DOWN:
 		divisor = ns / TIMER_BASE;
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 2a063f07fe7b..ccfd642998be 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -480,11 +480,11 @@ static void waveform_ao_timer(unsigned long arg)
 			/* output the last scan */
 			for (i = 0; i < cmd->scan_end_arg; i++) {
 				unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+				unsigned short *pd;
 
-				if (comedi_buf_read_samples(s,
-							    &devpriv->
-							     ao_loopbacks[chan],
-							    1) == 0) {
+				pd = &devpriv->ao_loopbacks[chan];
+
+				if (!comedi_buf_read_samples(s, pd, 1)) {
 					/* unexpected underrun! (cancelled?) */
 					async->events |= COMEDI_CB_OVERFLOW;
 					goto underrun;
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 70390de66e0e..f1c2a20a7d4d 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -95,27 +95,30 @@ struct jr3_pci_poll_delay {
 };
 
 struct jr3_pci_dev_private {
-	struct jr3_t __iomem *iobase;
 	struct timer_list timer;
 };
 
+union jr3_pci_single_range {
+	struct comedi_lrange l;
+	char _reserved[offsetof(struct comedi_lrange, range[1])];
+};
+
+enum jr3_pci_poll_state {
+	state_jr3_poll,
+	state_jr3_init_wait_for_offset,
+	state_jr3_init_transform_complete,
+	state_jr3_init_set_full_scale_complete,
+	state_jr3_init_use_offset_complete,
+	state_jr3_done
+};
+
 struct jr3_pci_subdev_private {
-	struct jr3_channel __iomem *channel;
+	struct jr3_sensor __iomem *sensor;
 	unsigned long next_time_min;
-	unsigned long next_time_max;
-	enum { state_jr3_poll,
-		state_jr3_init_wait_for_offset,
-		state_jr3_init_transform_complete,
-		state_jr3_init_set_full_scale_complete,
-		state_jr3_init_use_offset_complete,
-		state_jr3_done
-	} state;
+	enum jr3_pci_poll_state state;
 	int serial_no;
 	int model_no;
-	struct {
-		int length;
-		struct comedi_krange range;
-	} range[9];
+	union jr3_pci_single_range range[9];
 	const struct comedi_lrange *range_table_list[8 * 7 + 2];
 	unsigned int maxdata_list[8 * 7 + 2];
 	u16 errors;
@@ -131,43 +134,43 @@ static struct jr3_pci_poll_delay poll_delay_min_max(int min, int max)
 	return result;
 }
 
-static int is_complete(struct jr3_channel __iomem *channel)
+static int is_complete(struct jr3_sensor __iomem *sensor)
 {
-	return get_s16(&channel->command_word0) == 0;
+	return get_s16(&sensor->command_word0) == 0;
 }
 
-static void set_transforms(struct jr3_channel __iomem *channel,
-			   struct jr3_pci_transform transf, short num)
+static void set_transforms(struct jr3_sensor __iomem *sensor,
+			   const struct jr3_pci_transform *transf, short num)
 {
 	int i;
 
 	num &= 0x000f;		/* Make sure that 0 <= num <= 15 */
 	for (i = 0; i < 8; i++) {
-		set_u16(&channel->transforms[num].link[i].link_type,
-			transf.link[i].link_type);
+		set_u16(&sensor->transforms[num].link[i].link_type,
+			transf->link[i].link_type);
 		udelay(1);
-		set_s16(&channel->transforms[num].link[i].link_amount,
-			transf.link[i].link_amount);
+		set_s16(&sensor->transforms[num].link[i].link_amount,
+			transf->link[i].link_amount);
 		udelay(1);
-		if (transf.link[i].link_type == end_x_form)
+		if (transf->link[i].link_type == end_x_form)
 			break;
 	}
 }
 
-static void use_transform(struct jr3_channel __iomem *channel,
+static void use_transform(struct jr3_sensor __iomem *sensor,
 			  short transf_num)
 {
-	set_s16(&channel->command_word0, 0x0500 + (transf_num & 0x000f));
+	set_s16(&sensor->command_word0, 0x0500 + (transf_num & 0x000f));
 }
 
-static void use_offset(struct jr3_channel __iomem *channel, short offset_num)
+static void use_offset(struct jr3_sensor __iomem *sensor, short offset_num)
 {
-	set_s16(&channel->command_word0, 0x0600 + (offset_num & 0x000f));
+	set_s16(&sensor->command_word0, 0x0600 + (offset_num & 0x000f));
 }
 
-static void set_offset(struct jr3_channel __iomem *channel)
+static void set_offset(struct jr3_sensor __iomem *sensor)
 {
-	set_s16(&channel->command_word0, 0x0700);
+	set_s16(&sensor->command_word0, 0x0700);
 }
 
 struct six_axis_t {
@@ -179,43 +182,41 @@ struct six_axis_t {
 	s16 mz;
 };
 
-static void set_full_scales(struct jr3_channel __iomem *channel,
+static void set_full_scales(struct jr3_sensor __iomem *sensor,
 			    struct six_axis_t full_scale)
 {
-	set_s16(&channel->full_scale.fx, full_scale.fx);
-	set_s16(&channel->full_scale.fy, full_scale.fy);
-	set_s16(&channel->full_scale.fz, full_scale.fz);
-	set_s16(&channel->full_scale.mx, full_scale.mx);
-	set_s16(&channel->full_scale.my, full_scale.my);
-	set_s16(&channel->full_scale.mz, full_scale.mz);
-	set_s16(&channel->command_word0, 0x0a00);
+	set_s16(&sensor->full_scale.fx, full_scale.fx);
+	set_s16(&sensor->full_scale.fy, full_scale.fy);
+	set_s16(&sensor->full_scale.fz, full_scale.fz);
+	set_s16(&sensor->full_scale.mx, full_scale.mx);
+	set_s16(&sensor->full_scale.my, full_scale.my);
+	set_s16(&sensor->full_scale.mz, full_scale.mz);
+	set_s16(&sensor->command_word0, 0x0a00);
 }
 
-static struct six_axis_t get_min_full_scales(struct jr3_channel __iomem
-					     *channel)
+static struct six_axis_t get_min_full_scales(struct jr3_sensor __iomem *sensor)
 {
 	struct six_axis_t result;
 
-	result.fx = get_s16(&channel->min_full_scale.fx);
-	result.fy = get_s16(&channel->min_full_scale.fy);
-	result.fz = get_s16(&channel->min_full_scale.fz);
-	result.mx = get_s16(&channel->min_full_scale.mx);
-	result.my = get_s16(&channel->min_full_scale.my);
-	result.mz = get_s16(&channel->min_full_scale.mz);
+	result.fx = get_s16(&sensor->min_full_scale.fx);
+	result.fy = get_s16(&sensor->min_full_scale.fy);
+	result.fz = get_s16(&sensor->min_full_scale.fz);
+	result.mx = get_s16(&sensor->min_full_scale.mx);
+	result.my = get_s16(&sensor->min_full_scale.my);
+	result.mz = get_s16(&sensor->min_full_scale.mz);
 	return result;
 }
 
-static struct six_axis_t get_max_full_scales(struct jr3_channel __iomem
-					     *channel)
+static struct six_axis_t get_max_full_scales(struct jr3_sensor __iomem *sensor)
 {
 	struct six_axis_t result;
 
-	result.fx = get_s16(&channel->max_full_scale.fx);
-	result.fy = get_s16(&channel->max_full_scale.fy);
-	result.fz = get_s16(&channel->max_full_scale.fz);
-	result.mx = get_s16(&channel->max_full_scale.mx);
-	result.my = get_s16(&channel->max_full_scale.my);
-	result.mz = get_s16(&channel->max_full_scale.mz);
+	result.fx = get_s16(&sensor->max_full_scale.fx);
+	result.fy = get_s16(&sensor->max_full_scale.fy);
+	result.fz = get_s16(&sensor->max_full_scale.fz);
+	result.mx = get_s16(&sensor->max_full_scale.mx);
+	result.my = get_s16(&sensor->max_full_scale.my);
+	result.mz = get_s16(&sensor->max_full_scale.mz);
 	return result;
 }
 
@@ -235,35 +236,35 @@ static unsigned int jr3_pci_ai_read_chan(struct comedi_device *dev,
 
 		switch (axis) {
 		case 0:
-			val = get_s16(&spriv->channel->filter[filter].fx);
+			val = get_s16(&spriv->sensor->filter[filter].fx);
 			break;
 		case 1:
-			val = get_s16(&spriv->channel->filter[filter].fy);
+			val = get_s16(&spriv->sensor->filter[filter].fy);
 			break;
 		case 2:
-			val = get_s16(&spriv->channel->filter[filter].fz);
+			val = get_s16(&spriv->sensor->filter[filter].fz);
 			break;
 		case 3:
-			val = get_s16(&spriv->channel->filter[filter].mx);
+			val = get_s16(&spriv->sensor->filter[filter].mx);
 			break;
 		case 4:
-			val = get_s16(&spriv->channel->filter[filter].my);
+			val = get_s16(&spriv->sensor->filter[filter].my);
 			break;
 		case 5:
-			val = get_s16(&spriv->channel->filter[filter].mz);
+			val = get_s16(&spriv->sensor->filter[filter].mz);
 			break;
 		case 6:
-			val = get_s16(&spriv->channel->filter[filter].v1);
+			val = get_s16(&spriv->sensor->filter[filter].v1);
 			break;
 		case 7:
-			val = get_s16(&spriv->channel->filter[filter].v2);
+			val = get_s16(&spriv->sensor->filter[filter].v2);
 			break;
 		}
 		val += 0x4000;
 	} else if (chan == 56) {
-		val = get_u16(&spriv->channel->model_no);
+		val = get_u16(&spriv->sensor->model_no);
 	} else if (chan == 57) {
-		val = get_u16(&spriv->channel->serial_no);
+		val = get_u16(&spriv->sensor->serial_no);
 	}
 
 	return val;
@@ -279,10 +280,7 @@ static int jr3_pci_ai_insn_read(struct comedi_device *dev,
 	u16 errors;
 	int i;
 
-	if (!spriv)
-		return -EINVAL;
-
-	errors = get_u16(&spriv->channel->errors);
+	errors = get_u16(&spriv->sensor->errors);
 	if (spriv->state != state_jr3_done ||
 	    (errors & (watch_dog | watch_dog2 | sensor_change))) {
 		/* No sensor or sensor changed */
@@ -309,9 +307,8 @@ static int jr3_pci_open(struct comedi_device *dev)
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = &dev->subdevices[i];
 		spriv = s->private;
-		if (spriv)
-			dev_dbg(dev->class_dev, "serial: %p %d (%d)\n",
-				spriv, spriv->serial_no, s->index);
+		dev_dbg(dev->class_dev, "serial[%d]: %d\n", s->index,
+			spriv->serial_no);
 	}
 	return 0;
 }
@@ -375,8 +372,7 @@ static int jr3_check_firmware(struct comedi_device *dev,
 static void jr3_write_firmware(struct comedi_device *dev,
 			       int subdev, const u8 *data, size_t size)
 {
-	struct jr3_pci_dev_private *devpriv = dev->private;
-	struct jr3_t __iomem *iobase = devpriv->iobase;
+	struct jr3_block __iomem *block = dev->mmio;
 	u32 __iomem *lo;
 	u32 __iomem *hi;
 	int more = 1;
@@ -409,8 +405,8 @@ static void jr3_write_firmware(struct comedi_device *dev,
 				unsigned int data1 = 0;
 				unsigned int data2 = 0;
 
-				lo = &iobase->channel[subdev].program_lo[addr];
-				hi = &iobase->channel[subdev].program_hi[addr];
+				lo = &block[subdev].program_lo[addr];
+				hi = &block[subdev].program_hi[addr];
 
 				more = more &&
 				       read_idm_word(data, size, &pos, &data1);
@@ -453,17 +449,14 @@ jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 {
 	struct jr3_pci_subdev_private *spriv = s->private;
 	struct jr3_pci_poll_delay result = poll_delay_min_max(1000, 2000);
-	struct jr3_channel __iomem *channel;
+	struct jr3_sensor __iomem *sensor;
 	u16 model_no;
 	u16 serial_no;
 	int errors;
 	int i;
 
-	if (!spriv)
-		return result;
-
-	channel = spriv->channel;
-	errors = get_u16(&channel->errors);
+	sensor = spriv->sensor;
+	errors = get_u16(&sensor->errors);
 
 	if (errors != spriv->errors)
 		spriv->errors = errors;
@@ -474,8 +467,8 @@ jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 
 	switch (spriv->state) {
 	case state_jr3_poll:
-		model_no = get_u16(&channel->model_no);
-		serial_no = get_u16(&channel->serial_no);
+		model_no = get_u16(&sensor->model_no);
+		serial_no = get_u16(&sensor->serial_no);
 
 		if ((errors & (watch_dog | watch_dog2)) ||
 		    model_no == 0 || serial_no == 0) {
@@ -499,8 +492,8 @@ jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 		} else {
 			struct jr3_pci_transform transf;
 
-			spriv->model_no = get_u16(&channel->model_no);
-			spriv->serial_no = get_u16(&channel->serial_no);
+			spriv->model_no = get_u16(&sensor->model_no);
+			spriv->serial_no = get_u16(&sensor->serial_no);
 
 			/* Transformation all zeros */
 			for (i = 0; i < ARRAY_SIZE(transf.link); i++) {
@@ -508,24 +501,24 @@ jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 				transf.link[i].link_amount = 0;
 			}
 
-			set_transforms(channel, transf, 0);
-			use_transform(channel, 0);
+			set_transforms(sensor, &transf, 0);
+			use_transform(sensor, 0);
 			spriv->state = state_jr3_init_transform_complete;
 			/* Allow 20 ms for completion */
 			result = poll_delay_min_max(20, 100);
 		}
 		break;
 	case state_jr3_init_transform_complete:
-		if (!is_complete(channel)) {
+		if (!is_complete(sensor)) {
 			result = poll_delay_min_max(20, 100);
 		} else {
 			/* Set full scale */
 			struct six_axis_t min_full_scale;
 			struct six_axis_t max_full_scale;
 
-			min_full_scale = get_min_full_scales(channel);
-			max_full_scale = get_max_full_scales(channel);
-			set_full_scales(channel, max_full_scale);
+			min_full_scale = get_min_full_scales(sensor);
+			max_full_scale = get_max_full_scales(sensor);
+			set_full_scales(sensor, max_full_scale);
 
 			spriv->state = state_jr3_init_set_full_scale_complete;
 			/* Allow 20 ms for completion */
@@ -533,50 +526,51 @@ jr3_pci_poll_subdevice(struct comedi_subdevice *s)
 		}
 		break;
 	case state_jr3_init_set_full_scale_complete:
-		if (!is_complete(channel)) {
+		if (!is_complete(sensor)) {
 			result = poll_delay_min_max(20, 100);
 		} else {
-			struct force_array __iomem *fs = &channel->full_scale;
+			struct force_array __iomem *fs = &sensor->full_scale;
+			union jr3_pci_single_range *r = spriv->range;
 
 			/* Use ranges in kN or we will overflow around 2000N! */
-			spriv->range[0].range.min = -get_s16(&fs->fx) * 1000;
-			spriv->range[0].range.max = get_s16(&fs->fx) * 1000;
-			spriv->range[1].range.min = -get_s16(&fs->fy) * 1000;
-			spriv->range[1].range.max = get_s16(&fs->fy) * 1000;
-			spriv->range[2].range.min = -get_s16(&fs->fz) * 1000;
-			spriv->range[2].range.max = get_s16(&fs->fz) * 1000;
-			spriv->range[3].range.min = -get_s16(&fs->mx) * 100;
-			spriv->range[3].range.max = get_s16(&fs->mx) * 100;
-			spriv->range[4].range.min = -get_s16(&fs->my) * 100;
-			spriv->range[4].range.max = get_s16(&fs->my) * 100;
-			spriv->range[5].range.min = -get_s16(&fs->mz) * 100;
+			r[0].l.range[0].min = -get_s16(&fs->fx) * 1000;
+			r[0].l.range[0].max = get_s16(&fs->fx) * 1000;
+			r[1].l.range[0].min = -get_s16(&fs->fy) * 1000;
+			r[1].l.range[0].max = get_s16(&fs->fy) * 1000;
+			r[2].l.range[0].min = -get_s16(&fs->fz) * 1000;
+			r[2].l.range[0].max = get_s16(&fs->fz) * 1000;
+			r[3].l.range[0].min = -get_s16(&fs->mx) * 100;
+			r[3].l.range[0].max = get_s16(&fs->mx) * 100;
+			r[4].l.range[0].min = -get_s16(&fs->my) * 100;
+			r[4].l.range[0].max = get_s16(&fs->my) * 100;
+			r[5].l.range[0].min = -get_s16(&fs->mz) * 100;
 			/* the next five are questionable */
-			spriv->range[5].range.max = get_s16(&fs->mz) * 100;
-			spriv->range[6].range.min = -get_s16(&fs->v1) * 100;
-			spriv->range[6].range.max = get_s16(&fs->v1) * 100;
-			spriv->range[7].range.min = -get_s16(&fs->v2) * 100;
-			spriv->range[7].range.max = get_s16(&fs->v2) * 100;
-			spriv->range[8].range.min = 0;
-			spriv->range[8].range.max = 65535;
-
-			use_offset(channel, 0);
+			r[5].l.range[0].max = get_s16(&fs->mz) * 100;
+			r[6].l.range[0].min = -get_s16(&fs->v1) * 100;
+			r[6].l.range[0].max = get_s16(&fs->v1) * 100;
+			r[7].l.range[0].min = -get_s16(&fs->v2) * 100;
+			r[7].l.range[0].max = get_s16(&fs->v2) * 100;
+			r[8].l.range[0].min = 0;
+			r[8].l.range[0].max = 65535;
+
+			use_offset(sensor, 0);
 			spriv->state = state_jr3_init_use_offset_complete;
 			/* Allow 40 ms for completion */
 			result = poll_delay_min_max(40, 100);
 		}
 		break;
 	case state_jr3_init_use_offset_complete:
-		if (!is_complete(channel)) {
+		if (!is_complete(sensor)) {
 			result = poll_delay_min_max(20, 100);
 		} else {
-			set_s16(&channel->offsets.fx, 0);
-			set_s16(&channel->offsets.fy, 0);
-			set_s16(&channel->offsets.fz, 0);
-			set_s16(&channel->offsets.mx, 0);
-			set_s16(&channel->offsets.my, 0);
-			set_s16(&channel->offsets.mz, 0);
+			set_s16(&sensor->offsets.fx, 0);
+			set_s16(&sensor->offsets.fy, 0);
+			set_s16(&sensor->offsets.fz, 0);
+			set_s16(&sensor->offsets.mx, 0);
+			set_s16(&sensor->offsets.my, 0);
+			set_s16(&sensor->offsets.mz, 0);
 
-			set_offset(channel);
+			set_offset(sensor);
 
 			spriv->state = state_jr3_done;
 		}
@@ -606,25 +600,23 @@ static void jr3_pci_poll_dev(unsigned long data)
 	delay = 1000;
 	now = jiffies;
 
-	/* Poll all channels that are ready to be polled */
+	/* Poll all sensors that are ready to be polled */
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = &dev->subdevices[i];
 		spriv = s->private;
 
-		if (now > spriv->next_time_min) {
+		if (time_after_eq(now, spriv->next_time_min)) {
 			struct jr3_pci_poll_delay sub_delay;
 
 			sub_delay = jr3_pci_poll_subdevice(s);
 
 			spriv->next_time_min = jiffies +
 					       msecs_to_jiffies(sub_delay.min);
-			spriv->next_time_max = jiffies +
-					       msecs_to_jiffies(sub_delay.max);
 
 			if (sub_delay.max && sub_delay.max < delay)
 				/*
 				 * Wake up as late as possible ->
-				 * poll as many channels as possible at once.
+				 * poll as many sensors as possible at once.
 				 */
 				delay = sub_delay.max;
 		}
@@ -638,7 +630,7 @@ static void jr3_pci_poll_dev(unsigned long data)
 static struct jr3_pci_subdev_private *
 jr3_pci_alloc_spriv(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	struct jr3_pci_dev_private *devpriv = dev->private;
+	struct jr3_block __iomem *block = dev->mmio;
 	struct jr3_pci_subdev_private *spriv;
 	int j;
 	int k;
@@ -647,36 +639,43 @@ jr3_pci_alloc_spriv(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (!spriv)
 		return NULL;
 
-	spriv->channel = &devpriv->iobase->channel[s->index].data;
+	spriv->sensor = &block[s->index].sensor;
 
 	for (j = 0; j < 8; j++) {
-		spriv->range[j].length = 1;
-		spriv->range[j].range.min = -1000000;
-		spriv->range[j].range.max = 1000000;
+		spriv->range[j].l.length = 1;
+		spriv->range[j].l.range[0].min = -1000000;
+		spriv->range[j].l.range[0].max = 1000000;
 
 		for (k = 0; k < 7; k++) {
-			spriv->range_table_list[j + k * 8] =
-				(struct comedi_lrange *)&spriv->range[j];
+			spriv->range_table_list[j + k * 8] = &spriv->range[j].l;
 			spriv->maxdata_list[j + k * 8] = 0x7fff;
 		}
 	}
-	spriv->range[8].length = 1;
-	spriv->range[8].range.min = 0;
-	spriv->range[8].range.max = 65536;
+	spriv->range[8].l.length = 1;
+	spriv->range[8].l.range[0].min = 0;
+	spriv->range[8].l.range[0].max = 65535;
 
-	spriv->range_table_list[56] = (struct comedi_lrange *)&spriv->range[8];
-	spriv->range_table_list[57] = (struct comedi_lrange *)&spriv->range[8];
+	spriv->range_table_list[56] = &spriv->range[8].l;
+	spriv->range_table_list[57] = &spriv->range[8].l;
 	spriv->maxdata_list[56] = 0xffff;
 	spriv->maxdata_list[57] = 0xffff;
 
-	dev_dbg(dev->class_dev, "p->channel %p %p (%tx)\n",
-		spriv->channel, devpriv->iobase,
-		((char __iomem *)spriv->channel -
-		 (char __iomem *)devpriv->iobase));
-
 	return spriv;
 }
 
+static void jr3_pci_show_copyright(struct comedi_device *dev)
+{
+	struct jr3_block __iomem *block = dev->mmio;
+	struct jr3_sensor __iomem *sensor0 = &block[0].sensor;
+	char copy[ARRAY_SIZE(sensor0->copyright) + 1];
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sensor0->copyright); i++)
+		copy[i] = (char)(get_u16(&sensor0->copyright[i]) >> 8);
+	copy[i] = '\0';
+	dev_dbg(dev->class_dev, "Firmware copyright: %s\n", copy);
+}
+
 static int jr3_pci_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
@@ -684,16 +683,12 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 	static const struct jr3_pci_board *board;
 	struct jr3_pci_dev_private *devpriv;
 	struct jr3_pci_subdev_private *spriv;
+	struct jr3_block __iomem *block;
 	struct comedi_subdevice *s;
 	int ret;
 	int i;
 
-	if (sizeof(struct jr3_channel) != 0xc00) {
-		dev_err(dev->class_dev,
-			"sizeof(struct jr3_channel) = %x [expected %x]\n",
-			(unsigned int)sizeof(struct jr3_channel), 0xc00);
-		return -EINVAL;
-	}
+	BUILD_BUG_ON(sizeof(struct jr3_block) != 0x80000);
 
 	if (context < ARRAY_SIZE(jr3_pci_boards))
 		board = &jr3_pci_boards[context];
@@ -710,10 +705,15 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	devpriv->iobase = pci_ioremap_bar(pcidev, 0);
-	if (!devpriv->iobase)
+	if (pci_resource_len(pcidev, 0) < board->n_subdevs * sizeof(*block))
+		return -ENXIO;
+
+	dev->mmio = pci_ioremap_bar(pcidev, 0);
+	if (!dev->mmio)
 		return -ENOMEM;
 
+	block = dev->mmio;
+
 	ret = comedi_alloc_subdevices(dev, board->n_subdevs);
 	if (ret)
 		return ret;
@@ -727,15 +727,17 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 		s->insn_read	= jr3_pci_ai_insn_read;
 
 		spriv = jr3_pci_alloc_spriv(dev, s);
-		if (spriv) {
-			/* Channel specific range and maxdata */
-			s->range_table_list	= spriv->range_table_list;
-			s->maxdata_list		= spriv->maxdata_list;
-		}
+		if (!spriv)
+			return -ENOMEM;
+
+		/* Channel specific range and maxdata */
+		s->range_table_list	= spriv->range_table_list;
+		s->maxdata_list		= spriv->maxdata_list;
 	}
 
 	/* Reset DSP card */
-	writel(0, &devpriv->iobase->channel[0].reset);
+	for (i = 0; i < dev->n_subdevices; i++)
+		writel(0, &block[i].reset);
 
 	ret = comedi_load_firmware(dev, &comedi_to_pci_dev(dev)->dev,
 				   "comedi/jr3pci.idm",
@@ -758,11 +760,7 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 	 * can read firmware version
 	 */
 	msleep_interruptible(25);
-	for (i = 0; i < 0x18; i++) {
-		dev_dbg(dev->class_dev, "%c\n",
-			get_u16(&devpriv->iobase->channel[0].
-				data.copyright[i]) >> 8);
-	}
+	jr3_pci_show_copyright(dev);
 
 	/* Start card timer */
 	for (i = 0; i < dev->n_subdevices; i++) {
@@ -770,7 +768,6 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 		spriv = s->private;
 
 		spriv->next_time_min = jiffies + msecs_to_jiffies(500);
-		spriv->next_time_max = jiffies + msecs_to_jiffies(2000);
 	}
 
 	setup_timer(&devpriv->timer, jr3_pci_poll_dev, (unsigned long)dev);
@@ -784,13 +781,10 @@ static void jr3_pci_detach(struct comedi_device *dev)
 {
 	struct jr3_pci_dev_private *devpriv = dev->private;
 
-	if (devpriv) {
+	if (devpriv)
 		del_timer_sync(&devpriv->timer);
 
-		if (devpriv->iobase)
-			iounmap(devpriv->iobase);
-	}
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver jr3_pci_driver = {
@@ -825,6 +819,6 @@ static struct pci_driver jr3_pci_pci_driver = {
 module_comedi_pci_driver(jr3_pci_driver, jr3_pci_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for JR3/PCI force sensor board");
 MODULE_LICENSE("GPL");
 MODULE_FIRMWARE("comedi/jr3pci.idm");
diff --git a/drivers/staging/comedi/drivers/jr3_pci.h b/drivers/staging/comedi/drivers/jr3_pci.h
index f10a84fb6c14..28ff0c2aa3b8 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.h
+++ b/drivers/staging/comedi/drivers/jr3_pci.h
@@ -280,7 +280,7 @@ struct intern_transform {
  * and hardware manuals.
  */
 
-struct jr3_channel {
+struct jr3_sensor {
 	/*
 	 * Raw_channels is the area used to store the raw data coming from
 	 * the sensor.
@@ -724,13 +724,11 @@ struct jr3_channel {
 	struct intern_transform transforms[0x10];	/* offset 0x0200 */
 };
 
-struct jr3_t {
-	struct {
-		u32 program_lo[0x4000];		/*  0x00000 - 0x10000 */
-		struct jr3_channel data;	/*  0x10000 - 0x10c00 */
-		char pad2[0x30000 - 0x00c00];	/*  0x10c00 - 0x40000 */
-		u32 program_hi[0x8000];		/*  0x40000 - 0x60000 */
-		u32 reset;			/*  0x60000 - 0x60004 */
-		char pad3[0x20000 - 0x00004];	/*  0x60004 - 0x80000 */
-	} channel[4];
+struct jr3_block {
+	u32 program_lo[0x4000];		/*  0x00000 - 0x10000 */
+	struct jr3_sensor sensor;	/*  0x10000 - 0x10c00 */
+	char pad2[0x30000 - 0x00c00];	/*  0x10c00 - 0x40000 */
+	u32 program_hi[0x8000];		/*  0x40000 - 0x60000 */
+	u32 reset;			/*  0x60000 - 0x60004 */
+	char pad3[0x20000 - 0x00004];	/*  0x60004 - 0x80000 */
 };
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index ffcf7afce684..2d62a8c57332 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -216,7 +216,7 @@ static const int ni_irqpin[] = {
 
 #include "ni_mio_common.c"
 
-static struct pnp_device_id device_ids[] = {
+static const struct pnp_device_id device_ids[] = {
 	{.id = "NIC1900", .driver_data = 0},
 	{.id = "NIC2400", .driver_data = 0},
 	{.id = "NIC2500", .driver_data = 0},
diff --git a/drivers/staging/comedi/drivers/ni_usb6501.c b/drivers/staging/comedi/drivers/ni_usb6501.c
index 5036eebb9162..9a0a96329a55 100644
--- a/drivers/staging/comedi/drivers/ni_usb6501.c
+++ b/drivers/staging/comedi/drivers/ni_usb6501.c
@@ -45,7 +45,7 @@
  *	byte 3 is the total packet length
  *
  *	byte 4 is always 00
- *	byte 5 is is the total packet length - 4
+ *	byte 5 is the total packet length - 4
  *	byte 6 is always 01
  *	byte 7 is the command
  *
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 97939b42cc00..4b9c22665748 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -74,26 +74,34 @@ struct s626_buffer_dma {
 	void *logical_base;
 };
 
+/**
+ * struct s626_private - Working data for s626 driver.
+ * @ai_cmd_running: non-zero if ai_cmd is running.
+ * @ai_sample_timer: time between samples in units of the timer.
+ * @ai_convert_count: conversion counter.
+ * @ai_convert_timer: time between conversion in units of the timer.
+ * @counter_int_enabs: counter interrupt enable mask for MISC2 register.
+ * @adc_items: number of items in ADC poll list.
+ * @rps_buf: DMA buffer used to hold ADC (RPS1) program.
+ * @ana_buf:  DMA buffer used to receive ADC data and hold DAC data.
+ * @dac_wbuf: pointer to logical adrs of DMA buffer used to hold DAC data.
+ * @dacpol: image of DAC polarity register.
+ * @trim_setpoint: images of TrimDAC setpoints.
+ * @i2c_adrs: I2C device address for onboard EEPROM (board rev dependent)
+ */
 struct s626_private {
-	u8 ai_cmd_running;		/* ai_cmd is running */
-	unsigned int ai_sample_timer;	/* time between samples in
-					 * units of the timer */
-	int ai_convert_count;		/* conversion counter */
-	unsigned int ai_convert_timer;	/* time between conversion in
-					 * units of the timer */
-	u16 counter_int_enabs;	        /* counter interrupt enable mask
-					 * for MISC2 register */
-	u8 adc_items;		        /* number of items in ADC poll list */
-	struct s626_buffer_dma rps_buf;	/* DMA buffer used to hold ADC (RPS1)
-					 * program */
-	struct s626_buffer_dma ana_buf;	/* DMA buffer used to receive ADC data
-					 * and hold DAC data */
-	u32 *dac_wbuf;		        /* pointer to logical adrs of DMA buffer
-					 * used to hold DAC data */
-	u16 dacpol;		        /* image of DAC polarity register */
-	u8 trim_setpoint[12];	        /* images of TrimDAC setpoints */
-	u32 i2c_adrs;		        /* I2C device address for onboard EEPROM
-					 * (board rev dependent) */
+	u8 ai_cmd_running;
+	unsigned int ai_sample_timer;
+	int ai_convert_count;
+	unsigned int ai_convert_timer;
+	u16 counter_int_enabs;
+	u8 adc_items;
+	struct s626_buffer_dma rps_buf;
+	struct s626_buffer_dma ana_buf;
+	u32 *dac_wbuf;
+	u16 dacpol;
+	u8 trim_setpoint[12];
+	u32 i2c_adrs;
 };
 
 /* Counter overflow/index event flag masks for RDMISC2. */
@@ -591,7 +599,7 @@ static int s626_write_trim_dac(struct comedi_device *dev,
 	 * Save the new setpoint in case the application needs to read it back
 	 * later.
 	 */
-	devpriv->trim_setpoint[logical_chan] = (u8)dac_data;
+	devpriv->trim_setpoint[logical_chan] = dac_data;
 
 	/* Map logical channel number to physical channel number. */
 	chan = s626_trimchan[logical_chan];
@@ -1928,7 +1936,7 @@ static int s626_ao_insn_write(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		int16_t dacdata = (int16_t)data[i];
+		s16 dacdata = (s16)data[i];
 		int ret;
 
 		dacdata -= (0x1fff);
diff --git a/drivers/staging/dgnc/TODO b/drivers/staging/dgnc/TODO
index e26d1d6a5941..d4cc65770513 100644
--- a/drivers/staging/dgnc/TODO
+++ b/drivers/staging/dgnc/TODO
@@ -1,7 +1,4 @@
 * remove unnecessary comments
-* remove unnecessary error messages. Example kzalloc() has its
-  own error message. Adding an extra one is useless.
-* use goto statements for error handling when appropriate
 * there is a lot of unnecessary code in the driver. It was
   originally a standalone driver. Remove unneeded code.
 
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index c20ffdd254d8..9639035fddd1 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -14,15 +14,15 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/sched.h>	/* For jiffies, task states */
-#include <linux/interrupt.h>	/* For tasklet and interrupt structs/defines */
-#include <linux/delay.h>	/* For udelay */
-#include <linux/io.h>		/* For read[bwl]/write[bwl] */
-#include <linux/serial.h>	/* For struct async_serial */
-#include <linux/serial_reg.h>	/* For the various UART offsets */
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/serial.h>
+#include <linux/serial_reg.h>
 #include <linux/pci.h>
 
-#include "dgnc_driver.h"	/* Driver main header file */
+#include "dgnc_driver.h"
 #include "dgnc_cls.h"
 #include "dgnc_tty.h"
 
@@ -273,7 +273,6 @@ static inline void cls_set_no_input_flow_control(struct channel_t *ch)
 }
 
 /*
- * cls_clear_break.
  * Determines whether its time to shut off break condition.
  *
  * No locks are assumed to be held when calling this function.
@@ -283,12 +282,11 @@ static inline void cls_clear_break(struct channel_t *ch, int force)
 {
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* Bail if we aren't currently sending a break. */
 	if (!ch->ch_stop_sending_break) {
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
@@ -316,16 +314,14 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 	ushort tail;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* cache head and tail of queue */
 	head = ch->ch_r_head;
 	tail = ch->ch_r_tail;
 
-	/* Store how much space we have left in the queue */
 	qleft = tail - head - 1;
 	if (qleft < 0)
 		qleft += RQUEUEMASK + 1;
@@ -343,9 +339,7 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		if (!(linestatus & (UART_LSR_DR)))
 			break;
 
-		/*
-		 * Discard character if we are ignoring the error mask.
-		 */
+		/* Discard character if we are ignoring the error mask. */
 		if (linestatus & error_mask)  {
 			linestatus = 0;
 			readb(&ch->ch_cls_uart->txrx);
@@ -356,9 +350,6 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * If our queue is full, we have no choice but to drop some
 		 * data. The assumption is that HWFLOW or SWFLOW should have
 		 * stopped things way way before we got to this point.
-		 *
-		 * I decided that I wanted to ditch the oldest data first,
-		 * I hope thats okay with everyone? Yes? Good.
 		 */
 		while (qleft < 1) {
 			tail = (tail + 1) & RQUEUEMASK;
@@ -380,13 +371,10 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		if (ch->ch_equeue[head] & UART_LSR_FE)
 			ch->ch_err_frame++;
 
-		/* Add to, and flip head if needed */
 		head = (head + 1) & RQUEUEMASK;
 		ch->ch_rxcount++;
 	}
 
-	/* Write new final heads to channel structure. */
-
 	ch->ch_r_head = head & RQUEUEMASK;
 	ch->ch_e_head = head & EQUEUEMASK;
 
@@ -398,7 +386,7 @@ static void cls_assert_modem_signals(struct channel_t *ch)
 {
 	unsigned char out;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	out = ch->ch_mostat;
@@ -421,12 +409,11 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 	uint len_written = 0;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* No data to write to the UART */
 	if (ch->ch_w_tail == ch->ch_w_head)
 		goto exit_unlock;
 
@@ -440,12 +427,10 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 
 	n = 32;
 
-	/* cache head and tail of queue */
 	head = ch->ch_w_head & WQUEUEMASK;
 	tail = ch->ch_w_tail & WQUEUEMASK;
 	qlen = (head - tail) & WQUEUEMASK;
 
-	/* Find minimum of the FIFO space, versus queue length */
 	n = min(n, qlen);
 
 	while (n > 0) {
@@ -494,7 +479,7 @@ static void cls_parse_modem(struct channel_t *ch, unsigned char signals)
 	unsigned char msignals = signals;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	/*
@@ -524,10 +509,7 @@ static void cls_parse_modem(struct channel_t *ch, unsigned char signals)
 	}
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	/*
-	 * Scrub off lower bits. They signify delta's, which I don't
-	 * care about
-	 */
+	/* Scrub off lower bits. They signify delta's */
 	signals &= 0xf0;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -569,34 +551,28 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 		return;
 
 	ch = brd->channels[port];
-	if (ch->magic != DGNC_CHANNEL_MAGIC)
-		return;
 
 	/* Here we try to figure out what caused the interrupt to happen */
 	while (1) {
 		isr = readb(&ch->ch_cls_uart->isr_fcr);
 
-		/* Bail if no pending interrupt on port */
 		if (isr & UART_IIR_NO_INT)
 			break;
 
 		/* Receive Interrupt pending */
 		if (isr & (UART_IIR_RDI | UART_IIR_RDI_TIMEOUT)) {
-			/* Read data from uart -> queue */
 			cls_copy_data_from_uart_to_queue(ch);
 			dgnc_check_queue_flow_control(ch);
 		}
 
 		/* Transmit Hold register empty pending */
 		if (isr & UART_IIR_THRI) {
-			/* Transfer data (if any) from Write Queue -> UART. */
 			spin_lock_irqsave(&ch->ch_lock, flags);
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			cls_copy_data_from_queue_to_uart(ch);
 		}
 
-		/* Parse any modem signal changes */
 		cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
 	}
 }
@@ -604,12 +580,14 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 /* Channel lock MUST be held before calling this function! */
 static void cls_flush_uart_write(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT),
 	       &ch->ch_cls_uart->isr_fcr);
-	usleep_range(10, 20);
+
+	/* Must use *delay family functions in atomic context */
+	udelay(10);
 
 	ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 }
@@ -617,7 +595,7 @@ static void cls_flush_uart_write(struct channel_t *ch)
 /* Channel lock MUST be held before calling this function! */
 static void cls_flush_uart_read(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	/*
@@ -634,10 +612,7 @@ static void cls_flush_uart_read(struct channel_t *ch)
 	udelay(10);
 }
 
-/*
- * cls_param()
- * Send any/all changes to the line to the UART.
- */
+/* Send any/all changes to the line to the UART. */
 static void cls_param(struct tty_struct *tty)
 {
 	unsigned char lcr = 0;
@@ -650,23 +625,22 @@ static void cls_param(struct tty_struct *tty)
 	struct channel_t *ch;
 	struct un_t   *un;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = (struct un_t *)tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	/* If baud rate is zero, flush queues, and set mval to drop DTR. */
-
 	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
 		ch->ch_r_head = 0;
 		ch->ch_r_tail = 0;
@@ -776,10 +750,6 @@ static void cls_param(struct tty_struct *tty)
 	if (!(ch->ch_c_cflag & PARODD))
 		lcr |= UART_LCR_EPAR;
 
-	/*
-	 * Not all platforms support mark/space parity,
-	 * so this will hide behind an ifdef.
-	 */
 #ifdef CMSPAR
 	if (ch->ch_c_cflag & CMSPAR)
 		lcr |= UART_LCR_SPAR;
@@ -850,10 +820,6 @@ static void cls_param(struct tty_struct *tty)
 	if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		cls_set_cts_flow_control(ch);
 	} else if (ch->ch_c_iflag & IXON) {
-		/*
-		 * If start/stop is set to disable, then we should
-		 * disable flow control
-		 */
 		if ((ch->ch_startc == _POSIX_VDISABLE) ||
 		    (ch->ch_stopc == _POSIX_VDISABLE))
 			cls_set_no_output_flow_control(ch);
@@ -866,10 +832,6 @@ static void cls_param(struct tty_struct *tty)
 	if (ch->ch_digi.digi_flags & RTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		cls_set_rts_flow_control(ch);
 	} else if (ch->ch_c_iflag & IXOFF) {
-		/*
-		 * If start/stop is set to disable, then we should disable
-		 * flow control
-		 */
 		if ((ch->ch_startc == _POSIX_VDISABLE) ||
 		    (ch->ch_stopc == _POSIX_VDISABLE))
 			cls_set_no_input_flow_control(ch);
@@ -881,12 +843,10 @@ static void cls_param(struct tty_struct *tty)
 
 	cls_assert_modem_signals(ch);
 
-	/* Get current status of the modem signals now */
 	cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
 }
 
-/* Our board poller function. */
-
+/* Board poller function. */
 static void cls_tasklet(unsigned long data)
 {
 	struct dgnc_board *bd = (struct dgnc_board *)data;
@@ -896,10 +856,9 @@ static void cls_tasklet(unsigned long data)
 	int state = 0;
 	int ports = 0;
 
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
-	/* Cache a couple board values */
 	spin_lock_irqsave(&bd->bd_lock, flags);
 	state = bd->state;
 	ports = bd->nasync;
@@ -911,10 +870,7 @@ static void cls_tasklet(unsigned long data)
 	 */
 	spin_lock_irqsave(&bd->bd_intr_lock, flags);
 
-	/* If board is ready, parse deeper to see if there is anything to do. */
-
 	if ((state == BOARD_READY) && (ports > 0)) {
-		/* Loop on each port */
 		for (i = 0; i < ports; i++) {
 			ch = bd->channels[i];
 
@@ -934,8 +890,6 @@ static void cls_tasklet(unsigned long data)
 			cls_copy_data_from_queue_to_uart(ch);
 			dgnc_wakeup_writes(ch);
 
-			/* Check carrier function. */
-
 			dgnc_carrier(ch);
 
 			/*
@@ -950,11 +904,7 @@ static void cls_tasklet(unsigned long data)
 	spin_unlock_irqrestore(&bd->bd_intr_lock, flags);
 }
 
-/*
- * cls_intr()
- *
- * Classic specific interrupt handler.
- */
+/* Classic specific interrupt handler. */
 static irqreturn_t cls_intr(int irq, void *voidbrd)
 {
 	struct dgnc_board *brd = voidbrd;
@@ -962,33 +912,20 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 	unsigned char poll_reg;
 	unsigned long flags;
 
-	/*
-	 * Check to make sure it didn't receive interrupt with a null board
-	 * associated or a board pointer that wasn't ours.
-	 */
-	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
+	if (!brd)
 		return IRQ_NONE;
 
 	spin_lock_irqsave(&brd->bd_intr_lock, flags);
 
-	/*
-	 * Check the board's global interrupt offset to see if we
-	 * we actually do have an interrupt pending for us.
-	 */
 	poll_reg = readb(brd->re_map_membase + UART_CLASSIC_POLL_ADDR_OFFSET);
-
-	/* If 0, no interrupts pending */
 	if (!poll_reg) {
 		spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
 		return IRQ_NONE;
 	}
 
-	/* Parse each port to find out what caused the interrupt */
 	for (i = 0; i < brd->nasync; i++)
 		cls_parse_isr(brd, i);
 
-	/* Schedule tasklet to more in-depth servicing at a better time. */
-
 	tasklet_schedule(&brd->helper_tasklet);
 
 	spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
@@ -1013,7 +950,7 @@ static void cls_enable_receiver(struct channel_t *ch)
 }
 
 /*
- * This function basically goes to sleep for secs, or until
+ * This function basically goes to sleep for seconds, or until
  * it gets signalled that the port has fully drained.
  */
 static int cls_drain(struct tty_struct *tty, uint seconds)
@@ -1022,15 +959,15 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 	struct channel_t *ch;
 	struct un_t *un;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return -ENXIO;
 
 	un = (struct un_t *)tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return -ENXIO;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -ENXIO;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -1047,7 +984,7 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 
 static void cls_send_start_character(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	if (ch->ch_startc != _POSIX_VDISABLE) {
@@ -1058,7 +995,7 @@ static void cls_send_start_character(struct channel_t *ch)
 
 static void cls_send_stop_character(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	if (ch->ch_stopc != _POSIX_VDISABLE) {
@@ -1067,7 +1004,6 @@ static void cls_send_stop_character(struct channel_t *ch)
 	}
 }
 
-/* Inits UART */
 static void cls_uart_init(struct channel_t *ch)
 {
 	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
@@ -1096,7 +1032,7 @@ static void cls_uart_init(struct channel_t *ch)
 
 	writeb(UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT,
 	       &ch->ch_cls_uart->isr_fcr);
-	udelay(10);
+	usleep_range(10, 20);
 
 	ch->ch_flags |= (CH_FIFO_ENABLED | CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 
@@ -1104,25 +1040,21 @@ static void cls_uart_init(struct channel_t *ch)
 	readb(&ch->ch_cls_uart->msr);
 }
 
-/* Turns off UART.  */
-
 static void cls_uart_off(struct channel_t *ch)
 {
 	writeb(0, &ch->ch_cls_uart->ier);
 }
 
 /*
- * cls_get_uarts_bytes_left.
- * Returns 0 is nothing left in the FIFO, returns 1 otherwise.
- *
  * The channel lock MUST be held by the calling function.
+ * Returns 0 is nothing left in the FIFO, returns 1 otherwise.
  */
 static uint cls_get_uart_bytes_left(struct channel_t *ch)
 {
 	unsigned char left = 0;
 	unsigned char lsr = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return 0;
 
 	lsr = readb(&ch->ch_cls_uart->lsr);
@@ -1141,20 +1073,16 @@ static uint cls_get_uart_bytes_left(struct channel_t *ch)
 }
 
 /*
- * cls_send_break.
  * Starts sending a break thru the UART.
- *
  * The channel lock MUST be held by the calling function.
  */
 static void cls_send_break(struct channel_t *ch, int msecs)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	/* If we receive a time of 0, this means turn off the break. */
-
 	if (msecs == 0) {
-		/* Turn break off, and unset some variables */
 		if (ch->ch_flags & CH_BREAK_SENDING) {
 			unsigned char temp = readb(&ch->ch_cls_uart->lcr);
 
@@ -1182,7 +1110,6 @@ static void cls_send_break(struct channel_t *ch, int msecs)
 }
 
 /*
- * cls_send_immediate_char.
  * Sends a specific character as soon as possible to the UART,
  * jumping over any bytes that might be in the write queue.
  *
@@ -1190,7 +1117,7 @@ static void cls_send_break(struct channel_t *ch, int msecs)
  */
 static void cls_send_immediate_char(struct channel_t *ch, unsigned char c)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	writeb(c, &ch->ch_cls_uart->txrx);
@@ -1203,8 +1130,6 @@ static void cls_vpd(struct dgnc_board *brd)
 	int i = 0;
 
 	vpdbase = pci_resource_start(brd->pdev, 3);
-
-	/* No VPD */
 	if (!vpdbase)
 		return;
 
@@ -1213,7 +1138,6 @@ static void cls_vpd(struct dgnc_board *brd)
 	if (!re_map_vpdbase)
 		return;
 
-	/* Store the VPD into our buffer */
 	for (i = 0; i < 0x40; i++) {
 		brd->vpd[i] = readb(re_map_vpdbase + i);
 		pr_info("%x ", brd->vpd[i]);
diff --git a/drivers/staging/dgnc/dgnc_cls.h b/drivers/staging/dgnc/dgnc_cls.h
index 463ad30efb3b..9dfa9682a897 100644
--- a/drivers/staging/dgnc/dgnc_cls.h
+++ b/drivers/staging/dgnc/dgnc_cls.h
@@ -13,27 +13,24 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DGNC_CLS_H
-#define __DGNC_CLS_H
+#ifndef _DGNC_CLS_H
+#define _DGNC_CLS_H
 
-/************************************************************************
- * Per channel/port Classic UART structure				*
- ************************************************************************
- *		Base Structure Entries Usage Meanings to Host		*
- *									*
- *	W = read write		R = read only				*
- *			U = Unused.					*
- ************************************************************************/
-
-/*
- * txrx    : WR RHR/THR - Holding reg
- * ier     : WR IER - Interrupt Enable Reg
- * isr_fcr : WR ISR/FCR - Interrupt Status Reg/Fifo Control Reg
- * lcr     : WR LCR - Line Control Reg
- * mcr     : WR MCR - Modem Control Reg
- * lsr     : WR LSR - Line Status Reg
- * msr     : WR MSG - Modem Status Reg
- * spr     : WR SPR - Scratch pad Reg
+/**
+ * struct cls_uart_struct - Per channel/port Classic UART.
+ *
+ * key - W = read write
+ *     - R = read only
+ *     - U = unused
+ *
+ * @txrx: (WR) Holding Register.
+ * @ier: (WR) Interrupt Enable Register.
+ * @isr_fcr: (WR) Interrupt Status Register/Fifo Control Register.
+ * @lcr: (WR) Line Control Register.
+ * @mcr: (WR) Modem Control Register.
+ * @lsr: (WR) Line Status Register.
+ * @msr: (WR) Modem Status Register.
+ * @spr: (WR) Scratch Pad Register.
  */
 struct cls_uart_struct {
 	u8 txrx;
@@ -74,9 +71,6 @@ struct cls_uart_struct {
 #define UART_EXAR654_IER_RTSDTR   0x40    /* Output Interrupt Enable */
 #define UART_EXAR654_IER_CTSDSR   0x80    /* Input Interrupt Enable */
 
-/*
- * Our Global Variables
- */
 extern struct board_ops dgnc_cls_ops;
 
-#endif
+#endif	/* _DGNC_CLS_H */
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index 5381dbddd8bb..253f38b25a54 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -30,8 +30,6 @@ MODULE_AUTHOR("Digi International, http://www.digi.com");
 MODULE_DESCRIPTION("Driver for the Digi International Neo and Classic PCI based product line");
 MODULE_SUPPORTED_DEVICE("dgnc");
 
-/* File operations permitted on Control/Management major. */
-
 static const struct file_operations dgnc_board_fops = {
 	.owner		=	THIS_MODULE,
 	.unlocked_ioctl =	dgnc_mgmt_ioctl,
@@ -39,8 +37,6 @@ static const struct file_operations dgnc_board_fops = {
 	.release	=	dgnc_mgmt_close
 };
 
-/* Globals */
-
 uint			dgnc_num_boards;
 struct dgnc_board		*dgnc_board[MAXBOARDS];
 DEFINE_SPINLOCK(dgnc_global_lock);
@@ -48,12 +44,8 @@ DEFINE_SPINLOCK(dgnc_poll_lock); /* Poll scheduling lock */
 uint			dgnc_major;
 int			dgnc_poll_tick = 20;	/* Poll interval - 20 ms */
 
-/* Static vars. */
-
 static struct class *dgnc_class;
 
-/* Poller stuff */
-
 static ulong		dgnc_poll_time; /* Time of next poll */
 static uint		dgnc_poll_stop; /* Used to tell poller to stop */
 static struct timer_list dgnc_poll_timer;
@@ -95,23 +87,17 @@ static const struct board_id dgnc_ids[] = {
 };
 
 /* Remap PCI memory. */
-
 static int dgnc_do_remap(struct dgnc_board *brd)
 {
-	int rc = 0;
-
 	brd->re_map_membase = ioremap(brd->membase, 0x1000);
 	if (!brd->re_map_membase)
-		rc = -ENOMEM;
+		return -ENOMEM;
 
-	return rc;
+	return 0;
 }
 
-/*
- * dgnc_found_board()
- *
- * A board has been found, init it.
- */
+
+/* A board has been found, initialize  it. */
 static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 {
 	struct dgnc_board *brd;
@@ -119,13 +105,11 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 	int i = 0;
 	int rc = 0;
 
-	/* get the board structure and prep it */
 	brd = kzalloc(sizeof(*brd), GFP_KERNEL);
 	if (!brd)
 		return ERR_PTR(-ENOMEM);
 
 	/* store the info for the board we've found */
-	brd->magic = DGNC_BOARD_MAGIC;
 	brd->boardnum = dgnc_num_boards;
 	brd->vendor = dgnc_pci_tbl[id].vendor;
 	brd->device = dgnc_pci_tbl[id].device;
@@ -170,7 +154,6 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 		 * 4	Memory Mapped UARTs and Status
 		 */
 
-		/* get the PCI Base Address Registers */
 		brd->membase = pci_resource_start(pdev, 4);
 
 		if (!brd->membase) {
@@ -191,14 +174,12 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 		brd->iobase_end = pci_resource_end(pdev, 1);
 		brd->iobase	= ((unsigned int)(brd->iobase)) & 0xFFFE;
 
-		/* Assign the board_ops struct */
 		brd->bd_ops = &dgnc_cls_ops;
 
 		brd->bd_uart_offset = 0x8;
 		brd->bd_dividend = 921600;
 
 		rc = dgnc_do_remap(brd);
-
 		if (rc < 0)
 			goto failed;
 
@@ -237,7 +218,6 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 		else
 			brd->dpatype = T_NEO | T_PCIBUS;
 
-		/* get the PCI Base Address Registers */
 		brd->membase     = pci_resource_start(pdev, 0);
 		brd->membase_end = pci_resource_end(pdev, 0);
 
@@ -246,7 +226,6 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 		else
 			brd->membase &= ~15;
 
-		/* Assign the board_ops struct */
 		brd->bd_ops = &dgnc_neo_ops;
 
 		brd->bd_uart_offset = 0x200;
@@ -272,7 +251,6 @@ static struct dgnc_board *dgnc_found_board(struct pci_dev *pdev, int id)
 		goto failed;
 	}
 
-	/* init our poll helper tasklet */
 	tasklet_init(&brd->helper_tasklet,
 		     brd->bd_ops->tasklet,
 		     (unsigned long)brd);
@@ -289,21 +267,18 @@ failed:
 
 static int dgnc_request_irq(struct dgnc_board *brd)
 {
-	int rc = 0;
-
 	if (brd->irq) {
-		rc = request_irq(brd->irq, brd->bd_ops->intr,
+		int rc = request_irq(brd->irq, brd->bd_ops->intr,
 				 IRQF_SHARED, "DGNC", brd);
-
 		if (rc) {
 			dev_err(&brd->pdev->dev,
 				"Failed to hook IRQ %d\n", brd->irq);
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
-			rc = -ENODEV;
+			return -ENODEV;
 		}
 	}
-	return rc;
+	return 0;
 }
 
 static void dgnc_free_irq(struct dgnc_board *brd)
@@ -312,30 +287,12 @@ static void dgnc_free_irq(struct dgnc_board *brd)
 		free_irq(brd->irq, brd);
 }
 
-/*
- * Function:
- *
- *    dgnc_poll_handler
- *
- * Author:
- *
- *    Scott H Kilau
- *
- * Parameters:
- *
- *    dummy -- ignored
- *
- * Return Values:
- *
- *    none
- *
- * Description:
- *
- *    As each timer expires, it determines (a) whether the "transmit"
- *    waiter needs to be woken up, and (b) whether the poller needs to
- *    be rescheduled.
- */
 
+ /*
+  * As each timer expires, it determines (a) whether the "transmit"
+  * waiter needs to be woken up, and (b) whether the poller needs to
+  * be rescheduled.
+  */
 static void dgnc_poll_handler(ulong dummy)
 {
 	struct dgnc_board *brd;
@@ -343,19 +300,16 @@ static void dgnc_poll_handler(ulong dummy)
 	int i;
 	unsigned long new_time;
 
-	/* Go thru each board, kicking off a tasklet for each if needed */
 	for (i = 0; i < dgnc_num_boards; i++) {
 		brd = dgnc_board[i];
 
 		spin_lock_irqsave(&brd->bd_lock, flags);
 
-		/* If board is in a failed state don't schedule a tasklet */
 		if (brd->state == BOARD_FAILED) {
 			spin_unlock_irqrestore(&brd->bd_lock, flags);
 			continue;
 		}
 
-		/* Schedule a poll helper task */
 		tasklet_schedule(&brd->helper_tasklet);
 
 		spin_unlock_irqrestore(&brd->bd_lock, flags);
@@ -385,9 +339,7 @@ static int dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	int rc;
 	struct dgnc_board *brd;
 
-	/* wake up and enable device */
 	rc = pci_enable_device(pdev);
-
 	if (rc)
 		return -EIO;
 
@@ -395,8 +347,6 @@ static int dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (IS_ERR(brd))
 		return PTR_ERR(brd);
 
-	/* Do tty device initialization. */
-
 	rc = dgnc_tty_register(brd);
 	if (rc < 0) {
 		pr_err(DRVSTR ": Can't register tty devices (%d)\n", rc);
@@ -426,7 +376,6 @@ free_irq:
 	dgnc_free_irq(brd);
 unregister_tty:
 	dgnc_tty_unregister(brd);
-
 failed:
 	kfree(brd);
 
@@ -439,24 +388,14 @@ static struct pci_driver dgnc_driver = {
 	.id_table       = dgnc_pci_tbl,
 };
 
-/* Start of driver. */
-
 static int dgnc_start(void)
 {
 	int rc = 0;
 	unsigned long flags;
 	struct device *dev;
 
-	/* make sure timer is initialized before we do anything else */
 	init_timer(&dgnc_poll_timer);
 
-	/*
-	 * Register our base character device into the kernel.
-	 * This allows the download daemon to connect to the downld device
-	 * before any of the boards are init'ed.
-	 *
-	 * Register management/dpa devices
-	 */
 	rc = register_chrdev(0, "dgnc", &dgnc_board_fops);
 	if (rc < 0) {
 		pr_err(DRVSTR ": Can't register dgnc driver device (%d)\n", rc);
@@ -495,19 +434,16 @@ failed_device:
 	class_destroy(dgnc_class);
 failed_class:
 	unregister_chrdev(dgnc_major, "dgnc");
+
 	return rc;
 }
 
-/*
- * dgnc_cleanup_board()
- *
- * Free all the memory associated with a board
- */
+/* Free all the memory associated with a board */
 static void dgnc_cleanup_board(struct dgnc_board *brd)
 {
 	int i = 0;
 
-	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
+	if (!brd)
 		return;
 
 	switch (brd->device) {
@@ -534,7 +470,6 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 		brd->re_map_membase = NULL;
 	}
 
-	/* Free all allocated channels structs */
 	for (i = 0; i < MAXPORTS ; i++) {
 		if (brd->channels[i]) {
 			kfree(brd->channels[i]->ch_rqueue);
@@ -574,42 +509,28 @@ static void cleanup(void)
 	}
 }
 
-/*
- * dgnc_cleanup_module()
- *
- * Module unload.  This is where it all ends.
- */
 static void __exit dgnc_cleanup_module(void)
 {
 	cleanup();
 	pci_unregister_driver(&dgnc_driver);
 }
 
-/*
- * init_module()
- *
- * Module load.  This is where it all starts.
- */
 static int __init dgnc_init_module(void)
 {
 	int rc;
 
 	/* Initialize global stuff */
-
 	rc = dgnc_start();
-
 	if (rc < 0)
 		return rc;
 
 	/* Find and configure all the cards */
-
 	rc = pci_register_driver(&dgnc_driver);
 	if (rc) {
 		pr_warn("WARNING: dgnc driver load failed.  No Digi Neo or Classic boards found.\n");
 		cleanup();
 		return rc;
 	}
-
 	return 0;
 }
 
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index c8119f2fe881..980410fc4801 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -11,12 +11,10 @@
  * but WITHOUT ANY WARRANTY, EXPRESS OR IMPLIED; without even the
  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  * PURPOSE.  See the GNU General Public License for more details.
- *
- * Driver includes
  */
 
-#ifndef __DGNC_DRIVER_H
-#define __DGNC_DRIVER_H
+#ifndef _DGNC_DRIVER_H
+#define _DGNC_DRIVER_H
 
 #include <linux/types.h>
 #include <linux/tty.h>
@@ -24,8 +22,6 @@
 
 #include "digi.h"		/* Digi specific ioctl header */
 
-/* Driver defines */
-
 /* Driver identification and error statements */
 #define	PROCSTR		"dgnc"			/* /proc entries */
 #define	DEVSTR		"/dev/dg/dgnc"		/* /dev entries */
@@ -39,11 +35,6 @@
 #define	MAXPORTS	8
 #define MAXTTYNAMELEN	200
 
-/* Our 3 magic numbers for our board, channel and unit structs */
-#define DGNC_BOARD_MAGIC	0x5c6df104
-#define DGNC_CHANNEL_MAGIC	0x6c6df104
-#define DGNC_UNIT_MAGIC		0x7c6df104
-
 /* Serial port types */
 #define DGNC_SERIAL		0
 #define DGNC_PRINT		1
@@ -53,10 +44,7 @@
 #define PORT_NUM(dev)	((dev) & 0x7f)
 #define IS_PRINT(dev)	(((dev) & 0xff) >= 0x80)
 
-/*
- *MAX number of stop characters we will send
- * when our read queue is getting full
- */
+/* MAX number of stop characters sent when our read queue is getting full */
 #define MAX_STOPS_SENT 5
 
 /* 4 extra for alignment play space */
@@ -82,27 +70,24 @@
 #endif
 
 /* All the possible states the driver can be while being loaded. */
-
 enum {
 	DRIVER_INITIALIZED = 0,
 	DRIVER_READY
 };
 
 /* All the possible states the board can be while booting up. */
-
 enum {
 	BOARD_FAILED = 0,
 	BOARD_FOUND,
 	BOARD_READY
 };
 
-/* Structures and closely related defines. */
-
 struct dgnc_board;
 struct channel_t;
 
-/* Per board operations structure */
-
+/**
+ * struct board_ops - Per board operations.
+ */
 struct board_ops {
 	void (*tasklet)(unsigned long data);
 	irqreturn_t (*intr)(int irq, void *voidbrd);
@@ -128,77 +113,107 @@ struct board_ops {
 
 #define BD_IS_PCI_EXPRESS     0x0001	  /* Is a PCI Express board */
 
-/*	Per-board information */
-
+/**
+ * struct dgnc_board - Per board information.
+ * @boardnum: Board number (0 - 32).
+ *
+ * @type: Type of board.
+ * @name: Product name.
+ * @pdev: Pointer to the pci_dev structure.
+ * @bd_flags: Board flags.
+ * @vendor: PCI vendor ID.
+ * @device: PCI device ID.
+ * @subvendor: PCI subsystem vendor ID.
+ * @subdevice: PCI subsystem device ID.
+ * @rev: PCI revision ID.
+ * @pci_bus: PCI bus value.
+ * @pci_slot: PCI slot value.
+ * @maxports: Maximum ports this board can handle.
+ * @dvid: Board specific device ID.
+ * @vpd: VPD of this board, if found.
+ * @serial_num: Serial number of this board, if found in VPD.
+ * @bd_lock: Used to protect board.
+ * @bd_intr_lock: Protect poller tasklet and interrupt routine from each other.
+ * @state: State of the card.
+ * @state_wait: Queue to sleep on for state change.
+ * @helper_tasklet: Poll helper tasklet.
+ * @nasync: Number of ports on card.
+ * @irq: Interrupt request number.
+ * @membase: Start of base memory of the card.
+ * @membase_end: End of base memory of the card.
+ * @iobase: Start of IO base of the card.
+ * @iobase_end: End of IO base of the card.
+ * @bd_uart_offset: Space between each UART.
+ * @channels: array of pointers to our channels.
+ * @serial_driver: Pointer to the serial driver.
+ * @serial_name: Serial driver name.
+ * @print_dirver: Pointer to the print driver.
+ * @print_name: Print driver name.
+ * @dpatype: Board type as defined by DPA.
+ * @dpastatus: Board status as defined by DPA.
+ * @bd_dividend: Board/UART's specific dividend.
+ * @bd_ops: Pointer to board operations structure.
+ * @proc_entry_pointer: Proc/<board> entry
+ * @dgnc_board_table: Proc/<board> entry
+ */
 struct dgnc_board {
-	int		magic;		/* Board Magic number. */
-	int		boardnum;	/* Board number: 0-32 */
+	int		boardnum;
 
-	int		type;		/* Type of board */
-	char		*name;		/* Product Name */
-	struct pci_dev	*pdev;		/* Pointer to the pci_dev struct */
-	unsigned long	bd_flags;	/* Board flags */
-	u16		vendor;		/* PCI vendor ID */
-	u16		device;		/* PCI device ID */
-	u16		subvendor;	/* PCI subsystem vendor ID */
-	u16		subdevice;	/* PCI subsystem device ID */
-	unsigned char	rev;		/* PCI revision ID */
-	uint		pci_bus;	/* PCI bus value */
-	uint		pci_slot;	/* PCI slot value */
-	uint		maxports;	/* MAX ports this board can handle */
-	unsigned char	dvid;		/* Board specific device id */
-	unsigned char	vpd[128];	/* VPD of board, if found */
-	unsigned char	serial_num[20];	/* Serial number of board,
-					 * if found in VPD
-					 */
+	int		type;
+	char		*name;
+	struct pci_dev	*pdev;
+	unsigned long	bd_flags;
+	u16		vendor;
+	u16		device;
+	u16		subvendor;
+	u16		subdevice;
+	unsigned char	rev;
+	uint		pci_bus;
+	uint		pci_slot;
+	uint		maxports;
+	unsigned char	dvid;
+	unsigned char	vpd[128];
+	unsigned char	serial_num[20];
 
-	spinlock_t	bd_lock;	/* Used to protect board */
+	/* used to protect the board */
+	spinlock_t	bd_lock;
 
-	spinlock_t	bd_intr_lock;	/* Used to protect the poller tasklet
-					 * and the interrupt routine from each
-					 * other.
-					 */
+	/*  Protect poller tasklet and interrupt routine from each other. */
+	spinlock_t	bd_intr_lock;
 
-	uint		state;		/* State of card. */
-	wait_queue_head_t state_wait;	/* Place to sleep on for state change */
+	uint		state;
+	wait_queue_head_t state_wait;
 
-	struct		tasklet_struct helper_tasklet; /* Poll helper tasklet */
+	struct tasklet_struct helper_tasklet;
 
-	uint		nasync;		/* Number of ports on card */
+	uint		nasync;
 
-	uint		irq;		/* Interrupt request number */
+	uint		irq;
 
-	ulong		membase;	/* Start of base memory of the card */
-	ulong		membase_end;	/* End of base memory of the card */
+	ulong		membase;
+	ulong		membase_end;
 
-	u8 __iomem	*re_map_membase; /* Remapped memory of the card */
+	u8 __iomem	*re_map_membase;
 
-	ulong		iobase;		/* Start of io base of the card */
-	ulong		iobase_end;	/* End of io base of the card */
+	ulong		iobase;
+	ulong		iobase_end;
 
-	uint		bd_uart_offset;	/* Space between each UART */
+	uint		bd_uart_offset;
 
-	struct channel_t *channels[MAXPORTS];	/* array of pointers
-						 * to our channels.
-						 */
+	struct channel_t *channels[MAXPORTS];
 
 	struct tty_driver *serial_driver;
 	char		serial_name[200];
 	struct tty_driver *print_driver;
 	char		print_name[200];
 
-	u16		dpatype;	/* The board "type",
-					 * as defined by DPA
-					 */
-	u16		dpastatus;	/* The board "status",
-					 * as defined by DPA
-					 */
+	u16		dpatype;
+	u16		dpastatus;
 
-	uint		bd_dividend;	/* Board/UARTs specific dividend */
+	uint		bd_dividend;
 
 	struct board_ops *bd_ops;
 
-	/* /proc/<board> entries */
 	struct proc_dir_entry *proc_entry_pointer;
 	struct dgnc_proc_entry *dgnc_board_table;
 
@@ -221,17 +236,23 @@ struct dgnc_board {
 
 struct device;
 
-/* Structure for terminal or printer unit. */
+/**
+ * struct un_t - terminal or printer unit
+ * @un_open_count: Counter of opens to port.
+ * @un_tty: Pointer to unit tty structure.
+ * @un_flags: Unit flags.
+ * @un_flags_wait: Place to sleep to wait on unit.
+ * @un_dev: Minor device number.
+ */
 struct un_t {
-	int	magic;		/* Unit Magic Number. */
 	struct	channel_t *un_ch;
 	ulong	un_time;
 	uint	un_type;
-	uint	un_open_count;		/* Counter of opens to port */
-	struct tty_struct *un_tty;	/* Pointer to unit tty structure */
-	uint	un_flags;		/* Unit flags */
-	wait_queue_head_t un_flags_wait; /* Place to sleep to wait on unit */
-	uint	un_dev;			/* Minor device number */
+	uint	un_open_count;
+	struct tty_struct *un_tty;
+	uint	un_flags;
+	wait_queue_head_t un_flags_wait;
+	uint	un_dev;
 	struct device *un_sysfs;
 };
 
@@ -263,102 +284,137 @@ struct un_t {
 #define EQUEUESIZE	RQUEUESIZE
 #define WQUEUESIZE	(WQUEUEMASK + 1)
 
-/* Channel information structure. */
+/**
+ * struct channel_t - Channel information.
+ * @dgnc_board: Pointer to board structure.
+ * @ch_bd: Transparent print structure.
+ * @ch_tun: Terminal unit information.
+ * @ch_pun: Printer unit information.
+ * @ch_lock: Provide for serialization.
+ * @ch_flags_wait: Channel flags wait queue.
+ * @ch_portnum: Port number, 0 offset.
+ * @ch_open_count: Open count.
+ * @ch_flags: Channel flags.
+ * @ch_close_delay: How long we should drop RTS/DTR for.
+ * @ch_cpstime: Time for CPS calculations.
+ * @ch_c_iflag: Channel iflags.
+ * @ch_c_cflag: Channel cflags.
+ * @ch_c_oflag: Channel oflags.
+ * @ch_c_lflag: Channel lflags.
+ * @ch_stopc: Stop character.
+ * @ch_startc: Start character.
+ * @ch_old_baud: Cache of the current baud rate.
+ * @ch_custom_speed: Custom baud rate, if set.
+ * @ch_wopen: Waiting for open process count.
+ * @ch_mostat: FEP output modem status.
+ * @ch_mistat: FEP input modem status.
+ * @chc_neo_uart: Pointer to the mapped neo UART struct
+ * @ch_cls_uart:  Pointer to the mapped cls UART struct
+ * @ch_cached_lsr: Cached value of the LSR register.
+ * @ch_rqueue: Read queue buffer, malloc'ed.
+ * @ch_r_head: Head location of the read queue.
+ * @ch_r_tail: Tail location of the read queue.
+ * @ch_equeue: Error queue buffer, malloc'ed.
+ * @ch_e_head: Head location of the error queue.
+ * @ch_e_tail: Tail location of the error queue.
+ * @ch_wqueue: Write queue buffer, malloc'ed.
+ * @ch_w_head: Head location of the write queue.
+ * @ch_w_tail: Tail location of the write queue.
+ * @ch_rxcount: Total of data received so far.
+ * @ch_txcount: Total of data transmitted so far.
+ * @ch_r_tlevel: Receive trigger level.
+ * @ch_t_tlevel: Transmit trigger level.
+ * @ch_r_watermark: Receive water mark.
+ * @ch_stop_sending_break: Time we should STOP sending a break.
+ * @ch_stops_sent: How many times I have send a stop character to try
+ *                 to stop the other guy sending.
+ * @ch_err_parity: Count of parity
+ * @ch_err_frame: Count of framing errors on channel.
+ * @ch_err_break: Count of breaks on channel.
+ * @ch_err_overrun: Count of overruns on channel.
+ * @ch_xon_sends: Count of xons transmitted.
+ * @ch_xoff_sends: Count of xoffs transmitted.
+ * @proc_entry_pointer: Proc/<board>/<channel> entry.
+ * @dgnc_channel_table: Proc/<board>/<channel> entry.
+ */
 struct channel_t {
-	int magic;			/* Channel Magic Number	*/
-	struct dgnc_board *ch_bd;	/* Board structure pointer */
-	struct digi_t	ch_digi;	/* Transparent Print structure  */
-	struct un_t	ch_tun;		/* Terminal unit info */
-	struct un_t	ch_pun;		/* Printer unit info */
+	struct dgnc_board *ch_bd;
+	struct digi_t	ch_digi;
+	struct un_t	ch_tun;
+	struct un_t	ch_pun;
 
-	spinlock_t	ch_lock;	/* provide for serialization */
+	spinlock_t	ch_lock; /* provide for serialization */
 	wait_queue_head_t ch_flags_wait;
 
-	uint		ch_portnum;	/* Port number, 0 offset. */
-	uint		ch_open_count;	/* open count */
-	uint		ch_flags;	/* Channel flags */
+	uint		ch_portnum;
+	uint		ch_open_count;
+	uint		ch_flags;
 
-	ulong		ch_close_delay;	/* How long we should
-					 * drop RTS/DTR for
-					 */
+	ulong		ch_close_delay;
 
-	ulong		ch_cpstime;	/* Time for CPS calculations */
+	ulong		ch_cpstime;
 
-	tcflag_t	ch_c_iflag;	/* channel iflags */
-	tcflag_t	ch_c_cflag;	/* channel cflags */
-	tcflag_t	ch_c_oflag;	/* channel oflags */
-	tcflag_t	ch_c_lflag;	/* channel lflags */
-	unsigned char	ch_stopc;	/* Stop character */
-	unsigned char	ch_startc;	/* Start character */
+	tcflag_t	ch_c_iflag;
+	tcflag_t	ch_c_cflag;
+	tcflag_t	ch_c_oflag;
+	tcflag_t	ch_c_lflag;
+	unsigned char	ch_stopc;
+	unsigned char	ch_startc;
 
-	uint		ch_old_baud;	/* Cache of the current baud */
-	uint		ch_custom_speed;/* Custom baud, if set */
+	uint		ch_old_baud;
+	uint		ch_custom_speed;
 
-	uint		ch_wopen;	/* Waiting for open process cnt */
+	uint		ch_wopen;
 
-	unsigned char	ch_mostat;	/* FEP output modem status */
-	unsigned char	ch_mistat;	/* FEP input modem status */
+	unsigned char	ch_mostat;
+	unsigned char	ch_mistat;
 
-	struct neo_uart_struct __iomem *ch_neo_uart;	/* Pointer to the
-							 * "mapped" UART struct
-							 */
-	struct cls_uart_struct __iomem *ch_cls_uart;	/* Pointer to the
-							 * "mapped" UART struct
-							 */
+	struct neo_uart_struct __iomem *ch_neo_uart;
+	struct cls_uart_struct __iomem *ch_cls_uart;
 
-	unsigned char	ch_cached_lsr;	/* Cached value of the LSR register */
+	unsigned char	ch_cached_lsr;
 
-	unsigned char	*ch_rqueue;	/* Our read queue buffer - malloc'ed */
-	ushort		ch_r_head;	/* Head location of the read queue */
-	ushort		ch_r_tail;	/* Tail location of the read queue */
+	unsigned char	*ch_rqueue;
+	ushort		ch_r_head;
+	ushort		ch_r_tail;
 
-	unsigned char	*ch_equeue;	/* Our error queue buffer - malloc'ed */
-	ushort		ch_e_head;	/* Head location of the error queue */
-	ushort		ch_e_tail;	/* Tail location of the error queue */
+	unsigned char	*ch_equeue;
+	ushort		ch_e_head;
+	ushort		ch_e_tail;
 
-	unsigned char	*ch_wqueue;	/* Our write queue buffer - malloc'ed */
-	ushort		ch_w_head;	/* Head location of the write queue */
-	ushort		ch_w_tail;	/* Tail location of the write queue */
+	unsigned char	*ch_wqueue;
+	ushort		ch_w_head;
+	ushort		ch_w_tail;
 
-	ulong		ch_rxcount;	/* total of data received so far */
-	ulong		ch_txcount;	/* total of data transmitted so far */
+	ulong		ch_rxcount;
+	ulong		ch_txcount;
 
-	unsigned char	ch_r_tlevel;	/* Receive Trigger level */
-	unsigned char	ch_t_tlevel;	/* Transmit Trigger level */
+	unsigned char	ch_r_tlevel;
+	unsigned char	ch_t_tlevel;
 
-	unsigned char	ch_r_watermark;	/* Receive Watermark */
+	unsigned char	ch_r_watermark;
 
-	ulong		ch_stop_sending_break;	/* Time we should STOP
-						 * sending a break
-						 */
+	ulong		ch_stop_sending_break;
+	uint		ch_stops_sent;
 
-	uint		ch_stops_sent;	/* How many times I have sent a stop
-					 * character to try to stop the other
-					 * guy sending.
-					 */
-	ulong		ch_err_parity;	/* Count of parity errors on channel */
-	ulong		ch_err_frame;	/* Count of framing errors on channel */
-	ulong		ch_err_break;	/* Count of breaks on channel */
-	ulong		ch_err_overrun; /* Count of overruns on channel */
+	ulong		ch_err_parity;
+	ulong		ch_err_frame;
+	ulong		ch_err_break;
+	ulong		ch_err_overrun;
 
-	ulong		ch_xon_sends;	/* Count of xons transmitted */
-	ulong		ch_xoff_sends;	/* Count of xoffs transmitted */
+	ulong		ch_xon_sends;
+	ulong		ch_xoff_sends;
 
-	/* /proc/<board>/<channel> entries */
 	struct proc_dir_entry *proc_entry_pointer;
 	struct dgnc_proc_entry *dgnc_channel_table;
 
 };
 
-/* Our Global Variables. */
-
 extern uint		dgnc_major;		/* Our driver/mgmt major */
 extern int		dgnc_poll_tick;		/* Poll interval - 20 ms */
 extern spinlock_t	dgnc_global_lock;	/* Driver global spinlock */
 extern spinlock_t	dgnc_poll_lock;		/* Poll scheduling lock */
 extern uint		dgnc_num_boards;	/* Total number of boards */
-extern struct dgnc_board *dgnc_board[MAXBOARDS];/* Array of board
-						 * structs
-						 */
+extern struct dgnc_board *dgnc_board[MAXBOARDS];/* Array of boards */
 
-#endif
+#endif	/* _DGNC_DRIVER_H */
diff --git a/drivers/staging/dgnc/dgnc_mgmt.c b/drivers/staging/dgnc/dgnc_mgmt.c
index 9d9b15d6358a..3ca473b47daf 100644
--- a/drivers/staging/dgnc/dgnc_mgmt.c
+++ b/drivers/staging/dgnc/dgnc_mgmt.c
@@ -20,11 +20,11 @@
 
 #include <linux/kernel.h>
 #include <linux/ctype.h>
-#include <linux/sched.h>	/* For jiffies, task states */
-#include <linux/interrupt.h>	/* For tasklet and interrupt structs/defines */
+#include <linux/sched.h>
+#include <linux/interrupt.h>
 #include <linux/serial_reg.h>
 #include <linux/termios.h>
-#include <linux/uaccess.h>	/* For copy_from_user/copy_to_user */
+#include <linux/uaccess.h>
 
 #include "dgnc_driver.h"
 #include "dgnc_pci.h"
@@ -33,64 +33,60 @@
 /* Our "in use" variables, to enforce 1 open only */
 static int dgnc_mgmt_in_use[MAXMGMTDEVICES];
 
-/*
- * dgnc_mgmt_open()
- *
- * Open the mgmt/downld/dpa device
+/**
+ * dgnc_mgmt_open() - Open the mgmt/downld/dpa device.
  */
 int dgnc_mgmt_open(struct inode *inode, struct file *file)
 {
 	unsigned long flags;
 	unsigned int minor = iminor(inode);
+	int rc = 0;
 
 	spin_lock_irqsave(&dgnc_global_lock, flags);
 
-	/* mgmt device */
-	if (minor < MAXMGMTDEVICES) {
-		/* Only allow 1 open at a time on mgmt device */
-		if (dgnc_mgmt_in_use[minor]) {
-			spin_unlock_irqrestore(&dgnc_global_lock, flags);
-			return -EBUSY;
-		}
-		dgnc_mgmt_in_use[minor]++;
-	} else {
-		spin_unlock_irqrestore(&dgnc_global_lock, flags);
-		return -ENXIO;
+	if (minor >= MAXMGMTDEVICES) {
+		rc = -ENXIO;
+		goto out;
 	}
+	/* Only allow 1 open at a time on mgmt device */
+	if (dgnc_mgmt_in_use[minor]) {
+		rc = -EBUSY;
+		goto out;
+	}
+	dgnc_mgmt_in_use[minor]++;
 
+out:
 	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
-	return 0;
+	return rc;
 }
 
-/*
- * dgnc_mgmt_close()
- *
- * Open the mgmt/dpa device
+/**
+ * dgnc_mgmt_close() - Close the mgmt/dpa device
  */
 int dgnc_mgmt_close(struct inode *inode, struct file *file)
 {
 	unsigned long flags;
 	unsigned int minor = iminor(inode);
+	int rc = 0;
 
 	spin_lock_irqsave(&dgnc_global_lock, flags);
 
-	/* mgmt device */
-	if (minor < MAXMGMTDEVICES) {
-		if (dgnc_mgmt_in_use[minor])
-			dgnc_mgmt_in_use[minor] = 0;
+	if (minor >= MAXMGMTDEVICES) {
+		rc = -ENXIO;
+		goto out;
 	}
+	dgnc_mgmt_in_use[minor] = 0;
+
+out:
 	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
-	return 0;
+	return rc;
 }
 
-/*
- * dgnc_mgmt_ioctl()
- *
- * ioctl the mgmt/dpa device
+/**
+ * dgnc_mgmt_ioctl() - Ioctl the mgmt/dpa device.
  */
-
 long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	unsigned long flags;
@@ -171,17 +167,15 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		board = ni.board;
 		channel = ni.channel;
 
-		/* Verify boundaries on board */
 		if (board >= dgnc_num_boards)
 			return -ENODEV;
 
-		/* Verify boundaries on channel */
 		if (channel >= dgnc_board[board]->nasync)
 			return -ENODEV;
 
 		ch = dgnc_board[board]->channels[channel];
 
-		if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+		if (!ch)
 			return -ENODEV;
 
 		memset(&ni, 0, sizeof(ni));
@@ -250,6 +244,5 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		break;
 	}
 	}
-
 	return 0;
 }
diff --git a/drivers/staging/dgnc/dgnc_mgmt.h b/drivers/staging/dgnc/dgnc_mgmt.h
index 708abe9594d4..a7a5770d7630 100644
--- a/drivers/staging/dgnc/dgnc_mgmt.h
+++ b/drivers/staging/dgnc/dgnc_mgmt.h
@@ -13,13 +13,14 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DGNC_MGMT_H
-#define __DGNC_MGMT_H
+#ifndef _DGNC_MGMT_H
+#define _DGNC_MGMT_H
 
 #define MAXMGMTDEVICES 8
 
 int dgnc_mgmt_open(struct inode *inode, struct file *file);
 int dgnc_mgmt_close(struct inode *inode, struct file *file);
 long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
-#endif
+
+#endif	/* _DGNC_MGMT_H */
 
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index 3eefefe53174..1943e66fec57 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -14,15 +14,15 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/sched.h>	/* For jiffies, task states */
-#include <linux/interrupt.h>    /* For tasklet and interrupt structs/defines */
-#include <linux/delay.h>	/* For udelay */
-#include <linux/io.h>		/* For read[bwl]/write[bwl] */
-#include <linux/serial.h>	/* For struct async_serial */
-#include <linux/serial_reg.h>	/* For the various UART offsets */
-
-#include "dgnc_driver.h"	/* Driver main header file */
-#include "dgnc_neo.h"		/* Our header file */
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/serial.h>
+#include <linux/serial_reg.h>
+
+#include "dgnc_driver.h"
+#include "dgnc_neo.h"
 #include "dgnc_tty.h"
 
 static inline void neo_parse_lsr(struct dgnc_board *brd, uint port);
@@ -96,12 +96,7 @@ static inline void neo_set_cts_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn on auto CTS flow control */
-#if 1
 	ier |= UART_17158_IER_CTSDSR;
-#else
-	ier &= ~(UART_17158_IER_CTSDSR);
-#endif
-
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_CTSDSR);
 
 	/* Turn off auto Xon flow control */
@@ -135,11 +130,7 @@ static inline void neo_set_rts_flow_control(struct channel_t *ch)
 	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn on auto RTS flow control */
-#if 1
 	ier |= UART_17158_IER_RTSDTR;
-#else
-	ier &= ~(UART_17158_IER_RTSDTR);
-#endif
 	efr |= (UART_17158_EFR_ECB | UART_17158_EFR_RTSDTR);
 
 	/* Turn off auto Xoff flow control */
@@ -358,20 +349,17 @@ static inline void neo_set_new_start_stop_chars(struct channel_t *ch)
 }
 
 /* No locks are assumed to be held when calling this function. */
-
 static inline void neo_clear_break(struct channel_t *ch, int force)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* Bail if we aren't currently sending a break. */
 	if (!ch->ch_stop_sending_break) {
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
-	/* Turn break off, and unset some variables */
 	if (ch->ch_flags & CH_BREAK_SENDING) {
 		if (force ||
 		    time_after_eq(jiffies, ch->ch_stop_sending_break)) {
@@ -387,7 +375,6 @@ static inline void neo_clear_break(struct channel_t *ch, int force)
 }
 
 /* Parse the ISR register. */
-
 static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 {
 	struct channel_t *ch;
@@ -396,14 +383,13 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 	unsigned long flags;
 
 	ch = brd->channels[port];
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	/* Here we try to figure out what caused the interrupt to happen */
 	while (1) {
 		isr = readb(&ch->ch_neo_uart->isr_fcr);
 
-		/* Bail if no pending interrupt */
 		if (isr & UART_IIR_NO_INT)
 			break;
 
@@ -426,7 +412,6 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 		}
 
 		if (isr & UART_IIR_THRI) {
-			/* Transfer data (if any) from Write Queue -> UART. */
 			spin_lock_irqsave(&ch->ch_lock, flags);
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 			spin_unlock_irqrestore(&ch->ch_lock, flags);
@@ -442,10 +427,7 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 			 * one it was, so we can suspend or resume data flow.
 			 */
 			if (cause == UART_17158_XON_DETECT) {
-				/*
-				 * Is output stopped right now, if so,
-				 * resume it
-				 */
+				/* resume output if stopped */
 				if (brd->channels[port]->ch_flags & CH_STOP) {
 					spin_lock_irqsave(&ch->ch_lock,
 							  flags);
@@ -504,7 +486,6 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 			}
 		}
 
-		/* Parse any modem signal changes */
 		neo_parse_modem(ch, readb(&ch->ch_neo_uart->msr));
 	}
 }
@@ -515,18 +496,14 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 	int linestatus;
 	unsigned long flags;
 
-	/*
-	 * Check to make sure it didn't receive interrupt with a null board
-	 * associated or a board pointer that wasn't ours.
-	 */
-	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
+	if (!brd)
 		return;
 
 	if (port >= brd->maxports)
 		return;
 
 	ch = brd->channels[port];
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	linestatus = readb(&ch->ch_neo_uart->lsr);
@@ -534,7 +511,6 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 	ch->ch_cached_lsr |= linestatus;
 
 	if (ch->ch_cached_lsr & UART_LSR_DR) {
-		/* Read data from uart -> queue */
 		neo_copy_data_from_uart_to_queue(ch);
 		spin_lock_irqsave(&ch->ch_lock, flags);
 		dgnc_check_queue_flow_control(ch);
@@ -571,22 +547,17 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-		/* Transfer data (if any) from Write Queue -> UART. */
 		neo_copy_data_from_queue_to_uart(ch);
 	} else if (linestatus & UART_17158_TX_AND_FIFO_CLR) {
 		spin_lock_irqsave(&ch->ch_lock, flags);
 		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-		/* Transfer data (if any) from Write Queue -> UART. */
 		neo_copy_data_from_queue_to_uart(ch);
 	}
 }
 
-/*
- * neo_param()
- * Send any/all changes to the line to the UART.
- */
+/* Send any/all changes to the line to the UART. */
 static void neo_param(struct tty_struct *tty)
 {
 	unsigned char lcr = 0;
@@ -599,23 +570,22 @@ static void neo_param(struct tty_struct *tty)
 	struct channel_t *ch;
 	struct un_t   *un;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = (struct un_t *)tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	/* If baud rate is zero, flush queues, and set mval to drop DTR. */
-
 	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
 		ch->ch_r_head = 0;
 		ch->ch_r_tail = 0;
@@ -724,10 +694,6 @@ static void neo_param(struct tty_struct *tty)
 	if (!(ch->ch_c_cflag & PARODD))
 		lcr |= UART_LCR_EPAR;
 
-	/*
-	 * Not all platforms support mark/space parity,
-	 * so this will hide behind an ifdef.
-	 */
 #ifdef CMSPAR
 	if (ch->ch_c_cflag & CMSPAR)
 		lcr |= UART_LCR_SPAR;
@@ -796,16 +762,11 @@ static void neo_param(struct tty_struct *tty)
 	if (ier != uart_ier)
 		writeb(ier, &ch->ch_neo_uart->ier);
 
-	/* Set new start/stop chars */
 	neo_set_new_start_stop_chars(ch);
 
 	if (ch->ch_digi.digi_flags & CTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_cts_flow_control(ch);
 	} else if (ch->ch_c_iflag & IXON) {
-		/*
-		 * If start/stop is set to disable, then we should
-		 * disable flow control
-		 */
 		if ((ch->ch_startc == _POSIX_VDISABLE) ||
 		    (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_output_flow_control(ch);
@@ -818,10 +779,6 @@ static void neo_param(struct tty_struct *tty)
 	if (ch->ch_digi.digi_flags & RTSPACE || ch->ch_c_cflag & CRTSCTS) {
 		neo_set_rts_flow_control(ch);
 	} else if (ch->ch_c_iflag & IXOFF) {
-		/*
-		 * If start/stop is set to disable, then we should
-		 * disable flow control
-		 */
 		if ((ch->ch_startc == _POSIX_VDISABLE) ||
 		    (ch->ch_stopc == _POSIX_VDISABLE))
 			neo_set_no_input_flow_control(ch);
@@ -843,12 +800,10 @@ static void neo_param(struct tty_struct *tty)
 
 	neo_assert_modem_signals(ch);
 
-	/* Get current status of the modem signals now */
 	neo_parse_modem(ch, readb(&ch->ch_neo_uart->msr));
 }
 
-/* Our board poller function. */
-
+/* Board poller function. */
 static void neo_tasklet(unsigned long data)
 {
 	struct dgnc_board *bd = (struct dgnc_board *)data;
@@ -858,10 +813,9 @@ static void neo_tasklet(unsigned long data)
 	int state = 0;
 	int ports = 0;
 
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
-	/* Cache a couple board values */
 	spin_lock_irqsave(&bd->bd_lock, flags);
 	state = bd->state;
 	ports = bd->nasync;
@@ -873,15 +827,10 @@ static void neo_tasklet(unsigned long data)
 	 */
 	spin_lock_irqsave(&bd->bd_intr_lock, flags);
 
-	/* If board is ready, parse deeper to see if there is anything to do. */
-
 	if ((state == BOARD_READY) && (ports > 0)) {
-		/* Loop on each port */
 		for (i = 0; i < ports; i++) {
 			ch = bd->channels[i];
-
-			/* Just being careful... */
-			if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+			if (!ch)
 				continue;
 
 			/*
@@ -903,10 +852,6 @@ static void neo_tasklet(unsigned long data)
 			neo_copy_data_from_queue_to_uart(ch);
 			dgnc_wakeup_writes(ch);
 
-			/*
-			 * Call carrier carrier function, in case something
-			 * has changed.
-			 */
 			dgnc_carrier(ch);
 
 			/*
@@ -918,15 +863,10 @@ static void neo_tasklet(unsigned long data)
 		}
 	}
 
-	/* Allow interrupt routine to access the interrupt register again */
 	spin_unlock_irqrestore(&bd->bd_intr_lock, flags);
 }
 
-/*
- * dgnc_neo_intr()
- *
- * Neo specific interrupt handler.
- */
+/* Neo specific interrupt handler. */
 static irqreturn_t neo_intr(int irq, void *voidbrd)
 {
 	struct dgnc_board *brd = voidbrd;
@@ -937,11 +877,7 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	unsigned long flags;
 	unsigned long flags2;
 
-	/*
-	 * Check to make sure it didn't receive interrupt with a null board
-	 * associated or a board pointer that wasn't ours.
-	 */
-	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
+	if (!brd)
 		return IRQ_NONE;
 
 	/* Lock out the slow poller from running on this board. */
@@ -964,19 +900,12 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 		return IRQ_NONE;
 	}
 
-	/*
-	 * At this point, we have at least SOMETHING to service, dig
-	 * further...
-	 */
-
-	/* Loop on each port */
 	while ((uart_poll & 0xff) != 0) {
 		type = uart_poll >> (8 + (port * 3));
 		type &= 0x7;
 
 		uart_poll &= ~(0x01 << port);
 
-		/* Switch on type of interrupt we have */
 		switch (type) {
 		case UART_17158_RXRDY_TIMEOUT:
 			/*
@@ -984,7 +913,6 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			 * RX FIFO until it falls below the trigger level.
 			 */
 
-			/* Verify the port is in range. */
 			if (port >= brd->nasync)
 				break;
 
@@ -1027,27 +955,17 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			break;
 
 		case UART_17158_MSR:
-
 			/* MSR or flow control was seen. */
-
 			neo_parse_isr(brd, port);
 			break;
 
 		default:
-			/*
-			 * The UART triggered us with a bogus interrupt type.
-			 * It appears the Exar chip, when REALLY bogged down,
-			 * will throw these once and awhile.
-			 * Its harmless, just ignore it and move on.
-			 */
 			break;
 		}
 
 		port++;
 	}
 
-	/* Schedule tasklet to more in-depth servicing at a better time. */
-
 	tasklet_schedule(&brd->helper_tasklet);
 
 	spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
@@ -1094,32 +1012,23 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 	ushort tail;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* cache head and tail of queue */
 	head = ch->ch_r_head & RQUEUEMASK;
 	tail = ch->ch_r_tail & RQUEUEMASK;
 
-	/* Get our cached LSR */
 	linestatus = ch->ch_cached_lsr;
 	ch->ch_cached_lsr = 0;
 
-	/* Store how much space we have left in the queue */
 	qleft = tail - head - 1;
 	if (qleft < 0)
 		qleft += RQUEUEMASK + 1;
 
-	/*
-	 * If the UART is not in FIFO mode, force the FIFO copy to
-	 * NOT be run, by setting total to 0.
-	 *
-	 * On the other hand, if the UART IS in FIFO mode, then ask
-	 * the UART to give us an approximation of data it has RX'ed.
-	 */
 	if (!(ch->ch_flags & CH_FIFO_ENABLED)) {
+		/* force the FIFO copy to NOT be run */
 		total = 0;
 	} else {
 		total = readb(&ch->ch_neo_uart->rfifo);
@@ -1138,26 +1047,11 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 			total -= 3;
 	}
 
-	/*
-	 * Finally, bound the copy to make sure we don't overflow
-	 * our own queue...
-	 * The byte by byte copy loop below this loop this will
-	 * deal with the queue overflow possibility.
-	 */
 	total = min(total, qleft);
 
 	while (total > 0) {
-		/*
-		 * Grab the linestatus register, we need to check
-		 * to see if there are any errors in the FIFO.
-		 */
 		linestatus = readb(&ch->ch_neo_uart->lsr);
 
-		/*
-		 * Break out if there is a FIFO error somewhere.
-		 * This will allow us to go byte by byte down below,
-		 * finding the exact location of the error.
-		 */
 		if (linestatus & UART_17158_RX_FIFO_DATA_ERROR)
 			break;
 
@@ -1182,7 +1076,6 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 
 		linestatus = 0;
 
-		/* Copy data from uart to the queue */
 		memcpy_fromio(ch->ch_rqueue + head,
 			      &ch->ch_neo_uart->txrxburst, n);
 
@@ -1193,7 +1086,6 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 */
 		memset(ch->ch_equeue + head, 0, n);
 
-		/* Add to and flip head if needed */
 		head = (head + n) & RQUEUEMASK;
 		total -= n;
 		qleft -= n;
@@ -1242,8 +1134,6 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 		}
 
-		/* Discard character if we are ignoring the error mask. */
-
 		if (linestatus & error_mask)  {
 			unsigned char discard;
 
@@ -1253,13 +1143,10 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		}
 
 		/*
-		 * If our queue is full, we have no choice but to drop some
-		 * data.
-		 * The assumption is that HWFLOW or SWFLOW should have stopped
-		 * things way way before we got to this point.
-		 *
-		 * I decided that I wanted to ditch the oldest data first,
-		 * I hope thats okay with everyone? Yes? Good.
+		 * If our queue is full, we have no choice but to drop
+		 * some data. The assumption is that HWFLOW or SWFLOW
+		 * should have stopped things way way before we got to
+		 * this point.
 		 */
 		while (qleft < 1) {
 			tail = (tail + 1) & RQUEUEMASK;
@@ -1272,18 +1159,14 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 			      &ch->ch_neo_uart->txrxburst, 1);
 		ch->ch_equeue[head] = (unsigned char)linestatus;
 
-		/* Ditch any remaining linestatus value. */
 		linestatus = 0;
 
-		/* Add to and flip head if needed */
 		head = (head + 1) & RQUEUEMASK;
 
 		qleft--;
 		ch->ch_rxcount++;
 	}
 
-	/* Write new final heads to channel structure. */
-
 	ch->ch_r_head = head & RQUEUEMASK;
 	ch->ch_e_head = head & EQUEUEMASK;
 
@@ -1301,25 +1184,21 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	struct un_t *un;
 	int rc = 0;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return -ENXIO;
 
 	un = (struct un_t *)tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return -ENXIO;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -ENXIO;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 	un->un_flags |= UN_EMPTY;
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	/*
-	 * Go to sleep waiting for the tty layer to wake me back up when
-	 * the empty flag goes away.
-	 */
 	rc = wait_event_interruptible_timeout(un->un_flags_wait,
 					      ((un->un_flags & UN_EMPTY) == 0),
 					      msecs_to_jiffies(seconds * 1000));
@@ -1330,15 +1209,14 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 
 /*
  * Flush the WRITE FIFO on the Neo.
- *
- * NOTE: Channel lock MUST be held before calling this function!
+ * Channel lock MUST be held before calling this function!
  */
 static void neo_flush_uart_write(struct channel_t *ch)
 {
 	unsigned char tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT),
@@ -1347,7 +1225,7 @@ static void neo_flush_uart_write(struct channel_t *ch)
 
 	for (i = 0; i < 10; i++) {
 		/*
-		 * Check to see if the UART feels it completely flushed the
+		 * Check to see if the UART completely flushed the FIFO
 		 * FIFO.
 		 */
 		tmp = readb(&ch->ch_neo_uart->isr_fcr);
@@ -1362,15 +1240,14 @@ static void neo_flush_uart_write(struct channel_t *ch)
 
 /*
  * Flush the READ FIFO on the Neo.
- *
- * NOTE: Channel lock MUST be held before calling this function!
+ * Channel lock MUST be held before calling this function!
  */
 static void neo_flush_uart_read(struct channel_t *ch)
 {
 	unsigned char tmp = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	writeb(UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR,
@@ -1400,12 +1277,11 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	uint len_written = 0;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* No data to write to the UART */
 	if (ch->ch_w_tail == ch->ch_w_head)
 		goto exit_unlock;
 
@@ -1414,12 +1290,10 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	    (ch->ch_flags & CH_BREAK_SENDING))
 		goto exit_unlock;
 
-	/* If FIFOs are disabled. Send data directly to txrx register */
-
 	if (!(ch->ch_flags & CH_FIFO_ENABLED)) {
+		/* Send data directly to txrx register */
 		unsigned char lsrbits = readb(&ch->ch_neo_uart->lsr);
 
-		/* Cache the LSR bits for later parsing */
 		ch->ch_cached_lsr |= lsrbits;
 		if (ch->ch_cached_lsr & UART_LSR_THRE) {
 			ch->ch_cached_lsr &= ~(UART_LSR_THRE);
@@ -1477,12 +1351,10 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		n = UART_17158_TX_FIFOSIZE - readb(&ch->ch_neo_uart->tfifo);
 	}
 
-	/* cache head and tail of queue */
 	head = ch->ch_w_head & WQUEUEMASK;
 	tail = ch->ch_w_tail & WQUEUEMASK;
 	qlen = (head - tail) & WQUEUEMASK;
 
-	/* Find minimum of the FIFO space, versus queue length */
 	n = min(n, qlen);
 
 	while (n > 0) {
@@ -1521,14 +1393,12 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		memcpy_toio(&ch->ch_neo_uart->txrxburst,
 			    ch->ch_wqueue + tail, s);
 
-		/* Add and flip queue if needed */
 		tail = (tail + s) & WQUEUEMASK;
 		n -= s;
 		ch->ch_txcount += s;
 		len_written += s;
 	}
 
-	/* Update the final tail */
 	ch->ch_w_tail = tail & WQUEUEMASK;
 
 	if (len_written > 0) {
@@ -1544,7 +1414,7 @@ static void neo_parse_modem(struct channel_t *ch, unsigned char signals)
 {
 	unsigned char msignals = signals;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	/*
@@ -1572,10 +1442,7 @@ static void neo_parse_modem(struct channel_t *ch, unsigned char signals)
 		}
 	}
 
-	/*
-	 * Scrub off lower bits. They signify delta's, which I don't care
-	 * about
-	 */
+	/* Scrub off lower bits. They signify delta's */
 	msignals &= 0xf0;
 
 	if (msignals & UART_MSR_DCD)
@@ -1604,7 +1471,7 @@ static void neo_assert_modem_signals(struct channel_t *ch)
 {
 	unsigned char out;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	out = ch->ch_mostat;
@@ -1621,7 +1488,7 @@ static void neo_assert_modem_signals(struct channel_t *ch)
 
 static void neo_send_start_character(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	if (ch->ch_startc != _POSIX_VDISABLE) {
@@ -1634,7 +1501,7 @@ static void neo_send_start_character(struct channel_t *ch)
 
 static void neo_send_stop_character(struct channel_t *ch)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	if (ch->ch_stopc != _POSIX_VDISABLE) {
@@ -1668,7 +1535,6 @@ static void neo_uart_init(struct channel_t *ch)
 }
 
 /* Make the UART completely turn off. */
-
 static void neo_uart_off(struct channel_t *ch)
 {
 	/* Turn off UART enhanced bits */
@@ -1735,8 +1601,6 @@ static void neo_send_break(struct channel_t *ch, int msecs)
 }
 
 /*
- * neo_send_immediate_char.
- *
  * Sends a specific character as soon as possible to the UART,
  * jumping over any bytes that might be in the write queue.
  *
@@ -1744,7 +1608,7 @@ static void neo_send_break(struct channel_t *ch, int msecs)
  */
 static void neo_send_immediate_char(struct channel_t *ch, unsigned char c)
 {
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	writeb(c, &ch->ch_neo_uart->txrx);
@@ -1797,7 +1661,7 @@ static void neo_vpd(struct dgnc_board *brd)
 	unsigned int i = 0;
 	unsigned int a;
 
-	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
+	if (!brd)
 		return;
 
 	if (!brd->re_map_membase)
diff --git a/drivers/staging/dgnc/dgnc_neo.h b/drivers/staging/dgnc/dgnc_neo.h
index 77ecd9baae45..c30a2c2b5eea 100644
--- a/drivers/staging/dgnc/dgnc_neo.h
+++ b/drivers/staging/dgnc/dgnc_neo.h
@@ -13,43 +13,62 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DGNC_NEO_H
-#define __DGNC_NEO_H
+#ifndef _DGNC_NEO_H
+#define _DGNC_NEO_H
 
 #include "dgnc_driver.h"
 
-/*
- *	Per channel/port NEO UART structure
- *	Base Structure Entries Usage Meanings to Host
+/**
+ * struct neo_uart_struct - Per channel/port NEO UART structure
+ *
+ * key - W = read write
+ *     - R = read only
+ *     - U = unused
  *
- *	W = read write		R = read only
- *			U = Unused.
+ * @txrx: (RW) Holding Register.
+ * @ier: (RW) Interrupt Enable Register.
+ * @isr_fcr: (RW) Interrupt Status Reg/Fifo Control Register.
+ * @lcr: (RW) Line Control Register.
+ * @mcr: (RW) Modem Control Register.
+ * @lsr: (RW) Line Status Register.
+ * @msr: (RW) Modem Status Register.
+ * @spr: (RW) Scratch Pad Register.
+ * @fctr: (RW) Feature Control Register.
+ * @efr: (RW) Enhanced Function Register.
+ * @tfifo: (RW) Transmit FIFO Register.
+ * @rfifo: (RW) Receive  FIFO Register.
+ * @xoffchar1: (RW) XOff Character 1 Register.
+ * @xoffchar2: (RW) XOff Character 2 Register.
+ * @xonchar1: (RW) Xon Character 1 Register.
+ * @xonchar2: (RW) XOn Character 2 Register.
+ * @reserved1: (U) Reserved by Exar.
+ * @txrxburst: (RW)  64 bytes of RX/TX FIFO Data.
+ * @reserved2: (U) Reserved by Exar.
+ * @rxburst_with_errors: (R) bytes of RX FIFO Data + LSR.
  */
-
 struct neo_uart_struct {
-	u8 txrx;	/* WR  RHR/THR - Holding Reg */
-	u8 ier;		/* WR  IER - Interrupt Enable Reg */
-	u8 isr_fcr;	/* WR  ISR/FCR - Interrupt Status Reg/Fifo
-			 * Control Reg
-			 */
-	u8 lcr;		/* WR  LCR - Line Control Reg */
-	u8 mcr;		/* WR  MCR - Modem Control Reg */
-	u8 lsr;		/* WR  LSR - Line Status Reg */
-	u8 msr;		/* WR  MSR - Modem Status Reg */
-	u8 spr;		/* WR  SPR - Scratch Pad Reg */
-	u8 fctr;	/* WR  FCTR - Feature Control Reg */
-	u8 efr;		/* WR  EFR - Enhanced Function Reg */
-	u8 tfifo;	/* WR  TXCNT/TXTRG - Transmit FIFO Reg */
-	u8 rfifo;	/* WR  RXCNT/RXTRG - Receive  FIFO Reg */
-	u8 xoffchar1;	/* WR  XOFF 1 - XOff Character 1 Reg */
-	u8 xoffchar2;	/* WR  XOFF 2 - XOff Character 2 Reg */
-	u8 xonchar1;	/* WR  XON 1 - Xon Character 1 Reg */
-	u8 xonchar2;	/* WR  XON 2 - XOn Character 2 Reg */
-
-	u8 reserved1[0x2ff - 0x200]; /* U   Reserved by Exar */
-	u8 txrxburst[64];	     /* RW  64 bytes of RX/TX FIFO Data */
-	u8 reserved2[0x37f - 0x340]; /* U   Reserved by Exar */
-	u8 rxburst_with_errors[64];  /* R  64 bytes of RX FIFO Data + LSR */
+	u8 txrx;
+	u8 ier;
+	u8 isr_fcr;
+
+	u8 lcr;
+	u8 mcr;
+	u8 lsr;
+	u8 msr;
+	u8 spr;
+	u8 fctr;
+	u8 efr;
+	u8 tfifo;
+	u8 rfifo;
+	u8 xoffchar1;
+	u8 xoffchar2;
+	u8 xonchar1;
+	u8 xonchar2;
+
+	u8 reserved1[0x2ff - 0x200];
+	u8 txrxburst[64];
+	u8 reserved2[0x37f - 0x340];
+	u8 rxburst_with_errors[64];
 };
 
 /* Where to read the extended interrupt register (32bits instead of 8bits) */
@@ -151,8 +170,6 @@ struct neo_uart_struct {
 #define UART_17158_IER_RTSDTR	0x40	/* Output Interrupt Enable */
 #define UART_17158_IER_CTSDSR	0x80	/* Input Interrupt Enable */
 
-/* Our Global Variables */
-
 extern struct board_ops dgnc_neo_ops;
 
-#endif
+#endif	/* _DGNC_NEO_H */
diff --git a/drivers/staging/dgnc/dgnc_pci.h b/drivers/staging/dgnc/dgnc_pci.h
index 4e170c47f4a3..59845916d90c 100644
--- a/drivers/staging/dgnc/dgnc_pci.h
+++ b/drivers/staging/dgnc/dgnc_pci.h
@@ -13,10 +13,11 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DGNC_PCI_H
-#define __DGNC_PCI_H
+#ifndef _DGNC_PCI_H
+#define _DGNC_PCI_H
 
-#define PCIMAX 32			/* maximum number of PCI boards */
+/* Maximum number of PCI boards */
+#define PCIMAX 32
 
 #define DIGI_VID				0x114F
 
@@ -59,10 +60,10 @@
 #define PCI_DEVICE_NEO_EXPRESS_8RJ45_PCI_NAME	"Neo 8 PCI Express RJ45"
 #define PCI_DEVICE_NEO_EXPRESS_4_IBM_PCI_NAME	"Neo 4 PCI Express IBM"
 
-/* Size of Memory and I/O for PCI (4 K) */
+/* Size of memory and I/O for PCI (4 K) */
 #define PCI_RAM_SIZE				0x1000
 
-/* Size of Memory (2MB) */
+/* Size of memory (2MB) */
 #define PCI_MEM_SIZE				0x1000
 
-#endif
+#endif	/* _DGNC_PCI_H */
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index c3b8fc54883d..9e98781ca6fe 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -58,16 +58,15 @@ static const struct digi_t dgnc_digi_init = {
  * This defines a raw port at 9600 baud, 8 data bits, no parity,
  * 1 stop bit.
  */
-static struct ktermios default_termios = {
-	.c_iflag =	(DEFAULT_IFLAGS),	/* iflags */
-	.c_oflag =	(DEFAULT_OFLAGS),	/* oflags */
-	.c_cflag =	(DEFAULT_CFLAGS),	/* cflags */
-	.c_lflag =	(DEFAULT_LFLAGS),	/* lflags */
+static const struct ktermios default_termios = {
+	.c_iflag =	(DEFAULT_IFLAGS),
+	.c_oflag =	(DEFAULT_OFLAGS),
+	.c_cflag =	(DEFAULT_CFLAGS),
+	.c_lflag =	(DEFAULT_LFLAGS),
 	.c_cc =		INIT_C_CC,
 	.c_line =	0,
 };
 
-/* Our function prototypes */
 static int dgnc_tty_open(struct tty_struct *tty, struct file *file);
 static void dgnc_tty_close(struct tty_struct *tty, struct file *file);
 static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file,
@@ -130,10 +129,8 @@ static const struct tty_operations dgnc_tty_ops = {
 
 /* TTY Initialization/Cleanup Functions */
 
-/*
- * dgnc_tty_register()
- *
- * Init the tty subsystem for this board.
+/**
+ * dgnc_tty_register() - Init the tty subsystem for this board.
  */
 int dgnc_tty_register(struct dgnc_board *brd)
 {
@@ -143,7 +140,6 @@ int dgnc_tty_register(struct dgnc_board *brd)
 					      TTY_DRIVER_REAL_RAW |
 					      TTY_DRIVER_DYNAMIC_DEV |
 					      TTY_DRIVER_HARDWARE_BREAK);
-
 	if (IS_ERR(brd->serial_driver))
 		return PTR_ERR(brd->serial_driver);
 
@@ -181,7 +177,6 @@ int dgnc_tty_register(struct dgnc_board *brd)
 					     TTY_DRIVER_REAL_RAW |
 					     TTY_DRIVER_DYNAMIC_DEV |
 					     TTY_DRIVER_HARDWARE_BREAK);
-
 	if (IS_ERR(brd->print_driver)) {
 		rc = PTR_ERR(brd->print_driver);
 		goto unregister_serial_driver;
@@ -232,15 +227,15 @@ void dgnc_tty_unregister(struct dgnc_board *brd)
 	put_tty_driver(brd->serial_driver);
 }
 
-/*
- * dgnc_tty_init()
+/**
+ * dgnc_tty_init() - Initialize the tty subsystem.
  *
- * Init the tty subsystem.  Called once per board after board has been
- * downloaded and init'ed.
+ * Called once per board after board has been downloaded and initialized.
  */
 int dgnc_tty_init(struct dgnc_board *brd)
 {
 	int i;
+	int rc;
 	void __iomem *vaddr;
 	struct channel_t *ch;
 
@@ -254,14 +249,12 @@ int dgnc_tty_init(struct dgnc_board *brd)
 	brd->nasync = brd->maxports;
 
 	for (i = 0; i < brd->nasync; i++) {
-		/*
-		 * Okay to malloc with GFP_KERNEL, we are not at
-		 * interrupt context, and there are no locks held.
-		 */
 		brd->channels[i] = kzalloc(sizeof(*brd->channels[i]),
 					   GFP_KERNEL);
-		if (!brd->channels[i])
+		if (!brd->channels[i]) {
+			rc = -ENOMEM;
 			goto err_free_channels;
+		}
 	}
 
 	ch = brd->channels[0];
@@ -271,14 +264,10 @@ int dgnc_tty_init(struct dgnc_board *brd)
 	for (i = 0; i < brd->nasync; i++, ch = brd->channels[i]) {
 		spin_lock_init(&ch->ch_lock);
 
-		/* Store all our magic numbers */
-		ch->magic = DGNC_CHANNEL_MAGIC;
-		ch->ch_tun.magic = DGNC_UNIT_MAGIC;
 		ch->ch_tun.un_ch = ch;
 		ch->ch_tun.un_type = DGNC_SERIAL;
 		ch->ch_tun.un_dev = i;
 
-		ch->ch_pun.magic = DGNC_UNIT_MAGIC;
 		ch->ch_pun.un_ch = ch;
 		ch->ch_pun.un_type = DGNC_PRINT;
 		ch->ch_pun.un_dev = i + 128;
@@ -319,11 +308,12 @@ err_free_channels:
 		kfree(brd->channels[i]);
 		brd->channels[i] = NULL;
 	}
-	return -ENOMEM;
+
+	return rc;
 }
 
-/*
- * dgnc_cleanup_tty()
+/**
+ * dgnc_cleanup_tty() - Cleanup driver.
  *
  * Uninitialize the TTY portion of this driver.  Free all memory and
  * resources.
@@ -346,19 +336,18 @@ void dgnc_cleanup_tty(struct dgnc_board *brd)
 	put_tty_driver(brd->print_driver);
 }
 
-/*
- *	dgnc_wmove - Write data to transmit queue.
- *
- *		ch	- Pointer to channel structure.
- *		buf	- Pointer to characters to be moved.
- *		n	- Number of characters to move.
+/**
+ * dgnc_wmove() - Write data to transmit queue.
+ * @ch: Pointer to channel structure.
+ * @buf: Pointer to characters to be moved.
+ * @n: Number of characters to move.
  */
 static void dgnc_wmove(struct channel_t *ch, char *buf, uint n)
 {
 	int	remain;
 	uint	head;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	head = ch->ch_w_head & WQUEUEMASK;
@@ -388,10 +377,9 @@ static void dgnc_wmove(struct channel_t *ch, char *buf, uint n)
 	ch->ch_w_head = head;
 }
 
-/*
- *      dgnc_input - Process received data.
- *
- *	      ch      - Pointer to channel structure.
+/**
+ * dgnc_input() - Process received data.
+ * @ch: Pointer to channel structure.
  */
 void dgnc_input(struct channel_t *ch)
 {
@@ -409,21 +397,17 @@ void dgnc_input(struct channel_t *ch)
 	int s = 0;
 	int i = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	tp = ch->ch_tun.un_tty;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/*
-	 *      Figure the number of characters in the buffer.
-	 *      Exit immediately if none.
-	 */
 	rmask = RQUEUEMASK;
 	head = ch->ch_r_head & rmask;
 	tail = ch->ch_r_tail & rmask;
@@ -436,7 +420,7 @@ void dgnc_input(struct channel_t *ch)
 	 * If the device is not open, or CREAD is off,
 	 * flush input data and return immediately.
 	 */
-	if (!tp || (tp->magic != TTY_MAGIC) ||
+	if (!tp ||
 	    !(ch->ch_tun.un_flags & UN_ISOPEN) ||
 	    !C_CREAD(tp) ||
 	    (ch->ch_tun.un_flags & UN_CLOSING)) {
@@ -448,32 +432,18 @@ void dgnc_input(struct channel_t *ch)
 		goto exit_unlock;
 	}
 
-	/* If we are throttled, simply don't read any data. */
-
 	if (ch->ch_flags & CH_FORCED_STOPI)
 		goto exit_unlock;
 
 	flip_len = TTY_FLIPBUF_SIZE;
 
-	/* Chop down the length, if needed */
 	len = min(data_len, flip_len);
 	len = min(len, (N_TTY_BUF_SIZE - 1));
 
 	ld = tty_ldisc_ref(tp);
-
-	/*
-	 * If we were unable to get a reference to the ld,
-	 * don't flush our buffer, and act like the ld doesn't
-	 * have any space to put the data right now.
-	 */
 	if (!ld) {
 		len = 0;
 	} else {
-		/*
-		 * If ld doesn't have a pointer to a receive_buf function,
-		 * flush the data, then act like the ld doesn't have any
-		 * space to put the data right now.
-		 */
 		if (!ld->ops->receive_buf) {
 			ch->ch_r_head = ch->ch_r_tail;
 			len = 0;
@@ -562,7 +532,9 @@ exit_unlock:
 		tty_ldisc_deref(ld);
 }
 
-/*
+/**
+ * dgnc_carrier()
+ *
  * Determines when CARRIER changes state and takes appropriate
  * action.
  */
@@ -571,7 +543,7 @@ void dgnc_carrier(struct channel_t *ch)
 	int virt_carrier = 0;
 	int phys_carrier = 0;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	if (ch->ch_mistat & UART_MSR_DCD)
@@ -652,7 +624,6 @@ void dgnc_carrier(struct channel_t *ch)
 }
 
 /*  Assign the custom baud rate to the channel structure */
-
 static void dgnc_set_custom_speed(struct channel_t *ch, uint newrate)
 {
 	int testdiv;
@@ -716,7 +687,6 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 {
 	int qleft;
 
-	/* Store how much space we have left in the queue */
 	qleft = ch->ch_r_tail - ch->ch_r_head - 1;
 	if (qleft < 0)
 		qleft += RQUEUEMASK + 1;
@@ -797,7 +767,7 @@ void dgnc_wakeup_writes(struct channel_t *ch)
 	int qlen = 0;
 	unsigned long flags;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -883,8 +853,6 @@ static struct dgnc_board *find_board_by_major(unsigned int major)
 
 /* TTY Entry points and helper functions */
 
-/* dgnc_tty_open() */
-
 static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 {
 	struct dgnc_board	*brd;
@@ -903,39 +871,30 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	if (major > 255)
 		return -ENXIO;
 
-	/* Get board pointer from our array of majors we have allocated */
 	brd = find_board_by_major(major);
 	if (!brd)
 		return -ENXIO;
 
-	/*
-	 * If board is not yet up to a state of READY, go to
-	 * sleep waiting for it to happen or they cancel the open.
-	 */
 	rc = wait_event_interruptible(brd->state_wait,
 				      (brd->state & BOARD_READY));
-
 	if (rc)
 		return rc;
 
 	spin_lock_irqsave(&brd->bd_lock, flags);
 
-	/* If opened device is greater than our number of ports, bail. */
 	if (PORT_NUM(minor) >= brd->nasync) {
-		spin_unlock_irqrestore(&brd->bd_lock, flags);
-		return -ENXIO;
+		rc = -ENXIO;
+		goto err_brd_unlock;
 	}
 
 	ch = brd->channels[PORT_NUM(minor)];
 	if (!ch) {
-		spin_unlock_irqrestore(&brd->bd_lock, flags);
-		return -ENXIO;
+		rc = -ENXIO;
+		goto err_brd_unlock;
 	}
 
-	/* Drop board lock */
 	spin_unlock_irqrestore(&brd->bd_lock, flags);
 
-	/* Grab channel lock */
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Figure out our type */
@@ -946,8 +905,8 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 		un = &brd->channels[PORT_NUM(minor)]->ch_pun;
 		un->un_type = DGNC_PRINT;
 	} else {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return -ENXIO;
+		rc = -ENXIO;
+		goto err_ch_unlock;
 	}
 
 	/*
@@ -959,7 +918,6 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 
 	rc = wait_event_interruptible(ch->ch_flags_wait,
 				      ((ch->ch_flags & CH_OPENING) == 0));
-
 	/* If ret is non-zero, user ctrl-c'ed us */
 	if (rc)
 		return -EINTR;
@@ -975,21 +933,18 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 				ch->ch_flags_wait,
 				(((ch->ch_tun.un_flags |
 				ch->ch_pun.un_flags) & UN_CLOSING) == 0));
-
 	/* If ret is non-zero, user ctrl-c'ed us */
 	if (rc)
 		return -EINTR;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* Store our unit into driver_data, so we always have it available. */
 	tty->driver_data = un;
 
 	/* Initialize tty's */
 
 	if (!(un->un_flags & UN_ISOPEN)) {
-		/* Store important variables. */
-		un->un_tty     = tty;
+		un->un_tty = tty;
 
 		/* Maybe do something here to the TTY struct as well? */
 	}
@@ -1000,7 +955,6 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	 */
 	ch->ch_flags |= (CH_OPENING);
 
-	/* Drop locks, as malloc with GFP_KERNEL can sleep */
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	if (!ch->ch_rqueue)
@@ -1014,7 +968,6 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 		kfree(ch->ch_rqueue);
 		kfree(ch->ch_equeue);
 		kfree(ch->ch_wqueue);
-
 		return -ENOMEM;
 	}
 
@@ -1062,19 +1015,14 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 		brd->bd_ops->uart_init(ch);
 	}
 
-	/* Run param in case we changed anything */
-
 	brd->bd_ops->param(tty);
 
 	dgnc_carrier(ch);
 
-	/* follow protocol for opening port */
-
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	rc = dgnc_block_til_ready(tty, file, ch);
 
-	/* No going back now, increment our unit and channel counters */
 	spin_lock_irqsave(&ch->ch_lock, flags);
 	ch->ch_open_count++;
 	un->un_open_count++;
@@ -1082,18 +1030,23 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return rc;
+
+err_brd_unlock:
+	spin_unlock_irqrestore(&brd->bd_lock, flags);
+
+	return rc;
+err_ch_unlock:
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
+
+	return rc;
 }
 
-/*
- * dgnc_block_til_ready()
- *
- * Wait for DCD, if needed.
- */
+/* Wait for DCD, if needed. */
 static int dgnc_block_til_ready(struct tty_struct *tty,
 				struct file *file,
 				struct channel_t *ch)
 {
-	int retval = 0;
+	int rc = 0;
 	struct un_t *un = tty->driver_data;
 	unsigned long flags;
 	uint	old_flags = 0;
@@ -1106,22 +1059,16 @@ static int dgnc_block_til_ready(struct tty_struct *tty,
 
 	ch->ch_wopen++;
 
-	/* Loop forever */
 	while (1) {
 		sleep_on_un_flags = 0;
 
-		/*
-		 * If board has failed somehow during our sleep,
-		 * bail with error.
-		 */
 		if (ch->ch_bd->state == BOARD_FAILED) {
-			retval = -ENXIO;
+			rc = -ENXIO;
 			break;
 		}
 
-		/* If tty was hung up, break out of loop and set error. */
 		if (tty_hung_up_p(file)) {
-			retval = -EAGAIN;
+			rc = -EAGAIN;
 			break;
 		}
 
@@ -1146,7 +1093,7 @@ static int dgnc_block_til_ready(struct tty_struct *tty,
 				break;
 
 			if (tty_io_error(tty)) {
-				retval = -EIO;
+				rc = -EIO;
 				break;
 			}
 
@@ -1162,15 +1109,12 @@ static int dgnc_block_til_ready(struct tty_struct *tty,
 		/*
 		 * If there is a signal pending, the user probably
 		 * interrupted (ctrl-c) us.
-		 * Leave loop with error set.
 		 */
 		if (signal_pending(current)) {
-			retval = -ERESTARTSYS;
+			rc = -ERESTARTSYS;
 			break;
 		}
 
-		/* Store the flags before we let go of channel lock */
-
 		if (sleep_on_un_flags)
 			old_flags = ch->ch_tun.un_flags | ch->ch_pun.un_flags;
 		else
@@ -1189,12 +1133,12 @@ static int dgnc_block_til_ready(struct tty_struct *tty,
 		 * from the current value.
 		 */
 		if (sleep_on_un_flags)
-			retval = wait_event_interruptible
+			rc = wait_event_interruptible
 				(un->un_flags_wait,
 				 (old_flags != (ch->ch_tun.un_flags |
 						ch->ch_pun.un_flags)));
 		else
-			retval = wait_event_interruptible(
+			rc = wait_event_interruptible(
 					ch->ch_flags_wait,
 					(old_flags != ch->ch_flags));
 
@@ -1209,25 +1153,19 @@ static int dgnc_block_til_ready(struct tty_struct *tty,
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	return retval;
+	return rc;
 }
 
-/*
- * dgnc_tty_hangup()
- *
- * Hangup the port.  Like a close, but don't wait for output to drain.
- */
+/* Hangup the port.  Like a close, but don't wait for output to drain. */
 static void dgnc_tty_hangup(struct tty_struct *tty)
 {
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	/* flush the transmit queues */
 	dgnc_tty_flush_buffer(tty);
 }
 
-/* dgnc_tty_close() */
-
 static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 {
 	struct dgnc_board *bd;
@@ -1235,19 +1173,19 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -1361,8 +1299,6 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 }
 
 /*
- * dgnc_tty_chars_in_buffer()
- *
  * Return number of characters that have not been transmitted yet.
  *
  * This routine is used by the line discipline to determine if there
@@ -1375,18 +1311,18 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
 	ushort thead;
 	ushort ttail;
 	uint tmask;
-	uint chars = 0;
+	uint chars;
 	unsigned long flags;
 
 	if (!tty)
 		return 0;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return 0;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return 0;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -1397,21 +1333,17 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	if (ttail == thead) {
+	if (ttail == thead)
 		chars = 0;
-	} else {
-		if (thead >= ttail)
-			chars = thead - ttail;
-		else
-			chars = thead - ttail + WQUEUESIZE;
-	}
+	else if (thead > ttail)
+		chars = thead - ttail;
+	else
+		chars = thead - ttail + WQUEUESIZE;
 
 	return chars;
 }
 
 /*
- * dgnc_maxcps_room
- *
  * Reduces bytes_available to the max number of characters
  * that can be sent currently given the maxcps value, and
  * returns the new bytes_available.  This only affects printer
@@ -1419,6 +1351,8 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
  */
 static int dgnc_maxcps_room(struct channel_t *ch, int bytes_available)
 {
+	int rc = bytes_available;
+
 	if (ch->ch_digi.digi_maxcps > 0 && ch->ch_digi.digi_bufsize > 0) {
 		int cps_limit = 0;
 		unsigned long current_time = jiffies;
@@ -1439,17 +1373,13 @@ static int dgnc_maxcps_room(struct channel_t *ch, int bytes_available)
 			cps_limit = 0;
 		}
 
-		bytes_available = min(cps_limit, bytes_available);
+		rc = min(cps_limit, bytes_available);
 	}
 
-	return bytes_available;
+	return rc;
 }
 
-/*
- * dgnc_tty_write_room()
- *
- * Return space available in Tx buffer
- */
+/* Return room available in Tx buffer */
 static int dgnc_tty_write_room(struct tty_struct *tty)
 {
 	struct channel_t *ch = NULL;
@@ -1457,18 +1387,18 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 	ushort head;
 	ushort tail;
 	ushort tmask;
-	int ret = 0;
+	int room = 0;
 	unsigned long flags;
 
 	if (!tty)
 		return 0;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return 0;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return 0;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -1477,53 +1407,45 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 	head = (ch->ch_w_head) & tmask;
 	tail = (ch->ch_w_tail) & tmask;
 
-	ret = tail - head - 1;
-	if (ret < 0)
-		ret += WQUEUESIZE;
+	room = tail - head - 1;
+	if (room < 0)
+		room += WQUEUESIZE;
 
 	/* Limit printer to maxcps */
 	if (un->un_type != DGNC_PRINT)
-		ret = dgnc_maxcps_room(ch, ret);
+		room = dgnc_maxcps_room(ch, room);
 
 	/*
-	 * If we are printer device, leave space for
+	 * If we are printer device, leave room for
 	 * possibly both the on and off strings.
 	 */
 	if (un->un_type == DGNC_PRINT) {
 		if (!(ch->ch_flags & CH_PRON))
-			ret -= ch->ch_digi.digi_onlen;
-		ret -= ch->ch_digi.digi_offlen;
+			room -= ch->ch_digi.digi_onlen;
+		room -= ch->ch_digi.digi_offlen;
 	} else {
 		if (ch->ch_flags & CH_PRON)
-			ret -= ch->ch_digi.digi_offlen;
+			room -= ch->ch_digi.digi_offlen;
 	}
 
-	if (ret < 0)
-		ret = 0;
+	if (room < 0)
+		room = 0;
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
-
-	return ret;
+	return room;
 }
 
 /*
- * dgnc_tty_put_char()
- *
  * Put a character into ch->ch_buf
- *
- *      - used by the line discipline for OPOST processing
+ * Used by the line discipline for OPOST processing
  */
 static int dgnc_tty_put_char(struct tty_struct *tty, unsigned char c)
 {
-	/* Simply call tty_write. */
-
 	dgnc_tty_write(tty, &c, 1);
 	return 1;
 }
 
 /*
- * dgnc_tty_write()
- *
  * Take data from the user or kernel and send it out to the FEP.
  * In here exists all the Transparent Print magic as well.
  */
@@ -1543,11 +1465,11 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		return 0;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return 0;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return 0;
 
 	if (!count)
@@ -1561,7 +1483,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	/* Get our space available for the channel from the board */
 	tmask = WQUEUEMASK;
 	head = (ch->ch_w_head) & tmask;
 	tail = (ch->ch_w_tail) & tmask;
@@ -1577,14 +1498,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	if (un->un_type != DGNC_PRINT)
 		bufcount = dgnc_maxcps_room(ch, bufcount);
 
-	/*
-	 * Take minimum of what the user wants to send, and the
-	 * space available in the FEP buffer.
-	 */
 	count = min(count, bufcount);
-
-	/* Bail if no space left. */
-
 	if (count <= 0)
 		goto exit_retry;
 
@@ -1646,42 +1560,36 @@ static int dgnc_tty_write(struct tty_struct *tty,
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	if (count) {
-		/*
-		 * Channel lock is grabbed and then released
-		 * inside this routine.
-		 */
+	if (count)
 		ch->ch_bd->bd_ops->copy_data_from_queue_to_uart(ch);
-	}
 
 	return count;
 
 exit_retry:
-
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
+
 	return 0;
 }
 
 /* Return modem signals to ld. */
-
 static int dgnc_tty_tiocmget(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	int result = -EIO;
+	int rc;
 	unsigned char mstat = 0;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
-		return result;
+	if (!tty)
+		return -EIO;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
-		return result;
+	if (!un)
+		return -EIO;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
-		return result;
+	if (!ch)
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
@@ -1689,53 +1597,47 @@ static int dgnc_tty_tiocmget(struct tty_struct *tty)
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	result = 0;
+	rc = 0;
 
 	if (mstat & UART_MCR_DTR)
-		result |= TIOCM_DTR;
+		rc |= TIOCM_DTR;
 	if (mstat & UART_MCR_RTS)
-		result |= TIOCM_RTS;
+		rc |= TIOCM_RTS;
 	if (mstat & UART_MSR_CTS)
-		result |= TIOCM_CTS;
+		rc |= TIOCM_CTS;
 	if (mstat & UART_MSR_DSR)
-		result |= TIOCM_DSR;
+		rc |= TIOCM_DSR;
 	if (mstat & UART_MSR_RI)
-		result |= TIOCM_RI;
+		rc |= TIOCM_RI;
 	if (mstat & UART_MSR_DCD)
-		result |= TIOCM_CD;
+		rc |= TIOCM_CD;
 
-	return result;
+	return rc;
 }
 
-/*
- * dgnc_tty_tiocmset()
- *
- * Set modem signals, called by ld.
- */
-
+/* Set modem signals, called by ld. */
 static int dgnc_tty_tiocmset(struct tty_struct *tty,
 			     unsigned int set, unsigned int clear)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+	if (!tty)
+		return -EIO;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
-		return ret;
+	if (!un)
+		return -EIO;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
-		return ret;
+	if (!ch)
+		return -EIO;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
-		return ret;
+	if (!bd)
+		return -EIO;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
@@ -1751,95 +1653,74 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 	if (clear & TIOCM_DTR)
 		ch->ch_mostat &= ~(UART_MCR_DTR);
 
-	ch->ch_bd->bd_ops->assert_modem_signals(ch);
+	bd->bd_ops->assert_modem_signals(ch);
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
 
-/*
- * dgnc_tty_send_break()
- *
- * Send a Break, called by ld.
- */
+/* Send a Break, called by ld. */
 static int dgnc_tty_send_break(struct tty_struct *tty, int msec)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	int ret = -EIO;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
-		return ret;
+	if (!tty)
+		return -EIO;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
-		return ret;
+	if (!un)
+		return -EIO;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
-		return ret;
+	if (!ch)
+		return -EIO;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
-		return ret;
+	if (!bd)
+		return -EIO;
 
-	switch (msec) {
-	case -1:
+	if (msec < 0)
 		msec = 0xFFFF;
-		break;
-	case 0:
-		msec = 0;
-		break;
-	default:
-		break;
-	}
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
-	ch->ch_bd->bd_ops->send_break(ch, msec);
+	bd->bd_ops->send_break(ch, msec);
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
 
-/*
- * dgnc_tty_wait_until_sent()
- *
- * wait until data has been transmitted, called by ld.
- */
+/* wait until data has been transmitted, called by ld. */
 static void dgnc_tty_wait_until_sent(struct tty_struct *tty, int timeout)
 {
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	bd->bd_ops->drain(tty, 0);
 }
 
-/*
- * dgnc_send_xchar()
- *
- * send a high priority character, called by ld.
- */
+/* send a high priority character, called by ld. */
 static void dgnc_tty_send_xchar(struct tty_struct *tty, char c)
 {
 	struct dgnc_board *bd;
@@ -1847,39 +1728,34 @@ static void dgnc_tty_send_xchar(struct tty_struct *tty, char c)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
-	dev_dbg(tty->dev, "dgnc_tty_send_xchar start\n");
-
 	spin_lock_irqsave(&ch->ch_lock, flags);
 	bd->bd_ops->send_immediate_char(ch, c);
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
-
-	dev_dbg(tty->dev, "dgnc_tty_send_xchar finish\n");
 }
 
 /* Return modem signals to ld. */
-
 static inline int dgnc_get_mstat(struct channel_t *ch)
 {
 	unsigned char mstat;
-	int result = 0;
 	unsigned long flags;
+	int rc;
 
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -ENXIO;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -1888,46 +1764,43 @@ static inline int dgnc_get_mstat(struct channel_t *ch)
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
+	rc = 0;
+
 	if (mstat & UART_MCR_DTR)
-		result |= TIOCM_DTR;
+		rc |= TIOCM_DTR;
 	if (mstat & UART_MCR_RTS)
-		result |= TIOCM_RTS;
+		rc |= TIOCM_RTS;
 	if (mstat & UART_MSR_CTS)
-		result |= TIOCM_CTS;
+		rc |= TIOCM_CTS;
 	if (mstat & UART_MSR_DSR)
-		result |= TIOCM_DSR;
+		rc |= TIOCM_DSR;
 	if (mstat & UART_MSR_RI)
-		result |= TIOCM_RI;
+		rc |= TIOCM_RI;
 	if (mstat & UART_MSR_DCD)
-		result |= TIOCM_CD;
+		rc |= TIOCM_CD;
 
-	return result;
+	return rc;
 }
 
 /* Return modem signals to ld. */
-
 static int dgnc_get_modem_info(struct channel_t *ch,
 			       unsigned int  __user *value)
 {
 	return put_user(dgnc_get_mstat(ch), value);
 }
 
-/*
- * dgnc_set_modem_info()
- *
- * Set modem signals, called by ld.
- */
+/* Set modem signals, called by ld. */
 static int dgnc_set_modem_info(struct channel_t *ch,
 			       unsigned int command,
 			       unsigned int __user *value)
 {
-	int ret = -ENXIO;
+	int rc;
 	unsigned int arg = 0;
 	unsigned long flags;
 
-	ret = get_user(arg, value);
-	if (ret)
-		return ret;
+	rc = get_user(arg, value);
+	if (rc)
+		return rc;
 
 	switch (command) {
 	case TIOCMBIS:
@@ -1975,11 +1848,7 @@ static int dgnc_set_modem_info(struct channel_t *ch,
 	return 0;
 }
 
-/*
- * dgnc_tty_digigeta()
- *
- * Ioctl to get the information for ditty.
- */
+/* Ioctl to get the information for ditty. */
 static int dgnc_tty_digigeta(struct tty_struct *tty,
 			     struct digi_t __user *retinfo)
 {
@@ -1991,15 +1860,15 @@ static int dgnc_tty_digigeta(struct tty_struct *tty,
 	if (!retinfo)
 		return -EFAULT;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return -EFAULT;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return -EFAULT;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -EFAULT;
 
 	memset(&tmp, 0, sizeof(tmp));
@@ -2014,11 +1883,7 @@ static int dgnc_tty_digigeta(struct tty_struct *tty,
 	return 0;
 }
 
-/*
- * dgnc_tty_digiseta()
- *
- * Ioctl to set the information for ditty.
- */
+/* Ioctl to set the information for ditty. */
 static int dgnc_tty_digiseta(struct tty_struct *tty,
 			     struct digi_t __user *new_info)
 {
@@ -2028,19 +1893,19 @@ static int dgnc_tty_digiseta(struct tty_struct *tty,
 	struct digi_t new_digi;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return -EFAULT;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return -EFAULT;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -EFAULT;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return -EFAULT;
 
 	if (copy_from_user(&new_digi, new_info, sizeof(new_digi)))
@@ -2089,15 +1954,13 @@ static int dgnc_tty_digiseta(struct tty_struct *tty,
 	if (ch->ch_digi.digi_offlen > DIGI_PLEN)
 		ch->ch_digi.digi_offlen = DIGI_PLEN;
 
-	ch->ch_bd->bd_ops->param(tty);
+	bd->bd_ops->param(tty);
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
 
-/* dgnc_set_termios() */
-
 static void dgnc_tty_set_termios(struct tty_struct *tty,
 				 struct ktermios *old_termios)
 {
@@ -2106,19 +1969,19 @@ static void dgnc_tty_set_termios(struct tty_struct *tty,
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2130,7 +1993,7 @@ static void dgnc_tty_set_termios(struct tty_struct *tty,
 	ch->ch_startc = tty->termios.c_cc[VSTART];
 	ch->ch_stopc  = tty->termios.c_cc[VSTOP];
 
-	ch->ch_bd->bd_ops->param(tty);
+	bd->bd_ops->param(tty);
 	dgnc_carrier(ch);
 
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
@@ -2142,15 +2005,15 @@ static void dgnc_tty_throttle(struct tty_struct *tty)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2166,15 +2029,15 @@ static void dgnc_tty_unthrottle(struct tty_struct *tty)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2191,19 +2054,19 @@ static void dgnc_tty_start(struct tty_struct *tty)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2220,19 +2083,19 @@ static void dgnc_tty_stop(struct tty_struct *tty)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2243,8 +2106,6 @@ static void dgnc_tty_stop(struct tty_struct *tty)
 }
 
 /*
- * dgnc_tty_flush_chars()
- *
  * Flush the cook buffer
  *
  * Note to self, and any other poor souls who venture here:
@@ -2262,19 +2123,19 @@ static void dgnc_tty_flush_chars(struct tty_struct *tty)
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2284,26 +2145,22 @@ static void dgnc_tty_flush_chars(struct tty_struct *tty)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-/*
- * dgnc_tty_flush_buffer()
- *
- * Flush Tx buffer (make in == out)
- */
+/* Flush Tx buffer (make in == out) */
 static void dgnc_tty_flush_buffer(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
 	unsigned long flags;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return;
 
 	spin_lock_irqsave(&ch->ch_lock, flags);
@@ -2328,11 +2185,7 @@ static void dgnc_tty_flush_buffer(struct tty_struct *tty)
 	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
-/*
- * dgnc_wake_up_unit()
- *
- * Wakes up processes waiting in the unit's (teminal/printer) wait queue
- */
+/* Wakes up processes waiting in the unit's (teminal/printer) wait queue */
 static void dgnc_wake_up_unit(struct un_t *unit)
 {
 	unit->un_flags &= ~(UN_LOW | UN_EMPTY);
@@ -2341,11 +2194,7 @@ static void dgnc_wake_up_unit(struct un_t *unit)
 
 /* The IOCTL function and all of its helpers */
 
-/*
- * dgnc_tty_ioctl()
- *
- * The usual assortment of ioctl's
- */
+/* The usual assortment of ioctl's */
 static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 			  unsigned long arg)
 {
@@ -2357,19 +2206,19 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	unsigned long flags;
 	void __user *uarg = (void __user *)arg;
 
-	if (!tty || tty->magic != TTY_MAGIC)
+	if (!tty)
 		return -ENODEV;
 
 	un = tty->driver_data;
-	if (!un || un->magic != DGNC_UNIT_MAGIC)
+	if (!un)
 		return -ENODEV;
 
 	ch = un->un_ch;
-	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
+	if (!ch)
 		return -ENODEV;
 
 	bd = ch->ch_bd;
-	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
+	if (!bd)
 		return -ENODEV;
 
 	ch_bd_ops = bd->bd_ops;
@@ -2377,8 +2226,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	if (un->un_open_count <= 0) {
-		spin_unlock_irqrestore(&ch->ch_lock, flags);
-		return -EIO;
+		rc = -EIO;
+		goto err_unlock;
 	}
 
 	switch (cmd) {
@@ -2399,7 +2248,6 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 			return rc;
 
 		rc = ch_bd_ops->drain(tty, 0);
-
 		if (rc)
 			return -EINTR;
 
@@ -2504,10 +2352,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 * also.
 		 */
 		rc = tty_check_change(tty);
-		if (rc) {
-			spin_unlock_irqrestore(&ch->ch_lock, flags);
-			return rc;
-		}
+		if (rc)
+			goto err_unlock;
 
 		if ((arg == TCIFLUSH) || (arg == TCIOFLUSH)) {
 			ch->ch_r_head = ch->ch_r_tail;
@@ -2588,7 +2434,6 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		if (cmd == (DIGI_SETAW)) {
 			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			rc = ch_bd_ops->drain(tty, 0);
-
 			if (rc)
 				return -EINTR;
 
@@ -2634,7 +2479,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	case DIGI_SETCUSTOMBAUD:
 	{
 		int new_rate;
-		/* Let go of locks when accessing user space, could sleep */
+
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = get_user(new_rate, (int __user *)arg);
 		if (rc)
@@ -2732,8 +2577,6 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-		/* Get data from user first. */
-
 		if (copy_from_user(&buf, uarg, sizeof(buf)))
 			return -EFAULT;
 
@@ -2787,4 +2630,8 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 
 		return -ENOIOCTLCMD;
 	}
+err_unlock:
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
+
+	return rc;
 }
diff --git a/drivers/staging/dgnc/dgnc_tty.h b/drivers/staging/dgnc/dgnc_tty.h
index 1ee0eeeb4730..6c58f1b3461a 100644
--- a/drivers/staging/dgnc/dgnc_tty.h
+++ b/drivers/staging/dgnc/dgnc_tty.h
@@ -13,8 +13,8 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DGNC_TTY_H
-#define __DGNC_TTY_H
+#ifndef _DGNC_TTY_H
+#define _DGNC_TTY_H
 
 #include "dgnc_driver.h"
 
@@ -30,4 +30,4 @@ void	dgnc_carrier(struct channel_t *ch);
 void	dgnc_wakeup_writes(struct channel_t *ch);
 void	dgnc_check_queue_flow_control(struct channel_t *ch);
 
-#endif
+#endif	/* _DGNC_TTY_H */
diff --git a/drivers/staging/dgnc/dgnc_utils.c b/drivers/staging/dgnc/dgnc_utils.c
index 6f59240024d1..e07ff8d2f972 100644
--- a/drivers/staging/dgnc/dgnc_utils.c
+++ b/drivers/staging/dgnc/dgnc_utils.c
@@ -2,12 +2,11 @@
 #include <linux/sched/signal.h>
 #include "dgnc_utils.h"
 
-/*
- * dgnc_ms_sleep()
+/**
+ * dgnc_ms_sleep - Put the driver to sleep
+ * @ms - milliseconds to sleep
  *
- * Put the driver to sleep for x ms's
- *
- * Returns 0 if timed out, !0 (showing signal) if interrupted by a signal.
+ * Return: 0 if timed out, if interrupted by a signal return signal.
  */
 int dgnc_ms_sleep(ulong ms)
 {
diff --git a/drivers/staging/dgnc/dgnc_utils.h b/drivers/staging/dgnc/dgnc_utils.h
index 1164c3a09c6b..d1f07a5735c6 100644
--- a/drivers/staging/dgnc/dgnc_utils.h
+++ b/drivers/staging/dgnc/dgnc_utils.h
@@ -1,6 +1,6 @@
-#ifndef __DGNC_UTILS_H
-#define __DGNC_UTILS_H
+#ifndef _DGNC_UTILS_H
+#define _DGNC_UTILS_H
 
 int dgnc_ms_sleep(ulong ms);
 
-#endif
+#endif	/* _DGNC_UTILS_H */
diff --git a/drivers/staging/dgnc/digi.h b/drivers/staging/dgnc/digi.h
index ec2e3dda6119..46b06b07bbf2 100644
--- a/drivers/staging/dgnc/digi.h
+++ b/drivers/staging/dgnc/digi.h
@@ -13,8 +13,8 @@
  * PURPOSE.  See the GNU General Public License for more details.
  */
 
-#ifndef __DIGI_H
-#define __DIGI_H
+#ifndef _DIGI_H
+#define _DIGI_H
 
 #ifndef TIOCM_LE
 #define		TIOCM_LE	0x01		/* line enable */
@@ -45,8 +45,7 @@
 #define DIGI_SETAW	(('e' << 8) | 96)	/* Drain & set params */
 #define DIGI_SETAF	(('e' << 8) | 97)	/* Drain, flush & set params */
 #define DIGI_GET_NI_INFO (('d' << 8) | 250)	/* Non-intelligent state info */
-#define DIGI_LOOPBACK (('d' << 8) | 252)	/*
-						 * Enable/disable UART
+#define DIGI_LOOPBACK (('d' << 8) | 252)	/* Enable/disable UART
 						 * internal loopback
 						 */
 #define DIGI_FAST	0x0002		/* Fast baud rates */
@@ -64,50 +63,77 @@
 /*
  * Structure used with ioctl commands for DIGI parameters.
  */
+/**
+ * struct digi_t - Ioctl commands for DIGI parameters.
+ * @digi_flags: Flags.
+ * @digi_maxcps: Maximum printer CPS.
+ * @digi_maxchar: Maximum characters in the print queue.
+ * @digi_bufsize: Buffer size.
+ * @digi_onlen: Length of ON string.
+ * @digi_offlen: Length of OFF string.
+ * @digi_onstr: Printer ON string.
+ * @digi_offstr: Printer OFF string.
+ * @digi_term: Terminal string.
+ */
 struct digi_t {
-	unsigned short	digi_flags;		/* Flags (see above) */
-	unsigned short	digi_maxcps;		/* Max printer CPS */
-	unsigned short	digi_maxchar;		/* Max chars in print queue */
-	unsigned short	digi_bufsize;		/* Buffer size */
-	unsigned char	digi_onlen;		/* Length of ON string */
-	unsigned char	digi_offlen;		/* Length of OFF string	*/
-	char		digi_onstr[DIGI_PLEN];	/* Printer on string */
-	char		digi_offstr[DIGI_PLEN];	/* Printer off string */
-	char		digi_term[DIGI_TSIZ];	/* terminal string */
+	unsigned short	digi_flags;
+	unsigned short	digi_maxcps;
+	unsigned short	digi_maxchar;
+	unsigned short	digi_bufsize;
+	unsigned char	digi_onlen;
+	unsigned char	digi_offlen;
+	char		digi_onstr[DIGI_PLEN];
+	char		digi_offstr[DIGI_PLEN];
+	char		digi_term[DIGI_TSIZ];
 };
 
-/* Structure to get driver status information */
-
+/**
+ * struct digi_dinfo - Driver status information.
+ * @dinfo_nboards: Number of boards configured.
+ * @dinfo_reserved: Not used, for future expansion.
+ * @dinfio_version: Driver version.
+ */
 struct digi_dinfo {
-	unsigned int	dinfo_nboards;		/* # boards configured */
-	char		dinfo_reserved[12];	/* for future expansion */
-	char		dinfo_version[16];	/* driver version */
+	unsigned int	dinfo_nboards;
+	char		dinfo_reserved[12];
+	char		dinfo_version[16];
 };
 
 #define	DIGI_GETDD	(('d' << 8) | 248)	/* get driver info */
 
-/*
- * Structure used with ioctl commands for per-board information
+/**
+ * struct digi_info - Ioctl commands for per board information.
+ *
+ * Physsize and memsize differ when board has "windowed" memory.
  *
- * physsize and memsize differ when board has "windowed" memory
+ * @info_bdnum: Board number (0 based).
+ * @info_ioport: IO port address.
+ * @indo_physaddr: Memory address.
+ * @info_physize: Size of host memory window.
+ * @info_memsize: Amount of dual-port memory on board.
+ * @info_bdtype: Board type.
+ * @info_nports: Number of ports.
+ * @info_bdstate: Board state.
+ * @info_reserved: Not used, for future expansion.
  */
 struct digi_info {
-	unsigned int	info_bdnum;		/* Board number (0 based) */
-	unsigned int	info_ioport;		/* io port address */
-	unsigned int	info_physaddr;		/* memory address */
-	unsigned int	info_physsize;		/* Size of host mem window */
-	unsigned int	info_memsize;		/* Amount of dual-port mem */
-						/* on board */
-	unsigned short	info_bdtype;		/* Board type */
-	unsigned short	info_nports;		/* number of ports */
-	char		info_bdstate;		/* board state */
-	char		info_reserved[7];	/* for future expansion */
+	unsigned int	info_bdnum;
+	unsigned int	info_ioport;
+	unsigned int	info_physaddr;
+	unsigned int	info_physsize;
+	unsigned int	info_memsize;
+	unsigned short	info_bdtype;
+	unsigned short	info_nports;
+	char		info_bdstate;
+	char		info_reserved[7];
 };
 
 #define	DIGI_GETBD	(('d' << 8) | 249)	/* get board info */
 
-struct digi_getbuffer /* Struct for holding buffer use counts */
-{
+/**
+ * struct digi_getbuffer - Holds buffer use counts.
+ */
+struct digi_getbuffer {
 	unsigned long tx_in;
 	unsigned long tx_out;
 	unsigned long rxbuf;
@@ -115,14 +141,24 @@ struct digi_getbuffer /* Struct for holding buffer use counts */
 	unsigned long txdone;
 };
 
+/**
+ * struct digi_getcounter
+ * @norun: Number of UART overrun errors.
+ * @noflow: Number of buffer overflow errors.
+ * @nframe: Number of framing errors.
+ * @nparity: Number of parity errors.
+ * @nbreak: Number of breaks received.
+ * @rbytes: Number of received bytes.
+ * @tbytes: Number of transmitted bytes.
+ */
 struct digi_getcounter {
-	unsigned long norun;		/* number of UART overrun errors */
-	unsigned long noflow;		/* number of buffer overflow errors */
-	unsigned long nframe;		/* number of framing errors */
-	unsigned long nparity;		/* number of parity errors */
-	unsigned long nbreak;		/* number of breaks received */
-	unsigned long rbytes;		/* number of received bytes */
-	unsigned long tbytes;		/* number of bytes transmitted fully */
+	unsigned long norun;
+	unsigned long noflow;
+	unsigned long nframe;
+	unsigned long nparity;
+	unsigned long nbreak;
+	unsigned long rbytes;
+	unsigned long tbytes;
 };
 
 /* Board State Definitions */
@@ -137,15 +173,14 @@ struct digi_getcounter {
 #define DIGI_REALPORT_GETCOUNTERS (('e' << 8) | 110)
 #define DIGI_REALPORT_GETEVENTS (('e' << 8) | 111)
 
-#define EV_OPU 0x0001 /* !<Output paused by client */
-#define EV_OPS 0x0002 /* !<Output paused by regular sw flowctrl */
-#define EV_IPU 0x0010 /* !<Input paused unconditionally by user */
-#define EV_IPS 0x0020 /* !<Input paused by high/low water marks */
-#define EV_TXB 0x0040 /* !<Transmit break pending */
+#define EV_OPU 0x0001 /* Output paused by client */
+#define EV_OPS 0x0002 /* Output paused by regular sw flowctrl */
+#define EV_IPU 0x0010 /* Input paused unconditionally by user */
+#define EV_IPS 0x0020 /* Input paused by high/low water marks */
+#define EV_TXB 0x0040 /* Transmit break pending */
 
-/*
- * This structure holds data needed for the intelligent <--> nonintelligent
- * DPA translation
+/**
+ * struct ni_info - intelligent <--> non-intelligent DPA translation.
  */
 struct ni_info {
 	int board;
@@ -175,4 +210,5 @@ struct ni_info {
 #define T_NEO 0000
 
 #define TTY_FLIPBUF_SIZE 512
-#endif /* DIGI_H */
+
+#endif	/* _DIGI_H */
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
index 789bfb97143c..78c08e15a1f9 100644
--- a/drivers/staging/emxx_udc/emxx_udc.h
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -567,7 +567,7 @@ struct nbu2ss_udc {
 	struct usb_gadget_driver *driver;
 	struct platform_device *pdev;
 	struct device *dev;
-	spinlock_t lock;
+	spinlock_t lock; /* Protects nbu2ss_udc structure fields */
 	struct completion		*pdone;
 
 	enum ep0_state			ep0state;
diff --git a/drivers/staging/fbtft/Kconfig b/drivers/staging/fbtft/Kconfig
index 6f5e82464d78..dba676761d72 100644
--- a/drivers/staging/fbtft/Kconfig
+++ b/drivers/staging/fbtft/Kconfig
@@ -111,6 +111,12 @@ config FB_TFT_S6D1121
 	help
 	  Generic Framebuffer support for S6D1121
 
+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
diff --git a/drivers/staging/fbtft/Makefile b/drivers/staging/fbtft/Makefile
index 2725ea9a4afc..05ae9fbf906e 100644
--- a/drivers/staging/fbtft/Makefile
+++ b/drivers/staging/fbtft/Makefile
@@ -20,6 +20,7 @@ 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_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
diff --git a/drivers/staging/fbtft/fb_agm1264k-fl.c b/drivers/staging/fbtft/fb_agm1264k-fl.c
index 4ee76dbd30b5..489151a6bf80 100644
--- a/drivers/staging/fbtft/fb_agm1264k-fl.c
+++ b/drivers/staging/fbtft/fb_agm1264k-fl.c
@@ -369,7 +369,7 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 			/* select left side (sc0)
 			 * set addr
 			 */
-			write_reg(par, 0x00, (1 << 6) | (u8)addr_win.xs);
+			write_reg(par, 0x00, BIT(6) | (u8)addr_win.xs);
 			write_reg(par, 0x00, (0x17 << 3) | (u8)y);
 
 			/* write bitmap */
diff --git a/drivers/staging/fbtft/fb_ili9163.c b/drivers/staging/fbtft/fb_ili9163.c
index 579e17734612..045cadc3bc65 100644
--- a/drivers/staging/fbtft/fb_ili9163.c
+++ b/drivers/staging/fbtft/fb_ili9163.c
@@ -194,7 +194,7 @@ static int set_var(struct fbtft_par *par)
 
 	/* Colorspcae */
 	if (par->bgr)
-		mactrl_data |= (1 << 2);
+		mactrl_data |= BIT(2);
 	write_reg(par, MIPI_DCS_SET_ADDRESS_MODE, mactrl_data);
 	write_reg(par, MIPI_DCS_WRITE_MEMORY_START);
 	return 0;
diff --git a/drivers/staging/fbtft/fb_ili9325.c b/drivers/staging/fbtft/fb_ili9325.c
index 7189de5ae4b3..7f9e9b25490e 100644
--- a/drivers/staging/fbtft/fb_ili9325.c
+++ b/drivers/staging/fbtft/fb_ili9325.c
@@ -126,7 +126,7 @@ static int init_display(struct fbtft_par *par)
 	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 */
-		(1 << 12) | (bt << 8) | (1 << 7) | (0x01 << 4));
+		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; */
diff --git a/drivers/staging/fbtft/fb_ili9481.c b/drivers/staging/fbtft/fb_ili9481.c
index 4e75f5abe2f9..7f182a1c084c 100644
--- a/drivers/staging/fbtft/fb_ili9481.c
+++ b/drivers/staging/fbtft/fb_ili9481.c
@@ -27,7 +27,7 @@
 #define WIDTH		320
 #define HEIGHT		480
 
-static s16 default_init_sequence[] = {
+static const s16 default_init_sequence[] = {
 	/* SLP_OUT - Sleep out */
 	-1, MIPI_DCS_EXIT_SLEEP_MODE,
 	-2, 50,
diff --git a/drivers/staging/fbtft/fb_ili9486.c b/drivers/staging/fbtft/fb_ili9486.c
index f4b314265f9e..ddd07a64c48a 100644
--- a/drivers/staging/fbtft/fb_ili9486.c
+++ b/drivers/staging/fbtft/fb_ili9486.c
@@ -26,7 +26,7 @@
 #define HEIGHT		480
 
 /* this init sequence matches PiScreen */
-static s16 default_init_sequence[] = {
+static const s16 default_init_sequence[] = {
 	/* Interface Mode Control */
 	-1, 0xb0, 0x0,
 	-1, MIPI_DCS_EXIT_SLEEP_MODE,
diff --git a/drivers/staging/fbtft/fb_ra8875.c b/drivers/staging/fbtft/fb_ra8875.c
index 89d36d6d0cd3..a899614ce829 100644
--- a/drivers/staging/fbtft/fb_ra8875.c
+++ b/drivers/staging/fbtft/fb_ra8875.c
@@ -253,7 +253,7 @@ static void write_reg8_bus8(struct fbtft_par *par, int len, ...)
 static int write_vmem16_bus8(struct fbtft_par *par, size_t offset, size_t len)
 {
 	u16 *vmem16;
-	u16 *txbuf16 = par->txbuf.buf;
+	__be16 *txbuf16 = par->txbuf.buf;
 	size_t remain;
 	size_t to_copy;
 	size_t tx_array_size;
diff --git a/drivers/staging/fbtft/fb_s6d02a1.c b/drivers/staging/fbtft/fb_s6d02a1.c
index eb712aa0d692..c12855bb6891 100644
--- a/drivers/staging/fbtft/fb_s6d02a1.c
+++ b/drivers/staging/fbtft/fb_s6d02a1.c
@@ -24,7 +24,7 @@
 
 #define DRVNAME "fb_s6d02a1"
 
-static s16 default_init_sequence[] = {
+static const s16 default_init_sequence[] = {
 
 	-1, 0xf0, 0x5a, 0x5a,
 
diff --git a/drivers/staging/fbtft/fb_sh1106.c b/drivers/staging/fbtft/fb_sh1106.c
new file mode 100644
index 000000000000..89c27a440305
--- /dev/null
+++ b/drivers/staging/fbtft/fb_sh1106.c
@@ -0,0 +1,195 @@
+/*
+ * FB driver for the SH1106 OLED Controller
+ * Based on the SSD1306 driver by Noralf Tronnes
+ *
+ * Copyright (C) 2017 Heiner Kallweit
+ *
+ * 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_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(blank=%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_DRIVER(DRVNAME, "sinowealth,sh1106", &display);
+
+MODULE_ALIAS("spi:" DRVNAME);
+MODULE_ALIAS("platform:" DRVNAME);
+MODULE_ALIAS("spi:sh1106");
+MODULE_ALIAS("platform:sh1106");
+
+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
index c603e1516e64..129e175fcd7e 100644
--- a/drivers/staging/fbtft/fb_ssd1289.c
+++ b/drivers/staging/fbtft/fb_ssd1289.c
@@ -47,7 +47,7 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, 0x0E, 0x2B00);
 	write_reg(par, 0x1E, 0x00B7);
 	write_reg(par, 0x01,
-		(1 << 13) | (par->bgr << 11) | (1 << 9) | (HEIGHT - 1));
+		BIT(13) | (par->bgr << 11) | BIT(9) | (HEIGHT - 1));
 	write_reg(par, 0x02, 0x0600);
 	write_reg(par, 0x10, 0x0000);
 	write_reg(par, 0x05, 0x0000);
diff --git a/drivers/staging/fbtft/fb_ssd1331.c b/drivers/staging/fbtft/fb_ssd1331.c
index 26f24e32d979..9aa9864fcf30 100644
--- a/drivers/staging/fbtft/fb_ssd1331.c
+++ b/drivers/staging/fbtft/fb_ssd1331.c
@@ -130,16 +130,16 @@ static int set_gamma(struct fbtft_par *par, u32 *curves)
 	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);
+				"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);
+				"Illegal value(s) in Grayscale Lookup Table. At index=%d, the accumulated value has exceeded 180\n",
+				i);
 			return -EINVAL;
 		}
 	}
diff --git a/drivers/staging/fbtft/fb_st7735r.c b/drivers/staging/fbtft/fb_st7735r.c
index 24d17cdc71ab..d98522a39344 100644
--- a/drivers/staging/fbtft/fb_st7735r.c
+++ b/drivers/staging/fbtft/fb_st7735r.c
@@ -25,7 +25,7 @@
 #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 s16 default_init_sequence[] = {
+static const s16 default_init_sequence[] = {
 	-1, MIPI_DCS_SOFT_RESET,
 	-2, 150,                               /* delay */
 
diff --git a/drivers/staging/fbtft/fb_watterott.c b/drivers/staging/fbtft/fb_watterott.c
index 429304546b44..180e5be6fa4f 100644
--- a/drivers/staging/fbtft/fb_watterott.c
+++ b/drivers/staging/fbtft/fb_watterott.c
@@ -69,8 +69,8 @@ static int write_vmem(struct fbtft_par *par, size_t offset, size_t len)
 {
 	unsigned int start_line, end_line;
 	u16 *vmem16 = (u16 *)(par->info->screen_buffer + offset);
-	u16 *pos = par->txbuf.buf + 1;
-	u16 *buf16 = par->txbuf.buf + 10;
+	__be16 *pos = par->txbuf.buf + 1;
+	__be16 *buf16 = par->txbuf.buf + 10;
 	int i, j;
 	int ret = 0;
 
@@ -106,7 +106,7 @@ static int write_vmem_8bit(struct fbtft_par *par, size_t offset, size_t len)
 {
 	unsigned int start_line, end_line;
 	u16 *vmem16 = (u16 *)(par->info->screen_buffer + offset);
-	u16 *pos = par->txbuf.buf + 1;
+	__be16 *pos = par->txbuf.buf + 1;
 	u8 *buf8 = par->txbuf.buf + 10;
 	int i, j;
 	int ret = 0;
diff --git a/drivers/staging/fbtft/fbtft-bus.c b/drivers/staging/fbtft/fbtft-bus.c
index ec45043c0830..a80b5d115ff8 100644
--- a/drivers/staging/fbtft/fbtft-bus.c
+++ b/drivers/staging/fbtft/fbtft-bus.c
@@ -36,14 +36,9 @@ void func(struct fbtft_par *par, int len, ...)                                \
 	}                                                                     \
 									      \
 	*buf = modifier((type)va_arg(args, unsigned int));                    \
-	if (par->gpio.dc != -1)                                               \
-		gpio_set_value(par->gpio.dc, 0);                              \
-	ret = par->fbtftops.write(par, par->buf, sizeof(type) + offset);      \
-	if (ret < 0) {                                                        \
-		va_end(args);                                                 \
-		dev_err(par->info->device, "%s: write() failed and returned %d\n", __func__, ret); \
-		return;                                                       \
-	}                                                                     \
+	ret = fbtft_write_buf_dc(par, par->buf, sizeof(type) + offset, 0);    \
+	if (ret < 0)							      \
+		goto out;						      \
 	len--;                                                                \
 									      \
 	if (par->startbyte)                                                   \
@@ -51,19 +46,12 @@ void func(struct fbtft_par *par, int len, ...)                                \
 									      \
 	if (len) {                                                            \
 		i = len;                                                      \
-		while (i--) {                                                 \
+		while (i--)						      \
 			*buf++ = modifier((type)va_arg(args, unsigned int));  \
-		}                                                             \
-		if (par->gpio.dc != -1)                                       \
-			gpio_set_value(par->gpio.dc, 1);                      \
-		ret = par->fbtftops.write(par, par->buf,		      \
-					  len * (sizeof(type) + offset));     \
-		if (ret < 0) {                                                \
-			va_end(args);                                         \
-			dev_err(par->info->device, "%s: write() failed and returned %d\n", __func__, ret); \
-			return;                                               \
-		}                                                             \
+		fbtft_write_buf_dc(par, par->buf,			      \
+				   len * (sizeof(type) + offset), 1);	      \
 	}                                                                     \
+out:									      \
 	va_end(args);                                                         \
 }                                                                             \
 EXPORT_SYMBOL(func);
@@ -126,7 +114,7 @@ EXPORT_SYMBOL(fbtft_write_reg8_bus9);
 int fbtft_write_vmem16_bus8(struct fbtft_par *par, size_t offset, size_t len)
 {
 	u16 *vmem16;
-	u16 *txbuf16 = par->txbuf.buf;
+	__be16 *txbuf16 = par->txbuf.buf;
 	size_t remain;
 	size_t to_copy;
 	size_t tx_array_size;
@@ -243,10 +231,7 @@ int fbtft_write_vmem16_bus16(struct fbtft_par *par, size_t offset, size_t len)
 
 	vmem16 = (u16 *)(par->info->screen_buffer + offset);
 
-	if (par->gpio.dc != -1)
-		gpio_set_value(par->gpio.dc, 1);
-
 	/* no need for buffered write with 16-bit bus */
-	return par->fbtftops.write(par, vmem16, len);
+	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
index 7c8af29cdb75..b742ee786615 100644
--- a/drivers/staging/fbtft/fbtft-core.c
+++ b/drivers/staging/fbtft/fbtft-core.c
@@ -43,8 +43,23 @@ 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;
+
+	if (gpio_is_valid(par->gpio.dc))
+		gpio_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,
-			void *buf, size_t len, const char *fmt, ...)
+		   void *buf, size_t len, const char *fmt, ...)
 {
 	va_list args;
 	static char textbuf[512];
@@ -56,7 +71,7 @@ void fbtft_dbg_hex(const struct device *dev, int groupsize,
 	va_end(args);
 
 	hex_dump_to_buffer(buf, len, 32, groupsize, text + text_len,
-				512 - text_len, false);
+			   512 - text_len, false);
 
 	if (len > 32)
 		dev_info(dev, "%s ...\n", text);
@@ -66,13 +81,13 @@ void fbtft_dbg_hex(const struct device *dev, int groupsize,
 EXPORT_SYMBOL(fbtft_dbg_hex);
 
 static unsigned long fbtft_request_gpios_match(struct fbtft_par *par,
-					const struct fbtft_gpio *gpio)
+					       const struct fbtft_gpio *gpio)
 {
 	int ret;
 	long val;
 
 	fbtft_par_dbg(DEBUG_REQUEST_GPIOS_MATCH, par, "%s('%s')\n",
-		__func__, gpio->name);
+		      __func__, gpio->name);
 
 	if (strcasecmp(gpio->name, "reset") == 0) {
 		par->gpio.reset = gpio->gpio;
@@ -141,8 +156,8 @@ static int fbtft_request_gpios(struct fbtft_par *par)
 				return ret;
 			}
 			fbtft_par_dbg(DEBUG_REQUEST_GPIOS, par,
-				"%s: '%s' = GPIO%d\n",
-				__func__, gpio->name, gpio->gpio);
+				      "%s: '%s' = GPIO%d\n",
+				      __func__, gpio->name, gpio->gpio);
 		}
 		gpio++;
 	}
@@ -175,7 +190,7 @@ static int fbtft_request_one_gpio(struct fbtft_par *par,
 		flags = (of_flags & OF_GPIO_ACTIVE_LOW) ? GPIOF_OUT_INIT_LOW :
 							GPIOF_OUT_INIT_HIGH;
 		ret = devm_gpio_request_one(dev, gpio, flags,
-						dev->driver->name);
+					    dev->driver->name);
 		if (ret) {
 			dev_err(dev,
 				"gpio_request_one('%s'=%d) failed with %d\n",
@@ -185,7 +200,7 @@ static int fbtft_request_one_gpio(struct fbtft_par *par,
 		if (gpiop)
 			*gpiop = gpio;
 		fbtft_par_dbg(DEBUG_REQUEST_GPIOS, par, "%s: '%s' = GPIO%d\n",
-							__func__, name, gpio);
+			      __func__, name, gpio);
 	}
 
 	return ret;
@@ -219,15 +234,15 @@ static int fbtft_request_gpios_dt(struct fbtft_par *par)
 		return ret;
 	for (i = 0; i < 16; i++) {
 		ret = fbtft_request_one_gpio(par, "db-gpios", i,
-						&par->gpio.db[i]);
+					     &par->gpio.db[i]);
 		if (ret)
 			return ret;
 		ret = fbtft_request_one_gpio(par, "led-gpios", i,
-						&par->gpio.led[i]);
+					     &par->gpio.led[i]);
 		if (ret)
 			return ret;
 		ret = fbtft_request_one_gpio(par, "aux-gpios", i,
-						&par->gpio.aux[i]);
+					     &par->gpio.aux[i]);
 		if (ret)
 			return ret;
 	}
@@ -282,7 +297,7 @@ void fbtft_register_backlight(struct fbtft_par *par)
 
 	if (par->gpio.led[0] == -1) {
 		fbtft_par_dbg(DEBUG_BACKLIGHT, par,
-			"%s(): led pin not set, exiting.\n", __func__);
+			      "%s(): led pin not set, exiting.\n", __func__);
 		return;
 	}
 
@@ -348,8 +363,8 @@ static void fbtft_update_display(struct fbtft_par *par, unsigned int start_line,
 	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)) {
+		    ((par->debug & DEBUG_TIME_FIRST_UPDATE) &&
+		    !par->first_update_done)) {
 			ts_start = ktime_get();
 			timeit = true;
 		}
@@ -374,7 +389,7 @@ static void fbtft_update_display(struct fbtft_par *par, unsigned int start_line,
 	}
 
 	fbtft_par_dbg(DEBUG_UPDATE_DISPLAY, par, "%s(start_line=%u, end_line=%u)\n",
-		__func__, start_line, end_line);
+		      __func__, start_line, end_line);
 
 	if (par->fbtftops.set_addr_win)
 		par->fbtftops.set_addr_win(par, 0, start_line,
@@ -402,8 +417,8 @@ static void fbtft_update_display(struct fbtft_par *par, unsigned int start_line,
 		throughput = throughput * 1000 / 1024;
 
 		dev_info(par->info->device,
-			"Display update: %ld kB/s, fps=%ld\n",
-			throughput, fps);
+			 "Display update: %ld kB/s, fps=%ld\n",
+			 throughput, fps);
 		par->first_update_done = true;
 	}
 }
@@ -556,7 +571,6 @@ static int fbtft_fb_setcolreg(unsigned int regno, unsigned int red, unsigned int
 			ret = 0;
 		}
 		break;
-
 	}
 	return ret;
 }
@@ -659,7 +673,7 @@ struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
 	unsigned int bpp = display->bpp;
 	unsigned int fps = display->fps;
 	int vmem_size, i;
-	s16 *init_sequence = display->init_sequence;
+	const s16 *init_sequence = display->init_sequence;
 	char *gamma = display->gamma;
 	u32 *gamma_curves = NULL;
 
@@ -957,16 +971,16 @@ int fbtft_register_framebuffer(struct fb_info *fb_info)
 
 	fbtft_sysfs_init(par);
 
-	if (par->txbuf.buf)
+	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);
+		 "%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);
 
 #ifdef CONFIG_FB_BACKLIGHT
 	/* Turn on backlight if available */
@@ -1049,7 +1063,7 @@ static int fbtft_init_display_dt(struct fbtft_par *par)
 			}
 			/* make debug message */
 			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
-				"init: write_register:\n");
+				      "init: write_register:\n");
 			for (j = 0; j < i; j++)
 				fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
 					      "buf[%d] = %02X\n", j, buf[j]);
@@ -1073,12 +1087,12 @@ static int fbtft_init_display_dt(struct fbtft_par *par)
 				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);
+				      "init: msleep(%u)\n", val & 0xFFFF);
 			msleep(val & 0xFFFF);
 			p = of_prop_next_u32(prop, p, &val);
 		} else {
 			dev_err(par->info->device, "illegal init value 0x%X\n",
-									val);
+				val);
 			return -EINVAL;
 		}
 	}
@@ -1153,8 +1167,8 @@ int fbtft_init_display(struct fbtft_par *par)
 				j++;
 			}
 			fbtft_par_dbg(DEBUG_INIT_DISPLAY, par,
-				"init: write(0x%02X) %s\n",
-				par->init_sequence[i], msg);
+				      "init: write(0x%02X) %s\n",
+				      par->init_sequence[i], msg);
 
 			/* Write */
 			j = 0;
@@ -1447,7 +1461,7 @@ int fbtft_remove_common(struct device *dev, struct fb_info *info)
 	par = info->par;
 	if (par)
 		fbtft_par_dbg(DEBUG_DRIVER_INIT_FUNCTIONS, par,
-			"%s()\n", __func__);
+			      "%s()\n", __func__);
 	fbtft_unregister_framebuffer(info);
 	fbtft_framebuffer_release(info);
 
diff --git a/drivers/staging/fbtft/fbtft-sysfs.c b/drivers/staging/fbtft/fbtft-sysfs.c
index 6b6fbaa794f4..5bfd67b526b5 100644
--- a/drivers/staging/fbtft/fbtft-sysfs.c
+++ b/drivers/staging/fbtft/fbtft-sysfs.c
@@ -17,7 +17,7 @@ static int get_next_ulong(char **str_p, unsigned long *val, char *sep, int base)
 }
 
 int fbtft_gamma_parse_str(struct fbtft_par *par, u32 *curves,
-						const char *str, int size)
+			  const char *str, int size)
 {
 	char *str_p, *curve_p = NULL;
 	char *tmp;
@@ -107,8 +107,8 @@ sprintf_gamma(struct fbtft_par *par, u32 *curves, char *buf)
 }
 
 static ssize_t store_gamma_curve(struct device *device,
-					struct device_attribute *attr,
-					const char *buf, size_t count)
+				 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;
@@ -125,7 +125,7 @@ static ssize_t store_gamma_curve(struct device *device,
 
 	mutex_lock(&par->gamma.lock);
 	memcpy(par->gamma.curves, tmp_curves,
-		par->gamma.num_curves * par->gamma.num_values * sizeof(tmp_curves[0]));
+	       par->gamma.num_curves * par->gamma.num_values * sizeof(tmp_curves[0]));
 	mutex_unlock(&par->gamma.lock);
 
 	return count;
@@ -172,8 +172,8 @@ void fbtft_expand_debug_value(unsigned long *debug)
 }
 
 static ssize_t store_debug(struct device *device,
-				struct device_attribute *attr,
-				const char *buf, size_t count)
+			   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;
@@ -188,7 +188,7 @@ static ssize_t store_debug(struct device *device,
 }
 
 static ssize_t show_debug(struct device *device,
-				struct device_attribute *attr, char *buf)
+			  struct device_attribute *attr, char *buf)
 {
 	struct fb_info *fb_info = dev_get_drvdata(device);
 	struct fbtft_par *par = fb_info->par;
@@ -196,7 +196,7 @@ static ssize_t show_debug(struct device *device,
 	return snprintf(buf, PAGE_SIZE, "%lu\n", par->debug);
 }
 
-static struct device_attribute debug_device_attr = \
+static struct device_attribute debug_device_attr =
 	__ATTR(debug, 0660, show_debug, store_debug);
 
 void fbtft_sysfs_init(struct fbtft_par *par)
diff --git a/drivers/staging/fbtft/fbtft.h b/drivers/staging/fbtft/fbtft.h
index 44cf94d160d4..488ab788138e 100644
--- a/drivers/staging/fbtft/fbtft.h
+++ b/drivers/staging/fbtft/fbtft.h
@@ -124,7 +124,7 @@ struct fbtft_display {
 	unsigned int bpp;
 	unsigned int fps;
 	int txbuflen;
-	s16 *init_sequence;
+	const s16 *init_sequence;
 	char *gamma;
 	int gamma_num;
 	int gamma_len;
@@ -228,7 +228,7 @@ struct fbtft_par {
 		int led[16];
 		int aux[16];
 	} gpio;
-	s16 *init_sequence;
+	const s16 *init_sequence;
 	struct {
 		struct mutex lock;
 		u32 *curves;
@@ -248,6 +248,7 @@ struct fbtft_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);
 void fbtft_dbg_hex(const struct device *dev, int groupsize,
 		   void *buf, size_t len, const char *fmt, ...);
 struct fb_info *fbtft_framebuffer_alloc(struct fbtft_display *display,
diff --git a/drivers/staging/fbtft/fbtft_device.c b/drivers/staging/fbtft/fbtft_device.c
index 9ffb9cecc465..0d974738c1c4 100644
--- a/drivers/staging/fbtft/fbtft_device.c
+++ b/drivers/staging/fbtft/fbtft_device.c
@@ -131,7 +131,7 @@ static void adafruit18_green_tab_set_addr_win(struct fbtft_par *par,
 		"D0 00 14 15 13 2C 42 43 4E 09 16 14 18 21\n" \
 		"D0 00 14 15 13 0B 43 55 53 0C 17 14 23 20"
 
-static s16 cberry28_init_sequence[] = {
+static const s16 cberry28_init_sequence[] = {
 	/* turn off sleep mode */
 	-1, MIPI_DCS_EXIT_SLEEP_MODE,
 	-2, 120,
@@ -180,7 +180,7 @@ static s16 cberry28_init_sequence[] = {
 	-3,
 };
 
-static s16 hy28b_init_sequence[] = {
+static const s16 hy28b_init_sequence[] = {
 	-1, 0x00e7, 0x0010, -1, 0x0000, 0x0001,
 	-1, 0x0001, 0x0100, -1, 0x0002, 0x0700,
 	-1, 0x0003, 0x1030, -1, 0x0004, 0x0000,
@@ -211,7 +211,7 @@ static s16 hy28b_init_sequence[] = {
 	"04 1F 4 7 7 0 7 7 6 0\n" \
 	"0F 00 1 7 4 0 0 0 6 7"
 
-static s16 pitft_init_sequence[] = {
+static const s16 pitft_init_sequence[] = {
 	-1, MIPI_DCS_SOFT_RESET,
 	-2, 5,
 	-1, MIPI_DCS_SET_DISPLAY_OFF,
@@ -242,7 +242,7 @@ static s16 pitft_init_sequence[] = {
 	-3
 };
 
-static s16 waveshare32b_init_sequence[] = {
+static const s16 waveshare32b_init_sequence[] = {
 	-1, 0xCB, 0x39, 0x2C, 0x00, 0x34, 0x02,
 	-1, 0xCF, 0x00, 0xC1, 0x30,
 	-1, 0xE8, 0x85, 0x00, 0x78,
@@ -1448,11 +1448,10 @@ static int __init fbtft_device_init(void)
 	if (fbtft_device_param_gpios[0].name[0])
 		gpio = fbtft_device_param_gpios;
 
-	if (verbose > 2)
+	if (verbose > 2) {
 		pr_spi_devices(); /* print list of registered SPI devices */
-
-	if (verbose > 2)
 		pr_p_devices(); /* print list of 'fb' platform devices */
+	}
 
 	pr_debug("name='%s', busnum=%d, cs=%d\n", name, busnum, cs);
 
@@ -1483,13 +1482,19 @@ static int __init fbtft_device_init(void)
 			displays[i].pdev->name = name;
 			displays[i].spi = NULL;
 		} else {
-			strncpy(displays[i].spi->modalias, name, SPI_NAME_SIZE);
+			size_t len;
+
+			len = strlcpy(displays[i].spi->modalias, name,
+				SPI_NAME_SIZE);
+			if (len >= SPI_NAME_SIZE)
+				pr_warn("modalias (name) truncated to: %s\n",
+					displays[i].spi->modalias);
 			displays[i].pdev = NULL;
 		}
 	}
 
 	for (i = 0; i < ARRAY_SIZE(displays); i++) {
-		if (strncmp(name, displays[i].name, 32) == 0) {
+		if (strncmp(name, displays[i].name, SPI_NAME_SIZE) == 0) {
 			if (displays[i].spi) {
 				spi = displays[i].spi;
 				spi->chip_select = cs;
diff --git a/drivers/staging/fbtft/flexfb.c b/drivers/staging/fbtft/flexfb.c
index af8422e18780..7134624a16c2 100644
--- a/drivers/staging/fbtft/flexfb.c
+++ b/drivers/staging/fbtft/flexfb.c
@@ -63,11 +63,11 @@ static bool latched;
 module_param(latched, bool, 0000);
 MODULE_PARM_DESC(latched, "Use with latched 16-bit databus");
 
-static s16 *initp;
+static const s16 *initp;
 static int initp_num;
 
 /* default init sequences */
-static s16 st7735r_init[] = {
+static const s16 st7735r_init[] = {
 	-1, 0x01,
 	-2, 150,
 	-1, 0x11,
@@ -96,7 +96,7 @@ static s16 st7735r_init[] = {
 	-3
 };
 
-static s16 ssd1289_init[] = {
+static const s16 ssd1289_init[] = {
 	-1, 0x00, 0x0001,
 	-1, 0x03, 0xA8A4,
 	-1, 0x0C, 0x0000,
@@ -142,7 +142,7 @@ static s16 ssd1289_init[] = {
 	-3
 };
 
-static s16 hx8340bn_init[] = {
+static const s16 hx8340bn_init[] = {
 	-1, 0xC1, 0xFF, 0x83, 0x40,
 	-1, 0x11,
 	-2, 150,
@@ -162,7 +162,7 @@ static s16 hx8340bn_init[] = {
 	-3
 };
 
-static s16 ili9225_init[] = {
+static const s16 ili9225_init[] = {
 	-1, 0x0001, 0x011C,
 	-1, 0x0002, 0x0100,
 	-1, 0x0003, 0x1030,
@@ -204,7 +204,7 @@ static s16 ili9225_init[] = {
 	-3
 };
 
-static s16 ili9320_init[] = {
+static const s16 ili9320_init[] = {
 	-1, 0x00E5, 0x8000,
 	-1, 0x0000, 0x0001,
 	-1, 0x0001, 0x0100,
@@ -265,7 +265,7 @@ static s16 ili9320_init[] = {
 	-3
 };
 
-static s16 ili9325_init[] = {
+static const s16 ili9325_init[] = {
 	-1, 0x00E3, 0x3008,
 	-1, 0x00E7, 0x0012,
 	-1, 0x00EF, 0x1231,
@@ -324,7 +324,7 @@ static s16 ili9325_init[] = {
 	-3
 };
 
-static s16 ili9341_init[] = {
+static const s16 ili9341_init[] = {
 	-1, 0x28,
 	-2, 20,
 	-1, 0xCF, 0x00, 0x83, 0x30,
@@ -349,7 +349,7 @@ static s16 ili9341_init[] = {
 	-3
 };
 
-static s16 ssd1351_init[] = {
+static const s16 ssd1351_init[] = {
 	-1, 0xfd, 0x12,
 	-1, 0xfd, 0xb1,
 	-1, 0xae,
@@ -390,7 +390,7 @@ struct flexfb_lcd_controller {
 	unsigned int height;
 	unsigned int setaddrwin;
 	unsigned int regwidth;
-	s16 *init_seq;
+	const s16 *init_seq;
 	int init_seq_sz;
 };
 
diff --git a/drivers/staging/fsl-dpaa2/Kconfig b/drivers/staging/fsl-dpaa2/Kconfig
new file mode 100644
index 000000000000..2e325cb747ae
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/Kconfig
@@ -0,0 +1,17 @@
+#
+# Freescale DataPath Acceleration Architecture Gen2 (DPAA2) drivers
+#
+
+config FSL_DPAA2
+	bool "Freescale DPAA2 devices"
+	depends on FSL_MC_BUS
+	---help---
+	  Build drivers for Freescale DataPath Acceleration
+	  Architecture (DPAA2) family of SoCs.
+
+config FSL_DPAA2_ETH
+	tristate "Freescale DPAA2 Ethernet"
+	depends on FSL_DPAA2 && FSL_MC_DPIO
+	---help---
+	  Ethernet driver for Freescale DPAA2 SoCs, using the
+	  Freescale MC bus driver
diff --git a/drivers/staging/fsl-dpaa2/Makefile b/drivers/staging/fsl-dpaa2/Makefile
new file mode 100644
index 000000000000..0836ba8977b1
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/Makefile
@@ -0,0 +1,5 @@
+#
+# Freescale DataPath Acceleration Architecture Gen2 (DPAA2) drivers
+#
+
+obj-$(CONFIG_FSL_DPAA2_ETH)	+= ethernet/
diff --git a/drivers/staging/fsl-dpaa2/ethernet/Makefile b/drivers/staging/fsl-dpaa2/ethernet/Makefile
new file mode 100644
index 000000000000..77b0b74f835a
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the Freescale DPAA2 Ethernet controller
+#
+
+obj-$(CONFIG_FSL_DPAA2_ETH) += fsl-dpaa2-eth.o
+
+fsl-dpaa2-eth-objs    := dpaa2-eth.o dpaa2-ethtool.o dpni.o
+
+# Needed by the tracing framework
+CFLAGS_dpaa2-eth.o := -I$(src)
diff --git a/drivers/staging/fsl-dpaa2/ethernet/README b/drivers/staging/fsl-dpaa2/ethernet/README
new file mode 100644
index 000000000000..410952ecf657
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/README
@@ -0,0 +1,186 @@
+Freescale DPAA2 Ethernet driver
+===============================
+
+This file provides documentation for the Freescale DPAA2 Ethernet driver.
+
+
+Contents
+========
+	Supported Platforms
+	Architecture Overview
+	Creating a Network Interface
+	Features & Offloads
+
+
+Supported Platforms
+===================
+This driver provides networking support for Freescale DPAA2 SoCs, e.g.
+LS2080A, LS2088A, LS1088A.
+
+
+Architecture Overview
+=====================
+Unlike regular NICs, in the DPAA2 architecture there is no single hardware block
+representing network interfaces; instead, several separate hardware resources
+concur to provide the networking functionality:
+        - network interfaces
+        - queues, channels
+        - buffer pools
+        - MAC/PHY
+
+All hardware resources are allocated and configured through the Management
+Complex (MC) portals. MC abstracts most of these resources as DPAA2 objects
+and exposes ABIs through which they can be configured and controlled. A few
+hardware resources, like queues, do not have a corresponding MC object and
+are treated as internal resources of other objects.
+
+For a more detailed description of the DPAA2 architecture and its object
+abstractions see:
+	drivers/staging/fsl-mc/README.txt
+
+Each Linux net device is built on top of a Datapath Network Interface (DPNI)
+object and uses Buffer Pools (DPBPs), I/O Portals (DPIOs) and Concentrators
+(DPCONs).
+
+Configuration interface:
+
+                 -----------------------
+                | DPAA2 Ethernet Driver |
+                 -----------------------
+                     .      .      .
+                     .      .      .
+             . . . . .      .      . . . . . .
+             .              .                .
+             .              .                .
+         ----------     ----------      -----------
+        | DPBP API |   | DPNI API |    | DPCON API |
+         ----------     ----------      -----------
+             .              .                .             software
+===========  .  ==========  .  ============  .  ===================
+             .              .                .             hardware
+         ------------------------------------------
+        |            MC hardware portals           |
+         ------------------------------------------
+             .              .                .
+             .              .                .
+          ------         ------            -------
+         | DPBP |       | DPNI |          | DPCON |
+          ------         ------            -------
+
+The DPNIs are network interfaces without a direct one-on-one mapping to PHYs.
+DPBPs represent hardware buffer pools. Packet I/O is performed in the context
+of DPCON objects, using DPIO portals for managing and communicating with the
+hardware resources.
+
+Datapath (I/O) interface:
+
+         -----------------------------------------------
+        |           DPAA2 Ethernet Driver               |
+          -----------------------------------------------
+          |          ^        ^         |            |
+          |          |        |         |            |
+   enqueue|   dequeue|   data |  dequeue|       seed |
+    (Tx)  | (Rx, TxC)|  avail.|  request|     buffers|
+          |          |  notify|         |            |
+          |          |        |         |            |
+          V          |        |         V            V
+         -----------------------------------------------
+        |                 DPIO Driver                   |
+         -----------------------------------------------
+          |          |        |         |            |          software
+          |          |        |         |            |  ================
+          |          |        |         |            |          hardware
+         -----------------------------------------------
+        |               I/O hardware portals            |
+         -----------------------------------------------
+          |          ^        ^         |            |
+          |          |        |         |            |
+          |          |        |         V            |
+          V          |    ================           V
+        ----------------------           |      -------------
+ queues  ----------------------          |     | Buffer pool |
+          ----------------------         |      -------------
+                   =======================
+                                Channel
+
+Datapath I/O (DPIO) portals provide enqueue and dequeue services, data
+availability notifications and buffer pool management. DPIOs are shared between
+all DPAA2 objects (and implicitly all DPAA2 kernel drivers) that work with data
+frames, but must be affine to the CPUs for the purpose of traffic distribution.
+
+Frames are transmitted and received through hardware frame queues, which can be
+grouped in channels for the purpose of hardware scheduling. The Ethernet driver
+enqueues TX frames on egress queues and after transmission is complete a TX
+confirmation frame is sent back to the CPU.
+
+When frames are available on ingress queues, a data availability notification
+is sent to the CPU; notifications are raised per channel, so even if multiple
+queues in the same channel have available frames, only one notification is sent.
+After a channel fires a notification, is must be explicitly rearmed.
+
+Each network interface can have multiple Rx, Tx and confirmation queues affined
+to CPUs, and one channel (DPCON) for each CPU that services at least one queue.
+DPCONs are used to distribute ingress traffic to different CPUs via the cores'
+affine DPIOs.
+
+The role of hardware buffer pools is storage of ingress frame data. Each network
+interface has a privately owned buffer pool which it seeds with kernel allocated
+buffers.
+
+
+DPNIs are decoupled from PHYs; a DPNI can be connected to a PHY through a DPMAC
+object or to another DPNI through an internal link, but the connection is
+managed by MC and completely transparent to the Ethernet driver.
+
+     ---------     ---------     ---------
+    | eth if1 |   | eth if2 |   | eth ifn |
+     ---------     ---------     ---------
+          .           .          .
+          .           .          .
+          .           .          .
+         ---------------------------
+        |   DPAA2 Ethernet Driver   |
+         ---------------------------
+          .           .          .
+          .           .          .
+          .           .          .
+       ------      ------      ------            -------
+      | DPNI |    | DPNI |    | DPNI |          | DPMAC |----+
+       ------      ------      ------            -------     |
+         |           |           |                  |        |
+         |           |           |                  |      -----
+          ===========             ==================      | PHY |
+                                                           -----
+
+Creating a Network Interface
+============================
+A net device is created for each DPNI object probed on the MC bus. Each DPNI has
+a number of properties which determine the network interface configuration
+options and associated hardware resources.
+
+DPNI objects (and the other DPAA2 objects needed for a network interface) can be
+added to a container on the MC bus in one of two ways: statically, through a
+Datapath Layout Binary file (DPL) that is parsed by MC at boot time; or created
+dynamically at runtime, via the DPAA2 objects APIs.
+
+
+Features & Offloads
+===================
+Hardware checksum offloading is supported for TCP and UDP over IPv4/6 frames.
+The checksum offloads can be independently configured on RX and TX through
+ethtool.
+
+Hardware offload of unicast and multicast MAC filtering is supported on the
+ingress path and permanently enabled.
+
+Scatter-gather frames are supported on both RX and TX paths. On TX, SG support
+is configurable via ethtool; on RX it is always enabled.
+
+The DPAA2 hardware can process jumbo Ethernet frames of up to 10K bytes.
+
+The Ethernet driver defines a static flow hashing scheme that distributes
+traffic based on a 5-tuple key: src IP, dst IP, IP proto, L4 src port,
+L4 dst port. No user configuration is supported for now.
+
+Hardware specific statistics for the network interface as well as some
+non-standard driver stats can be consulted through ethtool -S option.
diff --git a/drivers/staging/fsl-dpaa2/ethernet/TODO b/drivers/staging/fsl-dpaa2/ethernet/TODO
new file mode 100644
index 000000000000..e400a5e427a5
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/TODO
@@ -0,0 +1,18 @@
+* Add a DPAA2 MAC kernel driver in order to allow PHY management; currently
+  the DPMAC objects and their link to DPNIs are handled by MC internally
+  and all PHYs are seen as fixed-link
+* add more debug support: decide how to expose detailed debug statistics,
+  add ingress error queue support
+* MC firmware uprev; the DPAA2 objects used by the Ethernet driver need to
+  be kept in sync with binary interface changes in MC
+* refine README file
+* cleanup
+
+NOTE: None of the above is must-have before getting the DPAA2 Ethernet driver
+out of staging. The main requirement for that is to have the drivers it
+depends on, fsl-mc bus and DPIO driver, moved to drivers/bus and drivers/soc
+respectively.
+
+ Please send any patches to Greg Kroah-Hartman <gregkh@linuxfoundation.org>,
+ ruxandra.radulescu@nxp.com, devel@driverdev.osuosl.org,
+ linux-kernel@vger.kernel.org
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth-trace.h b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth-trace.h
new file mode 100644
index 000000000000..3b040e8d6a4e
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth-trace.h
@@ -0,0 +1,185 @@
+/* Copyright 2014-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM	dpaa2_eth
+
+#if !defined(_DPAA2_ETH_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _DPAA2_ETH_TRACE_H
+
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include "dpaa2-eth.h"
+#include <linux/tracepoint.h>
+
+#define TR_FMT "[%s] fd: addr=0x%llx, len=%u, off=%u"
+/* trace_printk format for raw buffer event class */
+#define TR_BUF_FMT "[%s] vaddr=%p size=%zu dma_addr=%pad map_size=%zu bpid=%d"
+
+/* This is used to declare a class of events.
+ * individual events of this type will be defined below.
+ */
+
+/* Store details about a frame descriptor */
+DECLARE_EVENT_CLASS(dpaa2_eth_fd,
+		    /* Trace function prototype */
+		    TP_PROTO(struct net_device *netdev,
+			     const struct dpaa2_fd *fd),
+
+		    /* Repeat argument list here */
+		    TP_ARGS(netdev, fd),
+
+		    /* A structure containing the relevant information we want
+		     * to record. Declare name and type for each normal element,
+		     * name, type and size for arrays. Use __string for variable
+		     * length strings.
+		     */
+		    TP_STRUCT__entry(
+				     __field(u64, fd_addr)
+				     __field(u32, fd_len)
+				     __field(u16, fd_offset)
+				     __string(name, netdev->name)
+		    ),
+
+		    /* The function that assigns values to the above declared
+		     * fields
+		     */
+		    TP_fast_assign(
+				   __entry->fd_addr = dpaa2_fd_get_addr(fd);
+				   __entry->fd_len = dpaa2_fd_get_len(fd);
+				   __entry->fd_offset = dpaa2_fd_get_offset(fd);
+				   __assign_str(name, netdev->name);
+		    ),
+
+		    /* This is what gets printed when the trace event is
+		     * triggered.
+		     */
+		    TP_printk(TR_FMT,
+			      __get_str(name),
+			      __entry->fd_addr,
+			      __entry->fd_len,
+			      __entry->fd_offset)
+);
+
+/* Now declare events of the above type. Format is:
+ * DEFINE_EVENT(class, name, proto, args), with proto and args same as for class
+ */
+
+/* Tx (egress) fd */
+DEFINE_EVENT(dpaa2_eth_fd, dpaa2_tx_fd,
+	     TP_PROTO(struct net_device *netdev,
+		      const struct dpaa2_fd *fd),
+
+	     TP_ARGS(netdev, fd)
+);
+
+/* Rx fd */
+DEFINE_EVENT(dpaa2_eth_fd, dpaa2_rx_fd,
+	     TP_PROTO(struct net_device *netdev,
+		      const struct dpaa2_fd *fd),
+
+	     TP_ARGS(netdev, fd)
+);
+
+/* Tx confirmation fd */
+DEFINE_EVENT(dpaa2_eth_fd, dpaa2_tx_conf_fd,
+	     TP_PROTO(struct net_device *netdev,
+		      const struct dpaa2_fd *fd),
+
+	     TP_ARGS(netdev, fd)
+);
+
+/* Log data about raw buffers. Useful for tracing DPBP content. */
+TRACE_EVENT(dpaa2_eth_buf_seed,
+	    /* Trace function prototype */
+	    TP_PROTO(struct net_device *netdev,
+		     /* virtual address and size */
+		     void *vaddr,
+		     size_t size,
+		     /* dma map address and size */
+		     dma_addr_t dma_addr,
+		     size_t map_size,
+		     /* buffer pool id, if relevant */
+		     u16 bpid),
+
+	    /* Repeat argument list here */
+	    TP_ARGS(netdev, vaddr, size, dma_addr, map_size, bpid),
+
+	    /* A structure containing the relevant information we want
+	     * to record. Declare name and type for each normal element,
+	     * name, type and size for arrays. Use __string for variable
+	     * length strings.
+	     */
+	    TP_STRUCT__entry(
+			     __field(void *, vaddr)
+			     __field(size_t, size)
+			     __field(dma_addr_t, dma_addr)
+			     __field(size_t, map_size)
+			     __field(u16, bpid)
+			     __string(name, netdev->name)
+	    ),
+
+	    /* The function that assigns values to the above declared
+	     * fields
+	     */
+	    TP_fast_assign(
+			   __entry->vaddr = vaddr;
+			   __entry->size = size;
+			   __entry->dma_addr = dma_addr;
+			   __entry->map_size = map_size;
+			   __entry->bpid = bpid;
+			   __assign_str(name, netdev->name);
+	    ),
+
+	    /* This is what gets printed when the trace event is
+	     * triggered.
+	     */
+	    TP_printk(TR_BUF_FMT,
+		      __get_str(name),
+		      __entry->vaddr,
+		      __entry->size,
+		      &__entry->dma_addr,
+		      __entry->map_size,
+		      __entry->bpid)
+);
+
+/* If only one event of a certain type needs to be declared, use TRACE_EVENT().
+ * The syntax is the same as for DECLARE_EVENT_CLASS().
+ */
+
+#endif /* _DPAA2_ETH_TRACE_H */
+
+/* This must be outside ifdef _DPAA2_ETH_TRACE_H */
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE	dpaa2-eth-trace
+#include <trace/define_trace.h>
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c
new file mode 100644
index 000000000000..6f9eed66c64d
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.c
@@ -0,0 +1,2520 @@
+/* Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016-2017 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/etherdevice.h>
+#include <linux/of_net.h>
+#include <linux/interrupt.h>
+#include <linux/msi.h>
+#include <linux/kthread.h>
+
+#include "../../fsl-mc/include/mc.h"
+#include "../../fsl-mc/include/mc-sys.h"
+#include "dpaa2-eth.h"
+
+/* CREATE_TRACE_POINTS only needs to be defined once. Other dpa files
+ * using trace events only need to #include <trace/events/sched.h>
+ */
+#define CREATE_TRACE_POINTS
+#include "dpaa2-eth-trace.h"
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Freescale Semiconductor, Inc");
+MODULE_DESCRIPTION("Freescale DPAA2 Ethernet Driver");
+
+const char dpaa2_eth_drv_version[] = "0.1";
+
+static void validate_rx_csum(struct dpaa2_eth_priv *priv,
+			     u32 fd_status,
+			     struct sk_buff *skb)
+{
+	skb_checksum_none_assert(skb);
+
+	/* HW checksum validation is disabled, nothing to do here */
+	if (!(priv->net_dev->features & NETIF_F_RXCSUM))
+		return;
+
+	/* Read checksum validation bits */
+	if (!((fd_status & DPAA2_FAS_L3CV) &&
+	      (fd_status & DPAA2_FAS_L4CV)))
+		return;
+
+	/* Inform the stack there's no need to compute L3/L4 csum anymore */
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+}
+
+/* Free a received FD.
+ * Not to be used for Tx conf FDs or on any other paths.
+ */
+static void free_rx_fd(struct dpaa2_eth_priv *priv,
+		       const struct dpaa2_fd *fd,
+		       void *vaddr)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	dma_addr_t addr = dpaa2_fd_get_addr(fd);
+	u8 fd_format = dpaa2_fd_get_format(fd);
+	struct dpaa2_sg_entry *sgt;
+	void *sg_vaddr;
+	int i;
+
+	/* If single buffer frame, just free the data buffer */
+	if (fd_format == dpaa2_fd_single)
+		goto free_buf;
+	else if (fd_format != dpaa2_fd_sg)
+		/* We don't support any other format */
+		return;
+
+	/* For S/G frames, we first need to free all SG entries */
+	sgt = vaddr + dpaa2_fd_get_offset(fd);
+	for (i = 0; i < DPAA2_ETH_MAX_SG_ENTRIES; i++) {
+		addr = dpaa2_sg_get_addr(&sgt[i]);
+		dma_unmap_single(dev, addr, DPAA2_ETH_RX_BUF_SIZE,
+				 DMA_FROM_DEVICE);
+
+		sg_vaddr = phys_to_virt(addr);
+		skb_free_frag(sg_vaddr);
+
+		if (dpaa2_sg_is_final(&sgt[i]))
+			break;
+	}
+
+free_buf:
+	skb_free_frag(vaddr);
+}
+
+/* Build a linear skb based on a single-buffer frame descriptor */
+static struct sk_buff *build_linear_skb(struct dpaa2_eth_priv *priv,
+					struct dpaa2_eth_channel *ch,
+					const struct dpaa2_fd *fd,
+					void *fd_vaddr)
+{
+	struct sk_buff *skb = NULL;
+	u16 fd_offset = dpaa2_fd_get_offset(fd);
+	u32 fd_length = dpaa2_fd_get_len(fd);
+
+	skb = build_skb(fd_vaddr, DPAA2_ETH_RX_BUF_SIZE +
+			SKB_DATA_ALIGN(sizeof(struct skb_shared_info)));
+	if (unlikely(!skb))
+		return NULL;
+
+	skb_reserve(skb, fd_offset);
+	skb_put(skb, fd_length);
+
+	ch->buf_count--;
+
+	return skb;
+}
+
+/* Build a non linear (fragmented) skb based on a S/G table */
+static struct sk_buff *build_frag_skb(struct dpaa2_eth_priv *priv,
+				      struct dpaa2_eth_channel *ch,
+				      struct dpaa2_sg_entry *sgt)
+{
+	struct sk_buff *skb = NULL;
+	struct device *dev = priv->net_dev->dev.parent;
+	void *sg_vaddr;
+	dma_addr_t sg_addr;
+	u16 sg_offset;
+	u32 sg_length;
+	struct page *page, *head_page;
+	int page_offset;
+	int i;
+
+	for (i = 0; i < DPAA2_ETH_MAX_SG_ENTRIES; i++) {
+		struct dpaa2_sg_entry *sge = &sgt[i];
+
+		/* NOTE: We only support SG entries in dpaa2_sg_single format,
+		 * but this is the only format we may receive from HW anyway
+		 */
+
+		/* Get the address and length from the S/G entry */
+		sg_addr = dpaa2_sg_get_addr(sge);
+		dma_unmap_single(dev, sg_addr, DPAA2_ETH_RX_BUF_SIZE,
+				 DMA_FROM_DEVICE);
+
+		sg_vaddr = phys_to_virt(sg_addr);
+		sg_length = dpaa2_sg_get_len(sge);
+
+		if (i == 0) {
+			/* We build the skb around the first data buffer */
+			skb = build_skb(sg_vaddr, DPAA2_ETH_RX_BUF_SIZE +
+				SKB_DATA_ALIGN(sizeof(struct skb_shared_info)));
+			if (unlikely(!skb))
+				return NULL;
+
+			sg_offset = dpaa2_sg_get_offset(sge);
+			skb_reserve(skb, sg_offset);
+			skb_put(skb, sg_length);
+		} else {
+			/* Rest of the data buffers are stored as skb frags */
+			page = virt_to_page(sg_vaddr);
+			head_page = virt_to_head_page(sg_vaddr);
+
+			/* Offset in page (which may be compound).
+			 * Data in subsequent SG entries is stored from the
+			 * beginning of the buffer, so we don't need to add the
+			 * sg_offset.
+			 */
+			page_offset = ((unsigned long)sg_vaddr &
+				(PAGE_SIZE - 1)) +
+				(page_address(page) - page_address(head_page));
+
+			skb_add_rx_frag(skb, i - 1, head_page, page_offset,
+					sg_length, DPAA2_ETH_RX_BUF_SIZE);
+		}
+
+		if (dpaa2_sg_is_final(sge))
+			break;
+	}
+
+	/* Count all data buffers + SG table buffer */
+	ch->buf_count -= i + 2;
+
+	return skb;
+}
+
+/* Main Rx frame processing routine */
+static void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
+			 struct dpaa2_eth_channel *ch,
+			 const struct dpaa2_fd *fd,
+			 struct napi_struct *napi)
+{
+	dma_addr_t addr = dpaa2_fd_get_addr(fd);
+	u8 fd_format = dpaa2_fd_get_format(fd);
+	void *vaddr;
+	struct sk_buff *skb;
+	struct rtnl_link_stats64 *percpu_stats;
+	struct dpaa2_eth_drv_stats *percpu_extras;
+	struct device *dev = priv->net_dev->dev.parent;
+	struct dpaa2_fas *fas;
+	u32 status = 0;
+
+	/* Tracing point */
+	trace_dpaa2_rx_fd(priv->net_dev, fd);
+
+	dma_unmap_single(dev, addr, DPAA2_ETH_RX_BUF_SIZE, DMA_FROM_DEVICE);
+	vaddr = phys_to_virt(addr);
+
+	prefetch(vaddr + priv->buf_layout.private_data_size);
+	prefetch(vaddr + dpaa2_fd_get_offset(fd));
+
+	percpu_stats = this_cpu_ptr(priv->percpu_stats);
+	percpu_extras = this_cpu_ptr(priv->percpu_extras);
+
+	if (fd_format == dpaa2_fd_single) {
+		skb = build_linear_skb(priv, ch, fd, vaddr);
+	} else if (fd_format == dpaa2_fd_sg) {
+		struct dpaa2_sg_entry *sgt =
+				vaddr + dpaa2_fd_get_offset(fd);
+		skb = build_frag_skb(priv, ch, sgt);
+		skb_free_frag(vaddr);
+		percpu_extras->rx_sg_frames++;
+		percpu_extras->rx_sg_bytes += dpaa2_fd_get_len(fd);
+	} else {
+		/* We don't support any other format */
+		goto err_frame_format;
+	}
+
+	if (unlikely(!skb))
+		goto err_build_skb;
+
+	prefetch(skb->data);
+
+	/* Check if we need to validate the L4 csum */
+	if (likely(dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV)) {
+		fas = (struct dpaa2_fas *)
+				(vaddr + priv->buf_layout.private_data_size);
+		status = le32_to_cpu(fas->status);
+		validate_rx_csum(priv, status, skb);
+	}
+
+	skb->protocol = eth_type_trans(skb, priv->net_dev);
+
+	percpu_stats->rx_packets++;
+	percpu_stats->rx_bytes += dpaa2_fd_get_len(fd);
+
+	if (priv->net_dev->features & NETIF_F_GRO)
+		napi_gro_receive(napi, skb);
+	else
+		netif_receive_skb(skb);
+
+	return;
+
+err_build_skb:
+	free_rx_fd(priv, fd, vaddr);
+err_frame_format:
+	percpu_stats->rx_dropped++;
+}
+
+/* Consume all frames pull-dequeued into the store. This is the simplest way to
+ * make sure we don't accidentally issue another volatile dequeue which would
+ * overwrite (leak) frames already in the store.
+ *
+ * Observance of NAPI budget is not our concern, leaving that to the caller.
+ */
+static int consume_frames(struct dpaa2_eth_channel *ch)
+{
+	struct dpaa2_eth_priv *priv = ch->priv;
+	struct dpaa2_eth_fq *fq;
+	struct dpaa2_dq *dq;
+	const struct dpaa2_fd *fd;
+	int cleaned = 0;
+	int is_last;
+
+	do {
+		dq = dpaa2_io_store_next(ch->store, &is_last);
+		if (unlikely(!dq)) {
+			/* If we're here, we *must* have placed a
+			 * volatile dequeue comnmand, so keep reading through
+			 * the store until we get some sort of valid response
+			 * token (either a valid frame or an "empty dequeue")
+			 */
+			continue;
+		}
+
+		fd = dpaa2_dq_fd(dq);
+		fq = (struct dpaa2_eth_fq *)dpaa2_dq_fqd_ctx(dq);
+		fq->stats.frames++;
+
+		fq->consume(priv, ch, fd, &ch->napi);
+		cleaned++;
+	} while (!is_last);
+
+	return cleaned;
+}
+
+/* Create a frame descriptor based on a fragmented skb */
+static int build_sg_fd(struct dpaa2_eth_priv *priv,
+		       struct sk_buff *skb,
+		       struct dpaa2_fd *fd)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	void *sgt_buf = NULL;
+	void *hwa;
+	dma_addr_t addr;
+	int nr_frags = skb_shinfo(skb)->nr_frags;
+	struct dpaa2_sg_entry *sgt;
+	int i, err;
+	int sgt_buf_size;
+	struct scatterlist *scl, *crt_scl;
+	int num_sg;
+	int num_dma_bufs;
+	struct dpaa2_eth_swa *swa;
+
+	/* Create and map scatterlist.
+	 * We don't advertise NETIF_F_FRAGLIST, so skb_to_sgvec() will not have
+	 * to go beyond nr_frags+1.
+	 * Note: We don't support chained scatterlists
+	 */
+	if (unlikely(PAGE_SIZE / sizeof(struct scatterlist) < nr_frags + 1))
+		return -EINVAL;
+
+	scl = kcalloc(nr_frags + 1, sizeof(struct scatterlist), GFP_ATOMIC);
+	if (unlikely(!scl))
+		return -ENOMEM;
+
+	sg_init_table(scl, nr_frags + 1);
+	num_sg = skb_to_sgvec(skb, scl, 0, skb->len);
+	num_dma_bufs = dma_map_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+	if (unlikely(!num_dma_bufs)) {
+		err = -ENOMEM;
+		goto dma_map_sg_failed;
+	}
+
+	/* Prepare the HW SGT structure */
+	sgt_buf_size = priv->tx_data_offset +
+		       sizeof(struct dpaa2_sg_entry) * (1 + num_dma_bufs);
+	sgt_buf = kzalloc(sgt_buf_size + DPAA2_ETH_TX_BUF_ALIGN, GFP_ATOMIC);
+	if (unlikely(!sgt_buf)) {
+		err = -ENOMEM;
+		goto sgt_buf_alloc_failed;
+	}
+	sgt_buf = PTR_ALIGN(sgt_buf, DPAA2_ETH_TX_BUF_ALIGN);
+
+	/* PTA from egress side is passed as is to the confirmation side so
+	 * we need to clear some fields here in order to find consistent values
+	 * on TX confirmation. We are clearing FAS (Frame Annotation Status)
+	 * field from the hardware annotation area
+	 */
+	hwa = sgt_buf + priv->buf_layout.private_data_size;
+	memset(hwa + DPAA2_FAS_OFFSET, 0, DPAA2_FAS_SIZE);
+
+	sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
+
+	/* Fill in the HW SGT structure.
+	 *
+	 * sgt_buf is zeroed out, so the following fields are implicit
+	 * in all sgt entries:
+	 *   - offset is 0
+	 *   - format is 'dpaa2_sg_single'
+	 */
+	for_each_sg(scl, crt_scl, num_dma_bufs, i) {
+		dpaa2_sg_set_addr(&sgt[i], sg_dma_address(crt_scl));
+		dpaa2_sg_set_len(&sgt[i], sg_dma_len(crt_scl));
+	}
+	dpaa2_sg_set_final(&sgt[i - 1], true);
+
+	/* Store the skb backpointer in the SGT buffer.
+	 * Fit the scatterlist and the number of buffers alongside the
+	 * skb backpointer in the software annotation area. We'll need
+	 * all of them on Tx Conf.
+	 */
+	swa = (struct dpaa2_eth_swa *)sgt_buf;
+	swa->skb = skb;
+	swa->scl = scl;
+	swa->num_sg = num_sg;
+	swa->num_dma_bufs = num_dma_bufs;
+
+	/* Separately map the SGT buffer */
+	addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(dev, addr))) {
+		err = -ENOMEM;
+		goto dma_map_single_failed;
+	}
+	dpaa2_fd_set_offset(fd, priv->tx_data_offset);
+	dpaa2_fd_set_format(fd, dpaa2_fd_sg);
+	dpaa2_fd_set_addr(fd, addr);
+	dpaa2_fd_set_len(fd, skb->len);
+	dpaa2_fd_set_ctrl(fd, DPAA2_FD_CTRL_ASAL | DPAA2_FD_CTRL_PTA |
+			  DPAA2_FD_CTRL_PTV1);
+
+	return 0;
+
+dma_map_single_failed:
+	kfree(sgt_buf);
+sgt_buf_alloc_failed:
+	dma_unmap_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+dma_map_sg_failed:
+	kfree(scl);
+	return err;
+}
+
+/* Create a frame descriptor based on a linear skb */
+static int build_single_fd(struct dpaa2_eth_priv *priv,
+			   struct sk_buff *skb,
+			   struct dpaa2_fd *fd)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	u8 *buffer_start;
+	void *hwa;
+	struct sk_buff **skbh;
+	dma_addr_t addr;
+
+	buffer_start = PTR_ALIGN(skb->data - priv->tx_data_offset -
+				 DPAA2_ETH_TX_BUF_ALIGN,
+				 DPAA2_ETH_TX_BUF_ALIGN);
+
+	/* PTA from egress side is passed as is to the confirmation side so
+	 * we need to clear some fields here in order to find consistent values
+	 * on TX confirmation. We are clearing FAS (Frame Annotation Status)
+	 * field from the hardware annotation area
+	 */
+	hwa = buffer_start + priv->buf_layout.private_data_size;
+	memset(hwa + DPAA2_FAS_OFFSET, 0, DPAA2_FAS_SIZE);
+
+	/* Store a backpointer to the skb at the beginning of the buffer
+	 * (in the private data area) such that we can release it
+	 * on Tx confirm
+	 */
+	skbh = (struct sk_buff **)buffer_start;
+	*skbh = skb;
+
+	addr = dma_map_single(dev, buffer_start,
+			      skb_tail_pointer(skb) - buffer_start,
+			      DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(dev, addr)))
+		return -ENOMEM;
+
+	dpaa2_fd_set_addr(fd, addr);
+	dpaa2_fd_set_offset(fd, (u16)(skb->data - buffer_start));
+	dpaa2_fd_set_len(fd, skb->len);
+	dpaa2_fd_set_format(fd, dpaa2_fd_single);
+	dpaa2_fd_set_ctrl(fd, DPAA2_FD_CTRL_ASAL | DPAA2_FD_CTRL_PTA |
+			  DPAA2_FD_CTRL_PTV1);
+
+	return 0;
+}
+
+/* FD freeing routine on the Tx path
+ *
+ * DMA-unmap and free FD and possibly SGT buffer allocated on Tx. The skb
+ * back-pointed to is also freed.
+ * This can be called either from dpaa2_eth_tx_conf() or on the error path of
+ * dpaa2_eth_tx().
+ * Optionally, return the frame annotation status word (FAS), which needs
+ * to be checked if we're on the confirmation path.
+ */
+static void free_tx_fd(const struct dpaa2_eth_priv *priv,
+		       const struct dpaa2_fd *fd,
+		       u32 *status)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	dma_addr_t fd_addr;
+	struct sk_buff **skbh, *skb;
+	unsigned char *buffer_start;
+	int unmap_size;
+	struct scatterlist *scl;
+	int num_sg, num_dma_bufs;
+	struct dpaa2_eth_swa *swa;
+	u8 fd_format = dpaa2_fd_get_format(fd);
+	struct dpaa2_fas *fas;
+
+	fd_addr = dpaa2_fd_get_addr(fd);
+	skbh = phys_to_virt(fd_addr);
+
+	if (fd_format == dpaa2_fd_single) {
+		skb = *skbh;
+		buffer_start = (unsigned char *)skbh;
+		/* Accessing the skb buffer is safe before dma unmap, because
+		 * we didn't map the actual skb shell.
+		 */
+		dma_unmap_single(dev, fd_addr,
+				 skb_tail_pointer(skb) - buffer_start,
+				 DMA_TO_DEVICE);
+	} else if (fd_format == dpaa2_fd_sg) {
+		swa = (struct dpaa2_eth_swa *)skbh;
+		skb = swa->skb;
+		scl = swa->scl;
+		num_sg = swa->num_sg;
+		num_dma_bufs = swa->num_dma_bufs;
+
+		/* Unmap the scatterlist */
+		dma_unmap_sg(dev, scl, num_sg, DMA_TO_DEVICE);
+		kfree(scl);
+
+		/* Unmap the SGT buffer */
+		unmap_size = priv->tx_data_offset +
+		       sizeof(struct dpaa2_sg_entry) * (1 + num_dma_bufs);
+		dma_unmap_single(dev, fd_addr, unmap_size, DMA_TO_DEVICE);
+	} else {
+		/* Unsupported format, mark it as errored and give up */
+		if (status)
+			*status = ~0;
+		return;
+	}
+
+	/* Read the status from the Frame Annotation after we unmap the first
+	 * buffer but before we free it. The caller function is responsible
+	 * for checking the status value.
+	 */
+	if (status && (dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV)) {
+		fas = (struct dpaa2_fas *)
+			((void *)skbh + priv->buf_layout.private_data_size);
+		*status = le32_to_cpu(fas->status);
+	}
+
+	/* Free SGT buffer kmalloc'ed on tx */
+	if (fd_format != dpaa2_fd_single)
+		kfree(skbh);
+
+	/* Move on with skb release */
+	dev_kfree_skb(skb);
+}
+
+static int dpaa2_eth_tx(struct sk_buff *skb, struct net_device *net_dev)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	struct dpaa2_fd fd;
+	struct rtnl_link_stats64 *percpu_stats;
+	struct dpaa2_eth_drv_stats *percpu_extras;
+	struct dpaa2_eth_fq *fq;
+	u16 queue_mapping;
+	int err, i;
+
+	percpu_stats = this_cpu_ptr(priv->percpu_stats);
+	percpu_extras = this_cpu_ptr(priv->percpu_extras);
+
+	if (unlikely(skb_headroom(skb) < DPAA2_ETH_NEEDED_HEADROOM(priv))) {
+		struct sk_buff *ns;
+
+		ns = skb_realloc_headroom(skb, DPAA2_ETH_NEEDED_HEADROOM(priv));
+		if (unlikely(!ns)) {
+			percpu_stats->tx_dropped++;
+			goto err_alloc_headroom;
+		}
+		dev_kfree_skb(skb);
+		skb = ns;
+	}
+
+	/* We'll be holding a back-reference to the skb until Tx Confirmation;
+	 * we don't want that overwritten by a concurrent Tx with a cloned skb.
+	 */
+	skb = skb_unshare(skb, GFP_ATOMIC);
+	if (unlikely(!skb)) {
+		/* skb_unshare() has already freed the skb */
+		percpu_stats->tx_dropped++;
+		return NETDEV_TX_OK;
+	}
+
+	/* Setup the FD fields */
+	memset(&fd, 0, sizeof(fd));
+
+	if (skb_is_nonlinear(skb)) {
+		err = build_sg_fd(priv, skb, &fd);
+		percpu_extras->tx_sg_frames++;
+		percpu_extras->tx_sg_bytes += skb->len;
+	} else {
+		err = build_single_fd(priv, skb, &fd);
+	}
+
+	if (unlikely(err)) {
+		percpu_stats->tx_dropped++;
+		goto err_build_fd;
+	}
+
+	/* Tracing point */
+	trace_dpaa2_tx_fd(net_dev, &fd);
+
+	/* TxConf FQ selection primarily based on cpu affinity; this is
+	 * non-migratable context, so it's safe to call smp_processor_id().
+	 */
+	queue_mapping = smp_processor_id() % dpaa2_eth_queue_count(priv);
+	fq = &priv->fq[queue_mapping];
+	for (i = 0; i < DPAA2_ETH_ENQUEUE_RETRIES; i++) {
+		err = dpaa2_io_service_enqueue_qd(NULL, priv->tx_qdid, 0,
+						  fq->tx_qdbin, &fd);
+		if (err != -EBUSY)
+			break;
+	}
+	percpu_extras->tx_portal_busy += i;
+	if (unlikely(err < 0)) {
+		percpu_stats->tx_errors++;
+		/* Clean up everything, including freeing the skb */
+		free_tx_fd(priv, &fd, NULL);
+	} else {
+		percpu_stats->tx_packets++;
+		percpu_stats->tx_bytes += skb->len;
+	}
+
+	return NETDEV_TX_OK;
+
+err_build_fd:
+err_alloc_headroom:
+	dev_kfree_skb(skb);
+
+	return NETDEV_TX_OK;
+}
+
+/* Tx confirmation frame processing routine */
+static void dpaa2_eth_tx_conf(struct dpaa2_eth_priv *priv,
+			      struct dpaa2_eth_channel *ch,
+			      const struct dpaa2_fd *fd,
+			      struct napi_struct *napi __always_unused)
+{
+	struct rtnl_link_stats64 *percpu_stats;
+	struct dpaa2_eth_drv_stats *percpu_extras;
+	u32 status = 0;
+
+	/* Tracing point */
+	trace_dpaa2_tx_conf_fd(priv->net_dev, fd);
+
+	percpu_extras = this_cpu_ptr(priv->percpu_extras);
+	percpu_extras->tx_conf_frames++;
+	percpu_extras->tx_conf_bytes += dpaa2_fd_get_len(fd);
+
+	free_tx_fd(priv, fd, &status);
+
+	if (unlikely(status & DPAA2_ETH_TXCONF_ERR_MASK)) {
+		percpu_stats = this_cpu_ptr(priv->percpu_stats);
+		/* Tx-conf logically pertains to the egress path. */
+		percpu_stats->tx_errors++;
+	}
+}
+
+static int set_rx_csum(struct dpaa2_eth_priv *priv, bool enable)
+{
+	int err;
+
+	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
+			       DPNI_OFF_RX_L3_CSUM, enable);
+	if (err) {
+		netdev_err(priv->net_dev,
+			   "dpni_set_offload(RX_L3_CSUM) failed\n");
+		return err;
+	}
+
+	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
+			       DPNI_OFF_RX_L4_CSUM, enable);
+	if (err) {
+		netdev_err(priv->net_dev,
+			   "dpni_set_offload(RX_L4_CSUM) failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int set_tx_csum(struct dpaa2_eth_priv *priv, bool enable)
+{
+	int err;
+
+	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
+			       DPNI_OFF_TX_L3_CSUM, enable);
+	if (err) {
+		netdev_err(priv->net_dev, "dpni_set_offload(TX_L3_CSUM) failed\n");
+		return err;
+	}
+
+	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
+			       DPNI_OFF_TX_L4_CSUM, enable);
+	if (err) {
+		netdev_err(priv->net_dev, "dpni_set_offload(TX_L4_CSUM) failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/* Perform a single release command to add buffers
+ * to the specified buffer pool
+ */
+static int add_bufs(struct dpaa2_eth_priv *priv, u16 bpid)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	u64 buf_array[DPAA2_ETH_BUFS_PER_CMD];
+	void *buf;
+	dma_addr_t addr;
+	int i;
+
+	for (i = 0; i < DPAA2_ETH_BUFS_PER_CMD; i++) {
+		/* Allocate buffer visible to WRIOP + skb shared info +
+		 * alignment padding
+		 */
+		buf = napi_alloc_frag(DPAA2_ETH_BUF_RAW_SIZE);
+		if (unlikely(!buf))
+			goto err_alloc;
+
+		buf = PTR_ALIGN(buf, DPAA2_ETH_RX_BUF_ALIGN);
+
+		addr = dma_map_single(dev, buf, DPAA2_ETH_RX_BUF_SIZE,
+				      DMA_FROM_DEVICE);
+		if (unlikely(dma_mapping_error(dev, addr)))
+			goto err_map;
+
+		buf_array[i] = addr;
+
+		/* tracing point */
+		trace_dpaa2_eth_buf_seed(priv->net_dev,
+					 buf, DPAA2_ETH_BUF_RAW_SIZE,
+					 addr, DPAA2_ETH_RX_BUF_SIZE,
+					 bpid);
+	}
+
+release_bufs:
+	/* In case the portal is busy, retry until successful.
+	 * The buffer release function would only fail if the QBMan portal
+	 * was busy, which implies portal contention (i.e. more CPUs than
+	 * portals, i.e. GPPs w/o affine DPIOs). For all practical purposes,
+	 * there is little we can realistically do, short of giving up -
+	 * in which case we'd risk depleting the buffer pool and never again
+	 * receiving the Rx interrupt which would kick-start the refill logic.
+	 * So just keep retrying, at the risk of being moved to ksoftirqd.
+	 */
+	while (dpaa2_io_service_release(NULL, bpid, buf_array, i))
+		cpu_relax();
+	return i;
+
+err_map:
+	skb_free_frag(buf);
+err_alloc:
+	if (i)
+		goto release_bufs;
+
+	return 0;
+}
+
+static int seed_pool(struct dpaa2_eth_priv *priv, u16 bpid)
+{
+	int i, j;
+	int new_count;
+
+	/* This is the lazy seeding of Rx buffer pools.
+	 * dpaa2_add_bufs() is also used on the Rx hotpath and calls
+	 * napi_alloc_frag(). The trouble with that is that it in turn ends up
+	 * calling this_cpu_ptr(), which mandates execution in atomic context.
+	 * Rather than splitting up the code, do a one-off preempt disable.
+	 */
+	preempt_disable();
+	for (j = 0; j < priv->num_channels; j++) {
+		for (i = 0; i < DPAA2_ETH_NUM_BUFS;
+		     i += DPAA2_ETH_BUFS_PER_CMD) {
+			new_count = add_bufs(priv, bpid);
+			priv->channel[j]->buf_count += new_count;
+
+			if (new_count < DPAA2_ETH_BUFS_PER_CMD) {
+				preempt_enable();
+				return -ENOMEM;
+			}
+		}
+	}
+	preempt_enable();
+
+	return 0;
+}
+
+/**
+ * Drain the specified number of buffers from the DPNI's private buffer pool.
+ * @count must not exceeed DPAA2_ETH_BUFS_PER_CMD
+ */
+static void drain_bufs(struct dpaa2_eth_priv *priv, int count)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	u64 buf_array[DPAA2_ETH_BUFS_PER_CMD];
+	void *vaddr;
+	int ret, i;
+
+	do {
+		ret = dpaa2_io_service_acquire(NULL, priv->dpbp_attrs.bpid,
+					       buf_array, count);
+		if (ret < 0) {
+			netdev_err(priv->net_dev, "dpaa2_io_service_acquire() failed\n");
+			return;
+		}
+		for (i = 0; i < ret; i++) {
+			/* Same logic as on regular Rx path */
+			dma_unmap_single(dev, buf_array[i],
+					 DPAA2_ETH_RX_BUF_SIZE,
+					 DMA_FROM_DEVICE);
+			vaddr = phys_to_virt(buf_array[i]);
+			skb_free_frag(vaddr);
+		}
+	} while (ret);
+}
+
+static void drain_pool(struct dpaa2_eth_priv *priv)
+{
+	int i;
+
+	drain_bufs(priv, DPAA2_ETH_BUFS_PER_CMD);
+	drain_bufs(priv, 1);
+
+	for (i = 0; i < priv->num_channels; i++)
+		priv->channel[i]->buf_count = 0;
+}
+
+/* Function is called from softirq context only, so we don't need to guard
+ * the access to percpu count
+ */
+static int refill_pool(struct dpaa2_eth_priv *priv,
+		       struct dpaa2_eth_channel *ch,
+		       u16 bpid)
+{
+	int new_count;
+
+	if (likely(ch->buf_count >= DPAA2_ETH_REFILL_THRESH))
+		return 0;
+
+	do {
+		new_count = add_bufs(priv, bpid);
+		if (unlikely(!new_count)) {
+			/* Out of memory; abort for now, we'll try later on */
+			break;
+		}
+		ch->buf_count += new_count;
+	} while (ch->buf_count < DPAA2_ETH_NUM_BUFS);
+
+	if (unlikely(ch->buf_count < DPAA2_ETH_NUM_BUFS))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int pull_channel(struct dpaa2_eth_channel *ch)
+{
+	int err;
+	int dequeues = -1;
+
+	/* Retry while portal is busy */
+	do {
+		err = dpaa2_io_service_pull_channel(NULL, ch->ch_id, ch->store);
+		dequeues++;
+		cpu_relax();
+	} while (err == -EBUSY);
+
+	ch->stats.dequeue_portal_busy += dequeues;
+	if (unlikely(err))
+		ch->stats.pull_err++;
+
+	return err;
+}
+
+/* NAPI poll routine
+ *
+ * Frames are dequeued from the QMan channel associated with this NAPI context.
+ * Rx, Tx confirmation and (if configured) Rx error frames all count
+ * towards the NAPI budget.
+ */
+static int dpaa2_eth_poll(struct napi_struct *napi, int budget)
+{
+	struct dpaa2_eth_channel *ch;
+	int cleaned = 0, store_cleaned;
+	struct dpaa2_eth_priv *priv;
+	int err;
+
+	ch = container_of(napi, struct dpaa2_eth_channel, napi);
+	priv = ch->priv;
+
+	while (cleaned < budget) {
+		err = pull_channel(ch);
+		if (unlikely(err))
+			break;
+
+		/* Refill pool if appropriate */
+		refill_pool(priv, ch, priv->dpbp_attrs.bpid);
+
+		store_cleaned = consume_frames(ch);
+		cleaned += store_cleaned;
+
+		/* If we have enough budget left for a full store,
+		 * try a new pull dequeue, otherwise we're done here
+		 */
+		if (store_cleaned == 0 ||
+		    cleaned > budget - DPAA2_ETH_STORE_SIZE)
+			break;
+	}
+
+	if (cleaned < budget) {
+		napi_complete_done(napi, cleaned);
+		/* Re-enable data available notifications */
+		do {
+			err = dpaa2_io_service_rearm(NULL, &ch->nctx);
+			cpu_relax();
+		} while (err == -EBUSY);
+	}
+
+	ch->stats.frames += cleaned;
+
+	return cleaned;
+}
+
+static void enable_ch_napi(struct dpaa2_eth_priv *priv)
+{
+	struct dpaa2_eth_channel *ch;
+	int i;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		napi_enable(&ch->napi);
+	}
+}
+
+static void disable_ch_napi(struct dpaa2_eth_priv *priv)
+{
+	struct dpaa2_eth_channel *ch;
+	int i;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		napi_disable(&ch->napi);
+	}
+}
+
+static int link_state_update(struct dpaa2_eth_priv *priv)
+{
+	struct dpni_link_state state;
+	int err;
+
+	err = dpni_get_link_state(priv->mc_io, 0, priv->mc_token, &state);
+	if (unlikely(err)) {
+		netdev_err(priv->net_dev,
+			   "dpni_get_link_state() failed\n");
+		return err;
+	}
+
+	/* Chech link state; speed / duplex changes are not treated yet */
+	if (priv->link_state.up == state.up)
+		return 0;
+
+	priv->link_state = state;
+	if (state.up) {
+		netif_carrier_on(priv->net_dev);
+		netif_tx_start_all_queues(priv->net_dev);
+	} else {
+		netif_tx_stop_all_queues(priv->net_dev);
+		netif_carrier_off(priv->net_dev);
+	}
+
+	netdev_info(priv->net_dev, "Link Event: state %s",
+		    state.up ? "up" : "down");
+
+	return 0;
+}
+
+static int dpaa2_eth_open(struct net_device *net_dev)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	int err;
+
+	err = seed_pool(priv, priv->dpbp_attrs.bpid);
+	if (err) {
+		/* Not much to do; the buffer pool, though not filled up,
+		 * may still contain some buffers which would enable us
+		 * to limp on.
+		 */
+		netdev_err(net_dev, "Buffer seeding failed for DPBP %d (bpid=%d)\n",
+			   priv->dpbp_dev->obj_desc.id, priv->dpbp_attrs.bpid);
+	}
+
+	/* We'll only start the txqs when the link is actually ready; make sure
+	 * we don't race against the link up notification, which may come
+	 * immediately after dpni_enable();
+	 */
+	netif_tx_stop_all_queues(net_dev);
+	enable_ch_napi(priv);
+	/* Also, explicitly set carrier off, otherwise netif_carrier_ok() will
+	 * return true and cause 'ip link show' to report the LOWER_UP flag,
+	 * even though the link notification wasn't even received.
+	 */
+	netif_carrier_off(net_dev);
+
+	err = dpni_enable(priv->mc_io, 0, priv->mc_token);
+	if (err < 0) {
+		netdev_err(net_dev, "dpni_enable() failed\n");
+		goto enable_err;
+	}
+
+	/* If the DPMAC object has already processed the link up interrupt,
+	 * we have to learn the link state ourselves.
+	 */
+	err = link_state_update(priv);
+	if (err < 0) {
+		netdev_err(net_dev, "Can't update link state\n");
+		goto link_state_err;
+	}
+
+	return 0;
+
+link_state_err:
+enable_err:
+	disable_ch_napi(priv);
+	drain_pool(priv);
+	return err;
+}
+
+/* The DPIO store must be empty when we call this,
+ * at the end of every NAPI cycle.
+ */
+static u32 drain_channel(struct dpaa2_eth_priv *priv,
+			 struct dpaa2_eth_channel *ch)
+{
+	u32 drained = 0, total = 0;
+
+	do {
+		pull_channel(ch);
+		drained = consume_frames(ch);
+		total += drained;
+	} while (drained);
+
+	return total;
+}
+
+static u32 drain_ingress_frames(struct dpaa2_eth_priv *priv)
+{
+	struct dpaa2_eth_channel *ch;
+	int i;
+	u32 drained = 0;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		drained += drain_channel(priv, ch);
+	}
+
+	return drained;
+}
+
+static int dpaa2_eth_stop(struct net_device *net_dev)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	int dpni_enabled;
+	int retries = 10;
+	u32 drained;
+
+	netif_tx_stop_all_queues(net_dev);
+	netif_carrier_off(net_dev);
+
+	/* Loop while dpni_disable() attempts to drain the egress FQs
+	 * and confirm them back to us.
+	 */
+	do {
+		dpni_disable(priv->mc_io, 0, priv->mc_token);
+		dpni_is_enabled(priv->mc_io, 0, priv->mc_token, &dpni_enabled);
+		if (dpni_enabled)
+			/* Allow the hardware some slack */
+			msleep(100);
+	} while (dpni_enabled && --retries);
+	if (!retries) {
+		netdev_warn(net_dev, "Retry count exceeded disabling DPNI\n");
+		/* Must go on and disable NAPI nonetheless, so we don't crash at
+		 * the next "ifconfig up"
+		 */
+	}
+
+	/* Wait for NAPI to complete on every core and disable it.
+	 * In particular, this will also prevent NAPI from being rescheduled if
+	 * a new CDAN is serviced, effectively discarding the CDAN. We therefore
+	 * don't even need to disarm the channels, except perhaps for the case
+	 * of a huge coalescing value.
+	 */
+	disable_ch_napi(priv);
+
+	 /* Manually drain the Rx and TxConf queues */
+	drained = drain_ingress_frames(priv);
+	if (drained)
+		netdev_dbg(net_dev, "Drained %d frames.\n", drained);
+
+	/* Empty the buffer pool */
+	drain_pool(priv);
+
+	return 0;
+}
+
+static int dpaa2_eth_init(struct net_device *net_dev)
+{
+	u64 supported = 0;
+	u64 not_supported = 0;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	u32 options = priv->dpni_attrs.options;
+
+	/* Capabilities listing */
+	supported |= IFF_LIVE_ADDR_CHANGE;
+
+	if (options & DPNI_OPT_NO_MAC_FILTER)
+		not_supported |= IFF_UNICAST_FLT;
+	else
+		supported |= IFF_UNICAST_FLT;
+
+	net_dev->priv_flags |= supported;
+	net_dev->priv_flags &= ~not_supported;
+
+	/* Features */
+	net_dev->features = NETIF_F_RXCSUM |
+			    NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
+			    NETIF_F_SG | NETIF_F_HIGHDMA |
+			    NETIF_F_LLTX;
+	net_dev->hw_features = net_dev->features;
+
+	return 0;
+}
+
+static int dpaa2_eth_set_addr(struct net_device *net_dev, void *addr)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	struct device *dev = net_dev->dev.parent;
+	int err;
+
+	err = eth_mac_addr(net_dev, addr);
+	if (err < 0) {
+		dev_err(dev, "eth_mac_addr() failed (%d)\n", err);
+		return err;
+	}
+
+	err = dpni_set_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
+					net_dev->dev_addr);
+	if (err) {
+		dev_err(dev, "dpni_set_primary_mac_addr() failed (%d)\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+/** Fill in counters maintained by the GPP driver. These may be different from
+ * the hardware counters obtained by ethtool.
+ */
+void dpaa2_eth_get_stats(struct net_device *net_dev,
+			 struct rtnl_link_stats64 *stats)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	struct rtnl_link_stats64 *percpu_stats;
+	u64 *cpustats;
+	u64 *netstats = (u64 *)stats;
+	int i, j;
+	int num = sizeof(struct rtnl_link_stats64) / sizeof(u64);
+
+	for_each_possible_cpu(i) {
+		percpu_stats = per_cpu_ptr(priv->percpu_stats, i);
+		cpustats = (u64 *)percpu_stats;
+		for (j = 0; j < num; j++)
+			netstats[j] += cpustats[j];
+	}
+}
+
+static int dpaa2_eth_change_mtu(struct net_device *net_dev, int mtu)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	int err;
+
+	/* Set the maximum Rx frame length to match the transmit side;
+	 * account for L2 headers when computing the MFL
+	 */
+	err = dpni_set_max_frame_length(priv->mc_io, 0, priv->mc_token,
+					(u16)DPAA2_ETH_L2_MAX_FRM(mtu));
+	if (err) {
+		netdev_err(net_dev, "dpni_set_max_frame_length() failed\n");
+		return err;
+	}
+
+	net_dev->mtu = mtu;
+	return 0;
+}
+
+/* Copy mac unicast addresses from @net_dev to @priv.
+ * Its sole purpose is to make dpaa2_eth_set_rx_mode() more readable.
+ */
+static void add_uc_hw_addr(const struct net_device *net_dev,
+			   struct dpaa2_eth_priv *priv)
+{
+	struct netdev_hw_addr *ha;
+	int err;
+
+	netdev_for_each_uc_addr(ha, net_dev) {
+		err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token,
+					ha->addr);
+		if (err)
+			netdev_warn(priv->net_dev,
+				    "Could not add ucast MAC %pM to the filtering table (err %d)\n",
+				    ha->addr, err);
+	}
+}
+
+/* Copy mac multicast addresses from @net_dev to @priv
+ * Its sole purpose is to make dpaa2_eth_set_rx_mode() more readable.
+ */
+static void add_mc_hw_addr(const struct net_device *net_dev,
+			   struct dpaa2_eth_priv *priv)
+{
+	struct netdev_hw_addr *ha;
+	int err;
+
+	netdev_for_each_mc_addr(ha, net_dev) {
+		err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token,
+					ha->addr);
+		if (err)
+			netdev_warn(priv->net_dev,
+				    "Could not add mcast MAC %pM to the filtering table (err %d)\n",
+				    ha->addr, err);
+	}
+}
+
+static void dpaa2_eth_set_rx_mode(struct net_device *net_dev)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	int uc_count = netdev_uc_count(net_dev);
+	int mc_count = netdev_mc_count(net_dev);
+	u8 max_mac = priv->dpni_attrs.mac_filter_entries;
+	u32 options = priv->dpni_attrs.options;
+	u16 mc_token = priv->mc_token;
+	struct fsl_mc_io *mc_io = priv->mc_io;
+	int err;
+
+	/* Basic sanity checks; these probably indicate a misconfiguration */
+	if (options & DPNI_OPT_NO_MAC_FILTER && max_mac != 0)
+		netdev_info(net_dev,
+			    "mac_filter_entries=%d, DPNI_OPT_NO_MAC_FILTER option must be disabled\n",
+			    max_mac);
+
+	/* Force promiscuous if the uc or mc counts exceed our capabilities. */
+	if (uc_count > max_mac) {
+		netdev_info(net_dev,
+			    "Unicast addr count reached %d, max allowed is %d; forcing promisc\n",
+			    uc_count, max_mac);
+		goto force_promisc;
+	}
+	if (mc_count + uc_count > max_mac) {
+		netdev_info(net_dev,
+			    "Unicast + multicast addr count reached %d, max allowed is %d; forcing promisc\n",
+			    uc_count + mc_count, max_mac);
+		goto force_mc_promisc;
+	}
+
+	/* Adjust promisc settings due to flag combinations */
+	if (net_dev->flags & IFF_PROMISC)
+		goto force_promisc;
+	if (net_dev->flags & IFF_ALLMULTI) {
+		/* First, rebuild unicast filtering table. This should be done
+		 * in promisc mode, in order to avoid frame loss while we
+		 * progressively add entries to the table.
+		 * We don't know whether we had been in promisc already, and
+		 * making an MC call to find out is expensive; so set uc promisc
+		 * nonetheless.
+		 */
+		err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
+		if (err)
+			netdev_warn(net_dev, "Can't set uc promisc\n");
+
+		/* Actual uc table reconstruction. */
+		err = dpni_clear_mac_filters(mc_io, 0, mc_token, 1, 0);
+		if (err)
+			netdev_warn(net_dev, "Can't clear uc filters\n");
+		add_uc_hw_addr(net_dev, priv);
+
+		/* Finally, clear uc promisc and set mc promisc as requested. */
+		err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 0);
+		if (err)
+			netdev_warn(net_dev, "Can't clear uc promisc\n");
+		goto force_mc_promisc;
+	}
+
+	/* Neither unicast, nor multicast promisc will be on... eventually.
+	 * For now, rebuild mac filtering tables while forcing both of them on.
+	 */
+	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
+	if (err)
+		netdev_warn(net_dev, "Can't set uc promisc (%d)\n", err);
+	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 1);
+	if (err)
+		netdev_warn(net_dev, "Can't set mc promisc (%d)\n", err);
+
+	/* Actual mac filtering tables reconstruction */
+	err = dpni_clear_mac_filters(mc_io, 0, mc_token, 1, 1);
+	if (err)
+		netdev_warn(net_dev, "Can't clear mac filters\n");
+	add_mc_hw_addr(net_dev, priv);
+	add_uc_hw_addr(net_dev, priv);
+
+	/* Now we can clear both ucast and mcast promisc, without risking
+	 * to drop legitimate frames anymore.
+	 */
+	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 0);
+	if (err)
+		netdev_warn(net_dev, "Can't clear ucast promisc\n");
+	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 0);
+	if (err)
+		netdev_warn(net_dev, "Can't clear mcast promisc\n");
+
+	return;
+
+force_promisc:
+	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
+	if (err)
+		netdev_warn(net_dev, "Can't set ucast promisc\n");
+force_mc_promisc:
+	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 1);
+	if (err)
+		netdev_warn(net_dev, "Can't set mcast promisc\n");
+}
+
+static int dpaa2_eth_set_features(struct net_device *net_dev,
+				  netdev_features_t features)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	netdev_features_t changed = features ^ net_dev->features;
+	bool enable;
+	int err;
+
+	if (changed & NETIF_F_RXCSUM) {
+		enable = !!(features & NETIF_F_RXCSUM);
+		err = set_rx_csum(priv, enable);
+		if (err)
+			return err;
+	}
+
+	if (changed & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)) {
+		enable = !!(features & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM));
+		err = set_tx_csum(priv, enable);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static const struct net_device_ops dpaa2_eth_ops = {
+	.ndo_open = dpaa2_eth_open,
+	.ndo_start_xmit = dpaa2_eth_tx,
+	.ndo_stop = dpaa2_eth_stop,
+	.ndo_init = dpaa2_eth_init,
+	.ndo_set_mac_address = dpaa2_eth_set_addr,
+	.ndo_get_stats64 = dpaa2_eth_get_stats,
+	.ndo_change_mtu = dpaa2_eth_change_mtu,
+	.ndo_set_rx_mode = dpaa2_eth_set_rx_mode,
+	.ndo_set_features = dpaa2_eth_set_features,
+};
+
+static void cdan_cb(struct dpaa2_io_notification_ctx *ctx)
+{
+	struct dpaa2_eth_channel *ch;
+
+	ch = container_of(ctx, struct dpaa2_eth_channel, nctx);
+
+	/* Update NAPI statistics */
+	ch->stats.cdan++;
+
+	napi_schedule_irqoff(&ch->napi);
+}
+
+/* Allocate and configure a DPCON object */
+static struct fsl_mc_device *setup_dpcon(struct dpaa2_eth_priv *priv)
+{
+	struct fsl_mc_device *dpcon;
+	struct device *dev = priv->net_dev->dev.parent;
+	struct dpcon_attr attrs;
+	int err;
+
+	err = fsl_mc_object_allocate(to_fsl_mc_device(dev),
+				     FSL_MC_POOL_DPCON, &dpcon);
+	if (err) {
+		dev_info(dev, "Not enough DPCONs, will go on as-is\n");
+		return NULL;
+	}
+
+	err = dpcon_open(priv->mc_io, 0, dpcon->obj_desc.id, &dpcon->mc_handle);
+	if (err) {
+		dev_err(dev, "dpcon_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpcon_reset(priv->mc_io, 0, dpcon->mc_handle);
+	if (err) {
+		dev_err(dev, "dpcon_reset() failed\n");
+		goto err_reset;
+	}
+
+	err = dpcon_get_attributes(priv->mc_io, 0, dpcon->mc_handle, &attrs);
+	if (err) {
+		dev_err(dev, "dpcon_get_attributes() failed\n");
+		goto err_get_attr;
+	}
+
+	err = dpcon_enable(priv->mc_io, 0, dpcon->mc_handle);
+	if (err) {
+		dev_err(dev, "dpcon_enable() failed\n");
+		goto err_enable;
+	}
+
+	return dpcon;
+
+err_enable:
+err_get_attr:
+err_reset:
+	dpcon_close(priv->mc_io, 0, dpcon->mc_handle);
+err_open:
+	fsl_mc_object_free(dpcon);
+
+	return NULL;
+}
+
+static void free_dpcon(struct dpaa2_eth_priv *priv,
+		       struct fsl_mc_device *dpcon)
+{
+	dpcon_disable(priv->mc_io, 0, dpcon->mc_handle);
+	dpcon_close(priv->mc_io, 0, dpcon->mc_handle);
+	fsl_mc_object_free(dpcon);
+}
+
+static struct dpaa2_eth_channel *
+alloc_channel(struct dpaa2_eth_priv *priv)
+{
+	struct dpaa2_eth_channel *channel;
+	struct dpcon_attr attr;
+	struct device *dev = priv->net_dev->dev.parent;
+	int err;
+
+	channel = kzalloc(sizeof(*channel), GFP_KERNEL);
+	if (!channel)
+		return NULL;
+
+	channel->dpcon = setup_dpcon(priv);
+	if (!channel->dpcon)
+		goto err_setup;
+
+	err = dpcon_get_attributes(priv->mc_io, 0, channel->dpcon->mc_handle,
+				   &attr);
+	if (err) {
+		dev_err(dev, "dpcon_get_attributes() failed\n");
+		goto err_get_attr;
+	}
+
+	channel->dpcon_id = attr.id;
+	channel->ch_id = attr.qbman_ch_id;
+	channel->priv = priv;
+
+	return channel;
+
+err_get_attr:
+	free_dpcon(priv, channel->dpcon);
+err_setup:
+	kfree(channel);
+	return NULL;
+}
+
+static void free_channel(struct dpaa2_eth_priv *priv,
+			 struct dpaa2_eth_channel *channel)
+{
+	free_dpcon(priv, channel->dpcon);
+	kfree(channel);
+}
+
+/* DPIO setup: allocate and configure QBMan channels, setup core affinity
+ * and register data availability notifications
+ */
+static int setup_dpio(struct dpaa2_eth_priv *priv)
+{
+	struct dpaa2_io_notification_ctx *nctx;
+	struct dpaa2_eth_channel *channel;
+	struct dpcon_notification_cfg dpcon_notif_cfg;
+	struct device *dev = priv->net_dev->dev.parent;
+	int i, err;
+
+	/* We want the ability to spread ingress traffic (RX, TX conf) to as
+	 * many cores as possible, so we need one channel for each core
+	 * (unless there's fewer queues than cores, in which case the extra
+	 * channels would be wasted).
+	 * Allocate one channel per core and register it to the core's
+	 * affine DPIO. If not enough channels are available for all cores
+	 * or if some cores don't have an affine DPIO, there will be no
+	 * ingress frame processing on those cores.
+	 */
+	cpumask_clear(&priv->dpio_cpumask);
+	for_each_online_cpu(i) {
+		/* Try to allocate a channel */
+		channel = alloc_channel(priv);
+		if (!channel) {
+			dev_info(dev,
+				 "No affine channel for cpu %d and above\n", i);
+			goto err_alloc_ch;
+		}
+
+		priv->channel[priv->num_channels] = channel;
+
+		nctx = &channel->nctx;
+		nctx->is_cdan = 1;
+		nctx->cb = cdan_cb;
+		nctx->id = channel->ch_id;
+		nctx->desired_cpu = i;
+
+		/* Register the new context */
+		err = dpaa2_io_service_register(NULL, nctx);
+		if (err) {
+			dev_info(dev, "No affine DPIO for cpu %d\n", i);
+			/* If no affine DPIO for this core, there's probably
+			 * none available for next cores either.
+			 */
+			goto err_service_reg;
+		}
+
+		/* Register DPCON notification with MC */
+		dpcon_notif_cfg.dpio_id = nctx->dpio_id;
+		dpcon_notif_cfg.priority = 0;
+		dpcon_notif_cfg.user_ctx = nctx->qman64;
+		err = dpcon_set_notification(priv->mc_io, 0,
+					     channel->dpcon->mc_handle,
+					     &dpcon_notif_cfg);
+		if (err) {
+			dev_err(dev, "dpcon_set_notification failed()\n");
+			goto err_set_cdan;
+		}
+
+		/* If we managed to allocate a channel and also found an affine
+		 * DPIO for this core, add it to the final mask
+		 */
+		cpumask_set_cpu(i, &priv->dpio_cpumask);
+		priv->num_channels++;
+
+		/* Stop if we already have enough channels to accommodate all
+		 * RX and TX conf queues
+		 */
+		if (priv->num_channels == dpaa2_eth_queue_count(priv))
+			break;
+	}
+
+	return 0;
+
+err_set_cdan:
+	dpaa2_io_service_deregister(NULL, nctx);
+err_service_reg:
+	free_channel(priv, channel);
+err_alloc_ch:
+	if (cpumask_empty(&priv->dpio_cpumask)) {
+		dev_err(dev, "No cpu with an affine DPIO/DPCON\n");
+		return -ENODEV;
+	}
+
+	dev_info(dev, "Cores %*pbl available for processing ingress traffic\n",
+		 cpumask_pr_args(&priv->dpio_cpumask));
+
+	return 0;
+}
+
+static void free_dpio(struct dpaa2_eth_priv *priv)
+{
+	int i;
+	struct dpaa2_eth_channel *ch;
+
+	/* deregister CDAN notifications and free channels */
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		dpaa2_io_service_deregister(NULL, &ch->nctx);
+		free_channel(priv, ch);
+	}
+}
+
+static struct dpaa2_eth_channel *get_affine_channel(struct dpaa2_eth_priv *priv,
+						    int cpu)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	int i;
+
+	for (i = 0; i < priv->num_channels; i++)
+		if (priv->channel[i]->nctx.desired_cpu == cpu)
+			return priv->channel[i];
+
+	/* We should never get here. Issue a warning and return
+	 * the first channel, because it's still better than nothing
+	 */
+	dev_warn(dev, "No affine channel found for cpu %d\n", cpu);
+
+	return priv->channel[0];
+}
+
+static void set_fq_affinity(struct dpaa2_eth_priv *priv)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	struct dpaa2_eth_fq *fq;
+	int rx_cpu, txc_cpu;
+	int i;
+
+	/* For each FQ, pick one channel/CPU to deliver frames to.
+	 * This may well change at runtime, either through irqbalance or
+	 * through direct user intervention.
+	 */
+	rx_cpu = txc_cpu = cpumask_first(&priv->dpio_cpumask);
+
+	for (i = 0; i < priv->num_fqs; i++) {
+		fq = &priv->fq[i];
+		switch (fq->type) {
+		case DPAA2_RX_FQ:
+			fq->target_cpu = rx_cpu;
+			rx_cpu = cpumask_next(rx_cpu, &priv->dpio_cpumask);
+			if (rx_cpu >= nr_cpu_ids)
+				rx_cpu = cpumask_first(&priv->dpio_cpumask);
+			break;
+		case DPAA2_TX_CONF_FQ:
+			fq->target_cpu = txc_cpu;
+			txc_cpu = cpumask_next(txc_cpu, &priv->dpio_cpumask);
+			if (txc_cpu >= nr_cpu_ids)
+				txc_cpu = cpumask_first(&priv->dpio_cpumask);
+			break;
+		default:
+			dev_err(dev, "Unknown FQ type: %d\n", fq->type);
+		}
+		fq->channel = get_affine_channel(priv, fq->target_cpu);
+	}
+}
+
+static void setup_fqs(struct dpaa2_eth_priv *priv)
+{
+	int i;
+
+	/* We have one TxConf FQ per Tx flow.
+	 * The number of Tx and Rx queues is the same.
+	 * Tx queues come first in the fq array.
+	 */
+	for (i = 0; i < dpaa2_eth_queue_count(priv); i++) {
+		priv->fq[priv->num_fqs].type = DPAA2_TX_CONF_FQ;
+		priv->fq[priv->num_fqs].consume = dpaa2_eth_tx_conf;
+		priv->fq[priv->num_fqs++].flowid = (u16)i;
+	}
+
+	for (i = 0; i < dpaa2_eth_queue_count(priv); i++) {
+		priv->fq[priv->num_fqs].type = DPAA2_RX_FQ;
+		priv->fq[priv->num_fqs].consume = dpaa2_eth_rx;
+		priv->fq[priv->num_fqs++].flowid = (u16)i;
+	}
+
+	/* For each FQ, decide on which core to process incoming frames */
+	set_fq_affinity(priv);
+}
+
+/* Allocate and configure one buffer pool for each interface */
+static int setup_dpbp(struct dpaa2_eth_priv *priv)
+{
+	int err;
+	struct fsl_mc_device *dpbp_dev;
+	struct device *dev = priv->net_dev->dev.parent;
+
+	err = fsl_mc_object_allocate(to_fsl_mc_device(dev), FSL_MC_POOL_DPBP,
+				     &dpbp_dev);
+	if (err) {
+		dev_err(dev, "DPBP device allocation failed\n");
+		return err;
+	}
+
+	priv->dpbp_dev = dpbp_dev;
+
+	err = dpbp_open(priv->mc_io, 0, priv->dpbp_dev->obj_desc.id,
+			&dpbp_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpbp_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpbp_enable(priv->mc_io, 0, dpbp_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpbp_enable() failed\n");
+		goto err_enable;
+	}
+
+	err = dpbp_get_attributes(priv->mc_io, 0, dpbp_dev->mc_handle,
+				  &priv->dpbp_attrs);
+	if (err) {
+		dev_err(dev, "dpbp_get_attributes() failed\n");
+		goto err_get_attr;
+	}
+
+	return 0;
+
+err_get_attr:
+	dpbp_disable(priv->mc_io, 0, dpbp_dev->mc_handle);
+err_enable:
+	dpbp_close(priv->mc_io, 0, dpbp_dev->mc_handle);
+err_open:
+	fsl_mc_object_free(dpbp_dev);
+
+	return err;
+}
+
+static void free_dpbp(struct dpaa2_eth_priv *priv)
+{
+	drain_pool(priv);
+	dpbp_disable(priv->mc_io, 0, priv->dpbp_dev->mc_handle);
+	dpbp_close(priv->mc_io, 0, priv->dpbp_dev->mc_handle);
+	fsl_mc_object_free(priv->dpbp_dev);
+}
+
+/* Configure the DPNI object this interface is associated with */
+static int setup_dpni(struct fsl_mc_device *ls_dev)
+{
+	struct device *dev = &ls_dev->dev;
+	struct dpaa2_eth_priv *priv;
+	struct net_device *net_dev;
+	int err;
+
+	net_dev = dev_get_drvdata(dev);
+	priv = netdev_priv(net_dev);
+
+	priv->dpni_id = ls_dev->obj_desc.id;
+
+	/* get a handle for the DPNI object */
+	err = dpni_open(priv->mc_io, 0, priv->dpni_id, &priv->mc_token);
+	if (err) {
+		dev_err(dev, "dpni_open() failed\n");
+		goto err_open;
+	}
+
+	ls_dev->mc_io = priv->mc_io;
+	ls_dev->mc_handle = priv->mc_token;
+
+	err = dpni_reset(priv->mc_io, 0, priv->mc_token);
+	if (err) {
+		dev_err(dev, "dpni_reset() failed\n");
+		goto err_reset;
+	}
+
+	err = dpni_get_attributes(priv->mc_io, 0, priv->mc_token,
+				  &priv->dpni_attrs);
+	if (err) {
+		dev_err(dev, "dpni_get_attributes() failed (err=%d)\n", err);
+		goto err_get_attr;
+	}
+
+	/* Configure buffer layouts */
+	/* rx buffer */
+	priv->buf_layout.pass_parser_result = true;
+	priv->buf_layout.pass_frame_status = true;
+	priv->buf_layout.private_data_size = DPAA2_ETH_SWA_SIZE;
+	priv->buf_layout.data_align = DPAA2_ETH_RX_BUF_ALIGN;
+	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_PARSER_RESULT |
+				   DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
+				   DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE |
+				   DPNI_BUF_LAYOUT_OPT_DATA_ALIGN;
+	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
+				     DPNI_QUEUE_RX, &priv->buf_layout);
+	if (err) {
+		dev_err(dev, "dpni_set_buffer_layout(RX) failed\n");
+		goto err_buf_layout;
+	}
+
+	/* tx buffer */
+	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
+				   DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE;
+	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
+				     DPNI_QUEUE_TX, &priv->buf_layout);
+	if (err) {
+		dev_err(dev, "dpni_set_buffer_layout(TX) failed\n");
+		goto err_buf_layout;
+	}
+
+	/* tx-confirm buffer */
+	priv->buf_layout.options = DPNI_BUF_LAYOUT_OPT_FRAME_STATUS;
+	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
+				     DPNI_QUEUE_TX_CONFIRM, &priv->buf_layout);
+	if (err) {
+		dev_err(dev, "dpni_set_buffer_layout(TX_CONF) failed\n");
+		goto err_buf_layout;
+	}
+
+	/* Now that we've set our tx buffer layout, retrieve the minimum
+	 * required tx data offset.
+	 */
+	err = dpni_get_tx_data_offset(priv->mc_io, 0, priv->mc_token,
+				      &priv->tx_data_offset);
+	if (err) {
+		dev_err(dev, "dpni_get_tx_data_offset() failed\n");
+		goto err_data_offset;
+	}
+
+	if ((priv->tx_data_offset % 64) != 0)
+		dev_warn(dev, "Tx data offset (%d) not a multiple of 64B",
+			 priv->tx_data_offset);
+
+	/* Accommodate software annotation space (SWA) */
+	priv->tx_data_offset += DPAA2_ETH_SWA_SIZE;
+
+	return 0;
+
+err_data_offset:
+err_buf_layout:
+err_get_attr:
+err_reset:
+	dpni_close(priv->mc_io, 0, priv->mc_token);
+err_open:
+	return err;
+}
+
+static void free_dpni(struct dpaa2_eth_priv *priv)
+{
+	int err;
+
+	err = dpni_reset(priv->mc_io, 0, priv->mc_token);
+	if (err)
+		netdev_warn(priv->net_dev, "dpni_reset() failed (err %d)\n",
+			    err);
+
+	dpni_close(priv->mc_io, 0, priv->mc_token);
+}
+
+static int setup_rx_flow(struct dpaa2_eth_priv *priv,
+			 struct dpaa2_eth_fq *fq)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	struct dpni_queue queue;
+	struct dpni_queue_id qid;
+	struct dpni_taildrop td;
+	int err;
+
+	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
+			     DPNI_QUEUE_RX, 0, fq->flowid, &queue, &qid);
+	if (err) {
+		dev_err(dev, "dpni_get_queue(RX) failed\n");
+		return err;
+	}
+
+	fq->fqid = qid.fqid;
+
+	queue.destination.id = fq->channel->dpcon_id;
+	queue.destination.type = DPNI_DEST_DPCON;
+	queue.destination.priority = 1;
+	queue.user_context = (u64)fq;
+	err = dpni_set_queue(priv->mc_io, 0, priv->mc_token,
+			     DPNI_QUEUE_RX, 0, fq->flowid,
+			     DPNI_QUEUE_OPT_USER_CTX | DPNI_QUEUE_OPT_DEST,
+			     &queue);
+	if (err) {
+		dev_err(dev, "dpni_set_queue(RX) failed\n");
+		return err;
+	}
+
+	td.enable = 1;
+	td.threshold = DPAA2_ETH_TAILDROP_THRESH;
+	err = dpni_set_taildrop(priv->mc_io, 0, priv->mc_token, DPNI_CP_QUEUE,
+				DPNI_QUEUE_RX, 0, fq->flowid, &td);
+	if (err) {
+		dev_err(dev, "dpni_set_threshold() failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int setup_tx_flow(struct dpaa2_eth_priv *priv,
+			 struct dpaa2_eth_fq *fq)
+{
+	struct device *dev = priv->net_dev->dev.parent;
+	struct dpni_queue queue;
+	struct dpni_queue_id qid;
+	int err;
+
+	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
+			     DPNI_QUEUE_TX, 0, fq->flowid, &queue, &qid);
+	if (err) {
+		dev_err(dev, "dpni_get_queue(TX) failed\n");
+		return err;
+	}
+
+	fq->tx_qdbin = qid.qdbin;
+
+	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
+			     DPNI_QUEUE_TX_CONFIRM, 0, fq->flowid,
+			     &queue, &qid);
+	if (err) {
+		dev_err(dev, "dpni_get_queue(TX_CONF) failed\n");
+		return err;
+	}
+
+	fq->fqid = qid.fqid;
+
+	queue.destination.id = fq->channel->dpcon_id;
+	queue.destination.type = DPNI_DEST_DPCON;
+	queue.destination.priority = 0;
+	queue.user_context = (u64)fq;
+	err = dpni_set_queue(priv->mc_io, 0, priv->mc_token,
+			     DPNI_QUEUE_TX_CONFIRM, 0, fq->flowid,
+			     DPNI_QUEUE_OPT_USER_CTX | DPNI_QUEUE_OPT_DEST,
+			     &queue);
+	if (err) {
+		dev_err(dev, "dpni_set_queue(TX_CONF) failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/* Hash key is a 5-tuple: IPsrc, IPdst, IPnextproto, L4src, L4dst */
+static const struct dpaa2_eth_hash_fields hash_fields[] = {
+	{
+		/* IP header */
+		.rxnfc_field = RXH_IP_SRC,
+		.cls_prot = NET_PROT_IP,
+		.cls_field = NH_FLD_IP_SRC,
+		.size = 4,
+	}, {
+		.rxnfc_field = RXH_IP_DST,
+		.cls_prot = NET_PROT_IP,
+		.cls_field = NH_FLD_IP_DST,
+		.size = 4,
+	}, {
+		.rxnfc_field = RXH_L3_PROTO,
+		.cls_prot = NET_PROT_IP,
+		.cls_field = NH_FLD_IP_PROTO,
+		.size = 1,
+	}, {
+		/* Using UDP ports, this is functionally equivalent to raw
+		 * byte pairs from L4 header.
+		 */
+		.rxnfc_field = RXH_L4_B_0_1,
+		.cls_prot = NET_PROT_UDP,
+		.cls_field = NH_FLD_UDP_PORT_SRC,
+		.size = 2,
+	}, {
+		.rxnfc_field = RXH_L4_B_2_3,
+		.cls_prot = NET_PROT_UDP,
+		.cls_field = NH_FLD_UDP_PORT_DST,
+		.size = 2,
+	},
+};
+
+/* Set RX hash options
+ * flags is a combination of RXH_ bits
+ */
+int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
+{
+	struct device *dev = net_dev->dev.parent;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	struct dpkg_profile_cfg cls_cfg;
+	struct dpni_rx_tc_dist_cfg dist_cfg;
+	u8 *dma_mem;
+	int i;
+	int err = 0;
+
+	if (!dpaa2_eth_hash_enabled(priv)) {
+		dev_err(dev, "Hashing support is not enabled\n");
+		return -EOPNOTSUPP;
+	}
+
+	memset(&cls_cfg, 0, sizeof(cls_cfg));
+
+	for (i = 0; i < ARRAY_SIZE(hash_fields); i++) {
+		struct dpkg_extract *key =
+			&cls_cfg.extracts[cls_cfg.num_extracts];
+
+		if (!(flags & hash_fields[i].rxnfc_field))
+			continue;
+
+		if (cls_cfg.num_extracts >= DPKG_MAX_NUM_OF_EXTRACTS) {
+			dev_err(dev, "error adding key extraction rule, too many rules?\n");
+			return -E2BIG;
+		}
+
+		key->type = DPKG_EXTRACT_FROM_HDR;
+		key->extract.from_hdr.prot = hash_fields[i].cls_prot;
+		key->extract.from_hdr.type = DPKG_FULL_FIELD;
+		key->extract.from_hdr.field = hash_fields[i].cls_field;
+		cls_cfg.num_extracts++;
+
+		priv->rx_hash_fields |= hash_fields[i].rxnfc_field;
+	}
+
+	dma_mem = kzalloc(DPAA2_CLASSIFIER_DMA_SIZE, GFP_DMA | GFP_KERNEL);
+	if (!dma_mem)
+		return -ENOMEM;
+
+	err = dpni_prepare_key_cfg(&cls_cfg, dma_mem);
+	if (err) {
+		dev_err(dev, "dpni_prepare_key_cfg error %d", err);
+		goto err_prep_key;
+	}
+
+	memset(&dist_cfg, 0, sizeof(dist_cfg));
+
+	/* Prepare for setting the rx dist */
+	dist_cfg.key_cfg_iova = dma_map_single(net_dev->dev.parent, dma_mem,
+					       DPAA2_CLASSIFIER_DMA_SIZE,
+					       DMA_TO_DEVICE);
+	if (dma_mapping_error(net_dev->dev.parent, dist_cfg.key_cfg_iova)) {
+		dev_err(dev, "DMA mapping failed\n");
+		err = -ENOMEM;
+		goto err_dma_map;
+	}
+
+	dist_cfg.dist_size = dpaa2_eth_queue_count(priv);
+	dist_cfg.dist_mode = DPNI_DIST_MODE_HASH;
+
+	err = dpni_set_rx_tc_dist(priv->mc_io, 0, priv->mc_token, 0, &dist_cfg);
+	dma_unmap_single(net_dev->dev.parent, dist_cfg.key_cfg_iova,
+			 DPAA2_CLASSIFIER_DMA_SIZE, DMA_TO_DEVICE);
+	if (err)
+		dev_err(dev, "dpni_set_rx_tc_dist() error %d\n", err);
+
+err_dma_map:
+err_prep_key:
+	kfree(dma_mem);
+	return err;
+}
+
+/* Bind the DPNI to its needed objects and resources: buffer pool, DPIOs,
+ * frame queues and channels
+ */
+static int bind_dpni(struct dpaa2_eth_priv *priv)
+{
+	struct net_device *net_dev = priv->net_dev;
+	struct device *dev = net_dev->dev.parent;
+	struct dpni_pools_cfg pools_params;
+	struct dpni_error_cfg err_cfg;
+	int err = 0;
+	int i;
+
+	pools_params.num_dpbp = 1;
+	pools_params.pools[0].dpbp_id = priv->dpbp_dev->obj_desc.id;
+	pools_params.pools[0].backup_pool = 0;
+	pools_params.pools[0].buffer_size = DPAA2_ETH_RX_BUF_SIZE;
+	err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
+	if (err) {
+		dev_err(dev, "dpni_set_pools() failed\n");
+		return err;
+	}
+
+	/* have the interface implicitly distribute traffic based on supported
+	 * header fields
+	 */
+	err = dpaa2_eth_set_hash(net_dev, DPAA2_RXH_SUPPORTED);
+	if (err)
+		netdev_err(net_dev, "Failed to configure hashing\n");
+
+	/* Configure handling of error frames */
+	err_cfg.errors = DPAA2_ETH_RX_ERR_MASK;
+	err_cfg.set_frame_annotation = 1;
+	err_cfg.error_action = DPNI_ERROR_ACTION_DISCARD;
+	err = dpni_set_errors_behavior(priv->mc_io, 0, priv->mc_token,
+				       &err_cfg);
+	if (err) {
+		dev_err(dev, "dpni_set_errors_behavior failed\n");
+		return err;
+	}
+
+	/* Configure Rx and Tx conf queues to generate CDANs */
+	for (i = 0; i < priv->num_fqs; i++) {
+		switch (priv->fq[i].type) {
+		case DPAA2_RX_FQ:
+			err = setup_rx_flow(priv, &priv->fq[i]);
+			break;
+		case DPAA2_TX_CONF_FQ:
+			err = setup_tx_flow(priv, &priv->fq[i]);
+			break;
+		default:
+			dev_err(dev, "Invalid FQ type %d\n", priv->fq[i].type);
+			return -EINVAL;
+		}
+		if (err)
+			return err;
+	}
+
+	err = dpni_get_qdid(priv->mc_io, 0, priv->mc_token,
+			    DPNI_QUEUE_TX, &priv->tx_qdid);
+	if (err) {
+		dev_err(dev, "dpni_get_qdid() failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+/* Allocate rings for storing incoming frame descriptors */
+static int alloc_rings(struct dpaa2_eth_priv *priv)
+{
+	struct net_device *net_dev = priv->net_dev;
+	struct device *dev = net_dev->dev.parent;
+	int i;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		priv->channel[i]->store =
+			dpaa2_io_store_create(DPAA2_ETH_STORE_SIZE, dev);
+		if (!priv->channel[i]->store) {
+			netdev_err(net_dev, "dpaa2_io_store_create() failed\n");
+			goto err_ring;
+		}
+	}
+
+	return 0;
+
+err_ring:
+	for (i = 0; i < priv->num_channels; i++) {
+		if (!priv->channel[i]->store)
+			break;
+		dpaa2_io_store_destroy(priv->channel[i]->store);
+	}
+
+	return -ENOMEM;
+}
+
+static void free_rings(struct dpaa2_eth_priv *priv)
+{
+	int i;
+
+	for (i = 0; i < priv->num_channels; i++)
+		dpaa2_io_store_destroy(priv->channel[i]->store);
+}
+
+static int netdev_init(struct net_device *net_dev)
+{
+	int err;
+	struct device *dev = net_dev->dev.parent;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	u8 mac_addr[ETH_ALEN], dpni_mac_addr[ETH_ALEN];
+	u8 bcast_addr[ETH_ALEN];
+
+	net_dev->netdev_ops = &dpaa2_eth_ops;
+
+	/* Get firmware address, if any */
+	err = dpni_get_port_mac_addr(priv->mc_io, 0, priv->mc_token, mac_addr);
+	if (err) {
+		dev_err(dev, "dpni_get_port_mac_addr() failed\n");
+		return err;
+	}
+
+	/* Get DPNI attributes address, if any */
+	err = dpni_get_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
+					dpni_mac_addr);
+	if (err) {
+		dev_err(dev, "dpni_get_primary_mac_addr() failed (%d)\n", err);
+		return err;
+	}
+
+	/* First check if firmware has any address configured by bootloader */
+	if (!is_zero_ether_addr(mac_addr)) {
+		/* If the DPMAC addr != DPNI addr, update it */
+		if (!ether_addr_equal(mac_addr, dpni_mac_addr)) {
+			err = dpni_set_primary_mac_addr(priv->mc_io, 0,
+							priv->mc_token,
+							mac_addr);
+			if (err) {
+				dev_err(dev, "dpni_set_primary_mac_addr() failed\n");
+				return err;
+			}
+		}
+		memcpy(net_dev->dev_addr, mac_addr, net_dev->addr_len);
+	} else if (is_zero_ether_addr(dpni_mac_addr)) {
+		/* Fills in net_dev->dev_addr, as required by
+		 * register_netdevice()
+		 */
+		eth_hw_addr_random(net_dev);
+		/* Make the user aware, without cluttering the boot log */
+		dev_dbg_once(dev, " device(s) have all-zero hwaddr, replaced with random\n");
+		err = dpni_set_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
+						net_dev->dev_addr);
+		if (err) {
+			dev_err(dev, "dpni_set_primary_mac_addr(): %d\n", err);
+			return err;
+		}
+		/* Override NET_ADDR_RANDOM set by eth_hw_addr_random(); for all
+		 * practical purposes, this will be our "permanent" mac address,
+		 * at least until the next reboot. This move will also permit
+		 * register_netdevice() to properly fill up net_dev->perm_addr.
+		 */
+		net_dev->addr_assign_type = NET_ADDR_PERM;
+	} else {
+		/* NET_ADDR_PERM is default, all we have to do is
+		 * fill in the device addr.
+		 */
+		memcpy(net_dev->dev_addr, dpni_mac_addr, net_dev->addr_len);
+	}
+
+	/* Explicitly add the broadcast address to the MAC filtering table;
+	 * the MC won't do that for us.
+	 */
+	eth_broadcast_addr(bcast_addr);
+	err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token, bcast_addr);
+	if (err) {
+		dev_warn(dev, "dpni_add_mac_addr() failed (%d)\n", err);
+		/* Won't return an error; at least, we'd have egress traffic */
+	}
+
+	/* Reserve enough space to align buffer as per hardware requirement;
+	 * NOTE: priv->tx_data_offset MUST be initialized at this point.
+	 */
+	net_dev->needed_headroom = DPAA2_ETH_NEEDED_HEADROOM(priv);
+
+	/* Set MTU limits */
+	net_dev->min_mtu = 68;
+	net_dev->max_mtu = DPAA2_ETH_MAX_MTU;
+
+	/* Our .ndo_init will be called herein */
+	err = register_netdev(net_dev);
+	if (err < 0) {
+		dev_err(dev, "register_netdev() failed\n");
+		return err;
+	}
+
+	return 0;
+}
+
+static int poll_link_state(void *arg)
+{
+	struct dpaa2_eth_priv *priv = (struct dpaa2_eth_priv *)arg;
+	int err;
+
+	while (!kthread_should_stop()) {
+		err = link_state_update(priv);
+		if (unlikely(err))
+			return err;
+
+		msleep(DPAA2_ETH_LINK_STATE_REFRESH);
+	}
+
+	return 0;
+}
+
+static irqreturn_t dpni_irq0_handler(int irq_num, void *arg)
+{
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t dpni_irq0_handler_thread(int irq_num, void *arg)
+{
+	u32 status, clear = 0;
+	struct device *dev = (struct device *)arg;
+	struct fsl_mc_device *dpni_dev = to_fsl_mc_device(dev);
+	struct net_device *net_dev = dev_get_drvdata(dev);
+	int err;
+
+	err = dpni_get_irq_status(dpni_dev->mc_io, 0, dpni_dev->mc_handle,
+				  DPNI_IRQ_INDEX, &status);
+	if (unlikely(err)) {
+		netdev_err(net_dev, "Can't get irq status (err %d)", err);
+		clear = 0xffffffff;
+		goto out;
+	}
+
+	if (status & DPNI_IRQ_EVENT_LINK_CHANGED) {
+		clear |= DPNI_IRQ_EVENT_LINK_CHANGED;
+		link_state_update(netdev_priv(net_dev));
+	}
+
+out:
+	dpni_clear_irq_status(dpni_dev->mc_io, 0, dpni_dev->mc_handle,
+			      DPNI_IRQ_INDEX, clear);
+	return IRQ_HANDLED;
+}
+
+static int setup_irqs(struct fsl_mc_device *ls_dev)
+{
+	int err = 0;
+	struct fsl_mc_device_irq *irq;
+
+	err = fsl_mc_allocate_irqs(ls_dev);
+	if (err) {
+		dev_err(&ls_dev->dev, "MC irqs allocation failed\n");
+		return err;
+	}
+
+	irq = ls_dev->irqs[0];
+	err = devm_request_threaded_irq(&ls_dev->dev, irq->msi_desc->irq,
+					dpni_irq0_handler,
+					dpni_irq0_handler_thread,
+					IRQF_NO_SUSPEND | IRQF_ONESHOT,
+					dev_name(&ls_dev->dev), &ls_dev->dev);
+	if (err < 0) {
+		dev_err(&ls_dev->dev, "devm_request_threaded_irq(): %d", err);
+		goto free_mc_irq;
+	}
+
+	err = dpni_set_irq_mask(ls_dev->mc_io, 0, ls_dev->mc_handle,
+				DPNI_IRQ_INDEX, DPNI_IRQ_EVENT_LINK_CHANGED);
+	if (err < 0) {
+		dev_err(&ls_dev->dev, "dpni_set_irq_mask(): %d", err);
+		goto free_irq;
+	}
+
+	err = dpni_set_irq_enable(ls_dev->mc_io, 0, ls_dev->mc_handle,
+				  DPNI_IRQ_INDEX, 1);
+	if (err < 0) {
+		dev_err(&ls_dev->dev, "dpni_set_irq_enable(): %d", err);
+		goto free_irq;
+	}
+
+	return 0;
+
+free_irq:
+	devm_free_irq(&ls_dev->dev, irq->msi_desc->irq, &ls_dev->dev);
+free_mc_irq:
+	fsl_mc_free_irqs(ls_dev);
+
+	return err;
+}
+
+static void add_ch_napi(struct dpaa2_eth_priv *priv)
+{
+	int i;
+	struct dpaa2_eth_channel *ch;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		/* NAPI weight *MUST* be a multiple of DPAA2_ETH_STORE_SIZE */
+		netif_napi_add(priv->net_dev, &ch->napi, dpaa2_eth_poll,
+			       NAPI_POLL_WEIGHT);
+	}
+}
+
+static void del_ch_napi(struct dpaa2_eth_priv *priv)
+{
+	int i;
+	struct dpaa2_eth_channel *ch;
+
+	for (i = 0; i < priv->num_channels; i++) {
+		ch = priv->channel[i];
+		netif_napi_del(&ch->napi);
+	}
+}
+
+static int dpaa2_eth_probe(struct fsl_mc_device *dpni_dev)
+{
+	struct device *dev;
+	struct net_device *net_dev = NULL;
+	struct dpaa2_eth_priv *priv = NULL;
+	int err = 0;
+
+	dev = &dpni_dev->dev;
+
+	/* Net device */
+	net_dev = alloc_etherdev_mq(sizeof(*priv), DPAA2_ETH_MAX_TX_QUEUES);
+	if (!net_dev) {
+		dev_err(dev, "alloc_etherdev_mq() failed\n");
+		return -ENOMEM;
+	}
+
+	SET_NETDEV_DEV(net_dev, dev);
+	dev_set_drvdata(dev, net_dev);
+
+	priv = netdev_priv(net_dev);
+	priv->net_dev = net_dev;
+
+	/* Obtain a MC portal */
+	err = fsl_mc_portal_allocate(dpni_dev, FSL_MC_IO_ATOMIC_CONTEXT_PORTAL,
+				     &priv->mc_io);
+	if (err) {
+		dev_err(dev, "MC portal allocation failed\n");
+		goto err_portal_alloc;
+	}
+
+	/* MC objects initialization and configuration */
+	err = setup_dpni(dpni_dev);
+	if (err)
+		goto err_dpni_setup;
+
+	err = setup_dpio(priv);
+	if (err)
+		goto err_dpio_setup;
+
+	setup_fqs(priv);
+
+	err = setup_dpbp(priv);
+	if (err)
+		goto err_dpbp_setup;
+
+	err = bind_dpni(priv);
+	if (err)
+		goto err_bind;
+
+	/* Add a NAPI context for each channel */
+	add_ch_napi(priv);
+
+	/* Percpu statistics */
+	priv->percpu_stats = alloc_percpu(*priv->percpu_stats);
+	if (!priv->percpu_stats) {
+		dev_err(dev, "alloc_percpu(percpu_stats) failed\n");
+		err = -ENOMEM;
+		goto err_alloc_percpu_stats;
+	}
+	priv->percpu_extras = alloc_percpu(*priv->percpu_extras);
+	if (!priv->percpu_extras) {
+		dev_err(dev, "alloc_percpu(percpu_extras) failed\n");
+		err = -ENOMEM;
+		goto err_alloc_percpu_extras;
+	}
+
+	err = netdev_init(net_dev);
+	if (err)
+		goto err_netdev_init;
+
+	/* Configure checksum offload based on current interface flags */
+	err = set_rx_csum(priv, !!(net_dev->features & NETIF_F_RXCSUM));
+	if (err)
+		goto err_csum;
+
+	err = set_tx_csum(priv, !!(net_dev->features &
+				   (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)));
+	if (err)
+		goto err_csum;
+
+	err = alloc_rings(priv);
+	if (err)
+		goto err_alloc_rings;
+
+	net_dev->ethtool_ops = &dpaa2_ethtool_ops;
+
+	err = setup_irqs(dpni_dev);
+	if (err) {
+		netdev_warn(net_dev, "Failed to set link interrupt, fall back to polling\n");
+		priv->poll_thread = kthread_run(poll_link_state, priv,
+						"%s_poll_link", net_dev->name);
+		if (IS_ERR(priv->poll_thread)) {
+			netdev_err(net_dev, "Error starting polling thread\n");
+			goto err_poll_thread;
+		}
+		priv->do_link_poll = true;
+	}
+
+	dev_info(dev, "Probed interface %s\n", net_dev->name);
+	return 0;
+
+err_poll_thread:
+	free_rings(priv);
+err_alloc_rings:
+err_csum:
+	unregister_netdev(net_dev);
+err_netdev_init:
+	free_percpu(priv->percpu_extras);
+err_alloc_percpu_extras:
+	free_percpu(priv->percpu_stats);
+err_alloc_percpu_stats:
+	del_ch_napi(priv);
+err_bind:
+	free_dpbp(priv);
+err_dpbp_setup:
+	free_dpio(priv);
+err_dpio_setup:
+	free_dpni(priv);
+err_dpni_setup:
+	fsl_mc_portal_free(priv->mc_io);
+err_portal_alloc:
+	dev_set_drvdata(dev, NULL);
+	free_netdev(net_dev);
+
+	return err;
+}
+
+static int dpaa2_eth_remove(struct fsl_mc_device *ls_dev)
+{
+	struct device *dev;
+	struct net_device *net_dev;
+	struct dpaa2_eth_priv *priv;
+
+	dev = &ls_dev->dev;
+	net_dev = dev_get_drvdata(dev);
+	priv = netdev_priv(net_dev);
+
+	unregister_netdev(net_dev);
+	dev_info(net_dev->dev.parent, "Removed interface %s\n", net_dev->name);
+
+	if (priv->do_link_poll)
+		kthread_stop(priv->poll_thread);
+	else
+		fsl_mc_free_irqs(ls_dev);
+
+	free_rings(priv);
+	free_percpu(priv->percpu_stats);
+	free_percpu(priv->percpu_extras);
+
+	del_ch_napi(priv);
+	free_dpbp(priv);
+	free_dpio(priv);
+	free_dpni(priv);
+
+	fsl_mc_portal_free(priv->mc_io);
+
+	dev_set_drvdata(dev, NULL);
+	free_netdev(net_dev);
+
+	return 0;
+}
+
+static const struct fsl_mc_device_id dpaa2_eth_match_id_table[] = {
+	{
+		.vendor = FSL_MC_VENDOR_FREESCALE,
+		.obj_type = "dpni",
+	},
+	{ .vendor = 0x0 }
+};
+MODULE_DEVICE_TABLE(fslmc, dpaa2_eth_match_id_table);
+
+static struct fsl_mc_driver dpaa2_eth_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = dpaa2_eth_probe,
+	.remove = dpaa2_eth_remove,
+	.match_id_table = dpaa2_eth_match_id_table
+};
+
+module_fsl_mc_driver(dpaa2_eth_driver);
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h
new file mode 100644
index 000000000000..c67cced55b72
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-eth.h
@@ -0,0 +1,348 @@
+/* Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#ifndef __DPAA2_ETH_H
+#define __DPAA2_ETH_H
+
+#include <linux/netdevice.h>
+#include <linux/if_vlan.h>
+
+#include "../../fsl-mc/include/dpaa2-io.h"
+#include "../../fsl-mc/include/dpaa2-fd.h"
+#include "../../fsl-mc/include/dpbp.h"
+#include "../../fsl-mc/include/dpcon.h"
+#include "dpni.h"
+#include "dpni-cmd.h"
+
+#include "dpaa2-eth-trace.h"
+
+#define DPAA2_ETH_STORE_SIZE		16
+
+/* Maximum number of scatter-gather entries in an ingress frame,
+ * considering the maximum receive frame size is 64K
+ */
+#define DPAA2_ETH_MAX_SG_ENTRIES	((64 * 1024) / DPAA2_ETH_RX_BUF_SIZE)
+
+/* Maximum acceptable MTU value. It is in direct relation with the hardware
+ * enforced Max Frame Length (currently 10k).
+ */
+#define DPAA2_ETH_MFL			(10 * 1024)
+#define DPAA2_ETH_MAX_MTU		(DPAA2_ETH_MFL - VLAN_ETH_HLEN)
+/* Convert L3 MTU to L2 MFL */
+#define DPAA2_ETH_L2_MAX_FRM(mtu)	((mtu) + VLAN_ETH_HLEN)
+
+/* Set the taildrop threshold (in bytes) to allow the enqueue of several jumbo
+ * frames in the Rx queues (length of the current frame is not
+ * taken into account when making the taildrop decision)
+ */
+#define DPAA2_ETH_TAILDROP_THRESH	(64 * 1024)
+
+/* Buffer quota per queue. Must be large enough such that for minimum sized
+ * frames taildrop kicks in before the bpool gets depleted, so we compute
+ * how many 64B frames fit inside the taildrop threshold and add a margin
+ * to accommodate the buffer refill delay.
+ */
+#define DPAA2_ETH_MAX_FRAMES_PER_QUEUE	(DPAA2_ETH_TAILDROP_THRESH / 64)
+#define DPAA2_ETH_NUM_BUFS		(DPAA2_ETH_MAX_FRAMES_PER_QUEUE + 256)
+#define DPAA2_ETH_REFILL_THRESH		DPAA2_ETH_MAX_FRAMES_PER_QUEUE
+
+/* Maximum number of buffers that can be acquired/released through a single
+ * QBMan command
+ */
+#define DPAA2_ETH_BUFS_PER_CMD		7
+
+/* Hardware requires alignment for ingress/egress buffer addresses
+ * and ingress buffer lengths.
+ */
+#define DPAA2_ETH_RX_BUF_SIZE		2048
+#define DPAA2_ETH_TX_BUF_ALIGN		64
+#define DPAA2_ETH_RX_BUF_ALIGN		256
+#define DPAA2_ETH_NEEDED_HEADROOM(p_priv) \
+	((p_priv)->tx_data_offset + DPAA2_ETH_TX_BUF_ALIGN)
+
+/* Hardware only sees DPAA2_ETH_RX_BUF_SIZE, but we need to allocate ingress
+ * buffers large enough to allow building an skb around them and also account
+ * for alignment restrictions
+ */
+#define DPAA2_ETH_BUF_RAW_SIZE \
+	(DPAA2_ETH_RX_BUF_SIZE + \
+	SKB_DATA_ALIGN(sizeof(struct skb_shared_info)) + \
+	DPAA2_ETH_RX_BUF_ALIGN)
+
+/* We are accommodating a skb backpointer and some S/G info
+ * in the frame's software annotation. The hardware
+ * options are either 0 or 64, so we choose the latter.
+ */
+#define DPAA2_ETH_SWA_SIZE		64
+
+/* Must keep this struct smaller than DPAA2_ETH_SWA_SIZE */
+struct dpaa2_eth_swa {
+	struct sk_buff *skb;
+	struct scatterlist *scl;
+	int num_sg;
+	int num_dma_bufs;
+};
+
+/* Annotation valid bits in FD FRC */
+#define DPAA2_FD_FRC_FASV		0x8000
+#define DPAA2_FD_FRC_FAEADV		0x4000
+#define DPAA2_FD_FRC_FAPRV		0x2000
+#define DPAA2_FD_FRC_FAIADV		0x1000
+#define DPAA2_FD_FRC_FASWOV		0x0800
+#define DPAA2_FD_FRC_FAICFDV		0x0400
+
+/* Annotation bits in FD CTRL */
+#define DPAA2_FD_CTRL_ASAL		0x00020000	/* ASAL = 128 */
+#define DPAA2_FD_CTRL_PTA		0x00800000
+#define DPAA2_FD_CTRL_PTV1		0x00400000
+
+/* Frame annotation status */
+struct dpaa2_fas {
+	u8 reserved;
+	u8 ppid;
+	__le16 ifpid;
+	__le32 status;
+} __packed;
+
+/* Frame annotation status word is located in the first 8 bytes
+ * of the buffer's hardware annoatation area
+ */
+#define DPAA2_FAS_OFFSET		0
+#define DPAA2_FAS_SIZE			(sizeof(struct dpaa2_fas))
+
+/* Error and status bits in the frame annotation status word */
+/* Debug frame, otherwise supposed to be discarded */
+#define DPAA2_FAS_DISC			0x80000000
+/* MACSEC frame */
+#define DPAA2_FAS_MS			0x40000000
+#define DPAA2_FAS_PTP			0x08000000
+/* Ethernet multicast frame */
+#define DPAA2_FAS_MC			0x04000000
+/* Ethernet broadcast frame */
+#define DPAA2_FAS_BC			0x02000000
+#define DPAA2_FAS_KSE			0x00040000
+#define DPAA2_FAS_EOFHE			0x00020000
+#define DPAA2_FAS_MNLE			0x00010000
+#define DPAA2_FAS_TIDE			0x00008000
+#define DPAA2_FAS_PIEE			0x00004000
+/* Frame length error */
+#define DPAA2_FAS_FLE			0x00002000
+/* Frame physical error */
+#define DPAA2_FAS_FPE			0x00001000
+#define DPAA2_FAS_PTE			0x00000080
+#define DPAA2_FAS_ISP			0x00000040
+#define DPAA2_FAS_PHE			0x00000020
+#define DPAA2_FAS_BLE			0x00000010
+/* L3 csum validation performed */
+#define DPAA2_FAS_L3CV			0x00000008
+/* L3 csum error */
+#define DPAA2_FAS_L3CE			0x00000004
+/* L4 csum validation performed */
+#define DPAA2_FAS_L4CV			0x00000002
+/* L4 csum error */
+#define DPAA2_FAS_L4CE			0x00000001
+/* Possible errors on the ingress path */
+#define DPAA2_ETH_RX_ERR_MASK		(DPAA2_FAS_KSE		| \
+					 DPAA2_FAS_EOFHE	| \
+					 DPAA2_FAS_MNLE		| \
+					 DPAA2_FAS_TIDE		| \
+					 DPAA2_FAS_PIEE		| \
+					 DPAA2_FAS_FLE		| \
+					 DPAA2_FAS_FPE		| \
+					 DPAA2_FAS_PTE		| \
+					 DPAA2_FAS_ISP		| \
+					 DPAA2_FAS_PHE		| \
+					 DPAA2_FAS_BLE		| \
+					 DPAA2_FAS_L3CE		| \
+					 DPAA2_FAS_L4CE)
+/* Tx errors */
+#define DPAA2_ETH_TXCONF_ERR_MASK	(DPAA2_FAS_KSE		| \
+					 DPAA2_FAS_EOFHE	| \
+					 DPAA2_FAS_MNLE		| \
+					 DPAA2_FAS_TIDE)
+
+/* Time in milliseconds between link state updates */
+#define DPAA2_ETH_LINK_STATE_REFRESH	1000
+
+/* Number of times to retry a frame enqueue before giving up.
+ * Value determined empirically, in order to minimize the number
+ * of frames dropped on Tx
+ */
+#define DPAA2_ETH_ENQUEUE_RETRIES	10
+
+/* Driver statistics, other than those in struct rtnl_link_stats64.
+ * These are usually collected per-CPU and aggregated by ethtool.
+ */
+struct dpaa2_eth_drv_stats {
+	__u64	tx_conf_frames;
+	__u64	tx_conf_bytes;
+	__u64	tx_sg_frames;
+	__u64	tx_sg_bytes;
+	__u64	rx_sg_frames;
+	__u64	rx_sg_bytes;
+	/* Enqueues retried due to portal busy */
+	__u64	tx_portal_busy;
+};
+
+/* Per-FQ statistics */
+struct dpaa2_eth_fq_stats {
+	/* Number of frames received on this queue */
+	__u64 frames;
+};
+
+/* Per-channel statistics */
+struct dpaa2_eth_ch_stats {
+	/* Volatile dequeues retried due to portal busy */
+	__u64 dequeue_portal_busy;
+	/* Number of CDANs; useful to estimate avg NAPI len */
+	__u64 cdan;
+	/* Number of frames received on queues from this channel */
+	__u64 frames;
+	/* Pull errors */
+	__u64 pull_err;
+};
+
+/* Maximum number of queues associated with a DPNI */
+#define DPAA2_ETH_MAX_RX_QUEUES		16
+#define DPAA2_ETH_MAX_TX_QUEUES		NR_CPUS
+#define DPAA2_ETH_MAX_QUEUES		(DPAA2_ETH_MAX_RX_QUEUES + \
+					DPAA2_ETH_MAX_TX_QUEUES)
+
+#define DPAA2_ETH_MAX_DPCONS		NR_CPUS
+
+enum dpaa2_eth_fq_type {
+	DPAA2_RX_FQ = 0,
+	DPAA2_TX_CONF_FQ,
+};
+
+struct dpaa2_eth_priv;
+
+struct dpaa2_eth_fq {
+	u32 fqid;
+	u32 tx_qdbin;
+	u16 flowid;
+	int target_cpu;
+	struct dpaa2_eth_channel *channel;
+	enum dpaa2_eth_fq_type type;
+
+	void (*consume)(struct dpaa2_eth_priv *,
+			struct dpaa2_eth_channel *,
+			const struct dpaa2_fd *,
+			struct napi_struct *);
+	struct dpaa2_eth_fq_stats stats;
+};
+
+struct dpaa2_eth_channel {
+	struct dpaa2_io_notification_ctx nctx;
+	struct fsl_mc_device *dpcon;
+	int dpcon_id;
+	int ch_id;
+	int dpio_id;
+	struct napi_struct napi;
+	struct dpaa2_io_store *store;
+	struct dpaa2_eth_priv *priv;
+	int buf_count;
+	struct dpaa2_eth_ch_stats stats;
+};
+
+struct dpaa2_eth_hash_fields {
+	u64 rxnfc_field;
+	enum net_prot cls_prot;
+	int cls_field;
+	int size;
+};
+
+/* Driver private data */
+struct dpaa2_eth_priv {
+	struct net_device *net_dev;
+
+	u8 num_fqs;
+	struct dpaa2_eth_fq fq[DPAA2_ETH_MAX_QUEUES];
+
+	u8 num_channels;
+	struct dpaa2_eth_channel *channel[DPAA2_ETH_MAX_DPCONS];
+
+	int dpni_id;
+	struct dpni_attr dpni_attrs;
+	/* Insofar as the MC is concerned, we're using one layout on all 3 types
+	 * of buffers (Rx, Tx, Tx-Conf).
+	 */
+	struct dpni_buffer_layout buf_layout;
+	u16 tx_data_offset;
+
+	struct fsl_mc_device *dpbp_dev;
+	struct dpbp_attr dpbp_attrs;
+
+	u16 tx_qdid;
+	struct fsl_mc_io *mc_io;
+	/* Cores which have an affine DPIO/DPCON.
+	 * This is the cpu set on which Rx and Tx conf frames are processed
+	 */
+	struct cpumask dpio_cpumask;
+
+	/* Standard statistics */
+	struct rtnl_link_stats64 __percpu *percpu_stats;
+	/* Extra stats, in addition to the ones known by the kernel */
+	struct dpaa2_eth_drv_stats __percpu *percpu_extras;
+
+	u16 mc_token;
+
+	struct dpni_link_state link_state;
+	bool do_link_poll;
+	struct task_struct *poll_thread;
+
+	/* enabled ethtool hashing bits */
+	u64 rx_hash_fields;
+};
+
+/* default Rx hash options, set during probing */
+#define DPAA2_RXH_SUPPORTED	(RXH_L2DA | RXH_VLAN | RXH_L3_PROTO \
+				| RXH_IP_SRC | RXH_IP_DST | RXH_L4_B_0_1 \
+				| RXH_L4_B_2_3)
+
+#define dpaa2_eth_hash_enabled(priv)	\
+	((priv)->dpni_attrs.num_queues > 1)
+
+/* Required by struct dpni_rx_tc_dist_cfg::key_cfg_iova */
+#define DPAA2_CLASSIFIER_DMA_SIZE 256
+
+extern const struct ethtool_ops dpaa2_ethtool_ops;
+extern const char dpaa2_eth_drv_version[];
+
+int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags);
+
+static int dpaa2_eth_queue_count(struct dpaa2_eth_priv *priv)
+{
+	return priv->dpni_attrs.num_queues;
+}
+
+#endif	/* __DPAA2_H */
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c
new file mode 100644
index 000000000000..dd0cffa908ef
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpaa2-ethtool.c
@@ -0,0 +1,279 @@
+/* Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#include "dpni.h"	/* DPNI_LINK_OPT_* */
+#include "dpaa2-eth.h"
+
+/* To be kept in sync with DPNI statistics */
+char dpaa2_ethtool_stats[][ETH_GSTRING_LEN] = {
+	"rx frames",
+	"rx bytes",
+	"rx mcast frames",
+	"rx mcast bytes",
+	"rx bcast frames",
+	"rx bcast bytes",
+	"tx frames",
+	"tx bytes",
+	"tx mcast frames",
+	"tx mcast bytes",
+	"tx bcast frames",
+	"tx bcast bytes",
+	"rx filtered frames",
+	"rx discarded frames",
+	"rx nobuffer discards",
+	"tx discarded frames",
+	"tx confirmed frames",
+};
+
+#define DPAA2_ETH_NUM_STATS	ARRAY_SIZE(dpaa2_ethtool_stats)
+
+char dpaa2_ethtool_extras[][ETH_GSTRING_LEN] = {
+	/* per-cpu stats */
+	"tx conf frames",
+	"tx conf bytes",
+	"tx sg frames",
+	"tx sg bytes",
+	"rx sg frames",
+	"rx sg bytes",
+	"enqueue portal busy",
+	/* Channel stats */
+	"dequeue portal busy",
+	"channel pull errors",
+	"cdan",
+};
+
+#define DPAA2_ETH_NUM_EXTRA_STATS	ARRAY_SIZE(dpaa2_ethtool_extras)
+
+static void dpaa2_eth_get_drvinfo(struct net_device *net_dev,
+				  struct ethtool_drvinfo *drvinfo)
+{
+	strlcpy(drvinfo->driver, KBUILD_MODNAME, sizeof(drvinfo->driver));
+	strlcpy(drvinfo->version, dpaa2_eth_drv_version,
+		sizeof(drvinfo->version));
+	strlcpy(drvinfo->fw_version, "N/A", sizeof(drvinfo->fw_version));
+	strlcpy(drvinfo->bus_info, dev_name(net_dev->dev.parent->parent),
+		sizeof(drvinfo->bus_info));
+}
+
+static int
+dpaa2_eth_get_link_ksettings(struct net_device *net_dev,
+			     struct ethtool_link_ksettings *link_settings)
+{
+	struct dpni_link_state state = {0};
+	int err = 0;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+
+	err = dpni_get_link_state(priv->mc_io, 0, priv->mc_token, &state);
+	if (err) {
+		netdev_err(net_dev, "ERROR %d getting link state", err);
+		goto out;
+	}
+
+	/* At the moment, we have no way of interrogating the DPMAC
+	 * from the DPNI side - and for that matter there may exist
+	 * no DPMAC at all. So for now we just don't report anything
+	 * beyond the DPNI attributes.
+	 */
+	if (state.options & DPNI_LINK_OPT_AUTONEG)
+		link_settings->base.autoneg = AUTONEG_ENABLE;
+	if (!(state.options & DPNI_LINK_OPT_HALF_DUPLEX))
+		link_settings->base.duplex = DUPLEX_FULL;
+	link_settings->base.speed = state.rate;
+
+out:
+	return err;
+}
+
+static int
+dpaa2_eth_set_link_ksettings(struct net_device *net_dev,
+			     const struct ethtool_link_ksettings *link_settings)
+{
+	struct dpni_link_cfg cfg = {0};
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	int err = 0;
+
+	netdev_dbg(net_dev, "Setting link parameters...");
+
+	/* Due to a temporary MC limitation, the DPNI must be down
+	 * in order to be able to change link settings. Taking steps to let
+	 * the user know that.
+	 */
+	if (netif_running(net_dev)) {
+		netdev_info(net_dev, "Sorry, interface must be brought down first.\n");
+		return -EACCES;
+	}
+
+	cfg.rate = link_settings->base.speed;
+	if (link_settings->base.autoneg == AUTONEG_ENABLE)
+		cfg.options |= DPNI_LINK_OPT_AUTONEG;
+	else
+		cfg.options &= ~DPNI_LINK_OPT_AUTONEG;
+	if (link_settings->base.duplex  == DUPLEX_HALF)
+		cfg.options |= DPNI_LINK_OPT_HALF_DUPLEX;
+	else
+		cfg.options &= ~DPNI_LINK_OPT_HALF_DUPLEX;
+
+	err = dpni_set_link_cfg(priv->mc_io, 0, priv->mc_token, &cfg);
+	if (err)
+		/* ethtool will be loud enough if we return an error; no point
+		 * in putting our own error message on the console by default
+		 */
+		netdev_dbg(net_dev, "ERROR %d setting link cfg", err);
+
+	return err;
+}
+
+static void dpaa2_eth_get_strings(struct net_device *netdev, u32 stringset,
+				  u8 *data)
+{
+	u8 *p = data;
+	int i;
+
+	switch (stringset) {
+	case ETH_SS_STATS:
+		for (i = 0; i < DPAA2_ETH_NUM_STATS; i++) {
+			strlcpy(p, dpaa2_ethtool_stats[i], ETH_GSTRING_LEN);
+			p += ETH_GSTRING_LEN;
+		}
+		for (i = 0; i < DPAA2_ETH_NUM_EXTRA_STATS; i++) {
+			strlcpy(p, dpaa2_ethtool_extras[i], ETH_GSTRING_LEN);
+			p += ETH_GSTRING_LEN;
+		}
+		break;
+	}
+}
+
+static int dpaa2_eth_get_sset_count(struct net_device *net_dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS: /* ethtool_get_stats(), ethtool_get_drvinfo() */
+		return DPAA2_ETH_NUM_STATS + DPAA2_ETH_NUM_EXTRA_STATS;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+/** Fill in hardware counters, as returned by MC.
+ */
+static void dpaa2_eth_get_ethtool_stats(struct net_device *net_dev,
+					struct ethtool_stats *stats,
+					u64 *data)
+{
+	int i = 0;
+	int j, k, err;
+	int num_cnt;
+	union dpni_statistics dpni_stats;
+	u64 cdan = 0;
+	u64 portal_busy = 0, pull_err = 0;
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+	struct dpaa2_eth_drv_stats *extras;
+	struct dpaa2_eth_ch_stats *ch_stats;
+
+	memset(data, 0,
+	       sizeof(u64) * (DPAA2_ETH_NUM_STATS + DPAA2_ETH_NUM_EXTRA_STATS));
+
+	/* Print standard counters, from DPNI statistics */
+	for (j = 0; j <= 2; j++) {
+		err = dpni_get_statistics(priv->mc_io, 0, priv->mc_token,
+					  j, &dpni_stats);
+		if (err != 0)
+			netdev_warn(net_dev, "dpni_get_stats(%d) failed", j);
+		switch (j) {
+		case 0:
+			num_cnt = sizeof(dpni_stats.page_0) / sizeof(u64);
+			break;
+		case 1:
+			num_cnt = sizeof(dpni_stats.page_1) / sizeof(u64);
+			break;
+		case 2:
+			num_cnt = sizeof(dpni_stats.page_2) / sizeof(u64);
+			break;
+		default:
+			break;
+		}
+		for (k = 0; k < num_cnt; k++)
+			*(data + i++) = dpni_stats.raw.counter[k];
+	}
+
+	/* Print per-cpu extra stats */
+	for_each_online_cpu(k) {
+		extras = per_cpu_ptr(priv->percpu_extras, k);
+		for (j = 0; j < sizeof(*extras) / sizeof(__u64); j++)
+			*((__u64 *)data + i + j) += *((__u64 *)extras + j);
+	}
+	i += j;
+
+	for (j = 0; j < priv->num_channels; j++) {
+		ch_stats = &priv->channel[j]->stats;
+		cdan += ch_stats->cdan;
+		portal_busy += ch_stats->dequeue_portal_busy;
+		pull_err += ch_stats->pull_err;
+	}
+
+	*(data + i++) = portal_busy;
+	*(data + i++) = pull_err;
+	*(data + i++) = cdan;
+}
+
+static int dpaa2_eth_get_rxnfc(struct net_device *net_dev,
+			       struct ethtool_rxnfc *rxnfc, u32 *rule_locs)
+{
+	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
+
+	switch (rxnfc->cmd) {
+	case ETHTOOL_GRXFH:
+		/* we purposely ignore cmd->flow_type for now, because the
+		 * classifier only supports a single set of fields for all
+		 * protocols
+		 */
+		rxnfc->data = priv->rx_hash_fields;
+		break;
+	case ETHTOOL_GRXRINGS:
+		rxnfc->data = dpaa2_eth_queue_count(priv);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+const struct ethtool_ops dpaa2_ethtool_ops = {
+	.get_drvinfo = dpaa2_eth_get_drvinfo,
+	.get_link = ethtool_op_get_link,
+	.get_link_ksettings = dpaa2_eth_get_link_ksettings,
+	.set_link_ksettings = dpaa2_eth_set_link_ksettings,
+	.get_sset_count = dpaa2_eth_get_sset_count,
+	.get_ethtool_stats = dpaa2_eth_get_ethtool_stats,
+	.get_strings = dpaa2_eth_get_strings,
+	.get_rxnfc = dpaa2_eth_get_rxnfc,
+};
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpkg.h b/drivers/staging/fsl-dpaa2/ethernet/dpkg.h
new file mode 100644
index 000000000000..02290a088391
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpkg.h
@@ -0,0 +1,176 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef __FSL_DPKG_H_
+#define __FSL_DPKG_H_
+
+#include <linux/types.h>
+#include "net.h"
+
+/* Data Path Key Generator API
+ * Contains initialization APIs and runtime APIs for the Key Generator
+ */
+
+/** Key Generator properties */
+
+/**
+ * Number of masks per key extraction
+ */
+#define DPKG_NUM_OF_MASKS		4
+/**
+ * Number of extractions per key profile
+ */
+#define DPKG_MAX_NUM_OF_EXTRACTS	10
+
+/**
+ * enum dpkg_extract_from_hdr_type - Selecting extraction by header types
+ * @DPKG_FROM_HDR: Extract selected bytes from header, by offset
+ * @DPKG_FROM_FIELD: Extract selected bytes from header, by offset from field
+ * @DPKG_FULL_FIELD: Extract a full field
+ */
+enum dpkg_extract_from_hdr_type {
+	DPKG_FROM_HDR = 0,
+	DPKG_FROM_FIELD = 1,
+	DPKG_FULL_FIELD = 2
+};
+
+/**
+ * enum dpkg_extract_type - Enumeration for selecting extraction type
+ * @DPKG_EXTRACT_FROM_HDR: Extract from the header
+ * @DPKG_EXTRACT_FROM_DATA: Extract from data not in specific header
+ * @DPKG_EXTRACT_FROM_PARSE: Extract from parser-result;
+ *	e.g. can be used to extract header existence;
+ *	please refer to 'Parse Result definition' section in the parser BG
+ */
+enum dpkg_extract_type {
+	DPKG_EXTRACT_FROM_HDR = 0,
+	DPKG_EXTRACT_FROM_DATA = 1,
+	DPKG_EXTRACT_FROM_PARSE = 3
+};
+
+/**
+ * struct dpkg_mask - A structure for defining a single extraction mask
+ * @mask: Byte mask for the extracted content
+ * @offset: Offset within the extracted content
+ */
+struct dpkg_mask {
+	u8 mask;
+	u8 offset;
+};
+
+/**
+ * struct dpkg_extract - A structure for defining a single extraction
+ * @type: Determines how the union below is interpreted:
+ *		DPKG_EXTRACT_FROM_HDR: selects 'from_hdr';
+ *		DPKG_EXTRACT_FROM_DATA: selects 'from_data';
+ *		DPKG_EXTRACT_FROM_PARSE: selects 'from_parse'
+ * @extract: Selects extraction method
+ * @num_of_byte_masks: Defines the number of valid entries in the array below;
+ *		This is	also the number of bytes to be used as masks
+ * @masks: Masks parameters
+ */
+struct dpkg_extract {
+	enum dpkg_extract_type type;
+	/**
+	 * union extract - Selects extraction method
+	 * @from_hdr - Used when 'type = DPKG_EXTRACT_FROM_HDR'
+	 * @from_data - Used when 'type = DPKG_EXTRACT_FROM_DATA'
+	 * @from_parse - Used when 'type = DPKG_EXTRACT_FROM_PARSE'
+	 */
+	union {
+		/**
+		 * struct from_hdr - Used when 'type = DPKG_EXTRACT_FROM_HDR'
+		 * @prot: Any of the supported headers
+		 * @type: Defines the type of header extraction:
+		 *	DPKG_FROM_HDR: use size & offset below;
+		 *	DPKG_FROM_FIELD: use field, size and offset below;
+		 *	DPKG_FULL_FIELD: use field below
+		 * @field: One of the supported fields (NH_FLD_)
+		 *
+		 * @size: Size in bytes
+		 * @offset: Byte offset
+		 * @hdr_index: Clear for cases not listed below;
+		 *	Used for protocols that may have more than a single
+		 *	header, 0 indicates an outer header;
+		 *	Supported protocols (possible values):
+		 *	NET_PROT_VLAN (0, HDR_INDEX_LAST);
+		 *	NET_PROT_MPLS (0, 1, HDR_INDEX_LAST);
+		 *	NET_PROT_IP(0, HDR_INDEX_LAST);
+		 *	NET_PROT_IPv4(0, HDR_INDEX_LAST);
+		 *	NET_PROT_IPv6(0, HDR_INDEX_LAST);
+		 */
+
+		struct {
+			enum net_prot			prot;
+			enum dpkg_extract_from_hdr_type type;
+			u32			field;
+			u8			size;
+			u8			offset;
+			u8			hdr_index;
+		} from_hdr;
+		/**
+		 * struct from_data - Used when 'type = DPKG_EXTRACT_FROM_DATA'
+		 * @size: Size in bytes
+		 * @offset: Byte offset
+		 */
+		struct {
+			u8 size;
+			u8 offset;
+		} from_data;
+
+		/**
+		 * struct from_parse - Used when
+		 *		       'type = DPKG_EXTRACT_FROM_PARSE'
+		 * @size: Size in bytes
+		 * @offset: Byte offset
+		 */
+		struct {
+			u8 size;
+			u8 offset;
+		} from_parse;
+	} extract;
+
+	u8		num_of_byte_masks;
+	struct dpkg_mask	masks[DPKG_NUM_OF_MASKS];
+};
+
+/**
+ * struct dpkg_profile_cfg - A structure for defining a full Key Generation
+ *				profile (rule)
+ * @num_extracts: Defines the number of valid entries in the array below
+ * @extracts: Array of required extractions
+ */
+struct dpkg_profile_cfg {
+	u8 num_extracts;
+	struct dpkg_extract extracts[DPKG_MAX_NUM_OF_EXTRACTS];
+};
+
+#endif /* __FSL_DPKG_H_ */
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpni-cmd.h b/drivers/staging/fsl-dpaa2/ethernet/dpni-cmd.h
new file mode 100644
index 000000000000..57df22292233
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpni-cmd.h
@@ -0,0 +1,541 @@
+/* Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef _FSL_DPNI_CMD_H
+#define _FSL_DPNI_CMD_H
+
+#include "dpni.h"
+
+/* DPNI Version */
+#define DPNI_VER_MAJOR				7
+#define DPNI_VER_MINOR				0
+#define DPNI_CMD_BASE_VERSION			1
+#define DPNI_CMD_ID_OFFSET			4
+
+#define DPNI_CMD(id)	(((id) << DPNI_CMD_ID_OFFSET) | DPNI_CMD_BASE_VERSION)
+
+#define DPNI_CMDID_OPEN					DPNI_CMD(0x801)
+#define DPNI_CMDID_CLOSE				DPNI_CMD(0x800)
+#define DPNI_CMDID_CREATE				DPNI_CMD(0x901)
+#define DPNI_CMDID_DESTROY				DPNI_CMD(0x900)
+#define DPNI_CMDID_GET_API_VERSION			DPNI_CMD(0xa01)
+
+#define DPNI_CMDID_ENABLE				DPNI_CMD(0x002)
+#define DPNI_CMDID_DISABLE				DPNI_CMD(0x003)
+#define DPNI_CMDID_GET_ATTR				DPNI_CMD(0x004)
+#define DPNI_CMDID_RESET				DPNI_CMD(0x005)
+#define DPNI_CMDID_IS_ENABLED				DPNI_CMD(0x006)
+
+#define DPNI_CMDID_SET_IRQ				DPNI_CMD(0x010)
+#define DPNI_CMDID_GET_IRQ				DPNI_CMD(0x011)
+#define DPNI_CMDID_SET_IRQ_ENABLE			DPNI_CMD(0x012)
+#define DPNI_CMDID_GET_IRQ_ENABLE			DPNI_CMD(0x013)
+#define DPNI_CMDID_SET_IRQ_MASK				DPNI_CMD(0x014)
+#define DPNI_CMDID_GET_IRQ_MASK				DPNI_CMD(0x015)
+#define DPNI_CMDID_GET_IRQ_STATUS			DPNI_CMD(0x016)
+#define DPNI_CMDID_CLEAR_IRQ_STATUS			DPNI_CMD(0x017)
+
+#define DPNI_CMDID_SET_POOLS				DPNI_CMD(0x200)
+#define DPNI_CMDID_SET_ERRORS_BEHAVIOR			DPNI_CMD(0x20B)
+
+#define DPNI_CMDID_GET_QDID				DPNI_CMD(0x210)
+#define DPNI_CMDID_GET_TX_DATA_OFFSET			DPNI_CMD(0x212)
+#define DPNI_CMDID_GET_LINK_STATE			DPNI_CMD(0x215)
+#define DPNI_CMDID_SET_MAX_FRAME_LENGTH			DPNI_CMD(0x216)
+#define DPNI_CMDID_GET_MAX_FRAME_LENGTH			DPNI_CMD(0x217)
+#define DPNI_CMDID_SET_LINK_CFG				DPNI_CMD(0x21A)
+#define DPNI_CMDID_SET_TX_SHAPING			DPNI_CMD(0x21B)
+
+#define DPNI_CMDID_SET_MCAST_PROMISC			DPNI_CMD(0x220)
+#define DPNI_CMDID_GET_MCAST_PROMISC			DPNI_CMD(0x221)
+#define DPNI_CMDID_SET_UNICAST_PROMISC			DPNI_CMD(0x222)
+#define DPNI_CMDID_GET_UNICAST_PROMISC			DPNI_CMD(0x223)
+#define DPNI_CMDID_SET_PRIM_MAC				DPNI_CMD(0x224)
+#define DPNI_CMDID_GET_PRIM_MAC				DPNI_CMD(0x225)
+#define DPNI_CMDID_ADD_MAC_ADDR				DPNI_CMD(0x226)
+#define DPNI_CMDID_REMOVE_MAC_ADDR			DPNI_CMD(0x227)
+#define DPNI_CMDID_CLR_MAC_FILTERS			DPNI_CMD(0x228)
+
+#define DPNI_CMDID_SET_RX_TC_DIST			DPNI_CMD(0x235)
+
+#define DPNI_CMDID_ADD_FS_ENT				DPNI_CMD(0x244)
+#define DPNI_CMDID_REMOVE_FS_ENT			DPNI_CMD(0x245)
+#define DPNI_CMDID_CLR_FS_ENT				DPNI_CMD(0x246)
+
+#define DPNI_CMDID_GET_STATISTICS			DPNI_CMD(0x25D)
+#define DPNI_CMDID_GET_QUEUE				DPNI_CMD(0x25F)
+#define DPNI_CMDID_SET_QUEUE				DPNI_CMD(0x260)
+#define DPNI_CMDID_GET_TAILDROP				DPNI_CMD(0x261)
+#define DPNI_CMDID_SET_TAILDROP				DPNI_CMD(0x262)
+
+#define DPNI_CMDID_GET_PORT_MAC_ADDR			DPNI_CMD(0x263)
+
+#define DPNI_CMDID_GET_BUFFER_LAYOUT			DPNI_CMD(0x264)
+#define DPNI_CMDID_SET_BUFFER_LAYOUT			DPNI_CMD(0x265)
+
+#define DPNI_CMDID_SET_TX_CONFIRMATION_MODE		DPNI_CMD(0x266)
+#define DPNI_CMDID_SET_CONGESTION_NOTIFICATION		DPNI_CMD(0x267)
+#define DPNI_CMDID_GET_CONGESTION_NOTIFICATION		DPNI_CMD(0x268)
+#define DPNI_CMDID_SET_EARLY_DROP			DPNI_CMD(0x269)
+#define DPNI_CMDID_GET_EARLY_DROP			DPNI_CMD(0x26A)
+#define DPNI_CMDID_GET_OFFLOAD				DPNI_CMD(0x26B)
+#define DPNI_CMDID_SET_OFFLOAD				DPNI_CMD(0x26C)
+
+/* Macros for accessing command fields smaller than 1byte */
+#define DPNI_MASK(field)	\
+	GENMASK(DPNI_##field##_SHIFT + DPNI_##field##_SIZE - 1, \
+		DPNI_##field##_SHIFT)
+
+#define dpni_set_field(var, field, val)	\
+	((var) |= (((val) << DPNI_##field##_SHIFT) & DPNI_MASK(field)))
+#define dpni_get_field(var, field)	\
+	(((var) & DPNI_MASK(field)) >> DPNI_##field##_SHIFT)
+
+struct dpni_cmd_open {
+	__le32 dpni_id;
+};
+
+#define DPNI_BACKUP_POOL(val, order)	(((val) & 0x1) << (order))
+struct dpni_cmd_set_pools {
+	/* cmd word 0 */
+	u8 num_dpbp;
+	u8 backup_pool_mask;
+	__le16 pad;
+	/* cmd word 0..4 */
+	__le32 dpbp_id[DPNI_MAX_DPBP];
+	/* cmd word 4..6 */
+	__le16 buffer_size[DPNI_MAX_DPBP];
+};
+
+/* The enable indication is always the least significant bit */
+#define DPNI_ENABLE_SHIFT		0
+#define DPNI_ENABLE_SIZE		1
+
+struct dpni_rsp_is_enabled {
+	u8 enabled;
+};
+
+struct dpni_rsp_get_irq {
+	/* response word 0 */
+	__le32 irq_val;
+	__le32 pad;
+	/* response word 1 */
+	__le64 irq_addr;
+	/* response word 2 */
+	__le32 irq_num;
+	__le32 type;
+};
+
+struct dpni_cmd_set_irq_enable {
+	u8 enable;
+	u8 pad[3];
+	u8 irq_index;
+};
+
+struct dpni_cmd_get_irq_enable {
+	__le32 pad;
+	u8 irq_index;
+};
+
+struct dpni_rsp_get_irq_enable {
+	u8 enabled;
+};
+
+struct dpni_cmd_set_irq_mask {
+	__le32 mask;
+	u8 irq_index;
+};
+
+struct dpni_cmd_get_irq_mask {
+	__le32 pad;
+	u8 irq_index;
+};
+
+struct dpni_rsp_get_irq_mask {
+	__le32 mask;
+};
+
+struct dpni_cmd_get_irq_status {
+	__le32 status;
+	u8 irq_index;
+};
+
+struct dpni_rsp_get_irq_status {
+	__le32 status;
+};
+
+struct dpni_cmd_clear_irq_status {
+	__le32 status;
+	u8 irq_index;
+};
+
+struct dpni_rsp_get_attr {
+	/* response word 0 */
+	__le32 options;
+	u8 num_queues;
+	u8 num_tcs;
+	u8 mac_filter_entries;
+	u8 pad0;
+	/* response word 1 */
+	u8 vlan_filter_entries;
+	u8 pad1;
+	u8 qos_entries;
+	u8 pad2;
+	__le16 fs_entries;
+	__le16 pad3;
+	/* response word 2 */
+	u8 qos_key_size;
+	u8 fs_key_size;
+	__le16 wriop_version;
+};
+
+#define DPNI_ERROR_ACTION_SHIFT		0
+#define DPNI_ERROR_ACTION_SIZE		4
+#define DPNI_FRAME_ANN_SHIFT		4
+#define DPNI_FRAME_ANN_SIZE		1
+
+struct dpni_cmd_set_errors_behavior {
+	__le32 errors;
+	/* from least significant bit: error_action:4, set_frame_annotation:1 */
+	u8 flags;
+};
+
+/* There are 3 separate commands for configuring Rx, Tx and Tx confirmation
+ * buffer layouts, but they all share the same parameters.
+ * If one of the functions changes, below structure needs to be split.
+ */
+
+#define DPNI_PASS_TS_SHIFT		0
+#define DPNI_PASS_TS_SIZE		1
+#define DPNI_PASS_PR_SHIFT		1
+#define DPNI_PASS_PR_SIZE		1
+#define DPNI_PASS_FS_SHIFT		2
+#define DPNI_PASS_FS_SIZE		1
+
+struct dpni_cmd_get_buffer_layout {
+	u8 qtype;
+};
+
+struct dpni_rsp_get_buffer_layout {
+	/* response word 0 */
+	u8 pad0[6];
+	/* from LSB: pass_timestamp:1, parser_result:1, frame_status:1 */
+	u8 flags;
+	u8 pad1;
+	/* response word 1 */
+	__le16 private_data_size;
+	__le16 data_align;
+	__le16 head_room;
+	__le16 tail_room;
+};
+
+struct dpni_cmd_set_buffer_layout {
+	/* cmd word 0 */
+	u8 qtype;
+	u8 pad0[3];
+	__le16 options;
+	/* from LSB: pass_timestamp:1, parser_result:1, frame_status:1 */
+	u8 flags;
+	u8 pad1;
+	/* cmd word 1 */
+	__le16 private_data_size;
+	__le16 data_align;
+	__le16 head_room;
+	__le16 tail_room;
+};
+
+struct dpni_cmd_set_offload {
+	u8 pad[3];
+	u8 dpni_offload;
+	__le32 config;
+};
+
+struct dpni_cmd_get_offload {
+	u8 pad[3];
+	u8 dpni_offload;
+};
+
+struct dpni_rsp_get_offload {
+	__le32 pad;
+	__le32 config;
+};
+
+struct dpni_cmd_get_qdid {
+	u8 qtype;
+};
+
+struct dpni_rsp_get_qdid {
+	__le16 qdid;
+};
+
+struct dpni_rsp_get_tx_data_offset {
+	__le16 data_offset;
+};
+
+struct dpni_cmd_get_statistics {
+	u8 page_number;
+};
+
+struct dpni_rsp_get_statistics {
+	__le64 counter[DPNI_STATISTICS_CNT];
+};
+
+struct dpni_cmd_set_link_cfg {
+	/* cmd word 0 */
+	__le64 pad0;
+	/* cmd word 1 */
+	__le32 rate;
+	__le32 pad1;
+	/* cmd word 2 */
+	__le64 options;
+};
+
+#define DPNI_LINK_STATE_SHIFT		0
+#define DPNI_LINK_STATE_SIZE		1
+
+struct dpni_rsp_get_link_state {
+	/* response word 0 */
+	__le32 pad0;
+	/* from LSB: up:1 */
+	u8 flags;
+	u8 pad1[3];
+	/* response word 1 */
+	__le32 rate;
+	__le32 pad2;
+	/* response word 2 */
+	__le64 options;
+};
+
+struct dpni_cmd_set_max_frame_length {
+	__le16 max_frame_length;
+};
+
+struct dpni_rsp_get_max_frame_length {
+	__le16 max_frame_length;
+};
+
+struct dpni_cmd_set_multicast_promisc {
+	u8 enable;
+};
+
+struct dpni_rsp_get_multicast_promisc {
+	u8 enabled;
+};
+
+struct dpni_cmd_set_unicast_promisc {
+	u8 enable;
+};
+
+struct dpni_rsp_get_unicast_promisc {
+	u8 enabled;
+};
+
+struct dpni_cmd_set_primary_mac_addr {
+	__le16 pad;
+	u8 mac_addr[6];
+};
+
+struct dpni_rsp_get_primary_mac_addr {
+	__le16 pad;
+	u8 mac_addr[6];
+};
+
+struct dpni_rsp_get_port_mac_addr {
+	__le16 pad;
+	u8 mac_addr[6];
+};
+
+struct dpni_cmd_add_mac_addr {
+	__le16 pad;
+	u8 mac_addr[6];
+};
+
+struct dpni_cmd_remove_mac_addr {
+	__le16 pad;
+	u8 mac_addr[6];
+};
+
+#define DPNI_UNICAST_FILTERS_SHIFT	0
+#define DPNI_UNICAST_FILTERS_SIZE	1
+#define DPNI_MULTICAST_FILTERS_SHIFT	1
+#define DPNI_MULTICAST_FILTERS_SIZE	1
+
+struct dpni_cmd_clear_mac_filters {
+	/* from LSB: unicast:1, multicast:1 */
+	u8 flags;
+};
+
+#define DPNI_DIST_MODE_SHIFT		0
+#define DPNI_DIST_MODE_SIZE		4
+#define DPNI_MISS_ACTION_SHIFT		4
+#define DPNI_MISS_ACTION_SIZE		4
+
+struct dpni_cmd_set_rx_tc_dist {
+	/* cmd word 0 */
+	__le16 dist_size;
+	u8 tc_id;
+	/* from LSB: dist_mode:4, miss_action:4 */
+	u8 flags;
+	__le16 pad0;
+	__le16 default_flow_id;
+	/* cmd word 1..5 */
+	__le64 pad1[5];
+	/* cmd word 6 */
+	__le64 key_cfg_iova;
+};
+
+/* dpni_set_rx_tc_dist extension (structure of the DMA-able memory at
+ * key_cfg_iova)
+ */
+struct dpni_mask_cfg {
+	u8 mask;
+	u8 offset;
+};
+
+#define DPNI_EFH_TYPE_SHIFT		0
+#define DPNI_EFH_TYPE_SIZE		4
+#define DPNI_EXTRACT_TYPE_SHIFT		0
+#define DPNI_EXTRACT_TYPE_SIZE		4
+
+struct dpni_dist_extract {
+	/* word 0 */
+	u8 prot;
+	/* EFH type stored in the 4 least significant bits */
+	u8 efh_type;
+	u8 size;
+	u8 offset;
+	__le32 field;
+	/* word 1 */
+	u8 hdr_index;
+	u8 constant;
+	u8 num_of_repeats;
+	u8 num_of_byte_masks;
+	/* Extraction type is stored in the 4 LSBs */
+	u8 extract_type;
+	u8 pad[3];
+	/* word 2 */
+	struct dpni_mask_cfg masks[4];
+};
+
+struct dpni_ext_set_rx_tc_dist {
+	/* extension word 0 */
+	u8 num_extracts;
+	u8 pad[7];
+	/* words 1..25 */
+	struct dpni_dist_extract extracts[DPKG_MAX_NUM_OF_EXTRACTS];
+};
+
+struct dpni_cmd_get_queue {
+	u8 qtype;
+	u8 tc;
+	u8 index;
+};
+
+#define DPNI_DEST_TYPE_SHIFT		0
+#define DPNI_DEST_TYPE_SIZE		4
+#define DPNI_STASH_CTRL_SHIFT		6
+#define DPNI_STASH_CTRL_SIZE		1
+#define DPNI_HOLD_ACTIVE_SHIFT		7
+#define DPNI_HOLD_ACTIVE_SIZE		1
+
+struct dpni_rsp_get_queue {
+	/* response word 0 */
+	__le64 pad0;
+	/* response word 1 */
+	__le32 dest_id;
+	__le16 pad1;
+	u8 dest_prio;
+	/* From LSB: dest_type:4, pad:2, flc_stash_ctrl:1, hold_active:1 */
+	u8 flags;
+	/* response word 2 */
+	__le64 flc;
+	/* response word 3 */
+	__le64 user_context;
+	/* response word 4 */
+	__le32 fqid;
+	__le16 qdbin;
+};
+
+struct dpni_cmd_set_queue {
+	/* cmd word 0 */
+	u8 qtype;
+	u8 tc;
+	u8 index;
+	u8 options;
+	__le32 pad0;
+	/* cmd word 1 */
+	__le32 dest_id;
+	__le16 pad1;
+	u8 dest_prio;
+	u8 flags;
+	/* cmd word 2 */
+	__le64 flc;
+	/* cmd word 3 */
+	__le64 user_context;
+};
+
+struct dpni_cmd_set_taildrop {
+	/* cmd word 0 */
+	u8 congestion_point;
+	u8 qtype;
+	u8 tc;
+	u8 index;
+	__le32 pad0;
+	/* cmd word 1 */
+	/* Only least significant bit is relevant */
+	u8 enable;
+	u8 pad1;
+	u8 units;
+	u8 pad2;
+	__le32 threshold;
+};
+
+struct dpni_cmd_get_taildrop {
+	u8 congestion_point;
+	u8 qtype;
+	u8 tc;
+	u8 index;
+};
+
+struct dpni_rsp_get_taildrop {
+	/* cmd word 0 */
+	__le64 pad0;
+	/* cmd word 1 */
+	/* only least significant bit is relevant */
+	u8 enable;
+	u8 pad1;
+	u8 units;
+	u8 pad2;
+	__le32 threshold;
+};
+
+#endif /* _FSL_DPNI_CMD_H */
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpni.c b/drivers/staging/fsl-dpaa2/ethernet/dpni.c
new file mode 100644
index 000000000000..cea46edec6a2
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpni.c
@@ -0,0 +1,1595 @@
+/* Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#include "../../fsl-mc/include/mc-sys.h"
+#include "../../fsl-mc/include/mc-cmd.h"
+#include "dpni.h"
+#include "dpni-cmd.h"
+
+/**
+ * dpni_prepare_key_cfg() - function prepare extract parameters
+ * @cfg: defining a full Key Generation profile (rule)
+ * @key_cfg_buf: Zeroed 256 bytes of memory before mapping it to DMA
+ *
+ * This function has to be called before the following functions:
+ *	- dpni_set_rx_tc_dist()
+ *	- dpni_set_qos_table()
+ */
+int dpni_prepare_key_cfg(const struct dpkg_profile_cfg *cfg, u8 *key_cfg_buf)
+{
+	int i, j;
+	struct dpni_ext_set_rx_tc_dist *dpni_ext;
+	struct dpni_dist_extract *extr;
+
+	if (cfg->num_extracts > DPKG_MAX_NUM_OF_EXTRACTS)
+		return -EINVAL;
+
+	dpni_ext = (struct dpni_ext_set_rx_tc_dist *)key_cfg_buf;
+	dpni_ext->num_extracts = cfg->num_extracts;
+
+	for (i = 0; i < cfg->num_extracts; i++) {
+		extr = &dpni_ext->extracts[i];
+
+		switch (cfg->extracts[i].type) {
+		case DPKG_EXTRACT_FROM_HDR:
+			extr->prot = cfg->extracts[i].extract.from_hdr.prot;
+			dpni_set_field(extr->efh_type, EFH_TYPE,
+				       cfg->extracts[i].extract.from_hdr.type);
+			extr->size = cfg->extracts[i].extract.from_hdr.size;
+			extr->offset = cfg->extracts[i].extract.from_hdr.offset;
+			extr->field = cpu_to_le32(
+				cfg->extracts[i].extract.from_hdr.field);
+			extr->hdr_index =
+				cfg->extracts[i].extract.from_hdr.hdr_index;
+			break;
+		case DPKG_EXTRACT_FROM_DATA:
+			extr->size = cfg->extracts[i].extract.from_data.size;
+			extr->offset =
+				cfg->extracts[i].extract.from_data.offset;
+			break;
+		case DPKG_EXTRACT_FROM_PARSE:
+			extr->size = cfg->extracts[i].extract.from_parse.size;
+			extr->offset =
+				cfg->extracts[i].extract.from_parse.offset;
+			break;
+		default:
+			return -EINVAL;
+		}
+
+		extr->num_of_byte_masks = cfg->extracts[i].num_of_byte_masks;
+		dpni_set_field(extr->extract_type, EXTRACT_TYPE,
+			       cfg->extracts[i].type);
+
+		for (j = 0; j < DPKG_NUM_OF_MASKS; j++) {
+			extr->masks[j].mask = cfg->extracts[i].masks[j].mask;
+			extr->masks[j].offset =
+				cfg->extracts[i].masks[j].offset;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * dpni_open() - Open a control session for the specified object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @dpni_id:	DPNI unique ID
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * This function can be used to open a control session for an
+ * already created object; an object may have been declared in
+ * the DPL or by calling the dpni_create() function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent commands for
+ * this specific object.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_open(struct fsl_mc_io *mc_io,
+	      u32 cmd_flags,
+	      int dpni_id,
+	      u16 *token)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_open *cmd_params;
+
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_OPEN,
+					  cmd_flags,
+					  0);
+	cmd_params = (struct dpni_cmd_open *)cmd.params;
+	cmd_params->dpni_id = cpu_to_le32(dpni_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = mc_cmd_hdr_read_token(&cmd);
+
+	return 0;
+}
+
+/**
+ * dpni_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ *
+ * After this function is called, no further operations are
+ * allowed on the object without opening a new control session.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_close(struct fsl_mc_io *mc_io,
+	       u32 cmd_flags,
+	       u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_CLOSE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_set_pools() - Set buffer pools configuration
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @cfg:	Buffer pools configuration
+ *
+ * mandatory for DPNI operation
+ * warning:Allowed only when DPNI is disabled
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_pools(struct fsl_mc_io *mc_io,
+		   u32 cmd_flags,
+		   u16 token,
+		   const struct dpni_pools_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_pools *cmd_params;
+	int i;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_POOLS,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_pools *)cmd.params;
+	cmd_params->num_dpbp = cfg->num_dpbp;
+	for (i = 0; i < DPNI_MAX_DPBP; i++) {
+		cmd_params->dpbp_id[i] = cpu_to_le32(cfg->pools[i].dpbp_id);
+		cmd_params->buffer_size[i] =
+			cpu_to_le16(cfg->pools[i].buffer_size);
+		cmd_params->backup_pool_mask |=
+			DPNI_BACKUP_POOL(cfg->pools[i].backup_pool, i);
+	}
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_enable() - Enable the DPNI, allow sending and receiving frames.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:		Token of DPNI object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_enable(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_ENABLE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_disable() - Disable the DPNI, stop sending and receiving frames.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_disable(struct fsl_mc_io *mc_io,
+		 u32 cmd_flags,
+		 u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_DISABLE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_is_enabled() - Check if the DPNI is enabled.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @en:		Returns '1' if object is enabled; '0' otherwise
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_is_enabled(struct fsl_mc_io *mc_io,
+		    u32 cmd_flags,
+		    u16 token,
+		    int *en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_is_enabled *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_IS_ENABLED,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_is_enabled *)cmd.params;
+	*en = dpni_get_field(rsp_params->enabled, ENABLE);
+
+	return 0;
+}
+
+/**
+ * dpni_reset() - Reset the DPNI, returns the object to initial state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_reset(struct fsl_mc_io *mc_io,
+	       u32 cmd_flags,
+	       u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_RESET,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_set_irq_enable() - Set overall interrupt state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @en:		Interrupt state: - enable = 1, disable = 0
+ *
+ * Allows GPP software to control when interrupts are generated.
+ * Each interrupt can have up to 32 causes.  The enable/disable control's the
+ * overall interrupt state. if the interrupt is disabled no causes will cause
+ * an interrupt.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_irq_enable(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			u8 irq_index,
+			u8 en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_irq_enable *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_IRQ_ENABLE,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_irq_enable *)cmd.params;
+	dpni_set_field(cmd_params->enable, ENABLE, en);
+	cmd_params->irq_index = irq_index;
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_irq_enable() - Get overall interrupt state
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @en:		Returned interrupt state - enable = 1, disable = 0
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_irq_enable(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			u8 irq_index,
+			u8 *en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_irq_enable *cmd_params;
+	struct dpni_rsp_get_irq_enable *rsp_params;
+
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_IRQ_ENABLE,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_irq_enable *)cmd.params;
+	cmd_params->irq_index = irq_index;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_irq_enable *)cmd.params;
+	*en = dpni_get_field(rsp_params->enabled, ENABLE);
+
+	return 0;
+}
+
+/**
+ * dpni_set_irq_mask() - Set interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	event mask to trigger interrupt;
+ *			each bit:
+ *				0 = ignore event
+ *				1 = consider event for asserting IRQ
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_irq_mask(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      u8 irq_index,
+		      u32 mask)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_irq_mask *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_IRQ_MASK,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_irq_mask *)cmd.params;
+	cmd_params->mask = cpu_to_le32(mask);
+	cmd_params->irq_index = irq_index;
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_irq_mask() - Get interrupt mask.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @mask:	Returned event mask to trigger interrupt
+ *
+ * Every interrupt can have up to 32 causes and the interrupt model supports
+ * masking/unmasking each cause independently
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_irq_mask(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      u8 irq_index,
+		      u32 *mask)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_irq_mask *cmd_params;
+	struct dpni_rsp_get_irq_mask *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_IRQ_MASK,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_irq_mask *)cmd.params;
+	cmd_params->irq_index = irq_index;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_irq_mask *)cmd.params;
+	*mask = le32_to_cpu(rsp_params->mask);
+
+	return 0;
+}
+
+/**
+ * dpni_get_irq_status() - Get the current status of any pending interrupts.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @status:	Returned interrupts status - one bit per cause:
+ *			0 = no interrupt pending
+ *			1 = interrupt pending
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_irq_status(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			u8 irq_index,
+			u32 *status)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_irq_status *cmd_params;
+	struct dpni_rsp_get_irq_status *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_IRQ_STATUS,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_irq_status *)cmd.params;
+	cmd_params->status = cpu_to_le32(*status);
+	cmd_params->irq_index = irq_index;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_irq_status *)cmd.params;
+	*status = le32_to_cpu(rsp_params->status);
+
+	return 0;
+}
+
+/**
+ * dpni_clear_irq_status() - Clear a pending interrupt's status
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @irq_index:	The interrupt index to configure
+ * @status:	bits to clear (W1C) - one bit per cause:
+ *			0 = don't change
+ *			1 = clear status bit
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_clear_irq_status(struct fsl_mc_io *mc_io,
+			  u32 cmd_flags,
+			  u16 token,
+			  u8 irq_index,
+			  u32 status)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_clear_irq_status *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_CLEAR_IRQ_STATUS,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_clear_irq_status *)cmd.params;
+	cmd_params->irq_index = irq_index;
+	cmd_params->status = cpu_to_le32(status);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_attributes() - Retrieve DPNI attributes.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @attr:	Object's attributes
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_attributes(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			struct dpni_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_attr *rsp_params;
+
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_ATTR,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_attr *)cmd.params;
+	attr->options = le32_to_cpu(rsp_params->options);
+	attr->num_queues = rsp_params->num_queues;
+	attr->num_tcs = rsp_params->num_tcs;
+	attr->mac_filter_entries = rsp_params->mac_filter_entries;
+	attr->vlan_filter_entries = rsp_params->vlan_filter_entries;
+	attr->qos_entries = rsp_params->qos_entries;
+	attr->fs_entries = le16_to_cpu(rsp_params->fs_entries);
+	attr->qos_key_size = rsp_params->qos_key_size;
+	attr->fs_key_size = rsp_params->fs_key_size;
+	attr->wriop_version = le16_to_cpu(rsp_params->wriop_version);
+
+	return 0;
+}
+
+/**
+ * dpni_set_errors_behavior() - Set errors behavior
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @cfg:	Errors configuration
+ *
+ * this function may be called numerous times with different
+ * error masks
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_errors_behavior(struct fsl_mc_io *mc_io,
+			     u32 cmd_flags,
+			     u16 token,
+			     struct dpni_error_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_errors_behavior *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_ERRORS_BEHAVIOR,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_errors_behavior *)cmd.params;
+	cmd_params->errors = cpu_to_le32(cfg->errors);
+	dpni_set_field(cmd_params->flags, ERROR_ACTION, cfg->error_action);
+	dpni_set_field(cmd_params->flags, FRAME_ANN, cfg->set_frame_annotation);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_buffer_layout() - Retrieve buffer layout attributes.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @qtype:	Type of queue to retrieve configuration for
+ * @layout:	Returns buffer layout attributes
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_buffer_layout(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   enum dpni_queue_type qtype,
+			   struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_buffer_layout *cmd_params;
+	struct dpni_rsp_get_buffer_layout *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_BUFFER_LAYOUT,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_buffer_layout *)cmd.params;
+	cmd_params->qtype = qtype;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_buffer_layout *)cmd.params;
+	layout->pass_timestamp = dpni_get_field(rsp_params->flags, PASS_TS);
+	layout->pass_parser_result = dpni_get_field(rsp_params->flags, PASS_PR);
+	layout->pass_frame_status = dpni_get_field(rsp_params->flags, PASS_FS);
+	layout->private_data_size = le16_to_cpu(rsp_params->private_data_size);
+	layout->data_align = le16_to_cpu(rsp_params->data_align);
+	layout->data_head_room = le16_to_cpu(rsp_params->head_room);
+	layout->data_tail_room = le16_to_cpu(rsp_params->tail_room);
+
+	return 0;
+}
+
+/**
+ * dpni_set_buffer_layout() - Set buffer layout configuration.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @qtype:	Type of queue this configuration applies to
+ * @layout:	Buffer layout configuration
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ *
+ * @warning	Allowed only when DPNI is disabled
+ */
+int dpni_set_buffer_layout(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   enum dpni_queue_type qtype,
+			   const struct dpni_buffer_layout *layout)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_buffer_layout *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_BUFFER_LAYOUT,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_buffer_layout *)cmd.params;
+	cmd_params->qtype = qtype;
+	cmd_params->options = cpu_to_le16(layout->options);
+	dpni_set_field(cmd_params->flags, PASS_TS, layout->pass_timestamp);
+	dpni_set_field(cmd_params->flags, PASS_PR, layout->pass_parser_result);
+	dpni_set_field(cmd_params->flags, PASS_FS, layout->pass_frame_status);
+	cmd_params->private_data_size = cpu_to_le16(layout->private_data_size);
+	cmd_params->data_align = cpu_to_le16(layout->data_align);
+	cmd_params->head_room = cpu_to_le16(layout->data_head_room);
+	cmd_params->tail_room = cpu_to_le16(layout->data_tail_room);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_set_offload() - Set DPNI offload configuration.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @type:	Type of DPNI offload
+ * @config:	Offload configuration.
+ *		For checksum offloads, non-zero value enables the offload
+ *
+ * Return:     '0' on Success; Error code otherwise.
+ *
+ * @warning    Allowed only when DPNI is disabled
+ */
+
+int dpni_set_offload(struct fsl_mc_io *mc_io,
+		     u32 cmd_flags,
+		     u16 token,
+		     enum dpni_offload type,
+		     u32 config)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_offload *cmd_params;
+
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_OFFLOAD,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_offload *)cmd.params;
+	cmd_params->dpni_offload = type;
+	cmd_params->config = cpu_to_le32(config);
+
+	return mc_send_command(mc_io, &cmd);
+}
+
+int dpni_get_offload(struct fsl_mc_io *mc_io,
+		     u32 cmd_flags,
+		     u16 token,
+		     enum dpni_offload type,
+		     u32 *config)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_offload *cmd_params;
+	struct dpni_rsp_get_offload *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_OFFLOAD,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_offload *)cmd.params;
+	cmd_params->dpni_offload = type;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_offload *)cmd.params;
+	*config = le32_to_cpu(rsp_params->config);
+
+	return 0;
+}
+
+/**
+ * dpni_get_qdid() - Get the Queuing Destination ID (QDID) that should be used
+ *			for enqueue operations
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @qtype:	Type of queue to receive QDID for
+ * @qdid:	Returned virtual QDID value that should be used as an argument
+ *			in all enqueue operations
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_qdid(struct fsl_mc_io *mc_io,
+		  u32 cmd_flags,
+		  u16 token,
+		  enum dpni_queue_type qtype,
+		  u16 *qdid)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_qdid *cmd_params;
+	struct dpni_rsp_get_qdid *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_QDID,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_qdid *)cmd.params;
+	cmd_params->qtype = qtype;
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_qdid *)cmd.params;
+	*qdid = le16_to_cpu(rsp_params->qdid);
+
+	return 0;
+}
+
+/**
+ * dpni_get_tx_data_offset() - Get the Tx data offset (from start of buffer)
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @data_offset: Tx data offset (from start of buffer)
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_tx_data_offset(struct fsl_mc_io *mc_io,
+			    u32 cmd_flags,
+			    u16 token,
+			    u16 *data_offset)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_tx_data_offset *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TX_DATA_OFFSET,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_tx_data_offset *)cmd.params;
+	*data_offset = le16_to_cpu(rsp_params->data_offset);
+
+	return 0;
+}
+
+/**
+ * dpni_set_link_cfg() - set the link configuration.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @cfg:	Link configuration
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_link_cfg(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      const struct dpni_link_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_link_cfg *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_LINK_CFG,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_link_cfg *)cmd.params;
+	cmd_params->rate = cpu_to_le32(cfg->rate);
+	cmd_params->options = cpu_to_le64(cfg->options);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_link_state() - Return the link state (either up or down)
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @state:	Returned link state;
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_link_state(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			struct dpni_link_state *state)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_link_state *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_LINK_STATE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_link_state *)cmd.params;
+	state->up = dpni_get_field(rsp_params->flags, LINK_STATE);
+	state->rate = le32_to_cpu(rsp_params->rate);
+	state->options = le64_to_cpu(rsp_params->options);
+
+	return 0;
+}
+
+/**
+ * dpni_set_max_frame_length() - Set the maximum received frame length.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @max_frame_length:	Maximum received frame length (in
+ *				bytes); frame is discarded if its
+ *				length exceeds this value
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_max_frame_length(struct fsl_mc_io *mc_io,
+			      u32 cmd_flags,
+			      u16 token,
+			      u16 max_frame_length)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_max_frame_length *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_MAX_FRAME_LENGTH,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_max_frame_length *)cmd.params;
+	cmd_params->max_frame_length = cpu_to_le16(max_frame_length);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_max_frame_length() - Get the maximum received frame length.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @max_frame_length:	Maximum received frame length (in
+ *				bytes); frame is discarded if its
+ *				length exceeds this value
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_max_frame_length(struct fsl_mc_io *mc_io,
+			      u32 cmd_flags,
+			      u16 token,
+			      u16 *max_frame_length)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_max_frame_length *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_MAX_FRAME_LENGTH,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_max_frame_length *)cmd.params;
+	*max_frame_length = le16_to_cpu(rsp_params->max_frame_length);
+
+	return 0;
+}
+
+/**
+ * dpni_set_multicast_promisc() - Enable/disable multicast promiscuous mode
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @en:		Set to '1' to enable; '0' to disable
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_multicast_promisc(struct fsl_mc_io *mc_io,
+			       u32 cmd_flags,
+			       u16 token,
+			       int en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_multicast_promisc *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_MCAST_PROMISC,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_multicast_promisc *)cmd.params;
+	dpni_set_field(cmd_params->enable, ENABLE, en);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_multicast_promisc() - Get multicast promiscuous mode
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @en:		Returns '1' if enabled; '0' otherwise
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_multicast_promisc(struct fsl_mc_io *mc_io,
+			       u32 cmd_flags,
+			       u16 token,
+			       int *en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_multicast_promisc *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_MCAST_PROMISC,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_multicast_promisc *)cmd.params;
+	*en = dpni_get_field(rsp_params->enabled, ENABLE);
+
+	return 0;
+}
+
+/**
+ * dpni_set_unicast_promisc() - Enable/disable unicast promiscuous mode
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @en:		Set to '1' to enable; '0' to disable
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_unicast_promisc(struct fsl_mc_io *mc_io,
+			     u32 cmd_flags,
+			     u16 token,
+			     int en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_unicast_promisc *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_UNICAST_PROMISC,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_unicast_promisc *)cmd.params;
+	dpni_set_field(cmd_params->enable, ENABLE, en);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_unicast_promisc() - Get unicast promiscuous mode
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @en:		Returns '1' if enabled; '0' otherwise
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_unicast_promisc(struct fsl_mc_io *mc_io,
+			     u32 cmd_flags,
+			     u16 token,
+			     int *en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_unicast_promisc *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_UNICAST_PROMISC,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_unicast_promisc *)cmd.params;
+	*en = dpni_get_field(rsp_params->enabled, ENABLE);
+
+	return 0;
+}
+
+/**
+ * dpni_set_primary_mac_addr() - Set the primary MAC address
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @mac_addr:	MAC address to set as primary address
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_primary_mac_addr(struct fsl_mc_io *mc_io,
+			      u32 cmd_flags,
+			      u16 token,
+			      const u8 mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_primary_mac_addr *cmd_params;
+	int i;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_PRIM_MAC,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_primary_mac_addr *)cmd.params;
+	for (i = 0; i < 6; i++)
+		cmd_params->mac_addr[i] = mac_addr[5 - i];
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_primary_mac_addr() - Get the primary MAC address
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @mac_addr:	Returned MAC address
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_primary_mac_addr(struct fsl_mc_io *mc_io,
+			      u32 cmd_flags,
+			      u16 token,
+			      u8 mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_primary_mac_addr *rsp_params;
+	int i, err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_PRIM_MAC,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_primary_mac_addr *)cmd.params;
+	for (i = 0; i < 6; i++)
+		mac_addr[5 - i] = rsp_params->mac_addr[i];
+
+	return 0;
+}
+
+/**
+ * dpni_get_port_mac_addr() - Retrieve MAC address associated to the physical
+ *			port the DPNI is attached to
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @mac_addr:	MAC address of the physical port, if any, otherwise 0
+ *
+ * The primary MAC address is not cleared by this operation.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_port_mac_addr(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   u8 mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_rsp_get_port_mac_addr *rsp_params;
+	int i, err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_PORT_MAC_ADDR,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_port_mac_addr *)cmd.params;
+	for (i = 0; i < 6; i++)
+		mac_addr[5 - i] = rsp_params->mac_addr[i];
+
+	return 0;
+}
+
+/**
+ * dpni_add_mac_addr() - Add MAC address filter
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @mac_addr:	MAC address to add
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_add_mac_addr(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      const u8 mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_add_mac_addr *cmd_params;
+	int i;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_ADD_MAC_ADDR,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_add_mac_addr *)cmd.params;
+	for (i = 0; i < 6; i++)
+		cmd_params->mac_addr[i] = mac_addr[5 - i];
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_remove_mac_addr() - Remove MAC address filter
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @mac_addr:	MAC address to remove
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_remove_mac_addr(struct fsl_mc_io *mc_io,
+			 u32 cmd_flags,
+			 u16 token,
+			 const u8 mac_addr[6])
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_remove_mac_addr *cmd_params;
+	int i;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_REMOVE_MAC_ADDR,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_remove_mac_addr *)cmd.params;
+	for (i = 0; i < 6; i++)
+		cmd_params->mac_addr[i] = mac_addr[5 - i];
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_clear_mac_filters() - Clear all unicast and/or multicast MAC filters
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @unicast:	Set to '1' to clear unicast addresses
+ * @multicast:	Set to '1' to clear multicast addresses
+ *
+ * The primary MAC address is not cleared by this operation.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_clear_mac_filters(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   int unicast,
+			   int multicast)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_clear_mac_filters *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_CLR_MAC_FILTERS,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_clear_mac_filters *)cmd.params;
+	dpni_set_field(cmd_params->flags, UNICAST_FILTERS, unicast);
+	dpni_set_field(cmd_params->flags, MULTICAST_FILTERS, multicast);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_set_rx_tc_dist() - Set Rx traffic class distribution configuration
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @tc_id:	Traffic class selection (0-7)
+ * @cfg:	Traffic class distribution configuration
+ *
+ * warning: if 'dist_mode != DPNI_DIST_MODE_NONE', call dpni_prepare_key_cfg()
+ *			first to prepare the key_cfg_iova parameter
+ *
+ * Return:	'0' on Success; error code otherwise.
+ */
+int dpni_set_rx_tc_dist(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			u8 tc_id,
+			const struct dpni_rx_tc_dist_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_rx_tc_dist *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_RX_TC_DIST,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_rx_tc_dist *)cmd.params;
+	cmd_params->dist_size = cpu_to_le16(cfg->dist_size);
+	cmd_params->tc_id = tc_id;
+	dpni_set_field(cmd_params->flags, DIST_MODE, cfg->dist_mode);
+	dpni_set_field(cmd_params->flags, MISS_ACTION, cfg->fs_cfg.miss_action);
+	cmd_params->default_flow_id = cpu_to_le16(cfg->fs_cfg.default_flow_id);
+	cmd_params->key_cfg_iova = cpu_to_le64(cfg->key_cfg_iova);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_set_queue() - Set queue parameters
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @qtype:	Type of queue - all queue types are supported, although
+ *		the command is ignored for Tx
+ * @tc:		Traffic class, in range 0 to NUM_TCS - 1
+ * @index:	Selects the specific queue out of the set allocated for the
+ *		same TC. Value must be in range 0 to NUM_QUEUES - 1
+ * @options:	A combination of DPNI_QUEUE_OPT_ values that control what
+ *		configuration options are set on the queue
+ * @queue:	Queue structure
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_queue(struct fsl_mc_io *mc_io,
+		   u32 cmd_flags,
+		   u16 token,
+		   enum dpni_queue_type qtype,
+		   u8 tc,
+		   u8 index,
+		   u8 options,
+		   const struct dpni_queue *queue)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_queue *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_QUEUE,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_queue *)cmd.params;
+	cmd_params->qtype = qtype;
+	cmd_params->tc = tc;
+	cmd_params->index = index;
+	cmd_params->options = options;
+	cmd_params->dest_id = cpu_to_le32(queue->destination.id);
+	cmd_params->dest_prio = queue->destination.priority;
+	dpni_set_field(cmd_params->flags, DEST_TYPE, queue->destination.type);
+	dpni_set_field(cmd_params->flags, STASH_CTRL, queue->flc.stash_control);
+	dpni_set_field(cmd_params->flags, HOLD_ACTIVE,
+		       queue->destination.hold_active);
+	cmd_params->flc = cpu_to_le64(queue->flc.value);
+	cmd_params->user_context = cpu_to_le64(queue->user_context);
+
+	/* send command to mc */
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_queue() - Get queue parameters
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @qtype:	Type of queue - all queue types are supported
+ * @tc:		Traffic class, in range 0 to NUM_TCS - 1
+ * @index:	Selects the specific queue out of the set allocated for the
+ *		same TC. Value must be in range 0 to NUM_QUEUES - 1
+ * @queue:	Queue configuration structure
+ * @qid:	Queue identification
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_queue(struct fsl_mc_io *mc_io,
+		   u32 cmd_flags,
+		   u16 token,
+		   enum dpni_queue_type qtype,
+		   u8 tc,
+		   u8 index,
+		   struct dpni_queue *queue,
+		   struct dpni_queue_id *qid)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_queue *cmd_params;
+	struct dpni_rsp_get_queue *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_QUEUE,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_queue *)cmd.params;
+	cmd_params->qtype = qtype;
+	cmd_params->tc = tc;
+	cmd_params->index = index;
+
+	/* send command to mc */
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_queue *)cmd.params;
+	queue->destination.id = le32_to_cpu(rsp_params->dest_id);
+	queue->destination.priority = rsp_params->dest_prio;
+	queue->destination.type = dpni_get_field(rsp_params->flags,
+						     DEST_TYPE);
+	queue->flc.stash_control = dpni_get_field(rsp_params->flags,
+						  STASH_CTRL);
+	queue->destination.hold_active = dpni_get_field(rsp_params->flags,
+							HOLD_ACTIVE);
+	queue->flc.value = le64_to_cpu(rsp_params->flc);
+	queue->user_context = le64_to_cpu(rsp_params->user_context);
+	qid->fqid = le32_to_cpu(rsp_params->fqid);
+	qid->qdbin = le16_to_cpu(rsp_params->qdbin);
+
+	return 0;
+}
+
+/**
+ * dpni_get_statistics() - Get DPNI statistics
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @page:	Selects the statistics page to retrieve, see
+ *		DPNI_GET_STATISTICS output. Pages are numbered 0 to 2.
+ * @stat:	Structure containing the statistics
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_statistics(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			u8 page,
+			union dpni_statistics *stat)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_statistics *cmd_params;
+	struct dpni_rsp_get_statistics *rsp_params;
+	int i, err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_STATISTICS,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_statistics *)cmd.params;
+	cmd_params->page_number = page;
+
+	/* send command to mc */
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_statistics *)cmd.params;
+	for (i = 0; i < DPNI_STATISTICS_CNT; i++)
+		stat->raw.counter[i] = le64_to_cpu(rsp_params->counter[i]);
+
+	return 0;
+}
+
+/**
+ * dpni_set_taildrop() - Set taildrop per queue or TC
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @cg_point:	Congestion point
+ * @q_type:	Queue type on which the taildrop is configured.
+ *		Only Rx queues are supported for now
+ * @tc:		Traffic class to apply this taildrop to
+ * @q_index:	Index of the queue if the DPNI supports multiple queues for
+ *		traffic distribution. Ignored if CONGESTION_POINT is not 0.
+ * @taildrop:	Taildrop structure
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_set_taildrop(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      enum dpni_congestion_point cg_point,
+		      enum dpni_queue_type qtype,
+		      u8 tc,
+		      u8 index,
+		      struct dpni_taildrop *taildrop)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_set_taildrop *cmd_params;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_SET_TAILDROP,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_set_taildrop *)cmd.params;
+	cmd_params->congestion_point = cg_point;
+	cmd_params->qtype = qtype;
+	cmd_params->tc = tc;
+	cmd_params->index = index;
+	dpni_set_field(cmd_params->enable, ENABLE, taildrop->enable);
+	cmd_params->units = taildrop->units;
+	cmd_params->threshold = cpu_to_le32(taildrop->threshold);
+
+	/* send command to mc */
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpni_get_taildrop() - Get taildrop information
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPNI object
+ * @cg_point:	Congestion point
+ * @q_type:	Queue type on which the taildrop is configured.
+ *		Only Rx queues are supported for now
+ * @tc:		Traffic class to apply this taildrop to
+ * @q_index:	Index of the queue if the DPNI supports multiple queues for
+ *		traffic distribution. Ignored if CONGESTION_POINT is not 0.
+ * @taildrop:	Taildrop structure
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpni_get_taildrop(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      enum dpni_congestion_point cg_point,
+		      enum dpni_queue_type qtype,
+		      u8 tc,
+		      u8 index,
+		      struct dpni_taildrop *taildrop)
+{
+	struct mc_command cmd = { 0 };
+	struct dpni_cmd_get_taildrop *cmd_params;
+	struct dpni_rsp_get_taildrop *rsp_params;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPNI_CMDID_GET_TAILDROP,
+					  cmd_flags,
+					  token);
+	cmd_params = (struct dpni_cmd_get_taildrop *)cmd.params;
+	cmd_params->congestion_point = cg_point;
+	cmd_params->qtype = qtype;
+	cmd_params->tc = tc;
+	cmd_params->index = index;
+
+	/* send command to mc */
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	rsp_params = (struct dpni_rsp_get_taildrop *)cmd.params;
+	taildrop->enable = dpni_get_field(rsp_params->enable, ENABLE);
+	taildrop->units = rsp_params->units;
+	taildrop->threshold = le32_to_cpu(rsp_params->threshold);
+
+	return 0;
+}
diff --git a/drivers/staging/fsl-dpaa2/ethernet/dpni.h b/drivers/staging/fsl-dpaa2/ethernet/dpni.h
new file mode 100644
index 000000000000..282e5e85ffa7
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/dpni.h
@@ -0,0 +1,832 @@
+/* Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef __FSL_DPNI_H
+#define __FSL_DPNI_H
+
+#include "dpkg.h"
+
+struct fsl_mc_io;
+
+/**
+ * Data Path Network Interface API
+ * Contains initialization APIs and runtime control APIs for DPNI
+ */
+
+/** General DPNI macros */
+
+/**
+ * Maximum number of traffic classes
+ */
+#define DPNI_MAX_TC				8
+/**
+ * Maximum number of buffer pools per DPNI
+ */
+#define DPNI_MAX_DPBP				8
+
+/**
+ * All traffic classes considered; see dpni_set_queue()
+ */
+#define DPNI_ALL_TCS				(u8)(-1)
+/**
+ * All flows within traffic class considered; see dpni_set_queue()
+ */
+#define DPNI_ALL_TC_FLOWS			(u16)(-1)
+/**
+ * Generate new flow ID; see dpni_set_queue()
+ */
+#define DPNI_NEW_FLOW_ID			(u16)(-1)
+
+/**
+ * Tx traffic is always released to a buffer pool on transmit, there are no
+ * resources allocated to have the frames confirmed back to the source after
+ * transmission.
+ */
+#define DPNI_OPT_TX_FRM_RELEASE			0x000001
+/**
+ * Disables support for MAC address filtering for addresses other than primary
+ * MAC address. This affects both unicast and multicast. Promiscuous mode can
+ * still be enabled/disabled for both unicast and multicast. If promiscuous mode
+ * is disabled, only traffic matching the primary MAC address will be accepted.
+ */
+#define DPNI_OPT_NO_MAC_FILTER			0x000002
+/**
+ * Allocate policers for this DPNI. They can be used to rate-limit traffic per
+ * traffic class (TC) basis.
+ */
+#define DPNI_OPT_HAS_POLICING			0x000004
+/**
+ * Congestion can be managed in several ways, allowing the buffer pool to
+ * deplete on ingress, taildrop on each queue or use congestion groups for sets
+ * of queues. If set, it configures a single congestion groups across all TCs.
+ * If reset, a congestion group is allocated for each TC. Only relevant if the
+ * DPNI has multiple traffic classes.
+ */
+#define DPNI_OPT_SHARED_CONGESTION		0x000008
+/**
+ * Enables TCAM for Flow Steering and QoS look-ups. If not specified, all
+ * look-ups are exact match. Note that TCAM is not available on LS1088 and its
+ * variants. Setting this bit on these SoCs will trigger an error.
+ */
+#define DPNI_OPT_HAS_KEY_MASKING		0x000010
+/**
+ * Disables the flow steering table.
+ */
+#define DPNI_OPT_NO_FS				0x000020
+
+int dpni_open(struct fsl_mc_io	*mc_io,
+	      u32		cmd_flags,
+	      int		dpni_id,
+	      u16		*token);
+
+int dpni_close(struct fsl_mc_io	*mc_io,
+	       u32		cmd_flags,
+	       u16		token);
+
+/**
+ * struct dpni_pools_cfg - Structure representing buffer pools configuration
+ * @num_dpbp: Number of DPBPs
+ * @pools: Array of buffer pools parameters; The number of valid entries
+ *	must match 'num_dpbp' value
+ */
+struct dpni_pools_cfg {
+	u8		num_dpbp;
+	/**
+	 * struct pools - Buffer pools parameters
+	 * @dpbp_id: DPBP object ID
+	 * @buffer_size: Buffer size
+	 * @backup_pool: Backup pool
+	 */
+	struct {
+		int	dpbp_id;
+		u16	buffer_size;
+		int	backup_pool;
+	} pools[DPNI_MAX_DPBP];
+};
+
+int dpni_set_pools(struct fsl_mc_io		*mc_io,
+		   u32				cmd_flags,
+		   u16				token,
+		   const struct dpni_pools_cfg	*cfg);
+
+int dpni_enable(struct fsl_mc_io	*mc_io,
+		u32			cmd_flags,
+		u16			token);
+
+int dpni_disable(struct fsl_mc_io	*mc_io,
+		 u32			cmd_flags,
+		 u16			token);
+
+int dpni_is_enabled(struct fsl_mc_io	*mc_io,
+		    u32			cmd_flags,
+		    u16			token,
+		    int			*en);
+
+int dpni_reset(struct fsl_mc_io	*mc_io,
+	       u32		cmd_flags,
+	       u16		token);
+
+/**
+ * DPNI IRQ Index and Events
+ */
+
+/**
+ * IRQ index
+ */
+#define DPNI_IRQ_INDEX				0
+/**
+ * IRQ event - indicates a change in link state
+ */
+#define DPNI_IRQ_EVENT_LINK_CHANGED		0x00000001
+
+int dpni_set_irq_enable(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			u8			irq_index,
+			u8			en);
+
+int dpni_get_irq_enable(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			u8			irq_index,
+			u8			*en);
+
+int dpni_set_irq_mask(struct fsl_mc_io	*mc_io,
+		      u32		cmd_flags,
+		      u16		token,
+		      u8		irq_index,
+		      u32		mask);
+
+int dpni_get_irq_mask(struct fsl_mc_io	*mc_io,
+		      u32		cmd_flags,
+		      u16		token,
+		      u8		irq_index,
+		      u32		*mask);
+
+int dpni_get_irq_status(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			u8			irq_index,
+			u32			*status);
+
+int dpni_clear_irq_status(struct fsl_mc_io	*mc_io,
+			  u32			cmd_flags,
+			  u16			token,
+			  u8			irq_index,
+			  u32			status);
+
+/**
+ * struct dpni_attr - Structure representing DPNI attributes
+ * @options: Any combination of the following options:
+ *		DPNI_OPT_TX_FRM_RELEASE
+ *		DPNI_OPT_NO_MAC_FILTER
+ *		DPNI_OPT_HAS_POLICING
+ *		DPNI_OPT_SHARED_CONGESTION
+ *		DPNI_OPT_HAS_KEY_MASKING
+ *		DPNI_OPT_NO_FS
+ * @num_queues: Number of Tx and Rx queues used for traffic distribution.
+ * @num_tcs: Number of traffic classes (TCs), reserved for the DPNI.
+ * @mac_filter_entries: Number of entries in the MAC address filtering table.
+ * @vlan_filter_entries: Number of entries in the VLAN address filtering table.
+ * @qos_entries: Number of entries in the QoS classification table.
+ * @fs_entries: Number of entries in the flow steering table.
+ * @qos_key_size: Size, in bytes, of the QoS look-up key. Defining a key larger
+ *		than this when adding QoS entries will result in an error.
+ * @fs_key_size: Size, in bytes, of the flow steering look-up key. Defining a
+ *		key larger than this when composing the hash + FS key will
+ *		result in an error.
+ * @wriop_version: Version of WRIOP HW block. The 3 version values are stored
+ *		on 6, 5, 5 bits respectively.
+ */
+struct dpni_attr {
+	u32 options;
+	u8 num_queues;
+	u8 num_tcs;
+	u8 mac_filter_entries;
+	u8 vlan_filter_entries;
+	u8 qos_entries;
+	u16 fs_entries;
+	u8 qos_key_size;
+	u8 fs_key_size;
+	u16 wriop_version;
+};
+
+int dpni_get_attributes(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			struct dpni_attr	*attr);
+
+/**
+ * DPNI errors
+ */
+
+/**
+ * Extract out of frame header error
+ */
+#define DPNI_ERROR_EOFHE	0x00020000
+/**
+ * Frame length error
+ */
+#define DPNI_ERROR_FLE		0x00002000
+/**
+ * Frame physical error
+ */
+#define DPNI_ERROR_FPE		0x00001000
+/**
+ * Parsing header error
+ */
+#define DPNI_ERROR_PHE		0x00000020
+/**
+ * Parser L3 checksum error
+ */
+#define DPNI_ERROR_L3CE		0x00000004
+/**
+ * Parser L3 checksum error
+ */
+#define DPNI_ERROR_L4CE		0x00000001
+
+/**
+ * enum dpni_error_action - Defines DPNI behavior for errors
+ * @DPNI_ERROR_ACTION_DISCARD: Discard the frame
+ * @DPNI_ERROR_ACTION_CONTINUE: Continue with the normal flow
+ * @DPNI_ERROR_ACTION_SEND_TO_ERROR_QUEUE: Send the frame to the error queue
+ */
+enum dpni_error_action {
+	DPNI_ERROR_ACTION_DISCARD = 0,
+	DPNI_ERROR_ACTION_CONTINUE = 1,
+	DPNI_ERROR_ACTION_SEND_TO_ERROR_QUEUE = 2
+};
+
+/**
+ * struct dpni_error_cfg - Structure representing DPNI errors treatment
+ * @errors: Errors mask; use 'DPNI_ERROR__<X>
+ * @error_action: The desired action for the errors mask
+ * @set_frame_annotation: Set to '1' to mark the errors in frame annotation
+ *		status (FAS); relevant only for the non-discard action
+ */
+struct dpni_error_cfg {
+	u32			errors;
+	enum dpni_error_action	error_action;
+	int			set_frame_annotation;
+};
+
+int dpni_set_errors_behavior(struct fsl_mc_io		*mc_io,
+			     u32			cmd_flags,
+			     u16			token,
+			     struct dpni_error_cfg	*cfg);
+
+/**
+ * DPNI buffer layout modification options
+ */
+
+/**
+ * Select to modify the time-stamp setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_TIMESTAMP		0x00000001
+/**
+ * Select to modify the parser-result setting; not applicable for Tx
+ */
+#define DPNI_BUF_LAYOUT_OPT_PARSER_RESULT	0x00000002
+/**
+ * Select to modify the frame-status setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_FRAME_STATUS	0x00000004
+/**
+ * Select to modify the private-data-size setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE	0x00000008
+/**
+ * Select to modify the data-alignment setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_DATA_ALIGN		0x00000010
+/**
+ * Select to modify the data-head-room setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_DATA_HEAD_ROOM	0x00000020
+/**
+ * Select to modify the data-tail-room setting
+ */
+#define DPNI_BUF_LAYOUT_OPT_DATA_TAIL_ROOM	0x00000040
+
+/**
+ * struct dpni_buffer_layout - Structure representing DPNI buffer layout
+ * @options: Flags representing the suggested modifications to the buffer
+ *		layout; Use any combination of 'DPNI_BUF_LAYOUT_OPT_<X>' flags
+ * @pass_timestamp: Pass timestamp value
+ * @pass_parser_result: Pass parser results
+ * @pass_frame_status: Pass frame status
+ * @private_data_size: Size kept for private data (in bytes)
+ * @data_align: Data alignment
+ * @data_head_room: Data head room
+ * @data_tail_room: Data tail room
+ */
+struct dpni_buffer_layout {
+	u32	options;
+	int	pass_timestamp;
+	int	pass_parser_result;
+	int	pass_frame_status;
+	u16	private_data_size;
+	u16	data_align;
+	u16	data_head_room;
+	u16	data_tail_room;
+};
+
+/**
+ * enum dpni_queue_type - Identifies a type of queue targeted by the command
+ * @DPNI_QUEUE_RX: Rx queue
+ * @DPNI_QUEUE_TX: Tx queue
+ * @DPNI_QUEUE_TX_CONFIRM: Tx confirmation queue
+ * @DPNI_QUEUE_RX_ERR: Rx error queue
+ */enum dpni_queue_type {
+	DPNI_QUEUE_RX,
+	DPNI_QUEUE_TX,
+	DPNI_QUEUE_TX_CONFIRM,
+	DPNI_QUEUE_RX_ERR,
+};
+
+int dpni_get_buffer_layout(struct fsl_mc_io		*mc_io,
+			   u32				cmd_flags,
+			   u16				token,
+			   enum dpni_queue_type		qtype,
+			   struct dpni_buffer_layout	*layout);
+
+int dpni_set_buffer_layout(struct fsl_mc_io		   *mc_io,
+			   u32				   cmd_flags,
+			   u16				   token,
+			   enum dpni_queue_type		   qtype,
+			   const struct dpni_buffer_layout *layout);
+
+/**
+ * enum dpni_offload - Identifies a type of offload targeted by the command
+ * @DPNI_OFF_RX_L3_CSUM: Rx L3 checksum validation
+ * @DPNI_OFF_RX_L4_CSUM: Rx L4 checksum validation
+ * @DPNI_OFF_TX_L3_CSUM: Tx L3 checksum generation
+ * @DPNI_OFF_TX_L4_CSUM: Tx L4 checksum generation
+ */
+enum dpni_offload {
+	DPNI_OFF_RX_L3_CSUM,
+	DPNI_OFF_RX_L4_CSUM,
+	DPNI_OFF_TX_L3_CSUM,
+	DPNI_OFF_TX_L4_CSUM,
+};
+
+int dpni_set_offload(struct fsl_mc_io	*mc_io,
+		     u32		cmd_flags,
+		     u16		token,
+		     enum dpni_offload	type,
+		     u32		config);
+
+int dpni_get_offload(struct fsl_mc_io	*mc_io,
+		     u32		cmd_flags,
+		     u16		token,
+		     enum dpni_offload	type,
+		     u32		*config);
+
+int dpni_get_qdid(struct fsl_mc_io	*mc_io,
+		  u32			cmd_flags,
+		  u16			token,
+		  enum dpni_queue_type	qtype,
+		  u16			*qdid);
+
+int dpni_get_tx_data_offset(struct fsl_mc_io	*mc_io,
+			    u32			cmd_flags,
+			    u16			token,
+			    u16			*data_offset);
+
+#define DPNI_STATISTICS_CNT		7
+
+union dpni_statistics {
+	/**
+	 * struct page_0 - Page_0 statistics structure
+	 * @ingress_all_frames: Ingress frame count
+	 * @ingress_all_bytes: Ingress byte count
+	 * @ingress_multicast_frames: Ingress multicast frame count
+	 * @ingress_multicast_bytes: Ingress multicast byte count
+	 * @ingress_broadcast_frames: Ingress broadcast frame count
+	 * @ingress_broadcast_bytes: Ingress broadcast byte count
+	 */
+	struct {
+		u64 ingress_all_frames;
+		u64 ingress_all_bytes;
+		u64 ingress_multicast_frames;
+		u64 ingress_multicast_bytes;
+		u64 ingress_broadcast_frames;
+		u64 ingress_broadcast_bytes;
+	} page_0;
+	/**
+	 * struct page_1 - Page_1 statistics structure
+	 * @egress_all_frames: Egress frame count
+	 * @egress_all_bytes: Egress byte count
+	 * @egress_multicast_frames: Egress multicast frame count
+	 * @egress_multicast_bytes: Egress multicast byte count
+	 * @egress_broadcast_frames: Egress broadcast frame count
+	 * @egress_broadcast_bytes: Egress broadcast byte count
+	 */
+	struct {
+		u64 egress_all_frames;
+		u64 egress_all_bytes;
+		u64 egress_multicast_frames;
+		u64 egress_multicast_bytes;
+		u64 egress_broadcast_frames;
+		u64 egress_broadcast_bytes;
+	} page_1;
+	/**
+	 * struct page_2 - Page_2 statistics structure
+	 * @ingress_filtered_frames: Ingress filtered frame count
+	 * @ingress_discarded_frames: Ingress discarded frame count
+	 * @ingress_nobuffer_discards: Ingress discarded frame count
+	 *				due to lack of buffers
+	 * @egress_discarded_frames: Egress discarded frame count
+	 * @egress_confirmed_frames: Egress confirmed frame count
+	 */
+	struct {
+		u64 ingress_filtered_frames;
+		u64 ingress_discarded_frames;
+		u64 ingress_nobuffer_discards;
+		u64 egress_discarded_frames;
+		u64 egress_confirmed_frames;
+	} page_2;
+	/**
+	 * struct raw - raw statistics structure
+	 */
+	struct {
+		u64 counter[DPNI_STATISTICS_CNT];
+	} raw;
+};
+
+int dpni_get_statistics(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			u8			page,
+			union dpni_statistics	*stat);
+
+/**
+ * Enable auto-negotiation
+ */
+#define DPNI_LINK_OPT_AUTONEG		0x0000000000000001ULL
+/**
+ * Enable half-duplex mode
+ */
+#define DPNI_LINK_OPT_HALF_DUPLEX	0x0000000000000002ULL
+/**
+ * Enable pause frames
+ */
+#define DPNI_LINK_OPT_PAUSE		0x0000000000000004ULL
+/**
+ * Enable a-symmetric pause frames
+ */
+#define DPNI_LINK_OPT_ASYM_PAUSE	0x0000000000000008ULL
+
+/**
+ * struct - Structure representing DPNI link configuration
+ * @rate: Rate
+ * @options: Mask of available options; use 'DPNI_LINK_OPT_<X>' values
+ */
+struct dpni_link_cfg {
+	u32 rate;
+	u64 options;
+};
+
+int dpni_set_link_cfg(struct fsl_mc_io			*mc_io,
+		      u32				cmd_flags,
+		      u16				token,
+		      const struct dpni_link_cfg	*cfg);
+
+/**
+ * struct dpni_link_state - Structure representing DPNI link state
+ * @rate: Rate
+ * @options: Mask of available options; use 'DPNI_LINK_OPT_<X>' values
+ * @up: Link state; '0' for down, '1' for up
+ */
+struct dpni_link_state {
+	u32	rate;
+	u64	options;
+	int	up;
+};
+
+int dpni_get_link_state(struct fsl_mc_io	*mc_io,
+			u32			cmd_flags,
+			u16			token,
+			struct dpni_link_state	*state);
+
+int dpni_set_max_frame_length(struct fsl_mc_io	*mc_io,
+			      u32		cmd_flags,
+			      u16		token,
+			      u16		max_frame_length);
+
+int dpni_get_max_frame_length(struct fsl_mc_io	*mc_io,
+			      u32		cmd_flags,
+			      u16		token,
+			      u16		*max_frame_length);
+
+int dpni_set_multicast_promisc(struct fsl_mc_io *mc_io,
+			       u32		cmd_flags,
+			       u16		token,
+			       int		en);
+
+int dpni_get_multicast_promisc(struct fsl_mc_io *mc_io,
+			       u32		cmd_flags,
+			       u16		token,
+			       int		*en);
+
+int dpni_set_unicast_promisc(struct fsl_mc_io	*mc_io,
+			     u32		cmd_flags,
+			     u16		token,
+			     int		en);
+
+int dpni_get_unicast_promisc(struct fsl_mc_io	*mc_io,
+			     u32		cmd_flags,
+			     u16		token,
+			     int		*en);
+
+int dpni_set_primary_mac_addr(struct fsl_mc_io *mc_io,
+			      u32		cmd_flags,
+			      u16		token,
+			      const u8		mac_addr[6]);
+
+int dpni_get_primary_mac_addr(struct fsl_mc_io	*mc_io,
+			      u32		cmd_flags,
+			      u16		token,
+			      u8		mac_addr[6]);
+
+int dpni_get_port_mac_addr(struct fsl_mc_io	*mc_io,
+			   u32			cm_flags,
+			   u16			token,
+			   u8			mac_addr[6]);
+
+int dpni_add_mac_addr(struct fsl_mc_io	*mc_io,
+		      u32		cmd_flags,
+		      u16		token,
+		      const u8		mac_addr[6]);
+
+int dpni_remove_mac_addr(struct fsl_mc_io	*mc_io,
+			 u32			cmd_flags,
+			 u16			token,
+			 const u8		mac_addr[6]);
+
+int dpni_clear_mac_filters(struct fsl_mc_io	*mc_io,
+			   u32			cmd_flags,
+			   u16			token,
+			   int			unicast,
+			   int			multicast);
+
+/**
+ * enum dpni_dist_mode - DPNI distribution mode
+ * @DPNI_DIST_MODE_NONE: No distribution
+ * @DPNI_DIST_MODE_HASH: Use hash distribution; only relevant if
+ *		the 'DPNI_OPT_DIST_HASH' option was set at DPNI creation
+ * @DPNI_DIST_MODE_FS:  Use explicit flow steering; only relevant if
+ *	 the 'DPNI_OPT_DIST_FS' option was set at DPNI creation
+ */
+enum dpni_dist_mode {
+	DPNI_DIST_MODE_NONE = 0,
+	DPNI_DIST_MODE_HASH = 1,
+	DPNI_DIST_MODE_FS = 2
+};
+
+/**
+ * enum dpni_fs_miss_action -   DPNI Flow Steering miss action
+ * @DPNI_FS_MISS_DROP: In case of no-match, drop the frame
+ * @DPNI_FS_MISS_EXPLICIT_FLOWID: In case of no-match, use explicit flow-id
+ * @DPNI_FS_MISS_HASH: In case of no-match, distribute using hash
+ */
+enum dpni_fs_miss_action {
+	DPNI_FS_MISS_DROP = 0,
+	DPNI_FS_MISS_EXPLICIT_FLOWID = 1,
+	DPNI_FS_MISS_HASH = 2
+};
+
+/**
+ * struct dpni_fs_tbl_cfg - Flow Steering table configuration
+ * @miss_action: Miss action selection
+ * @default_flow_id: Used when 'miss_action = DPNI_FS_MISS_EXPLICIT_FLOWID'
+ */
+struct dpni_fs_tbl_cfg {
+	enum dpni_fs_miss_action	miss_action;
+	u16				default_flow_id;
+};
+
+int dpni_prepare_key_cfg(const struct dpkg_profile_cfg *cfg,
+			 u8 *key_cfg_buf);
+
+/**
+ * struct dpni_rx_tc_dist_cfg - Rx traffic class distribution configuration
+ * @dist_size: Set the distribution size;
+ *	supported values: 1,2,3,4,6,7,8,12,14,16,24,28,32,48,56,64,96,
+ *	112,128,192,224,256,384,448,512,768,896,1024
+ * @dist_mode: Distribution mode
+ * @key_cfg_iova: I/O virtual address of 256 bytes DMA-able memory filled with
+ *		the extractions to be used for the distribution key by calling
+ *		dpni_prepare_key_cfg() relevant only when
+ *		'dist_mode != DPNI_DIST_MODE_NONE', otherwise it can be '0'
+ * @fs_cfg: Flow Steering table configuration; only relevant if
+ *		'dist_mode = DPNI_DIST_MODE_FS'
+ */
+struct dpni_rx_tc_dist_cfg {
+	u16			dist_size;
+	enum dpni_dist_mode	dist_mode;
+	u64			key_cfg_iova;
+	struct dpni_fs_tbl_cfg	fs_cfg;
+};
+
+int dpni_set_rx_tc_dist(struct fsl_mc_io			*mc_io,
+			u32					cmd_flags,
+			u16					token,
+			u8					tc_id,
+			const struct dpni_rx_tc_dist_cfg	*cfg);
+
+/**
+ * enum dpni_dest - DPNI destination types
+ * @DPNI_DEST_NONE: Unassigned destination; The queue is set in parked mode and
+ *		does not generate FQDAN notifications; user is expected to
+ *		dequeue from the queue based on polling or other user-defined
+ *		method
+ * @DPNI_DEST_DPIO: The queue is set in schedule mode and generates FQDAN
+ *		notifications to the specified DPIO; user is expected to dequeue
+ *		from the queue only after notification is received
+ * @DPNI_DEST_DPCON: The queue is set in schedule mode and does not generate
+ *		FQDAN notifications, but is connected to the specified DPCON
+ *		object; user is expected to dequeue from the DPCON channel
+ */
+enum dpni_dest {
+	DPNI_DEST_NONE = 0,
+	DPNI_DEST_DPIO = 1,
+	DPNI_DEST_DPCON = 2
+};
+
+/**
+ * struct dpni_queue - Queue structure
+ * @user_context: User data, presented to the user along with any frames from
+ *		this queue. Not relevant for Tx queues.
+ */
+struct dpni_queue {
+/**
+ * struct destination - Destination structure
+ * @id: ID of the destination, only relevant if DEST_TYPE is > 0.
+ *		Identifies either a DPIO or a DPCON object. Not relevant for
+ *		Tx queues.
+ * @type: May be one of the following:
+ *	0 - No destination, queue can be manually queried, but will not
+ *		push traffic or notifications to a DPIO;
+ *	1 - The destination is a DPIO. When traffic becomes available in
+ *		the queue a FQDAN (FQ data available notification) will be
+ *		generated to selected DPIO;
+ *	2 - The destination is a DPCON. The queue is associated with a
+ *		DPCON object for the purpose of scheduling between multiple
+ *		queues. The DPCON may be independently configured to
+ *		generate notifications. Not relevant for Tx queues.
+ * @hold_active: Hold active, maintains a queue scheduled for longer
+ *		in a DPIO during dequeue to reduce spread of traffic.
+ *		Only relevant if queues are not affined to a single DPIO.
+ */
+	struct {
+		u16 id;
+		enum dpni_dest type;
+		char hold_active;
+		u8 priority;
+	} destination;
+	u64 user_context;
+	struct {
+		u64 value;
+		char stash_control;
+	} flc;
+};
+
+/**
+ * struct dpni_queue_id - Queue identification, used for enqueue commands
+ *			or queue control
+ * @fqid: FQID used for enqueueing to and/or configuration of this specific FQ
+ * @qdbin: Queueing bin, used to enqueue using QDID, DQBIN, QPRI. Only relevant
+ *		for Tx queues.
+ */
+struct dpni_queue_id {
+	u32 fqid;
+	u16 qdbin;
+};
+
+/**
+ * Set User Context
+ */
+#define DPNI_QUEUE_OPT_USER_CTX		0x00000001
+#define DPNI_QUEUE_OPT_DEST		0x00000002
+#define DPNI_QUEUE_OPT_FLC		0x00000004
+#define DPNI_QUEUE_OPT_HOLD_ACTIVE	0x00000008
+
+int dpni_set_queue(struct fsl_mc_io	*mc_io,
+		   u32			cmd_flags,
+		   u16			token,
+		   enum dpni_queue_type	qtype,
+		   u8			tc,
+		   u8			index,
+		   u8			options,
+		   const struct dpni_queue *queue);
+
+int dpni_get_queue(struct fsl_mc_io	*mc_io,
+		   u32			cmd_flags,
+		   u16			token,
+		   enum dpni_queue_type	qtype,
+		   u8			tc,
+		   u8			index,
+		   struct dpni_queue	*queue,
+		   struct dpni_queue_id	*qid);
+
+/**
+ * enum dpni_congestion_unit - DPNI congestion units
+ * @DPNI_CONGESTION_UNIT_BYTES: bytes units
+ * @DPNI_CONGESTION_UNIT_FRAMES: frames units
+ */
+enum dpni_congestion_unit {
+	DPNI_CONGESTION_UNIT_BYTES = 0,
+	DPNI_CONGESTION_UNIT_FRAMES
+};
+
+/**
+ * enum dpni_congestion_point - Structure representing congestion point
+ * @DPNI_CP_QUEUE: Set taildrop per queue, identified by QUEUE_TYPE, TC and
+ *		QUEUE_INDEX
+ * @DPNI_CP_GROUP: Set taildrop per queue group. Depending on options used to
+ *		define the DPNI this can be either per TC (default) or per
+ *		interface (DPNI_OPT_SHARED_CONGESTION set at DPNI create).
+ *		QUEUE_INDEX is ignored if this type is used.
+ */
+enum dpni_congestion_point {
+	DPNI_CP_QUEUE,
+	DPNI_CP_GROUP,
+};
+
+/**
+ * struct dpni_taildrop - Structure representing the taildrop
+ * @enable:	Indicates whether the taildrop is active or not.
+ * @units:	Indicates the unit of THRESHOLD. Queue taildrop only supports
+ *		byte units, this field is ignored and assumed = 0 if
+ *		CONGESTION_POINT is 0.
+ * @threshold:	Threshold value, in units identified by UNITS field. Value 0
+ *		cannot be used as a valid taildrop threshold, THRESHOLD must
+ *		be > 0 if the taildrop is enabled.
+ */
+struct dpni_taildrop {
+	char enable;
+	enum dpni_congestion_unit units;
+	u32 threshold;
+};
+
+int dpni_set_taildrop(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      enum dpni_congestion_point cg_point,
+		      enum dpni_queue_type q_type,
+		      u8 tc,
+		      u8 q_index,
+		      struct dpni_taildrop *taildrop);
+
+int dpni_get_taildrop(struct fsl_mc_io *mc_io,
+		      u32 cmd_flags,
+		      u16 token,
+		      enum dpni_congestion_point cg_point,
+		      enum dpni_queue_type q_type,
+		      u8 tc,
+		      u8 q_index,
+		      struct dpni_taildrop *taildrop);
+
+/**
+ * struct dpni_rule_cfg - Rule configuration for table lookup
+ * @key_iova: I/O virtual address of the key (must be in DMA-able memory)
+ * @mask_iova: I/O virtual address of the mask (must be in DMA-able memory)
+ * @key_size: key and mask size (in bytes)
+ */
+struct dpni_rule_cfg {
+	u64	key_iova;
+	u64	mask_iova;
+	u8	key_size;
+};
+
+#endif /* __FSL_DPNI_H */
diff --git a/drivers/staging/fsl-dpaa2/ethernet/net.h b/drivers/staging/fsl-dpaa2/ethernet/net.h
new file mode 100644
index 000000000000..5020dee1730c
--- /dev/null
+++ b/drivers/staging/fsl-dpaa2/ethernet/net.h
@@ -0,0 +1,480 @@
+/* Copyright 2013-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef __FSL_NET_H
+#define __FSL_NET_H
+
+#define LAST_HDR_INDEX 0xFFFFFFFF
+
+/*****************************************************************************/
+/*                Protocol fields                                            */
+/*****************************************************************************/
+
+/*************************  Ethernet fields  *********************************/
+#define NH_FLD_ETH_DA                         (1)
+#define NH_FLD_ETH_SA                         (NH_FLD_ETH_DA << 1)
+#define NH_FLD_ETH_LENGTH                     (NH_FLD_ETH_DA << 2)
+#define NH_FLD_ETH_TYPE                       (NH_FLD_ETH_DA << 3)
+#define NH_FLD_ETH_FINAL_CKSUM                (NH_FLD_ETH_DA << 4)
+#define NH_FLD_ETH_PADDING                    (NH_FLD_ETH_DA << 5)
+#define NH_FLD_ETH_ALL_FIELDS                 ((NH_FLD_ETH_DA << 6) - 1)
+
+#define NH_FLD_ETH_ADDR_SIZE                 6
+
+/***************************  VLAN fields  ***********************************/
+#define NH_FLD_VLAN_VPRI                      (1)
+#define NH_FLD_VLAN_CFI                       (NH_FLD_VLAN_VPRI << 1)
+#define NH_FLD_VLAN_VID                       (NH_FLD_VLAN_VPRI << 2)
+#define NH_FLD_VLAN_LENGTH                    (NH_FLD_VLAN_VPRI << 3)
+#define NH_FLD_VLAN_TYPE                      (NH_FLD_VLAN_VPRI << 4)
+#define NH_FLD_VLAN_ALL_FIELDS                ((NH_FLD_VLAN_VPRI << 5) - 1)
+
+#define NH_FLD_VLAN_TCI                       (NH_FLD_VLAN_VPRI | \
+					       NH_FLD_VLAN_CFI | \
+					       NH_FLD_VLAN_VID)
+
+/************************  IP (generic) fields  ******************************/
+#define NH_FLD_IP_VER                         (1)
+#define NH_FLD_IP_DSCP                        (NH_FLD_IP_VER << 2)
+#define NH_FLD_IP_ECN                         (NH_FLD_IP_VER << 3)
+#define NH_FLD_IP_PROTO                       (NH_FLD_IP_VER << 4)
+#define NH_FLD_IP_SRC                         (NH_FLD_IP_VER << 5)
+#define NH_FLD_IP_DST                         (NH_FLD_IP_VER << 6)
+#define NH_FLD_IP_TOS_TC                      (NH_FLD_IP_VER << 7)
+#define NH_FLD_IP_ID                          (NH_FLD_IP_VER << 8)
+#define NH_FLD_IP_ALL_FIELDS                  ((NH_FLD_IP_VER << 9) - 1)
+
+#define NH_FLD_IP_PROTO_SIZE                  1
+
+/*****************************  IPV4 fields  *********************************/
+#define NH_FLD_IPV4_VER                       (1)
+#define NH_FLD_IPV4_HDR_LEN                   (NH_FLD_IPV4_VER << 1)
+#define NH_FLD_IPV4_TOS                       (NH_FLD_IPV4_VER << 2)
+#define NH_FLD_IPV4_TOTAL_LEN                 (NH_FLD_IPV4_VER << 3)
+#define NH_FLD_IPV4_ID                        (NH_FLD_IPV4_VER << 4)
+#define NH_FLD_IPV4_FLAG_D                    (NH_FLD_IPV4_VER << 5)
+#define NH_FLD_IPV4_FLAG_M                    (NH_FLD_IPV4_VER << 6)
+#define NH_FLD_IPV4_OFFSET                    (NH_FLD_IPV4_VER << 7)
+#define NH_FLD_IPV4_TTL                       (NH_FLD_IPV4_VER << 8)
+#define NH_FLD_IPV4_PROTO                     (NH_FLD_IPV4_VER << 9)
+#define NH_FLD_IPV4_CKSUM                     (NH_FLD_IPV4_VER << 10)
+#define NH_FLD_IPV4_SRC_IP                    (NH_FLD_IPV4_VER << 11)
+#define NH_FLD_IPV4_DST_IP                    (NH_FLD_IPV4_VER << 12)
+#define NH_FLD_IPV4_OPTS                      (NH_FLD_IPV4_VER << 13)
+#define NH_FLD_IPV4_OPTS_COUNT                (NH_FLD_IPV4_VER << 14)
+#define NH_FLD_IPV4_ALL_FIELDS                ((NH_FLD_IPV4_VER << 15) - 1)
+
+#define NH_FLD_IPV4_ADDR_SIZE                 4
+#define NH_FLD_IPV4_PROTO_SIZE                1
+
+/*****************************  IPV6 fields  *********************************/
+#define NH_FLD_IPV6_VER                       (1)
+#define NH_FLD_IPV6_TC                        (NH_FLD_IPV6_VER << 1)
+#define NH_FLD_IPV6_SRC_IP                    (NH_FLD_IPV6_VER << 2)
+#define NH_FLD_IPV6_DST_IP                    (NH_FLD_IPV6_VER << 3)
+#define NH_FLD_IPV6_NEXT_HDR                  (NH_FLD_IPV6_VER << 4)
+#define NH_FLD_IPV6_FL                        (NH_FLD_IPV6_VER << 5)
+#define NH_FLD_IPV6_HOP_LIMIT                 (NH_FLD_IPV6_VER << 6)
+#define NH_FLD_IPV6_ID			      (NH_FLD_IPV6_VER << 7)
+#define NH_FLD_IPV6_ALL_FIELDS                ((NH_FLD_IPV6_VER << 8) - 1)
+
+#define NH_FLD_IPV6_ADDR_SIZE                 16
+#define NH_FLD_IPV6_NEXT_HDR_SIZE             1
+
+/*****************************  ICMP fields  *********************************/
+#define NH_FLD_ICMP_TYPE                      (1)
+#define NH_FLD_ICMP_CODE                      (NH_FLD_ICMP_TYPE << 1)
+#define NH_FLD_ICMP_CKSUM                     (NH_FLD_ICMP_TYPE << 2)
+#define NH_FLD_ICMP_ID                        (NH_FLD_ICMP_TYPE << 3)
+#define NH_FLD_ICMP_SQ_NUM                    (NH_FLD_ICMP_TYPE << 4)
+#define NH_FLD_ICMP_ALL_FIELDS                ((NH_FLD_ICMP_TYPE << 5) - 1)
+
+#define NH_FLD_ICMP_CODE_SIZE                 1
+#define NH_FLD_ICMP_TYPE_SIZE                 1
+
+/*****************************  IGMP fields  *********************************/
+#define NH_FLD_IGMP_VERSION                   (1)
+#define NH_FLD_IGMP_TYPE                      (NH_FLD_IGMP_VERSION << 1)
+#define NH_FLD_IGMP_CKSUM                     (NH_FLD_IGMP_VERSION << 2)
+#define NH_FLD_IGMP_DATA                      (NH_FLD_IGMP_VERSION << 3)
+#define NH_FLD_IGMP_ALL_FIELDS                ((NH_FLD_IGMP_VERSION << 4) - 1)
+
+/*****************************  TCP fields  **********************************/
+#define NH_FLD_TCP_PORT_SRC                   (1)
+#define NH_FLD_TCP_PORT_DST                   (NH_FLD_TCP_PORT_SRC << 1)
+#define NH_FLD_TCP_SEQ                        (NH_FLD_TCP_PORT_SRC << 2)
+#define NH_FLD_TCP_ACK                        (NH_FLD_TCP_PORT_SRC << 3)
+#define NH_FLD_TCP_OFFSET                     (NH_FLD_TCP_PORT_SRC << 4)
+#define NH_FLD_TCP_FLAGS                      (NH_FLD_TCP_PORT_SRC << 5)
+#define NH_FLD_TCP_WINDOW                     (NH_FLD_TCP_PORT_SRC << 6)
+#define NH_FLD_TCP_CKSUM                      (NH_FLD_TCP_PORT_SRC << 7)
+#define NH_FLD_TCP_URGPTR                     (NH_FLD_TCP_PORT_SRC << 8)
+#define NH_FLD_TCP_OPTS                       (NH_FLD_TCP_PORT_SRC << 9)
+#define NH_FLD_TCP_OPTS_COUNT                 (NH_FLD_TCP_PORT_SRC << 10)
+#define NH_FLD_TCP_ALL_FIELDS                 ((NH_FLD_TCP_PORT_SRC << 11) - 1)
+
+#define NH_FLD_TCP_PORT_SIZE                  2
+
+/*****************************  UDP fields  **********************************/
+#define NH_FLD_UDP_PORT_SRC                   (1)
+#define NH_FLD_UDP_PORT_DST                   (NH_FLD_UDP_PORT_SRC << 1)
+#define NH_FLD_UDP_LEN                        (NH_FLD_UDP_PORT_SRC << 2)
+#define NH_FLD_UDP_CKSUM                      (NH_FLD_UDP_PORT_SRC << 3)
+#define NH_FLD_UDP_ALL_FIELDS                 ((NH_FLD_UDP_PORT_SRC << 4) - 1)
+
+#define NH_FLD_UDP_PORT_SIZE                  2
+
+/***************************  UDP-lite fields  *******************************/
+#define NH_FLD_UDP_LITE_PORT_SRC              (1)
+#define NH_FLD_UDP_LITE_PORT_DST              (NH_FLD_UDP_LITE_PORT_SRC << 1)
+#define NH_FLD_UDP_LITE_ALL_FIELDS \
+	((NH_FLD_UDP_LITE_PORT_SRC << 2) - 1)
+
+#define NH_FLD_UDP_LITE_PORT_SIZE             2
+
+/***************************  UDP-encap-ESP fields  **************************/
+#define NH_FLD_UDP_ENC_ESP_PORT_SRC         (1)
+#define NH_FLD_UDP_ENC_ESP_PORT_DST         (NH_FLD_UDP_ENC_ESP_PORT_SRC << 1)
+#define NH_FLD_UDP_ENC_ESP_LEN              (NH_FLD_UDP_ENC_ESP_PORT_SRC << 2)
+#define NH_FLD_UDP_ENC_ESP_CKSUM            (NH_FLD_UDP_ENC_ESP_PORT_SRC << 3)
+#define NH_FLD_UDP_ENC_ESP_SPI              (NH_FLD_UDP_ENC_ESP_PORT_SRC << 4)
+#define NH_FLD_UDP_ENC_ESP_SEQUENCE_NUM     (NH_FLD_UDP_ENC_ESP_PORT_SRC << 5)
+#define NH_FLD_UDP_ENC_ESP_ALL_FIELDS \
+	((NH_FLD_UDP_ENC_ESP_PORT_SRC << 6) - 1)
+
+#define NH_FLD_UDP_ENC_ESP_PORT_SIZE        2
+#define NH_FLD_UDP_ENC_ESP_SPI_SIZE         4
+
+/*****************************  SCTP fields  *********************************/
+#define NH_FLD_SCTP_PORT_SRC                  (1)
+#define NH_FLD_SCTP_PORT_DST                  (NH_FLD_SCTP_PORT_SRC << 1)
+#define NH_FLD_SCTP_VER_TAG                   (NH_FLD_SCTP_PORT_SRC << 2)
+#define NH_FLD_SCTP_CKSUM                     (NH_FLD_SCTP_PORT_SRC << 3)
+#define NH_FLD_SCTP_ALL_FIELDS                ((NH_FLD_SCTP_PORT_SRC << 4) - 1)
+
+#define NH_FLD_SCTP_PORT_SIZE                 2
+
+/*****************************  DCCP fields  *********************************/
+#define NH_FLD_DCCP_PORT_SRC                  (1)
+#define NH_FLD_DCCP_PORT_DST                  (NH_FLD_DCCP_PORT_SRC << 1)
+#define NH_FLD_DCCP_ALL_FIELDS                ((NH_FLD_DCCP_PORT_SRC << 2) - 1)
+
+#define NH_FLD_DCCP_PORT_SIZE                 2
+
+/*****************************  IPHC fields  *********************************/
+#define NH_FLD_IPHC_CID                       (1)
+#define NH_FLD_IPHC_CID_TYPE                  (NH_FLD_IPHC_CID << 1)
+#define NH_FLD_IPHC_HCINDEX                   (NH_FLD_IPHC_CID << 2)
+#define NH_FLD_IPHC_GEN                       (NH_FLD_IPHC_CID << 3)
+#define NH_FLD_IPHC_D_BIT                     (NH_FLD_IPHC_CID << 4)
+#define NH_FLD_IPHC_ALL_FIELDS                ((NH_FLD_IPHC_CID << 5) - 1)
+
+/*****************************  SCTP fields  *********************************/
+#define NH_FLD_SCTP_CHUNK_DATA_TYPE           (1)
+#define NH_FLD_SCTP_CHUNK_DATA_FLAGS          (NH_FLD_SCTP_CHUNK_DATA_TYPE << 1)
+#define NH_FLD_SCTP_CHUNK_DATA_LENGTH         (NH_FLD_SCTP_CHUNK_DATA_TYPE << 2)
+#define NH_FLD_SCTP_CHUNK_DATA_TSN            (NH_FLD_SCTP_CHUNK_DATA_TYPE << 3)
+#define NH_FLD_SCTP_CHUNK_DATA_STREAM_ID      (NH_FLD_SCTP_CHUNK_DATA_TYPE << 4)
+#define NH_FLD_SCTP_CHUNK_DATA_STREAM_SQN     (NH_FLD_SCTP_CHUNK_DATA_TYPE << 5)
+#define NH_FLD_SCTP_CHUNK_DATA_PAYLOAD_PID    (NH_FLD_SCTP_CHUNK_DATA_TYPE << 6)
+#define NH_FLD_SCTP_CHUNK_DATA_UNORDERED      (NH_FLD_SCTP_CHUNK_DATA_TYPE << 7)
+#define NH_FLD_SCTP_CHUNK_DATA_BEGGINING      (NH_FLD_SCTP_CHUNK_DATA_TYPE << 8)
+#define NH_FLD_SCTP_CHUNK_DATA_END            (NH_FLD_SCTP_CHUNK_DATA_TYPE << 9)
+#define NH_FLD_SCTP_CHUNK_DATA_ALL_FIELDS \
+	((NH_FLD_SCTP_CHUNK_DATA_TYPE << 10) - 1)
+
+/***************************  L2TPV2 fields  *********************************/
+#define NH_FLD_L2TPV2_TYPE_BIT                (1)
+#define NH_FLD_L2TPV2_LENGTH_BIT              (NH_FLD_L2TPV2_TYPE_BIT << 1)
+#define NH_FLD_L2TPV2_SEQUENCE_BIT            (NH_FLD_L2TPV2_TYPE_BIT << 2)
+#define NH_FLD_L2TPV2_OFFSET_BIT              (NH_FLD_L2TPV2_TYPE_BIT << 3)
+#define NH_FLD_L2TPV2_PRIORITY_BIT            (NH_FLD_L2TPV2_TYPE_BIT << 4)
+#define NH_FLD_L2TPV2_VERSION                 (NH_FLD_L2TPV2_TYPE_BIT << 5)
+#define NH_FLD_L2TPV2_LEN                     (NH_FLD_L2TPV2_TYPE_BIT << 6)
+#define NH_FLD_L2TPV2_TUNNEL_ID               (NH_FLD_L2TPV2_TYPE_BIT << 7)
+#define NH_FLD_L2TPV2_SESSION_ID              (NH_FLD_L2TPV2_TYPE_BIT << 8)
+#define NH_FLD_L2TPV2_NS                      (NH_FLD_L2TPV2_TYPE_BIT << 9)
+#define NH_FLD_L2TPV2_NR                      (NH_FLD_L2TPV2_TYPE_BIT << 10)
+#define NH_FLD_L2TPV2_OFFSET_SIZE             (NH_FLD_L2TPV2_TYPE_BIT << 11)
+#define NH_FLD_L2TPV2_FIRST_BYTE              (NH_FLD_L2TPV2_TYPE_BIT << 12)
+#define NH_FLD_L2TPV2_ALL_FIELDS \
+	((NH_FLD_L2TPV2_TYPE_BIT << 13) - 1)
+
+/***************************  L2TPV3 fields  *********************************/
+#define NH_FLD_L2TPV3_CTRL_TYPE_BIT           (1)
+#define NH_FLD_L2TPV3_CTRL_LENGTH_BIT         (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 1)
+#define NH_FLD_L2TPV3_CTRL_SEQUENCE_BIT       (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 2)
+#define NH_FLD_L2TPV3_CTRL_VERSION            (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 3)
+#define NH_FLD_L2TPV3_CTRL_LENGTH             (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 4)
+#define NH_FLD_L2TPV3_CTRL_CONTROL            (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 5)
+#define NH_FLD_L2TPV3_CTRL_SENT               (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 6)
+#define NH_FLD_L2TPV3_CTRL_RECV               (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 7)
+#define NH_FLD_L2TPV3_CTRL_FIRST_BYTE         (NH_FLD_L2TPV3_CTRL_TYPE_BIT << 8)
+#define NH_FLD_L2TPV3_CTRL_ALL_FIELDS \
+	((NH_FLD_L2TPV3_CTRL_TYPE_BIT << 9) - 1)
+
+#define NH_FLD_L2TPV3_SESS_TYPE_BIT           (1)
+#define NH_FLD_L2TPV3_SESS_VERSION            (NH_FLD_L2TPV3_SESS_TYPE_BIT << 1)
+#define NH_FLD_L2TPV3_SESS_ID                 (NH_FLD_L2TPV3_SESS_TYPE_BIT << 2)
+#define NH_FLD_L2TPV3_SESS_COOKIE             (NH_FLD_L2TPV3_SESS_TYPE_BIT << 3)
+#define NH_FLD_L2TPV3_SESS_ALL_FIELDS \
+	((NH_FLD_L2TPV3_SESS_TYPE_BIT << 4) - 1)
+
+/****************************  PPP fields  ***********************************/
+#define NH_FLD_PPP_PID                        (1)
+#define NH_FLD_PPP_COMPRESSED                 (NH_FLD_PPP_PID << 1)
+#define NH_FLD_PPP_ALL_FIELDS                 ((NH_FLD_PPP_PID << 2) - 1)
+
+/**************************  PPPoE fields  ***********************************/
+#define NH_FLD_PPPOE_VER                      (1)
+#define NH_FLD_PPPOE_TYPE                     (NH_FLD_PPPOE_VER << 1)
+#define NH_FLD_PPPOE_CODE                     (NH_FLD_PPPOE_VER << 2)
+#define NH_FLD_PPPOE_SID                      (NH_FLD_PPPOE_VER << 3)
+#define NH_FLD_PPPOE_LEN                      (NH_FLD_PPPOE_VER << 4)
+#define NH_FLD_PPPOE_SESSION                  (NH_FLD_PPPOE_VER << 5)
+#define NH_FLD_PPPOE_PID                      (NH_FLD_PPPOE_VER << 6)
+#define NH_FLD_PPPOE_ALL_FIELDS               ((NH_FLD_PPPOE_VER << 7) - 1)
+
+/*************************  PPP-Mux fields  **********************************/
+#define NH_FLD_PPPMUX_PID                     (1)
+#define NH_FLD_PPPMUX_CKSUM                   (NH_FLD_PPPMUX_PID << 1)
+#define NH_FLD_PPPMUX_COMPRESSED              (NH_FLD_PPPMUX_PID << 2)
+#define NH_FLD_PPPMUX_ALL_FIELDS              ((NH_FLD_PPPMUX_PID << 3) - 1)
+
+/***********************  PPP-Mux sub-frame fields  **************************/
+#define NH_FLD_PPPMUX_SUBFRM_PFF            (1)
+#define NH_FLD_PPPMUX_SUBFRM_LXT            (NH_FLD_PPPMUX_SUBFRM_PFF << 1)
+#define NH_FLD_PPPMUX_SUBFRM_LEN            (NH_FLD_PPPMUX_SUBFRM_PFF << 2)
+#define NH_FLD_PPPMUX_SUBFRM_PID            (NH_FLD_PPPMUX_SUBFRM_PFF << 3)
+#define NH_FLD_PPPMUX_SUBFRM_USE_PID        (NH_FLD_PPPMUX_SUBFRM_PFF << 4)
+#define NH_FLD_PPPMUX_SUBFRM_ALL_FIELDS \
+	((NH_FLD_PPPMUX_SUBFRM_PFF << 5) - 1)
+
+/***************************  LLC fields  ************************************/
+#define NH_FLD_LLC_DSAP                       (1)
+#define NH_FLD_LLC_SSAP                       (NH_FLD_LLC_DSAP << 1)
+#define NH_FLD_LLC_CTRL                       (NH_FLD_LLC_DSAP << 2)
+#define NH_FLD_LLC_ALL_FIELDS                 ((NH_FLD_LLC_DSAP << 3) - 1)
+
+/***************************  NLPID fields  **********************************/
+#define NH_FLD_NLPID_NLPID                    (1)
+#define NH_FLD_NLPID_ALL_FIELDS               ((NH_FLD_NLPID_NLPID << 1) - 1)
+
+/***************************  SNAP fields  ***********************************/
+#define NH_FLD_SNAP_OUI                       (1)
+#define NH_FLD_SNAP_PID                       (NH_FLD_SNAP_OUI << 1)
+#define NH_FLD_SNAP_ALL_FIELDS                ((NH_FLD_SNAP_OUI << 2) - 1)
+
+/***************************  LLC SNAP fields  *******************************/
+#define NH_FLD_LLC_SNAP_TYPE                  (1)
+#define NH_FLD_LLC_SNAP_ALL_FIELDS            ((NH_FLD_LLC_SNAP_TYPE << 1) - 1)
+
+#define NH_FLD_ARP_HTYPE                      (1)
+#define NH_FLD_ARP_PTYPE                      (NH_FLD_ARP_HTYPE << 1)
+#define NH_FLD_ARP_HLEN                       (NH_FLD_ARP_HTYPE << 2)
+#define NH_FLD_ARP_PLEN                       (NH_FLD_ARP_HTYPE << 3)
+#define NH_FLD_ARP_OPER                       (NH_FLD_ARP_HTYPE << 4)
+#define NH_FLD_ARP_SHA                        (NH_FLD_ARP_HTYPE << 5)
+#define NH_FLD_ARP_SPA                        (NH_FLD_ARP_HTYPE << 6)
+#define NH_FLD_ARP_THA                        (NH_FLD_ARP_HTYPE << 7)
+#define NH_FLD_ARP_TPA                        (NH_FLD_ARP_HTYPE << 8)
+#define NH_FLD_ARP_ALL_FIELDS                 ((NH_FLD_ARP_HTYPE << 9) - 1)
+
+/***************************  RFC2684 fields  ********************************/
+#define NH_FLD_RFC2684_LLC                    (1)
+#define NH_FLD_RFC2684_NLPID                  (NH_FLD_RFC2684_LLC << 1)
+#define NH_FLD_RFC2684_OUI                    (NH_FLD_RFC2684_LLC << 2)
+#define NH_FLD_RFC2684_PID                    (NH_FLD_RFC2684_LLC << 3)
+#define NH_FLD_RFC2684_VPN_OUI                (NH_FLD_RFC2684_LLC << 4)
+#define NH_FLD_RFC2684_VPN_IDX                (NH_FLD_RFC2684_LLC << 5)
+#define NH_FLD_RFC2684_ALL_FIELDS             ((NH_FLD_RFC2684_LLC << 6) - 1)
+
+/***************************  User defined fields  ***************************/
+#define NH_FLD_USER_DEFINED_SRCPORT           (1)
+#define NH_FLD_USER_DEFINED_PCDID             (NH_FLD_USER_DEFINED_SRCPORT << 1)
+#define NH_FLD_USER_DEFINED_ALL_FIELDS \
+	((NH_FLD_USER_DEFINED_SRCPORT << 2) - 1)
+
+/***************************  Payload fields  ********************************/
+#define NH_FLD_PAYLOAD_BUFFER                 (1)
+#define NH_FLD_PAYLOAD_SIZE                   (NH_FLD_PAYLOAD_BUFFER << 1)
+#define NH_FLD_MAX_FRM_SIZE                   (NH_FLD_PAYLOAD_BUFFER << 2)
+#define NH_FLD_MIN_FRM_SIZE                   (NH_FLD_PAYLOAD_BUFFER << 3)
+#define NH_FLD_PAYLOAD_TYPE                   (NH_FLD_PAYLOAD_BUFFER << 4)
+#define NH_FLD_FRAME_SIZE                     (NH_FLD_PAYLOAD_BUFFER << 5)
+#define NH_FLD_PAYLOAD_ALL_FIELDS             ((NH_FLD_PAYLOAD_BUFFER << 6) - 1)
+
+/***************************  GRE fields  ************************************/
+#define NH_FLD_GRE_TYPE                       (1)
+#define NH_FLD_GRE_ALL_FIELDS                 ((NH_FLD_GRE_TYPE << 1) - 1)
+
+/***************************  MINENCAP fields  *******************************/
+#define NH_FLD_MINENCAP_SRC_IP                (1)
+#define NH_FLD_MINENCAP_DST_IP                (NH_FLD_MINENCAP_SRC_IP << 1)
+#define NH_FLD_MINENCAP_TYPE                  (NH_FLD_MINENCAP_SRC_IP << 2)
+#define NH_FLD_MINENCAP_ALL_FIELDS \
+	((NH_FLD_MINENCAP_SRC_IP << 3) - 1)
+
+/***************************  IPSEC AH fields  *******************************/
+#define NH_FLD_IPSEC_AH_SPI                   (1)
+#define NH_FLD_IPSEC_AH_NH                    (NH_FLD_IPSEC_AH_SPI << 1)
+#define NH_FLD_IPSEC_AH_ALL_FIELDS            ((NH_FLD_IPSEC_AH_SPI << 2) - 1)
+
+/***************************  IPSEC ESP fields  ******************************/
+#define NH_FLD_IPSEC_ESP_SPI                  (1)
+#define NH_FLD_IPSEC_ESP_SEQUENCE_NUM         (NH_FLD_IPSEC_ESP_SPI << 1)
+#define NH_FLD_IPSEC_ESP_ALL_FIELDS           ((NH_FLD_IPSEC_ESP_SPI << 2) - 1)
+
+#define NH_FLD_IPSEC_ESP_SPI_SIZE             4
+
+/***************************  MPLS fields  ***********************************/
+#define NH_FLD_MPLS_LABEL_STACK               (1)
+#define NH_FLD_MPLS_LABEL_STACK_ALL_FIELDS \
+	((NH_FLD_MPLS_LABEL_STACK << 1) - 1)
+
+/***************************  MACSEC fields  *********************************/
+#define NH_FLD_MACSEC_SECTAG                  (1)
+#define NH_FLD_MACSEC_ALL_FIELDS              ((NH_FLD_MACSEC_SECTAG << 1) - 1)
+
+/***************************  GTP fields  ************************************/
+#define NH_FLD_GTP_TEID                       (1)
+
+/* Protocol options */
+
+/* Ethernet options */
+#define	NH_OPT_ETH_BROADCAST			1
+#define	NH_OPT_ETH_MULTICAST			2
+#define	NH_OPT_ETH_UNICAST			3
+#define	NH_OPT_ETH_BPDU				4
+
+#define NH_ETH_IS_MULTICAST_ADDR(addr) (addr[0] & 0x01)
+/* also applicable for broadcast */
+
+/* VLAN options */
+#define	NH_OPT_VLAN_CFI				1
+
+/* IPV4 options */
+#define	NH_OPT_IPV4_UNICAST			1
+#define	NH_OPT_IPV4_MULTICAST			2
+#define	NH_OPT_IPV4_BROADCAST			3
+#define	NH_OPT_IPV4_OPTION			4
+#define	NH_OPT_IPV4_FRAG			5
+#define	NH_OPT_IPV4_INITIAL_FRAG		6
+
+/* IPV6 options */
+#define	NH_OPT_IPV6_UNICAST			1
+#define	NH_OPT_IPV6_MULTICAST			2
+#define	NH_OPT_IPV6_OPTION			3
+#define	NH_OPT_IPV6_FRAG			4
+#define	NH_OPT_IPV6_INITIAL_FRAG		5
+
+/* General IP options (may be used for any version) */
+#define	NH_OPT_IP_FRAG				1
+#define	NH_OPT_IP_INITIAL_FRAG			2
+#define	NH_OPT_IP_OPTION			3
+
+/* Minenc. options */
+#define	NH_OPT_MINENCAP_SRC_ADDR_PRESENT	1
+
+/* GRE. options */
+#define	NH_OPT_GRE_ROUTING_PRESENT		1
+
+/* TCP options */
+#define	NH_OPT_TCP_OPTIONS			1
+#define	NH_OPT_TCP_CONTROL_HIGH_BITS		2
+#define	NH_OPT_TCP_CONTROL_LOW_BITS		3
+
+/* CAPWAP options */
+#define	NH_OPT_CAPWAP_DTLS			1
+
+enum net_prot {
+	NET_PROT_NONE = 0,
+	NET_PROT_PAYLOAD,
+	NET_PROT_ETH,
+	NET_PROT_VLAN,
+	NET_PROT_IPV4,
+	NET_PROT_IPV6,
+	NET_PROT_IP,
+	NET_PROT_TCP,
+	NET_PROT_UDP,
+	NET_PROT_UDP_LITE,
+	NET_PROT_IPHC,
+	NET_PROT_SCTP,
+	NET_PROT_SCTP_CHUNK_DATA,
+	NET_PROT_PPPOE,
+	NET_PROT_PPP,
+	NET_PROT_PPPMUX,
+	NET_PROT_PPPMUX_SUBFRM,
+	NET_PROT_L2TPV2,
+	NET_PROT_L2TPV3_CTRL,
+	NET_PROT_L2TPV3_SESS,
+	NET_PROT_LLC,
+	NET_PROT_LLC_SNAP,
+	NET_PROT_NLPID,
+	NET_PROT_SNAP,
+	NET_PROT_MPLS,
+	NET_PROT_IPSEC_AH,
+	NET_PROT_IPSEC_ESP,
+	NET_PROT_UDP_ENC_ESP, /* RFC 3948 */
+	NET_PROT_MACSEC,
+	NET_PROT_GRE,
+	NET_PROT_MINENCAP,
+	NET_PROT_DCCP,
+	NET_PROT_ICMP,
+	NET_PROT_IGMP,
+	NET_PROT_ARP,
+	NET_PROT_CAPWAP_DATA,
+	NET_PROT_CAPWAP_CTRL,
+	NET_PROT_RFC2684,
+	NET_PROT_ICMPV6,
+	NET_PROT_FCOE,
+	NET_PROT_FIP,
+	NET_PROT_ISCSI,
+	NET_PROT_GTP,
+	NET_PROT_USER_DEFINED_L2,
+	NET_PROT_USER_DEFINED_L3,
+	NET_PROT_USER_DEFINED_L4,
+	NET_PROT_USER_DEFINED_L5,
+	NET_PROT_USER_DEFINED_SHIM1,
+	NET_PROT_USER_DEFINED_SHIM2,
+
+	NET_PROT_DUMMY_LAST
+};
+
+/*! IEEE8021.Q */
+#define NH_IEEE8021Q_ETYPE  0x8100
+#define NH_IEEE8021Q_HDR(etype, pcp, dei, vlan_id)	\
+	    ((((u32)((etype) & 0xFFFF)) << 16) |	\
+	    (((u32)((pcp) & 0x07)) << 13) |		\
+	    (((u32)((dei) & 0x01)) << 12) |		\
+	    (((u32)((vlan_id) & 0xFFF))))
+
+#endif /* __FSL_NET_H */
diff --git a/drivers/staging/fsl-mc/bus/Kconfig b/drivers/staging/fsl-mc/bus/Kconfig
index 5c009ab48f00..a10aaf03f314 100644
--- a/drivers/staging/fsl-mc/bus/Kconfig
+++ b/drivers/staging/fsl-mc/bus/Kconfig
@@ -15,3 +15,13 @@ config FSL_MC_BUS
 	  architecture.  The fsl-mc bus driver handles discovery of
 	  DPAA2 objects (which are represented as Linux devices) and
 	  binding objects to drivers.
+
+config FSL_MC_DPIO
+        tristate "QorIQ DPAA2 DPIO driver"
+        depends on FSL_MC_BUS
+        help
+	  Driver for the DPAA2 DPIO object.  A DPIO provides queue and
+	  buffer management facilities for software to interact with
+	  other DPAA2 objects. This driver does not expose the DPIO
+	  objects individually, but groups them under a service layer
+	  API.
diff --git a/drivers/staging/fsl-mc/bus/Makefile b/drivers/staging/fsl-mc/bus/Makefile
index 38716fd5cb58..659eccf52a4f 100644
--- a/drivers/staging/fsl-mc/bus/Makefile
+++ b/drivers/staging/fsl-mc/bus/Makefile
@@ -17,4 +17,8 @@ mc-bus-driver-objs := fsl-mc-bus.o \
 		      fsl-mc-msi.o \
 		      irq-gic-v3-its-fsl-mc-msi.o \
 		      dpmcp.o \
-		      dpbp.o
+		      dpbp.o \
+		      dpcon.o
+
+# MC DPIO driver
+obj-$(CONFIG_FSL_MC_DPIO) += dpio/
diff --git a/drivers/staging/fsl-mc/bus/dpcon-cmd.h b/drivers/staging/fsl-mc/bus/dpcon-cmd.h
index d0a5e194c5e1..2bb66988ecf6 100644
--- a/drivers/staging/fsl-mc/bus/dpcon-cmd.h
+++ b/drivers/staging/fsl-mc/bus/dpcon-cmd.h
@@ -33,30 +33,53 @@
 #define _FSL_DPCON_CMD_H
 
 /* DPCON Version */
-#define DPCON_VER_MAJOR				2
-#define DPCON_VER_MINOR				1
+#define DPCON_VER_MAJOR				3
+#define DPCON_VER_MINOR				2
+
+/* Command versioning */
+#define DPCON_CMD_BASE_VERSION			1
+#define DPCON_CMD_ID_OFFSET			4
+
+#define DPCON_CMD(id)	(((id) << DPCON_CMD_ID_OFFSET) | DPCON_CMD_BASE_VERSION)
 
 /* Command IDs */
-#define DPCON_CMDID_CLOSE				0x800
-#define DPCON_CMDID_OPEN				0x808
-#define DPCON_CMDID_CREATE				0x908
-#define DPCON_CMDID_DESTROY				0x900
-
-#define DPCON_CMDID_ENABLE				0x002
-#define DPCON_CMDID_DISABLE				0x003
-#define DPCON_CMDID_GET_ATTR				0x004
-#define DPCON_CMDID_RESET				0x005
-#define DPCON_CMDID_IS_ENABLED				0x006
-
-#define DPCON_CMDID_SET_IRQ				0x010
-#define DPCON_CMDID_GET_IRQ				0x011
-#define DPCON_CMDID_SET_IRQ_ENABLE			0x012
-#define DPCON_CMDID_GET_IRQ_ENABLE			0x013
-#define DPCON_CMDID_SET_IRQ_MASK			0x014
-#define DPCON_CMDID_GET_IRQ_MASK			0x015
-#define DPCON_CMDID_GET_IRQ_STATUS			0x016
-#define DPCON_CMDID_CLEAR_IRQ_STATUS			0x017
-
-#define DPCON_CMDID_SET_NOTIFICATION			0x100
+#define DPCON_CMDID_CLOSE			DPCON_CMD(0x800)
+#define DPCON_CMDID_OPEN			DPCON_CMD(0x808)
+#define DPCON_CMDID_GET_API_VERSION		DPCON_CMD(0xa08)
+
+#define DPCON_CMDID_ENABLE			DPCON_CMD(0x002)
+#define DPCON_CMDID_DISABLE			DPCON_CMD(0x003)
+#define DPCON_CMDID_GET_ATTR			DPCON_CMD(0x004)
+#define DPCON_CMDID_RESET			DPCON_CMD(0x005)
+#define DPCON_CMDID_IS_ENABLED			DPCON_CMD(0x006)
+
+#define DPCON_CMDID_SET_NOTIFICATION		DPCON_CMD(0x100)
+
+struct dpcon_cmd_open {
+	__le32 dpcon_id;
+};
+
+#define DPCON_ENABLE			1
+
+struct dpcon_rsp_is_enabled {
+	u8 enabled;
+};
+
+struct dpcon_rsp_get_attr {
+	/* response word 0 */
+	__le32 id;
+	__le16 qbman_ch_id;
+	u8 num_priorities;
+	u8 pad;
+};
+
+struct dpcon_cmd_set_notification {
+	/* cmd word 0 */
+	__le32 dpio_id;
+	u8 priority;
+	u8 pad[3];
+	/* cmd word 1 */
+	__le64 user_ctx;
+};
 
 #endif /* _FSL_DPCON_CMD_H */
diff --git a/drivers/staging/fsl-mc/bus/dpcon.c b/drivers/staging/fsl-mc/bus/dpcon.c
new file mode 100644
index 000000000000..eb713578b817
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpcon.c
@@ -0,0 +1,317 @@
+/* Copyright 2013-2016 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#include "../include/mc-sys.h"
+#include "../include/mc-cmd.h"
+#include "../include/dpcon.h"
+
+#include "dpcon-cmd.h"
+
+/**
+ * dpcon_open() - Open a control session for the specified object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @dpcon_id:	DPCON unique ID
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * This function can be used to open a control session for an
+ * already created object; an object may have been declared in
+ * the DPL or by calling the dpcon_create() function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent commands for
+ * this specific object.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpcon_open(struct fsl_mc_io *mc_io,
+	       u32 cmd_flags,
+	       int dpcon_id,
+	       u16 *token)
+{
+	struct mc_command cmd = { 0 };
+	struct dpcon_cmd_open *dpcon_cmd;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_OPEN,
+					  cmd_flags,
+					  0);
+	dpcon_cmd = (struct dpcon_cmd_open *)cmd.params;
+	dpcon_cmd->dpcon_id = cpu_to_le32(dpcon_id);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = mc_cmd_hdr_read_token(&cmd);
+
+	return 0;
+}
+EXPORT_SYMBOL(dpcon_open);
+
+/**
+ * dpcon_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ *
+ * After this function is called, no further operations are
+ * allowed on the object without opening a new control session.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpcon_close(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_CLOSE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpcon_close);
+
+/**
+ * dpcon_enable() - Enable the DPCON
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpcon_enable(struct fsl_mc_io *mc_io,
+		 u32 cmd_flags,
+		 u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_ENABLE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpcon_enable);
+
+/**
+ * dpcon_disable() - Disable the DPCON
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpcon_disable(struct fsl_mc_io *mc_io,
+		  u32 cmd_flags,
+		  u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_DISABLE,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpcon_disable);
+
+/**
+ * dpcon_is_enabled() -	Check if the DPCON is enabled.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ * @en:		Returns '1' if object is enabled; '0' otherwise
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpcon_is_enabled(struct fsl_mc_io *mc_io,
+		     u32 cmd_flags,
+		     u16 token,
+		     int *en)
+{
+	struct mc_command cmd = { 0 };
+	struct dpcon_rsp_is_enabled *dpcon_rsp;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_IS_ENABLED,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	dpcon_rsp = (struct dpcon_rsp_is_enabled *)cmd.params;
+	*en = dpcon_rsp->enabled & DPCON_ENABLE;
+
+	return 0;
+}
+EXPORT_SYMBOL(dpcon_is_enabled);
+
+/**
+ * dpcon_reset() - Reset the DPCON, returns the object to initial state.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpcon_reset(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_RESET,
+					  cmd_flags, token);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpcon_reset);
+
+/**
+ * dpcon_get_attributes() - Retrieve DPCON attributes.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ * @attr:	Object's attributes
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpcon_get_attributes(struct fsl_mc_io *mc_io,
+			 u32 cmd_flags,
+			 u16 token,
+			 struct dpcon_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	struct dpcon_rsp_get_attr *dpcon_rsp;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_GET_ATTR,
+					  cmd_flags,
+					  token);
+
+	/* send command to mc*/
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	dpcon_rsp = (struct dpcon_rsp_get_attr *)cmd.params;
+	attr->id = le32_to_cpu(dpcon_rsp->id);
+	attr->qbman_ch_id = le16_to_cpu(dpcon_rsp->qbman_ch_id);
+	attr->num_priorities = dpcon_rsp->num_priorities;
+
+	return 0;
+}
+EXPORT_SYMBOL(dpcon_get_attributes);
+
+/**
+ * dpcon_set_notification() - Set DPCON notification destination
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPCON object
+ * @cfg:	Notification parameters
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpcon_set_notification(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   struct dpcon_notification_cfg *cfg)
+{
+	struct mc_command cmd = { 0 };
+	struct dpcon_cmd_set_notification *dpcon_cmd;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_SET_NOTIFICATION,
+					  cmd_flags,
+					  token);
+	dpcon_cmd = (struct dpcon_cmd_set_notification *)cmd.params;
+	dpcon_cmd->dpio_id = cpu_to_le32(cfg->dpio_id);
+	dpcon_cmd->priority = cfg->priority;
+	dpcon_cmd->user_ctx = cpu_to_le64(cfg->user_ctx);
+
+	/* send command to mc*/
+	return mc_send_command(mc_io, &cmd);
+}
+EXPORT_SYMBOL(dpcon_set_notification);
+
+/**
+ * dpcon_get_api_version - Get Data Path Concentrator API version
+ * @mc_io:	Pointer to MC portal's DPCON object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @major_ver:	Major version of DPCON API
+ * @minor_ver:	Minor version of DPCON API
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpcon_get_api_version(struct fsl_mc_io *mc_io,
+			  u32 cmd_flags,
+			  u16 *major_ver,
+			  u16 *minor_ver)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPCON_CMDID_GET_API_VERSION,
+					  cmd_flags, 0);
+
+	/* send command to mc */
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	mc_cmd_read_api_version(&cmd, major_ver, minor_ver);
+
+	return 0;
+}
+EXPORT_SYMBOL(dpcon_get_api_version);
diff --git a/drivers/staging/fsl-mc/bus/dpio/Makefile b/drivers/staging/fsl-mc/bus/dpio/Makefile
new file mode 100644
index 000000000000..837d3303e11d
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/Makefile
@@ -0,0 +1,9 @@
+#
+# QorIQ DPAA2 DPIO driver
+#
+
+subdir-ccflags-y := -Werror
+
+obj-$(CONFIG_FSL_MC_DPIO) += fsl-mc-dpio.o
+
+fsl-mc-dpio-objs := dpio.o qbman-portal.o dpio-service.o dpio-driver.o
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio-cmd.h b/drivers/staging/fsl-mc/bus/dpio/dpio-cmd.h
new file mode 100644
index 000000000000..b2dc6e766f09
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio-cmd.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef _FSL_DPIO_CMD_H
+#define _FSL_DPIO_CMD_H
+
+/* DPIO Version */
+#define DPIO_VER_MAJOR			4
+#define DPIO_VER_MINOR			2
+
+/* Command Versioning */
+
+#define DPIO_CMD_ID_OFFSET		4
+#define DPIO_CMD_BASE_VERSION		1
+
+#define DPIO_CMD(id)	(((id) << DPIO_CMD_ID_OFFSET) | DPIO_CMD_BASE_VERSION)
+
+/* Command IDs */
+#define DPIO_CMDID_CLOSE				DPIO_CMD(0x800)
+#define DPIO_CMDID_OPEN					DPIO_CMD(0x803)
+#define DPIO_CMDID_GET_API_VERSION			DPIO_CMD(0xa03)
+#define DPIO_CMDID_ENABLE				DPIO_CMD(0x002)
+#define DPIO_CMDID_DISABLE				DPIO_CMD(0x003)
+#define DPIO_CMDID_GET_ATTR				DPIO_CMD(0x004)
+
+struct dpio_cmd_open {
+	__le32 dpio_id;
+};
+
+#define DPIO_CHANNEL_MODE_MASK		0x3
+
+struct dpio_rsp_get_attr {
+	/* cmd word 0 */
+	__le32 id;
+	__le16 qbman_portal_id;
+	u8 num_priorities;
+	u8 channel_mode;
+	/* cmd word 1 */
+	__le64 qbman_portal_ce_addr;
+	/* cmd word 2 */
+	__le64 qbman_portal_ci_addr;
+	/* cmd word 3 */
+	__le32 qbman_version;
+};
+
+#endif /* _FSL_DPIO_CMD_H */
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio-driver.c b/drivers/staging/fsl-mc/bus/dpio/dpio-driver.c
new file mode 100644
index 000000000000..e36da20a2796
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio-driver.c
@@ -0,0 +1,296 @@
+/*
+ * Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright NXP 2016
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/msi.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+
+#include "../../include/mc.h"
+#include "../../include/dpaa2-io.h"
+
+#include "qbman-portal.h"
+#include "dpio.h"
+#include "dpio-cmd.h"
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Freescale Semiconductor, Inc");
+MODULE_DESCRIPTION("DPIO Driver");
+
+struct dpio_priv {
+	struct dpaa2_io *io;
+};
+
+static irqreturn_t dpio_irq_handler(int irq_num, void *arg)
+{
+	struct device *dev = (struct device *)arg;
+	struct dpio_priv *priv = dev_get_drvdata(dev);
+
+	return dpaa2_io_irq(priv->io);
+}
+
+static void unregister_dpio_irq_handlers(struct fsl_mc_device *dpio_dev)
+{
+	struct fsl_mc_device_irq *irq;
+
+	irq = dpio_dev->irqs[0];
+
+	/* clear the affinity hint */
+	irq_set_affinity_hint(irq->msi_desc->irq, NULL);
+}
+
+static int register_dpio_irq_handlers(struct fsl_mc_device *dpio_dev, int cpu)
+{
+	struct dpio_priv *priv;
+	int error;
+	struct fsl_mc_device_irq *irq;
+	cpumask_t mask;
+
+	priv = dev_get_drvdata(&dpio_dev->dev);
+
+	irq = dpio_dev->irqs[0];
+	error = devm_request_irq(&dpio_dev->dev,
+				 irq->msi_desc->irq,
+				 dpio_irq_handler,
+				 0,
+				 dev_name(&dpio_dev->dev),
+				 &dpio_dev->dev);
+	if (error < 0) {
+		dev_err(&dpio_dev->dev,
+			"devm_request_irq() failed: %d\n",
+			error);
+		return error;
+	}
+
+	/* set the affinity hint */
+	cpumask_clear(&mask);
+	cpumask_set_cpu(cpu, &mask);
+	if (irq_set_affinity_hint(irq->msi_desc->irq, &mask))
+		dev_err(&dpio_dev->dev,
+			"irq_set_affinity failed irq %d cpu %d\n",
+			irq->msi_desc->irq, cpu);
+
+	return 0;
+}
+
+static int dpaa2_dpio_probe(struct fsl_mc_device *dpio_dev)
+{
+	struct dpio_attr dpio_attrs;
+	struct dpaa2_io_desc desc;
+	struct dpio_priv *priv;
+	int err = -ENOMEM;
+	struct device *dev = &dpio_dev->dev;
+	static int next_cpu = -1;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		goto err_priv_alloc;
+
+	dev_set_drvdata(dev, priv);
+
+	err = fsl_mc_portal_allocate(dpio_dev, 0, &dpio_dev->mc_io);
+	if (err) {
+		dev_dbg(dev, "MC portal allocation failed\n");
+		err = -EPROBE_DEFER;
+		goto err_mcportal;
+	}
+
+	err = dpio_open(dpio_dev->mc_io, 0, dpio_dev->obj_desc.id,
+			&dpio_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpio_open() failed\n");
+		goto err_open;
+	}
+
+	err = dpio_get_attributes(dpio_dev->mc_io, 0, dpio_dev->mc_handle,
+				  &dpio_attrs);
+	if (err) {
+		dev_err(dev, "dpio_get_attributes() failed %d\n", err);
+		goto err_get_attr;
+	}
+	desc.qman_version = dpio_attrs.qbman_version;
+
+	err = dpio_enable(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpio_enable() failed %d\n", err);
+		goto err_get_attr;
+	}
+
+	/* initialize DPIO descriptor */
+	desc.receives_notifications = dpio_attrs.num_priorities ? 1 : 0;
+	desc.has_8prio = dpio_attrs.num_priorities == 8 ? 1 : 0;
+	desc.dpio_id = dpio_dev->obj_desc.id;
+
+	/* get the cpu to use for the affinity hint */
+	if (next_cpu == -1)
+		next_cpu = cpumask_first(cpu_online_mask);
+	else
+		next_cpu = cpumask_next(next_cpu, cpu_online_mask);
+
+	if (!cpu_possible(next_cpu)) {
+		dev_err(dev, "probe failed. Number of DPIOs exceeds NR_CPUS.\n");
+		err = -ERANGE;
+		goto err_allocate_irqs;
+	}
+	desc.cpu = next_cpu;
+
+	/*
+	 * Set the CENA regs to be the cache inhibited area of the portal to
+	 * avoid coherency issues if a user migrates to another core.
+	 */
+	desc.regs_cena = ioremap_wc(dpio_dev->regions[1].start,
+		resource_size(&dpio_dev->regions[1]));
+	desc.regs_cinh = ioremap(dpio_dev->regions[1].start,
+		resource_size(&dpio_dev->regions[1]));
+
+	err = fsl_mc_allocate_irqs(dpio_dev);
+	if (err) {
+		dev_err(dev, "fsl_mc_allocate_irqs failed. err=%d\n", err);
+		goto err_allocate_irqs;
+	}
+
+	err = register_dpio_irq_handlers(dpio_dev, desc.cpu);
+	if (err)
+		goto err_register_dpio_irq;
+
+	priv->io = dpaa2_io_create(&desc);
+	if (!priv->io) {
+		dev_err(dev, "dpaa2_io_create failed\n");
+		goto err_dpaa2_io_create;
+	}
+
+	dev_info(dev, "probed\n");
+	dev_dbg(dev, "   receives_notifications = %d\n",
+		desc.receives_notifications);
+	dpio_close(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+	fsl_mc_portal_free(dpio_dev->mc_io);
+
+	return 0;
+
+err_dpaa2_io_create:
+	unregister_dpio_irq_handlers(dpio_dev);
+err_register_dpio_irq:
+	fsl_mc_free_irqs(dpio_dev);
+err_allocate_irqs:
+	dpio_disable(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+err_get_attr:
+	dpio_close(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+err_open:
+	fsl_mc_portal_free(dpio_dev->mc_io);
+err_mcportal:
+	dev_set_drvdata(dev, NULL);
+err_priv_alloc:
+	return err;
+}
+
+/* Tear down interrupts for a given DPIO object */
+static void dpio_teardown_irqs(struct fsl_mc_device *dpio_dev)
+{
+	unregister_dpio_irq_handlers(dpio_dev);
+	fsl_mc_free_irqs(dpio_dev);
+}
+
+static int dpaa2_dpio_remove(struct fsl_mc_device *dpio_dev)
+{
+	struct device *dev;
+	struct dpio_priv *priv;
+	int err;
+
+	dev = &dpio_dev->dev;
+	priv = dev_get_drvdata(dev);
+
+	dpaa2_io_down(priv->io);
+
+	dpio_teardown_irqs(dpio_dev);
+
+	err = fsl_mc_portal_allocate(dpio_dev, 0, &dpio_dev->mc_io);
+	if (err) {
+		dev_err(dev, "MC portal allocation failed\n");
+		goto err_mcportal;
+	}
+
+	err = dpio_open(dpio_dev->mc_io, 0, dpio_dev->obj_desc.id,
+			&dpio_dev->mc_handle);
+	if (err) {
+		dev_err(dev, "dpio_open() failed\n");
+		goto err_open;
+	}
+
+	dpio_disable(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+
+	dpio_close(dpio_dev->mc_io, 0, dpio_dev->mc_handle);
+
+	fsl_mc_portal_free(dpio_dev->mc_io);
+
+	dev_set_drvdata(dev, NULL);
+
+	return 0;
+
+err_open:
+	fsl_mc_portal_free(dpio_dev->mc_io);
+err_mcportal:
+	return err;
+}
+
+static const struct fsl_mc_device_id dpaa2_dpio_match_id_table[] = {
+	{
+		.vendor = FSL_MC_VENDOR_FREESCALE,
+		.obj_type = "dpio",
+	},
+	{ .vendor = 0x0 }
+};
+
+static struct fsl_mc_driver dpaa2_dpio_driver = {
+	.driver = {
+		.name		= KBUILD_MODNAME,
+		.owner		= THIS_MODULE,
+	},
+	.probe		= dpaa2_dpio_probe,
+	.remove		= dpaa2_dpio_remove,
+	.match_id_table = dpaa2_dpio_match_id_table
+};
+
+static int dpio_driver_init(void)
+{
+	return fsl_mc_driver_register(&dpaa2_dpio_driver);
+}
+
+static void dpio_driver_exit(void)
+{
+	fsl_mc_driver_unregister(&dpaa2_dpio_driver);
+}
+module_init(dpio_driver_init);
+module_exit(dpio_driver_exit);
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio-driver.txt b/drivers/staging/fsl-mc/bus/dpio/dpio-driver.txt
new file mode 100644
index 000000000000..0ba6771654f7
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio-driver.txt
@@ -0,0 +1,135 @@
+Copyright 2016 NXP
+
+Introduction
+------------
+
+A DPAA2 DPIO (Data Path I/O) is a hardware object that provides
+interfaces to enqueue and dequeue frames to/from network interfaces
+and other accelerators.  A DPIO also provides hardware buffer
+pool management for network interfaces.
+
+This document provides an overview the Linux DPIO driver, its
+subcomponents, and its APIs.
+
+See Documentation/dpaa2/overview.txt for a general overview of DPAA2
+and the general DPAA2 driver architecture in Linux.
+
+Driver Overview
+---------------
+
+The DPIO driver is bound to DPIO objects discovered on the fsl-mc bus and
+provides services that:
+  A) allow other drivers, such as the Ethernet driver, to enqueue and dequeue
+     frames for their respective objects
+  B) allow drivers to register callbacks for data availability notifications
+     when data becomes available on a queue or channel
+  C) allow drivers to manage hardware buffer pools
+
+The Linux DPIO driver consists of 3 primary components--
+   DPIO object driver-- fsl-mc driver that manages the DPIO object
+   DPIO service-- provides APIs to other Linux drivers for services
+   QBman portal interface-- sends portal commands, gets responses
+
+          fsl-mc          other
+           bus           drivers
+            |               |
+        +---+----+   +------+-----+
+        |DPIO obj|   |DPIO service|
+        | driver |---|  (DPIO)    |
+        +--------+   +------+-----+
+                            |
+                     +------+-----+
+                     |    QBman   |
+                     | portal i/f |
+                     +------------+
+                            |
+                         hardware
+
+The diagram below shows how the DPIO driver components fit with the other
+DPAA2 Linux driver components:
+                                                   +------------+
+                                                   | OS Network |
+                                                   |   Stack    |
+                 +------------+                    +------------+
+                 | Allocator  |. . . . . . .       |  Ethernet  |
+                 |(DPMCP,DPBP)|                    |   (DPNI)   |
+                 +-.----------+                    +---+---+----+
+                  .          .                         ^   |
+                 .            .           <data avail, |   |<enqueue,
+                .              .           tx confirm> |   | dequeue>
+    +-------------+             .                      |   |
+    | DPRC driver |              .    +--------+ +------------+
+    |   (DPRC)    |               . . |DPIO obj| |DPIO service|
+    +----------+--+                   | driver |-|  (DPIO)    |
+               |                      +--------+ +------+-----+
+               |<dev add/remove>                 +------|-----+
+               |                                 |   QBman    |
+          +----+--------------+                  | portal i/f |
+          |   MC-bus driver   |                  +------------+
+          |                   |                     |
+          | /soc/fsl-mc       |                     |
+          +-------------------+                     |
+                                                    |
+ =========================================|=========|========================
+                                        +-+--DPIO---|-----------+
+                                        |           |           |
+                                        |        QBman Portal   |
+                                        +-----------------------+
+
+ ============================================================================
+
+
+DPIO Object Driver (dpio-driver.c)
+----------------------------------
+
+   The dpio-driver component registers with the fsl-mc bus to handle objects of
+   type "dpio".  The implementation of probe() handles basic initialization
+   of the DPIO including mapping of the DPIO regions (the QBman SW portal)
+   and initializing interrupts and registering irq handlers.  The dpio-driver
+   registers the probed DPIO with dpio-service.
+
+DPIO service  (dpio-service.c, dpaa2-io.h)
+------------------------------------------
+
+   The dpio service component provides queuing, notification, and buffers
+   management services to DPAA2 drivers, such as the Ethernet driver.  A system
+   will typically allocate 1 DPIO object per CPU to allow queuing operations
+   to happen simultaneously across all CPUs.
+
+   Notification handling
+      dpaa2_io_service_register()
+      dpaa2_io_service_deregister()
+      dpaa2_io_service_rearm()
+
+   Queuing
+      dpaa2_io_service_pull_fq()
+      dpaa2_io_service_pull_channel()
+      dpaa2_io_service_enqueue_fq()
+      dpaa2_io_service_enqueue_qd()
+      dpaa2_io_store_create()
+      dpaa2_io_store_destroy()
+      dpaa2_io_store_next()
+
+   Buffer pool management
+      dpaa2_io_service_release()
+      dpaa2_io_service_acquire()
+
+QBman portal interface (qbman-portal.c)
+---------------------------------------
+
+   The qbman-portal component provides APIs to do the low level hardware
+   bit twiddling for operations such as:
+      -initializing Qman software portals
+      -building and sending portal commands
+      -portal interrupt configuration and processing
+
+   The qbman-portal APIs are not public to other drivers, and are
+   only used by dpio-service.
+
+Other (dpaa2-fd.h, dpaa2-global.h)
+----------------------------------
+
+   Frame descriptor and scatter-gather definitions and the APIs used to
+   manipulate them are defined in dpaa2-fd.h.
+
+   Dequeue result struct and parsing APIs are defined in dpaa2-global.h.
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio-service.c b/drivers/staging/fsl-mc/bus/dpio/dpio-service.c
new file mode 100644
index 000000000000..e5d66749614c
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio-service.c
@@ -0,0 +1,618 @@
+/*
+ * Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *	 names of its contributors may be used to endorse or promote products
+ *	 derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#include <linux/types.h>
+#include "../../include/mc.h"
+#include "../../include/dpaa2-io.h"
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/slab.h>
+
+#include "dpio.h"
+#include "qbman-portal.h"
+
+struct dpaa2_io {
+	atomic_t refs;
+	struct dpaa2_io_desc dpio_desc;
+	struct qbman_swp_desc swp_desc;
+	struct qbman_swp *swp;
+	struct list_head node;
+	/* protect against multiple management commands */
+	spinlock_t lock_mgmt_cmd;
+	/* protect notifications list */
+	spinlock_t lock_notifications;
+	struct list_head notifications;
+};
+
+struct dpaa2_io_store {
+	unsigned int max;
+	dma_addr_t paddr;
+	struct dpaa2_dq *vaddr;
+	void *alloced_addr;    /* unaligned value from kmalloc() */
+	unsigned int idx;      /* position of the next-to-be-returned entry */
+	struct qbman_swp *swp; /* portal used to issue VDQCR */
+	struct device *dev;    /* device used for DMA mapping */
+};
+
+/* keep a per cpu array of DPIOs for fast access */
+static struct dpaa2_io *dpio_by_cpu[NR_CPUS];
+static struct list_head dpio_list = LIST_HEAD_INIT(dpio_list);
+static DEFINE_SPINLOCK(dpio_list_lock);
+
+static inline struct dpaa2_io *service_select_by_cpu(struct dpaa2_io *d,
+						     int cpu)
+{
+	if (d)
+		return d;
+
+	if (unlikely(cpu >= num_possible_cpus()))
+		return NULL;
+
+	/*
+	 * If cpu == -1, choose the current cpu, with no guarantees about
+	 * potentially being migrated away.
+	 */
+	if (unlikely(cpu < 0))
+		cpu = smp_processor_id();
+
+	/* If a specific cpu was requested, pick it up immediately */
+	return dpio_by_cpu[cpu];
+}
+
+static inline struct dpaa2_io *service_select(struct dpaa2_io *d)
+{
+	if (d)
+		return d;
+
+	spin_lock(&dpio_list_lock);
+	d = list_entry(dpio_list.next, struct dpaa2_io, node);
+	list_del(&d->node);
+	list_add_tail(&d->node, &dpio_list);
+	spin_unlock(&dpio_list_lock);
+
+	return d;
+}
+
+/**
+ * dpaa2_io_create() - create a dpaa2_io object.
+ * @desc: the dpaa2_io descriptor
+ *
+ * Activates a "struct dpaa2_io" corresponding to the given config of an actual
+ * DPIO object.
+ *
+ * Return a valid dpaa2_io object for success, or NULL for failure.
+ */
+struct dpaa2_io *dpaa2_io_create(const struct dpaa2_io_desc *desc)
+{
+	struct dpaa2_io *obj = kmalloc(sizeof(*obj), GFP_KERNEL);
+
+	if (!obj)
+		return NULL;
+
+	/* check if CPU is out of range (-1 means any cpu) */
+	if (desc->cpu >= num_possible_cpus()) {
+		kfree(obj);
+		return NULL;
+	}
+
+	atomic_set(&obj->refs, 1);
+	obj->dpio_desc = *desc;
+	obj->swp_desc.cena_bar = obj->dpio_desc.regs_cena;
+	obj->swp_desc.cinh_bar = obj->dpio_desc.regs_cinh;
+	obj->swp_desc.qman_version = obj->dpio_desc.qman_version;
+	obj->swp = qbman_swp_init(&obj->swp_desc);
+
+	if (!obj->swp) {
+		kfree(obj);
+		return NULL;
+	}
+
+	INIT_LIST_HEAD(&obj->node);
+	spin_lock_init(&obj->lock_mgmt_cmd);
+	spin_lock_init(&obj->lock_notifications);
+	INIT_LIST_HEAD(&obj->notifications);
+
+	/* For now only enable DQRR interrupts */
+	qbman_swp_interrupt_set_trigger(obj->swp,
+					QBMAN_SWP_INTERRUPT_DQRI);
+	qbman_swp_interrupt_clear_status(obj->swp, 0xffffffff);
+	if (obj->dpio_desc.receives_notifications)
+		qbman_swp_push_set(obj->swp, 0, 1);
+
+	spin_lock(&dpio_list_lock);
+	list_add_tail(&obj->node, &dpio_list);
+	if (desc->cpu >= 0 && !dpio_by_cpu[desc->cpu])
+		dpio_by_cpu[desc->cpu] = obj;
+	spin_unlock(&dpio_list_lock);
+
+	return obj;
+}
+EXPORT_SYMBOL(dpaa2_io_create);
+
+/**
+ * dpaa2_io_down() - release the dpaa2_io object.
+ * @d: the dpaa2_io object to be released.
+ *
+ * The "struct dpaa2_io" type can represent an individual DPIO object (as
+ * described by "struct dpaa2_io_desc") or an instance of a "DPIO service",
+ * which can be used to group/encapsulate multiple DPIO objects. In all cases,
+ * each handle obtained should be released using this function.
+ */
+void dpaa2_io_down(struct dpaa2_io *d)
+{
+	if (!atomic_dec_and_test(&d->refs))
+		return;
+	kfree(d);
+}
+EXPORT_SYMBOL(dpaa2_io_down);
+
+#define DPAA_POLL_MAX 32
+
+/**
+ * dpaa2_io_irq() - ISR for DPIO interrupts
+ *
+ * @obj: the given DPIO object.
+ *
+ * Return IRQ_HANDLED for success or IRQ_NONE if there
+ * were no pending interrupts.
+ */
+irqreturn_t dpaa2_io_irq(struct dpaa2_io *obj)
+{
+	const struct dpaa2_dq *dq;
+	int max = 0;
+	struct qbman_swp *swp;
+	u32 status;
+
+	swp = obj->swp;
+	status = qbman_swp_interrupt_read_status(swp);
+	if (!status)
+		return IRQ_NONE;
+
+	dq = qbman_swp_dqrr_next(swp);
+	while (dq) {
+		if (qbman_result_is_SCN(dq)) {
+			struct dpaa2_io_notification_ctx *ctx;
+			u64 q64;
+
+			q64 = qbman_result_SCN_ctx(dq);
+			ctx = (void *)q64;
+			ctx->cb(ctx);
+		} else {
+			pr_crit("fsl-mc-dpio: Unrecognised/ignored DQRR entry\n");
+		}
+		qbman_swp_dqrr_consume(swp, dq);
+		++max;
+		if (max > DPAA_POLL_MAX)
+			goto done;
+		dq = qbman_swp_dqrr_next(swp);
+	}
+done:
+	qbman_swp_interrupt_clear_status(swp, status);
+	qbman_swp_interrupt_set_inhibit(swp, 0);
+	return IRQ_HANDLED;
+}
+EXPORT_SYMBOL(dpaa2_io_irq);
+
+/**
+ * dpaa2_io_service_register() - Prepare for servicing of FQDAN or CDAN
+ *                               notifications on the given DPIO service.
+ * @d:   the given DPIO service.
+ * @ctx: the notification context.
+ *
+ * The caller should make the MC command to attach a DPAA2 object to
+ * a DPIO after this function completes successfully.  In that way:
+ *    (a) The DPIO service is "ready" to handle a notification arrival
+ *        (which might happen before the "attach" command to MC has
+ *        returned control of execution back to the caller)
+ *    (b) The DPIO service can provide back to the caller the 'dpio_id' and
+ *        'qman64' parameters that it should pass along in the MC command
+ *        in order for the object to be configured to produce the right
+ *        notification fields to the DPIO service.
+ *
+ * Return 0 for success, or -ENODEV for failure.
+ */
+int dpaa2_io_service_register(struct dpaa2_io *d,
+			      struct dpaa2_io_notification_ctx *ctx)
+{
+	unsigned long irqflags;
+
+	d = service_select_by_cpu(d, ctx->desired_cpu);
+	if (!d)
+		return -ENODEV;
+
+	ctx->dpio_id = d->dpio_desc.dpio_id;
+	ctx->qman64 = (u64)ctx;
+	ctx->dpio_private = d;
+	spin_lock_irqsave(&d->lock_notifications, irqflags);
+	list_add(&ctx->node, &d->notifications);
+	spin_unlock_irqrestore(&d->lock_notifications, irqflags);
+
+	/* Enable the generation of CDAN notifications */
+	if (ctx->is_cdan)
+		qbman_swp_CDAN_set_context_enable(d->swp,
+						  (u16)ctx->id,
+						  ctx->qman64);
+	return 0;
+}
+EXPORT_SYMBOL(dpaa2_io_service_register);
+
+/**
+ * dpaa2_io_service_deregister - The opposite of 'register'.
+ * @service: the given DPIO service.
+ * @ctx: the notification context.
+ *
+ * This function should be called only after sending the MC command to
+ * to detach the notification-producing device from the DPIO.
+ */
+void dpaa2_io_service_deregister(struct dpaa2_io *service,
+				 struct dpaa2_io_notification_ctx *ctx)
+{
+	struct dpaa2_io *d = ctx->dpio_private;
+	unsigned long irqflags;
+
+	if (ctx->is_cdan)
+		qbman_swp_CDAN_disable(d->swp, (u16)ctx->id);
+
+	spin_lock_irqsave(&d->lock_notifications, irqflags);
+	list_del(&ctx->node);
+	spin_unlock_irqrestore(&d->lock_notifications, irqflags);
+}
+EXPORT_SYMBOL(dpaa2_io_service_deregister);
+
+/**
+ * dpaa2_io_service_rearm() - Rearm the notification for the given DPIO service.
+ * @d: the given DPIO service.
+ * @ctx: the notification context.
+ *
+ * Once a FQDAN/CDAN has been produced, the corresponding FQ/channel is
+ * considered "disarmed". Ie. the user can issue pull dequeue operations on that
+ * traffic source for as long as it likes. Eventually it may wish to "rearm"
+ * that source to allow it to produce another FQDAN/CDAN, that's what this
+ * function achieves.
+ *
+ * Return 0 for success.
+ */
+int dpaa2_io_service_rearm(struct dpaa2_io *d,
+			   struct dpaa2_io_notification_ctx *ctx)
+{
+	unsigned long irqflags;
+	int err;
+
+	d = service_select_by_cpu(d, ctx->desired_cpu);
+	if (!unlikely(d))
+		return -ENODEV;
+
+	spin_lock_irqsave(&d->lock_mgmt_cmd, irqflags);
+	if (ctx->is_cdan)
+		err = qbman_swp_CDAN_enable(d->swp, (u16)ctx->id);
+	else
+		err = qbman_swp_fq_schedule(d->swp, ctx->id);
+	spin_unlock_irqrestore(&d->lock_mgmt_cmd, irqflags);
+
+	return err;
+}
+EXPORT_SYMBOL(dpaa2_io_service_rearm);
+
+/**
+ * dpaa2_io_service_pull_fq() - pull dequeue functions from a fq.
+ * @d: the given DPIO service.
+ * @fqid: the given frame queue id.
+ * @s: the dpaa2_io_store object for the result.
+ *
+ * Return 0 for success, or error code for failure.
+ */
+int dpaa2_io_service_pull_fq(struct dpaa2_io *d, u32 fqid,
+			     struct dpaa2_io_store *s)
+{
+	struct qbman_pull_desc pd;
+	int err;
+
+	qbman_pull_desc_clear(&pd);
+	qbman_pull_desc_set_storage(&pd, s->vaddr, s->paddr, 1);
+	qbman_pull_desc_set_numframes(&pd, (u8)s->max);
+	qbman_pull_desc_set_fq(&pd, fqid);
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+	s->swp = d->swp;
+	err = qbman_swp_pull(d->swp, &pd);
+	if (err)
+		s->swp = NULL;
+
+	return err;
+}
+EXPORT_SYMBOL(dpaa2_io_service_pull_fq);
+
+/**
+ * dpaa2_io_service_pull_channel() - pull dequeue functions from a channel.
+ * @d: the given DPIO service.
+ * @channelid: the given channel id.
+ * @s: the dpaa2_io_store object for the result.
+ *
+ * Return 0 for success, or error code for failure.
+ */
+int dpaa2_io_service_pull_channel(struct dpaa2_io *d, u32 channelid,
+				  struct dpaa2_io_store *s)
+{
+	struct qbman_pull_desc pd;
+	int err;
+
+	qbman_pull_desc_clear(&pd);
+	qbman_pull_desc_set_storage(&pd, s->vaddr, s->paddr, 1);
+	qbman_pull_desc_set_numframes(&pd, (u8)s->max);
+	qbman_pull_desc_set_channel(&pd, channelid, qbman_pull_type_prio);
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+
+	s->swp = d->swp;
+	err = qbman_swp_pull(d->swp, &pd);
+	if (err)
+		s->swp = NULL;
+
+	return err;
+}
+EXPORT_SYMBOL(dpaa2_io_service_pull_channel);
+
+/**
+ * dpaa2_io_service_enqueue_fq() - Enqueue a frame to a frame queue.
+ * @d: the given DPIO service.
+ * @fqid: the given frame queue id.
+ * @fd: the frame descriptor which is enqueued.
+ *
+ * Return 0 for successful enqueue, -EBUSY if the enqueue ring is not ready,
+ * or -ENODEV if there is no dpio service.
+ */
+int dpaa2_io_service_enqueue_fq(struct dpaa2_io *d,
+				u32 fqid,
+				const struct dpaa2_fd *fd)
+{
+	struct qbman_eq_desc ed;
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+
+	qbman_eq_desc_clear(&ed);
+	qbman_eq_desc_set_no_orp(&ed, 0);
+	qbman_eq_desc_set_fq(&ed, fqid);
+
+	return qbman_swp_enqueue(d->swp, &ed, fd);
+}
+EXPORT_SYMBOL(dpaa2_io_service_enqueue_fq);
+
+/**
+ * dpaa2_io_service_enqueue_qd() - Enqueue a frame to a QD.
+ * @d: the given DPIO service.
+ * @qdid: the given queuing destination id.
+ * @prio: the given queuing priority.
+ * @qdbin: the given queuing destination bin.
+ * @fd: the frame descriptor which is enqueued.
+ *
+ * Return 0 for successful enqueue, or -EBUSY if the enqueue ring is not ready,
+ * or -ENODEV if there is no dpio service.
+ */
+int dpaa2_io_service_enqueue_qd(struct dpaa2_io *d,
+				u32 qdid, u8 prio, u16 qdbin,
+				const struct dpaa2_fd *fd)
+{
+	struct qbman_eq_desc ed;
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+
+	qbman_eq_desc_clear(&ed);
+	qbman_eq_desc_set_no_orp(&ed, 0);
+	qbman_eq_desc_set_qd(&ed, qdid, qdbin, prio);
+
+	return qbman_swp_enqueue(d->swp, &ed, fd);
+}
+EXPORT_SYMBOL(dpaa2_io_service_enqueue_qd);
+
+/**
+ * dpaa2_io_service_release() - Release buffers to a buffer pool.
+ * @d: the given DPIO object.
+ * @bpid: the buffer pool id.
+ * @buffers: the buffers to be released.
+ * @num_buffers: the number of the buffers to be released.
+ *
+ * Return 0 for success, and negative error code for failure.
+ */
+int dpaa2_io_service_release(struct dpaa2_io *d,
+			     u32 bpid,
+			     const u64 *buffers,
+			     unsigned int num_buffers)
+{
+	struct qbman_release_desc rd;
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+
+	qbman_release_desc_clear(&rd);
+	qbman_release_desc_set_bpid(&rd, bpid);
+
+	return qbman_swp_release(d->swp, &rd, buffers, num_buffers);
+}
+EXPORT_SYMBOL(dpaa2_io_service_release);
+
+/**
+ * dpaa2_io_service_acquire() - Acquire buffers from a buffer pool.
+ * @d: the given DPIO object.
+ * @bpid: the buffer pool id.
+ * @buffers: the buffer addresses for acquired buffers.
+ * @num_buffers: the expected number of the buffers to acquire.
+ *
+ * Return a negative error code if the command failed, otherwise it returns
+ * the number of buffers acquired, which may be less than the number requested.
+ * Eg. if the buffer pool is empty, this will return zero.
+ */
+int dpaa2_io_service_acquire(struct dpaa2_io *d,
+			     u32 bpid,
+			     u64 *buffers,
+			     unsigned int num_buffers)
+{
+	unsigned long irqflags;
+	int err;
+
+	d = service_select(d);
+	if (!d)
+		return -ENODEV;
+
+	spin_lock_irqsave(&d->lock_mgmt_cmd, irqflags);
+	err = qbman_swp_acquire(d->swp, bpid, buffers, num_buffers);
+	spin_unlock_irqrestore(&d->lock_mgmt_cmd, irqflags);
+
+	return err;
+}
+EXPORT_SYMBOL(dpaa2_io_service_acquire);
+
+/*
+ * 'Stores' are reusable memory blocks for holding dequeue results, and to
+ * assist with parsing those results.
+ */
+
+/**
+ * dpaa2_io_store_create() - Create the dma memory storage for dequeue result.
+ * @max_frames: the maximum number of dequeued result for frames, must be <= 16.
+ * @dev:        the device to allow mapping/unmapping the DMAable region.
+ *
+ * The size of the storage is "max_frames*sizeof(struct dpaa2_dq)".
+ * The 'dpaa2_io_store' returned is a DPIO service managed object.
+ *
+ * Return pointer to dpaa2_io_store struct for successfuly created storage
+ * memory, or NULL on error.
+ */
+struct dpaa2_io_store *dpaa2_io_store_create(unsigned int max_frames,
+					     struct device *dev)
+{
+	struct dpaa2_io_store *ret;
+	size_t size;
+
+	if (!max_frames || (max_frames > 16))
+		return NULL;
+
+	ret = kmalloc(sizeof(*ret), GFP_KERNEL);
+	if (!ret)
+		return NULL;
+
+	ret->max = max_frames;
+	size = max_frames * sizeof(struct dpaa2_dq) + 64;
+	ret->alloced_addr = kzalloc(size, GFP_KERNEL);
+	if (!ret->alloced_addr) {
+		kfree(ret);
+		return NULL;
+	}
+
+	ret->vaddr = PTR_ALIGN(ret->alloced_addr, 64);
+	ret->paddr = dma_map_single(dev, ret->vaddr,
+				    sizeof(struct dpaa2_dq) * max_frames,
+				    DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, ret->paddr)) {
+		kfree(ret->alloced_addr);
+		kfree(ret);
+		return NULL;
+	}
+
+	ret->idx = 0;
+	ret->dev = dev;
+
+	return ret;
+}
+EXPORT_SYMBOL(dpaa2_io_store_create);
+
+/**
+ * dpaa2_io_store_destroy() - Frees the dma memory storage for dequeue
+ *                            result.
+ * @s: the storage memory to be destroyed.
+ */
+void dpaa2_io_store_destroy(struct dpaa2_io_store *s)
+{
+	dma_unmap_single(s->dev, s->paddr, sizeof(struct dpaa2_dq) * s->max,
+			 DMA_FROM_DEVICE);
+	kfree(s->alloced_addr);
+	kfree(s);
+}
+EXPORT_SYMBOL(dpaa2_io_store_destroy);
+
+/**
+ * dpaa2_io_store_next() - Determine when the next dequeue result is available.
+ * @s: the dpaa2_io_store object.
+ * @is_last: indicate whether this is the last frame in the pull command.
+ *
+ * When an object driver performs dequeues to a dpaa2_io_store, this function
+ * can be used to determine when the next frame result is available. Once
+ * this function returns non-NULL, a subsequent call to it will try to find
+ * the next dequeue result.
+ *
+ * Note that if a pull-dequeue has a NULL result because the target FQ/channel
+ * was empty, then this function will also return NULL (rather than expecting
+ * the caller to always check for this. As such, "is_last" can be used to
+ * differentiate between "end-of-empty-dequeue" and "still-waiting".
+ *
+ * Return dequeue result for a valid dequeue result, or NULL for empty dequeue.
+ */
+struct dpaa2_dq *dpaa2_io_store_next(struct dpaa2_io_store *s, int *is_last)
+{
+	int match;
+	struct dpaa2_dq *ret = &s->vaddr[s->idx];
+
+	match = qbman_result_has_new_result(s->swp, ret);
+	if (!match) {
+		*is_last = 0;
+		return NULL;
+	}
+
+	s->idx++;
+
+	if (dpaa2_dq_is_pull_complete(ret)) {
+		*is_last = 1;
+		s->idx = 0;
+		/*
+		 * If we get an empty dequeue result to terminate a zero-results
+		 * vdqcr, return NULL to the caller rather than expecting him to
+		 * check non-NULL results every time.
+		 */
+		if (!(dpaa2_dq_flags(ret) & DPAA2_DQ_STAT_VALIDFRAME))
+			ret = NULL;
+	} else {
+		*is_last = 0;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(dpaa2_io_store_next);
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio.c b/drivers/staging/fsl-mc/bus/dpio/dpio.c
new file mode 100644
index 000000000000..d81e0232f6c1
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio.c
@@ -0,0 +1,224 @@
+/*
+ * Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#include "../../include/mc-sys.h"
+#include "../../include/mc-cmd.h"
+
+#include "dpio.h"
+#include "dpio-cmd.h"
+
+/*
+ * Data Path I/O Portal API
+ * Contains initialization APIs and runtime control APIs for DPIO
+ */
+
+/**
+ * dpio_open() - Open a control session for the specified object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @dpio_id:	DPIO unique ID
+ * @token:	Returned token; use in subsequent API calls
+ *
+ * This function can be used to open a control session for an
+ * already created object; an object may have been declared in
+ * the DPL or by calling the dpio_create() function.
+ * This function returns a unique authentication token,
+ * associated with the specific object ID and the specific MC
+ * portal; this token must be used in all subsequent commands for
+ * this specific object.
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpio_open(struct fsl_mc_io *mc_io,
+	      u32 cmd_flags,
+	      int dpio_id,
+	      u16 *token)
+{
+	struct mc_command cmd = { 0 };
+	struct dpio_cmd_open *dpio_cmd;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_OPEN,
+					  cmd_flags,
+					  0);
+	dpio_cmd = (struct dpio_cmd_open *)cmd.params;
+	dpio_cmd->dpio_id = cpu_to_le32(dpio_id);
+
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	*token = mc_cmd_hdr_read_token(&cmd);
+
+	return 0;
+}
+
+/**
+ * dpio_close() - Close the control session of the object
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPIO object
+ *
+ * Return:	'0' on Success; Error code otherwise.
+ */
+int dpio_close(struct fsl_mc_io *mc_io,
+	       u32 cmd_flags,
+	       u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_CLOSE,
+					  cmd_flags,
+					  token);
+
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpio_enable() - Enable the DPIO, allow I/O portal operations.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPIO object
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpio_enable(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_ENABLE,
+					  cmd_flags,
+					  token);
+
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpio_disable() - Disable the DPIO, stop any I/O portal operation.
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPIO object
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpio_disable(struct fsl_mc_io *mc_io,
+		 u32 cmd_flags,
+		 u16 token)
+{
+	struct mc_command cmd = { 0 };
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_DISABLE,
+					  cmd_flags,
+					  token);
+
+	return mc_send_command(mc_io, &cmd);
+}
+
+/**
+ * dpio_get_attributes() - Retrieve DPIO attributes
+ * @mc_io:	Pointer to MC portal's I/O object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @token:	Token of DPIO object
+ * @attr:	Returned object's attributes
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpio_get_attributes(struct fsl_mc_io *mc_io,
+			u32 cmd_flags,
+			u16 token,
+			struct dpio_attr *attr)
+{
+	struct mc_command cmd = { 0 };
+	struct dpio_rsp_get_attr *dpio_rsp;
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_GET_ATTR,
+					  cmd_flags,
+					  token);
+
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	dpio_rsp = (struct dpio_rsp_get_attr *)cmd.params;
+	attr->id = le32_to_cpu(dpio_rsp->id);
+	attr->qbman_portal_id = le16_to_cpu(dpio_rsp->qbman_portal_id);
+	attr->num_priorities = dpio_rsp->num_priorities;
+	attr->channel_mode = dpio_rsp->channel_mode & DPIO_CHANNEL_MODE_MASK;
+	attr->qbman_portal_ce_offset =
+		le64_to_cpu(dpio_rsp->qbman_portal_ce_addr);
+	attr->qbman_portal_ci_offset =
+		le64_to_cpu(dpio_rsp->qbman_portal_ci_addr);
+	attr->qbman_version = le32_to_cpu(dpio_rsp->qbman_version);
+
+	return 0;
+}
+
+/**
+ * dpio_get_api_version - Get Data Path I/O API version
+ * @mc_io:	Pointer to MC portal's DPIO object
+ * @cmd_flags:	Command flags; one or more of 'MC_CMD_FLAG_'
+ * @major_ver:	Major version of DPIO API
+ * @minor_ver:	Minor version of DPIO API
+ *
+ * Return:	'0' on Success; Error code otherwise
+ */
+int dpio_get_api_version(struct fsl_mc_io *mc_io,
+			 u32 cmd_flags,
+			 u16 *major_ver,
+			 u16 *minor_ver)
+{
+	struct mc_command cmd = { 0 };
+	int err;
+
+	/* prepare command */
+	cmd.header = mc_encode_cmd_header(DPIO_CMDID_GET_API_VERSION,
+					  cmd_flags, 0);
+
+	err = mc_send_command(mc_io, &cmd);
+	if (err)
+		return err;
+
+	/* retrieve response parameters */
+	mc_cmd_read_api_version(&cmd, major_ver, minor_ver);
+
+	return 0;
+}
diff --git a/drivers/staging/fsl-mc/bus/dpio/dpio.h b/drivers/staging/fsl-mc/bus/dpio/dpio.h
new file mode 100644
index 000000000000..ced1103d157c
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/dpio.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright 2013-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef __FSL_DPIO_H
+#define __FSL_DPIO_H
+
+struct fsl_mc_io;
+
+int dpio_open(struct fsl_mc_io	*mc_io,
+	      u32		cmd_flags,
+	      int		dpio_id,
+	      u16		*token);
+
+int dpio_close(struct fsl_mc_io	*mc_io,
+	       u32		cmd_flags,
+	       u16		token);
+
+/**
+ * enum dpio_channel_mode - DPIO notification channel mode
+ * @DPIO_NO_CHANNEL: No support for notification channel
+ * @DPIO_LOCAL_CHANNEL: Notifications on data availability can be received by a
+ *	dedicated channel in the DPIO; user should point the queue's
+ *	destination in the relevant interface to this DPIO
+ */
+enum dpio_channel_mode {
+	DPIO_NO_CHANNEL = 0,
+	DPIO_LOCAL_CHANNEL = 1,
+};
+
+/**
+ * struct dpio_cfg - Structure representing DPIO configuration
+ * @channel_mode: Notification channel mode
+ * @num_priorities: Number of priorities for the notification channel (1-8);
+ *			relevant only if 'channel_mode = DPIO_LOCAL_CHANNEL'
+ */
+struct dpio_cfg {
+	enum dpio_channel_mode	channel_mode;
+	u8		num_priorities;
+};
+
+int dpio_enable(struct fsl_mc_io	*mc_io,
+		u32		cmd_flags,
+		u16		token);
+
+int dpio_disable(struct fsl_mc_io	*mc_io,
+		 u32		cmd_flags,
+		 u16		token);
+
+/**
+ * struct dpio_attr - Structure representing DPIO attributes
+ * @id: DPIO object ID
+ * @qbman_portal_ce_offset: offset of the software portal cache-enabled area
+ * @qbman_portal_ci_offset: offset of the software portal cache-inhibited area
+ * @qbman_portal_id: Software portal ID
+ * @channel_mode: Notification channel mode
+ * @num_priorities: Number of priorities for the notification channel (1-8);
+ *			relevant only if 'channel_mode = DPIO_LOCAL_CHANNEL'
+ * @qbman_version: QBMAN version
+ */
+struct dpio_attr {
+	int			id;
+	u64		qbman_portal_ce_offset;
+	u64		qbman_portal_ci_offset;
+	u16		qbman_portal_id;
+	enum dpio_channel_mode	channel_mode;
+	u8			num_priorities;
+	u32		qbman_version;
+};
+
+int dpio_get_attributes(struct fsl_mc_io	*mc_io,
+			u32		cmd_flags,
+			u16		token,
+			struct dpio_attr	*attr);
+
+int dpio_get_api_version(struct fsl_mc_io *mc_io,
+			 u32 cmd_flags,
+			 u16 *major_ver,
+			 u16 *minor_ver);
+
+#endif /* __FSL_DPIO_H */
diff --git a/drivers/staging/fsl-mc/bus/dpio/qbman-portal.c b/drivers/staging/fsl-mc/bus/dpio/qbman-portal.c
new file mode 100644
index 000000000000..7988612aaecf
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/qbman-portal.c
@@ -0,0 +1,1035 @@
+/*
+ * Copyright (C) 2014-2016 Freescale Semiconductor, Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include "../../include/dpaa2-global.h"
+
+#include "qbman-portal.h"
+
+#define QMAN_REV_4000   0x04000000
+#define QMAN_REV_4100   0x04010000
+#define QMAN_REV_4101   0x04010001
+#define QMAN_REV_MASK   0xffff0000
+
+/* All QBMan command and result structures use this "valid bit" encoding */
+#define QB_VALID_BIT ((u32)0x80)
+
+/* QBMan portal management command codes */
+#define QBMAN_MC_ACQUIRE       0x30
+#define QBMAN_WQCHAN_CONFIGURE 0x46
+
+/* CINH register offsets */
+#define QBMAN_CINH_SWP_EQAR    0x8c0
+#define QBMAN_CINH_SWP_DQPI    0xa00
+#define QBMAN_CINH_SWP_DCAP    0xac0
+#define QBMAN_CINH_SWP_SDQCR   0xb00
+#define QBMAN_CINH_SWP_RAR     0xcc0
+#define QBMAN_CINH_SWP_ISR     0xe00
+#define QBMAN_CINH_SWP_IER     0xe40
+#define QBMAN_CINH_SWP_ISDR    0xe80
+#define QBMAN_CINH_SWP_IIR     0xec0
+
+/* CENA register offsets */
+#define QBMAN_CENA_SWP_EQCR(n) (0x000 + ((u32)(n) << 6))
+#define QBMAN_CENA_SWP_DQRR(n) (0x200 + ((u32)(n) << 6))
+#define QBMAN_CENA_SWP_RCR(n)  (0x400 + ((u32)(n) << 6))
+#define QBMAN_CENA_SWP_CR      0x600
+#define QBMAN_CENA_SWP_RR(vb)  (0x700 + ((u32)(vb) >> 1))
+#define QBMAN_CENA_SWP_VDQCR   0x780
+
+/* Reverse mapping of QBMAN_CENA_SWP_DQRR() */
+#define QBMAN_IDX_FROM_DQRR(p) (((unsigned long)(p) & 0x1ff) >> 6)
+
+/* Define token used to determine if response written to memory is valid */
+#define QMAN_DQ_TOKEN_VALID 1
+
+/* SDQCR attribute codes */
+#define QB_SDQCR_FC_SHIFT   29
+#define QB_SDQCR_FC_MASK    0x1
+#define QB_SDQCR_DCT_SHIFT  24
+#define QB_SDQCR_DCT_MASK   0x3
+#define QB_SDQCR_TOK_SHIFT  16
+#define QB_SDQCR_TOK_MASK   0xff
+#define QB_SDQCR_SRC_SHIFT  0
+#define QB_SDQCR_SRC_MASK   0xffff
+
+/* opaque token for static dequeues */
+#define QMAN_SDQCR_TOKEN    0xbb
+
+enum qbman_sdqcr_dct {
+	qbman_sdqcr_dct_null = 0,
+	qbman_sdqcr_dct_prio_ics,
+	qbman_sdqcr_dct_active_ics,
+	qbman_sdqcr_dct_active
+};
+
+enum qbman_sdqcr_fc {
+	qbman_sdqcr_fc_one = 0,
+	qbman_sdqcr_fc_up_to_3 = 1
+};
+
+/* Portal Access */
+
+static inline u32 qbman_read_register(struct qbman_swp *p, u32 offset)
+{
+	return readl_relaxed(p->addr_cinh + offset);
+}
+
+static inline void qbman_write_register(struct qbman_swp *p, u32 offset,
+					u32 value)
+{
+	writel_relaxed(value, p->addr_cinh + offset);
+}
+
+static inline void *qbman_get_cmd(struct qbman_swp *p, u32 offset)
+{
+	return p->addr_cena + offset;
+}
+
+#define QBMAN_CINH_SWP_CFG   0xd00
+
+#define SWP_CFG_DQRR_MF_SHIFT 20
+#define SWP_CFG_EST_SHIFT     16
+#define SWP_CFG_WN_SHIFT      14
+#define SWP_CFG_RPM_SHIFT     12
+#define SWP_CFG_DCM_SHIFT     10
+#define SWP_CFG_EPM_SHIFT     8
+#define SWP_CFG_SD_SHIFT      5
+#define SWP_CFG_SP_SHIFT      4
+#define SWP_CFG_SE_SHIFT      3
+#define SWP_CFG_DP_SHIFT      2
+#define SWP_CFG_DE_SHIFT      1
+#define SWP_CFG_EP_SHIFT      0
+
+static inline u32 qbman_set_swp_cfg(u8 max_fill, u8 wn,	u8 est, u8 rpm, u8 dcm,
+				    u8 epm, int sd, int sp, int se,
+				    int dp, int de, int ep)
+{
+	return cpu_to_le32 (max_fill << SWP_CFG_DQRR_MF_SHIFT |
+			    est << SWP_CFG_EST_SHIFT |
+			    wn << SWP_CFG_WN_SHIFT |
+			    rpm << SWP_CFG_RPM_SHIFT |
+			    dcm << SWP_CFG_DCM_SHIFT |
+			    epm << SWP_CFG_EPM_SHIFT |
+			    sd << SWP_CFG_SD_SHIFT |
+			    sp << SWP_CFG_SP_SHIFT |
+			    se << SWP_CFG_SE_SHIFT |
+			    dp << SWP_CFG_DP_SHIFT |
+			    de << SWP_CFG_DE_SHIFT |
+			    ep << SWP_CFG_EP_SHIFT);
+}
+
+/**
+ * qbman_swp_init() - Create a functional object representing the given
+ *                    QBMan portal descriptor.
+ * @d: the given qbman swp descriptor
+ *
+ * Return qbman_swp portal for success, NULL if the object cannot
+ * be created.
+ */
+struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
+{
+	struct qbman_swp *p = kmalloc(sizeof(*p), GFP_KERNEL);
+	u32 reg;
+
+	if (!p)
+		return NULL;
+	p->desc = d;
+	p->mc.valid_bit = QB_VALID_BIT;
+	p->sdq = 0;
+	p->sdq |= qbman_sdqcr_dct_prio_ics << QB_SDQCR_DCT_SHIFT;
+	p->sdq |= qbman_sdqcr_fc_up_to_3 << QB_SDQCR_FC_SHIFT;
+	p->sdq |= QMAN_SDQCR_TOKEN << QB_SDQCR_TOK_SHIFT;
+
+	atomic_set(&p->vdq.available, 1);
+	p->vdq.valid_bit = QB_VALID_BIT;
+	p->dqrr.next_idx = 0;
+	p->dqrr.valid_bit = QB_VALID_BIT;
+
+	if ((p->desc->qman_version & QMAN_REV_MASK) < QMAN_REV_4100) {
+		p->dqrr.dqrr_size = 4;
+		p->dqrr.reset_bug = 1;
+	} else {
+		p->dqrr.dqrr_size = 8;
+		p->dqrr.reset_bug = 0;
+	}
+
+	p->addr_cena = d->cena_bar;
+	p->addr_cinh = d->cinh_bar;
+
+	reg = qbman_set_swp_cfg(p->dqrr.dqrr_size,
+				1, /* Writes Non-cacheable */
+				0, /* EQCR_CI stashing threshold */
+				3, /* RPM: Valid bit mode, RCR in array mode */
+				2, /* DCM: Discrete consumption ack mode */
+				3, /* EPM: Valid bit mode, EQCR in array mode */
+				0, /* mem stashing drop enable == FALSE */
+				1, /* mem stashing priority == TRUE */
+				0, /* mem stashing enable == FALSE */
+				1, /* dequeue stashing priority == TRUE */
+				0, /* dequeue stashing enable == FALSE */
+				0); /* EQCR_CI stashing priority == FALSE */
+
+	qbman_write_register(p, QBMAN_CINH_SWP_CFG, reg);
+	reg = qbman_read_register(p, QBMAN_CINH_SWP_CFG);
+	if (!reg) {
+		pr_err("qbman: the portal is not enabled!\n");
+		return NULL;
+	}
+
+	/*
+	 * SDQCR needs to be initialized to 0 when no channels are
+	 * being dequeued from or else the QMan HW will indicate an
+	 * error.  The values that were calculated above will be
+	 * applied when dequeues from a specific channel are enabled.
+	 */
+	qbman_write_register(p, QBMAN_CINH_SWP_SDQCR, 0);
+	return p;
+}
+
+/**
+ * qbman_swp_finish() - Create and destroy a functional object representing
+ *                      the given QBMan portal descriptor.
+ * @p: the qbman_swp object to be destroyed
+ */
+void qbman_swp_finish(struct qbman_swp *p)
+{
+	kfree(p);
+}
+
+/**
+ * qbman_swp_interrupt_read_status()
+ * @p: the given software portal
+ *
+ * Return the value in the SWP_ISR register.
+ */
+u32 qbman_swp_interrupt_read_status(struct qbman_swp *p)
+{
+	return qbman_read_register(p, QBMAN_CINH_SWP_ISR);
+}
+
+/**
+ * qbman_swp_interrupt_clear_status()
+ * @p: the given software portal
+ * @mask: The mask to clear in SWP_ISR register
+ */
+void qbman_swp_interrupt_clear_status(struct qbman_swp *p, u32 mask)
+{
+	qbman_write_register(p, QBMAN_CINH_SWP_ISR, mask);
+}
+
+/**
+ * qbman_swp_interrupt_get_trigger() - read interrupt enable register
+ * @p: the given software portal
+ *
+ * Return the value in the SWP_IER register.
+ */
+u32 qbman_swp_interrupt_get_trigger(struct qbman_swp *p)
+{
+	return qbman_read_register(p, QBMAN_CINH_SWP_IER);
+}
+
+/**
+ * qbman_swp_interrupt_set_trigger() - enable interrupts for a swp
+ * @p: the given software portal
+ * @mask: The mask of bits to enable in SWP_IER
+ */
+void qbman_swp_interrupt_set_trigger(struct qbman_swp *p, u32 mask)
+{
+	qbman_write_register(p, QBMAN_CINH_SWP_IER, mask);
+}
+
+/**
+ * qbman_swp_interrupt_get_inhibit() - read interrupt mask register
+ * @p: the given software portal object
+ *
+ * Return the value in the SWP_IIR register.
+ */
+int qbman_swp_interrupt_get_inhibit(struct qbman_swp *p)
+{
+	return qbman_read_register(p, QBMAN_CINH_SWP_IIR);
+}
+
+/**
+ * qbman_swp_interrupt_set_inhibit() - write interrupt mask register
+ * @p: the given software portal object
+ * @mask: The mask to set in SWP_IIR register
+ */
+void qbman_swp_interrupt_set_inhibit(struct qbman_swp *p, int inhibit)
+{
+	qbman_write_register(p, QBMAN_CINH_SWP_IIR, inhibit ? 0xffffffff : 0);
+}
+
+/*
+ * Different management commands all use this common base layer of code to issue
+ * commands and poll for results.
+ */
+
+/*
+ * Returns a pointer to where the caller should fill in their management command
+ * (caller should ignore the verb byte)
+ */
+void *qbman_swp_mc_start(struct qbman_swp *p)
+{
+	return qbman_get_cmd(p, QBMAN_CENA_SWP_CR);
+}
+
+/*
+ * Commits merges in the caller-supplied command verb (which should not include
+ * the valid-bit) and submits the command to hardware
+ */
+void qbman_swp_mc_submit(struct qbman_swp *p, void *cmd, u8 cmd_verb)
+{
+	u8 *v = cmd;
+
+	dma_wmb();
+	*v = cmd_verb | p->mc.valid_bit;
+}
+
+/*
+ * Checks for a completed response (returns non-NULL if only if the response
+ * is complete).
+ */
+void *qbman_swp_mc_result(struct qbman_swp *p)
+{
+	u32 *ret, verb;
+
+	ret = qbman_get_cmd(p, QBMAN_CENA_SWP_RR(p->mc.valid_bit));
+
+	/* Remove the valid-bit - command completed if the rest is non-zero */
+	verb = ret[0] & ~QB_VALID_BIT;
+	if (!verb)
+		return NULL;
+	p->mc.valid_bit ^= QB_VALID_BIT;
+	return ret;
+}
+
+#define QB_ENQUEUE_CMD_OPTIONS_SHIFT    0
+enum qb_enqueue_commands {
+	enqueue_empty = 0,
+	enqueue_response_always = 1,
+	enqueue_rejects_to_fq = 2
+};
+
+#define QB_ENQUEUE_CMD_ORP_ENABLE_SHIFT      2
+#define QB_ENQUEUE_CMD_IRQ_ON_DISPATCH_SHIFT 3
+#define QB_ENQUEUE_CMD_TARGET_TYPE_SHIFT     4
+
+/**
+ * qbman_eq_desc_clear() - Clear the contents of a descriptor to
+ *                         default/starting state.
+ */
+void qbman_eq_desc_clear(struct qbman_eq_desc *d)
+{
+	memset(d, 0, sizeof(*d));
+}
+
+/**
+ * qbman_eq_desc_set_no_orp() - Set enqueue descriptor without orp
+ * @d:                the enqueue descriptor.
+ * @response_success: 1 = enqueue with response always; 0 = enqueue with
+ *                    rejections returned on a FQ.
+ */
+void qbman_eq_desc_set_no_orp(struct qbman_eq_desc *d, int respond_success)
+{
+	d->verb &= ~(1 << QB_ENQUEUE_CMD_ORP_ENABLE_SHIFT);
+	if (respond_success)
+		d->verb |= enqueue_response_always;
+	else
+		d->verb |= enqueue_rejects_to_fq;
+}
+
+/*
+ * Exactly one of the following descriptor "targets" should be set. (Calling any
+ * one of these will replace the effect of any prior call to one of these.)
+ *   -enqueue to a frame queue
+ *   -enqueue to a queuing destination
+ */
+
+/**
+ * qbman_eq_desc_set_fq() - set the FQ for the enqueue command
+ * @d:    the enqueue descriptor
+ * @fqid: the id of the frame queue to be enqueued
+ */
+void qbman_eq_desc_set_fq(struct qbman_eq_desc *d, u32 fqid)
+{
+	d->verb &= ~(1 << QB_ENQUEUE_CMD_TARGET_TYPE_SHIFT);
+	d->tgtid = cpu_to_le32(fqid);
+}
+
+/**
+ * qbman_eq_desc_set_qd() - Set Queuing Destination for the enqueue command
+ * @d:       the enqueue descriptor
+ * @qdid:    the id of the queuing destination to be enqueued
+ * @qd_bin:  the queuing destination bin
+ * @qd_prio: the queuing destination priority
+ */
+void qbman_eq_desc_set_qd(struct qbman_eq_desc *d, u32 qdid,
+			  u32 qd_bin, u32 qd_prio)
+{
+	d->verb |= 1 << QB_ENQUEUE_CMD_TARGET_TYPE_SHIFT;
+	d->tgtid = cpu_to_le32(qdid);
+	d->qdbin = cpu_to_le16(qd_bin);
+	d->qpri = qd_prio;
+}
+
+#define EQAR_IDX(eqar)     ((eqar) & 0x7)
+#define EQAR_VB(eqar)      ((eqar) & 0x80)
+#define EQAR_SUCCESS(eqar) ((eqar) & 0x100)
+
+/**
+ * qbman_swp_enqueue() - Issue an enqueue command
+ * @s:  the software portal used for enqueue
+ * @d:  the enqueue descriptor
+ * @fd: the frame descriptor to be enqueued
+ *
+ * Please note that 'fd' should only be NULL if the "action" of the
+ * descriptor is "orp_hole" or "orp_nesn".
+ *
+ * Return 0 for successful enqueue, -EBUSY if the EQCR is not ready.
+ */
+int qbman_swp_enqueue(struct qbman_swp *s, const struct qbman_eq_desc *d,
+		      const struct dpaa2_fd *fd)
+{
+	struct qbman_eq_desc *p;
+	u32 eqar = qbman_read_register(s, QBMAN_CINH_SWP_EQAR);
+
+	if (!EQAR_SUCCESS(eqar))
+		return -EBUSY;
+
+	p = qbman_get_cmd(s, QBMAN_CENA_SWP_EQCR(EQAR_IDX(eqar)));
+	memcpy(&p->dca, &d->dca, 31);
+	memcpy(&p->fd, fd, sizeof(*fd));
+
+	/* Set the verb byte, have to substitute in the valid-bit */
+	dma_wmb();
+	p->verb = d->verb | EQAR_VB(eqar);
+
+	return 0;
+}
+
+/* Static (push) dequeue */
+
+/**
+ * qbman_swp_push_get() - Get the push dequeue setup
+ * @p:           the software portal object
+ * @channel_idx: the channel index to query
+ * @enabled:     returned boolean to show whether the push dequeue is enabled
+ *               for the given channel
+ */
+void qbman_swp_push_get(struct qbman_swp *s, u8 channel_idx, int *enabled)
+{
+	u16 src = (s->sdq >> QB_SDQCR_SRC_SHIFT) & QB_SDQCR_SRC_MASK;
+
+	WARN_ON(channel_idx > 15);
+	*enabled = src | (1 << channel_idx);
+}
+
+/**
+ * qbman_swp_push_set() - Enable or disable push dequeue
+ * @p:           the software portal object
+ * @channel_idx: the channel index (0 to 15)
+ * @enable:      enable or disable push dequeue
+ */
+void qbman_swp_push_set(struct qbman_swp *s, u8 channel_idx, int enable)
+{
+	u16 dqsrc;
+
+	WARN_ON(channel_idx > 15);
+	if (enable)
+		s->sdq |= 1 << channel_idx;
+	else
+		s->sdq &= ~(1 << channel_idx);
+
+	/* Read make the complete src map.  If no channels are enabled
+	 * the SDQCR must be 0 or else QMan will assert errors
+	 */
+	dqsrc = (s->sdq >> QB_SDQCR_SRC_SHIFT) & QB_SDQCR_SRC_MASK;
+	if (dqsrc != 0)
+		qbman_write_register(s, QBMAN_CINH_SWP_SDQCR, s->sdq);
+	else
+		qbman_write_register(s, QBMAN_CINH_SWP_SDQCR, 0);
+}
+
+#define QB_VDQCR_VERB_DCT_SHIFT    0
+#define QB_VDQCR_VERB_DT_SHIFT     2
+#define QB_VDQCR_VERB_RLS_SHIFT    4
+#define QB_VDQCR_VERB_WAE_SHIFT    5
+
+enum qb_pull_dt_e {
+	qb_pull_dt_channel,
+	qb_pull_dt_workqueue,
+	qb_pull_dt_framequeue
+};
+
+/**
+ * qbman_pull_desc_clear() - Clear the contents of a descriptor to
+ *                           default/starting state
+ * @d: the pull dequeue descriptor to be cleared
+ */
+void qbman_pull_desc_clear(struct qbman_pull_desc *d)
+{
+	memset(d, 0, sizeof(*d));
+}
+
+/**
+ * qbman_pull_desc_set_storage()- Set the pull dequeue storage
+ * @d:            the pull dequeue descriptor to be set
+ * @storage:      the pointer of the memory to store the dequeue result
+ * @storage_phys: the physical address of the storage memory
+ * @stash:        to indicate whether write allocate is enabled
+ *
+ * If not called, or if called with 'storage' as NULL, the result pull dequeues
+ * will produce results to DQRR. If 'storage' is non-NULL, then results are
+ * produced to the given memory location (using the DMA address which
+ * the caller provides in 'storage_phys'), and 'stash' controls whether or not
+ * those writes to main-memory express a cache-warming attribute.
+ */
+void qbman_pull_desc_set_storage(struct qbman_pull_desc *d,
+				 struct dpaa2_dq *storage,
+				 dma_addr_t storage_phys,
+				 int stash)
+{
+	/* save the virtual address */
+	d->rsp_addr_virt = (u64)storage;
+
+	if (!storage) {
+		d->verb &= ~(1 << QB_VDQCR_VERB_RLS_SHIFT);
+		return;
+	}
+	d->verb |= 1 << QB_VDQCR_VERB_RLS_SHIFT;
+	if (stash)
+		d->verb |= 1 << QB_VDQCR_VERB_WAE_SHIFT;
+	else
+		d->verb &= ~(1 << QB_VDQCR_VERB_WAE_SHIFT);
+
+	d->rsp_addr = cpu_to_le64(storage_phys);
+}
+
+/**
+ * qbman_pull_desc_set_numframes() - Set the number of frames to be dequeued
+ * @d:         the pull dequeue descriptor to be set
+ * @numframes: number of frames to be set, must be between 1 and 16, inclusive
+ */
+void qbman_pull_desc_set_numframes(struct qbman_pull_desc *d, u8 numframes)
+{
+	d->numf = numframes - 1;
+}
+
+void qbman_pull_desc_set_token(struct qbman_pull_desc *d, u8 token)
+{
+	d->tok = token;
+}
+
+/*
+ * Exactly one of the following descriptor "actions" should be set. (Calling any
+ * one of these will replace the effect of any prior call to one of these.)
+ * - pull dequeue from the given frame queue (FQ)
+ * - pull dequeue from any FQ in the given work queue (WQ)
+ * - pull dequeue from any FQ in any WQ in the given channel
+ */
+
+/**
+ * qbman_pull_desc_set_fq() - Set fqid from which the dequeue command dequeues
+ * @fqid: the frame queue index of the given FQ
+ */
+void qbman_pull_desc_set_fq(struct qbman_pull_desc *d, u32 fqid)
+{
+	d->verb |= 1 << QB_VDQCR_VERB_DCT_SHIFT;
+	d->verb |= qb_pull_dt_framequeue << QB_VDQCR_VERB_DT_SHIFT;
+	d->dq_src = cpu_to_le32(fqid);
+}
+
+/**
+ * qbman_pull_desc_set_wq() - Set wqid from which the dequeue command dequeues
+ * @wqid: composed of channel id and wqid within the channel
+ * @dct:  the dequeue command type
+ */
+void qbman_pull_desc_set_wq(struct qbman_pull_desc *d, u32 wqid,
+			    enum qbman_pull_type_e dct)
+{
+	d->verb |= dct << QB_VDQCR_VERB_DCT_SHIFT;
+	d->verb |= qb_pull_dt_workqueue << QB_VDQCR_VERB_DT_SHIFT;
+	d->dq_src = cpu_to_le32(wqid);
+}
+
+/**
+ * qbman_pull_desc_set_channel() - Set channelid from which the dequeue command
+ *                                 dequeues
+ * @chid: the channel id to be dequeued
+ * @dct:  the dequeue command type
+ */
+void qbman_pull_desc_set_channel(struct qbman_pull_desc *d, u32 chid,
+				 enum qbman_pull_type_e dct)
+{
+	d->verb |= dct << QB_VDQCR_VERB_DCT_SHIFT;
+	d->verb |= qb_pull_dt_channel << QB_VDQCR_VERB_DT_SHIFT;
+	d->dq_src = cpu_to_le32(chid);
+}
+
+/**
+ * qbman_swp_pull() - Issue the pull dequeue command
+ * @s: the software portal object
+ * @d: the software portal descriptor which has been configured with
+ *     the set of qbman_pull_desc_set_*() calls
+ *
+ * Return 0 for success, and -EBUSY if the software portal is not ready
+ * to do pull dequeue.
+ */
+int qbman_swp_pull(struct qbman_swp *s, struct qbman_pull_desc *d)
+{
+	struct qbman_pull_desc *p;
+
+	if (!atomic_dec_and_test(&s->vdq.available)) {
+		atomic_inc(&s->vdq.available);
+		return -EBUSY;
+	}
+	s->vdq.storage = (void *)d->rsp_addr_virt;
+	p = qbman_get_cmd(s, QBMAN_CENA_SWP_VDQCR);
+	p->numf = d->numf;
+	p->tok = QMAN_DQ_TOKEN_VALID;
+	p->dq_src = d->dq_src;
+	p->rsp_addr = d->rsp_addr;
+	p->rsp_addr_virt = d->rsp_addr_virt;
+	dma_wmb();
+
+	/* Set the verb byte, have to substitute in the valid-bit */
+	p->verb = d->verb | s->vdq.valid_bit;
+	s->vdq.valid_bit ^= QB_VALID_BIT;
+
+	return 0;
+}
+
+#define QMAN_DQRR_PI_MASK   0xf
+
+/**
+ * qbman_swp_dqrr_next() - Get an valid DQRR entry
+ * @s: the software portal object
+ *
+ * Return NULL if there are no unconsumed DQRR entries. Return a DQRR entry
+ * only once, so repeated calls can return a sequence of DQRR entries, without
+ * requiring they be consumed immediately or in any particular order.
+ */
+const struct dpaa2_dq *qbman_swp_dqrr_next(struct qbman_swp *s)
+{
+	u32 verb;
+	u32 response_verb;
+	u32 flags;
+	struct dpaa2_dq *p;
+
+	/* Before using valid-bit to detect if something is there, we have to
+	 * handle the case of the DQRR reset bug...
+	 */
+	if (unlikely(s->dqrr.reset_bug)) {
+		/*
+		 * We pick up new entries by cache-inhibited producer index,
+		 * which means that a non-coherent mapping would require us to
+		 * invalidate and read *only* once that PI has indicated that
+		 * there's an entry here. The first trip around the DQRR ring
+		 * will be much less efficient than all subsequent trips around
+		 * it...
+		 */
+		u8 pi = qbman_read_register(s, QBMAN_CINH_SWP_DQPI) &
+			QMAN_DQRR_PI_MASK;
+
+		/* there are new entries if pi != next_idx */
+		if (pi == s->dqrr.next_idx)
+			return NULL;
+
+		/*
+		 * if next_idx is/was the last ring index, and 'pi' is
+		 * different, we can disable the workaround as all the ring
+		 * entries have now been DMA'd to so valid-bit checking is
+		 * repaired. Note: this logic needs to be based on next_idx
+		 * (which increments one at a time), rather than on pi (which
+		 * can burst and wrap-around between our snapshots of it).
+		 */
+		if (s->dqrr.next_idx == (s->dqrr.dqrr_size - 1)) {
+			pr_debug("next_idx=%d, pi=%d, clear reset bug\n",
+				 s->dqrr.next_idx, pi);
+			s->dqrr.reset_bug = 0;
+		}
+		prefetch(qbman_get_cmd(s,
+				       QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx)));
+	}
+
+	p = qbman_get_cmd(s, QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx));
+	verb = p->dq.verb;
+
+	/*
+	 * If the valid-bit isn't of the expected polarity, nothing there. Note,
+	 * in the DQRR reset bug workaround, we shouldn't need to skip these
+	 * check, because we've already determined that a new entry is available
+	 * and we've invalidated the cacheline before reading it, so the
+	 * valid-bit behaviour is repaired and should tell us what we already
+	 * knew from reading PI.
+	 */
+	if ((verb & QB_VALID_BIT) != s->dqrr.valid_bit) {
+		prefetch(qbman_get_cmd(s,
+				       QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx)));
+		return NULL;
+	}
+	/*
+	 * There's something there. Move "next_idx" attention to the next ring
+	 * entry (and prefetch it) before returning what we found.
+	 */
+	s->dqrr.next_idx++;
+	s->dqrr.next_idx &= s->dqrr.dqrr_size - 1; /* Wrap around */
+	if (!s->dqrr.next_idx)
+		s->dqrr.valid_bit ^= QB_VALID_BIT;
+
+	/*
+	 * If this is the final response to a volatile dequeue command
+	 * indicate that the vdq is available
+	 */
+	flags = p->dq.stat;
+	response_verb = verb & QBMAN_RESULT_MASK;
+	if ((response_verb == QBMAN_RESULT_DQ) &&
+	    (flags & DPAA2_DQ_STAT_VOLATILE) &&
+	    (flags & DPAA2_DQ_STAT_EXPIRED))
+		atomic_inc(&s->vdq.available);
+
+	prefetch(qbman_get_cmd(s, QBMAN_CENA_SWP_DQRR(s->dqrr.next_idx)));
+
+	return p;
+}
+
+/**
+ * qbman_swp_dqrr_consume() -  Consume DQRR entries previously returned from
+ *                             qbman_swp_dqrr_next().
+ * @s: the software portal object
+ * @dq: the DQRR entry to be consumed
+ */
+void qbman_swp_dqrr_consume(struct qbman_swp *s, const struct dpaa2_dq *dq)
+{
+	qbman_write_register(s, QBMAN_CINH_SWP_DCAP, QBMAN_IDX_FROM_DQRR(dq));
+}
+
+/**
+ * qbman_result_has_new_result() - Check and get the dequeue response from the
+ *                                 dq storage memory set in pull dequeue command
+ * @s: the software portal object
+ * @dq: the dequeue result read from the memory
+ *
+ * Return 1 for getting a valid dequeue result, or 0 for not getting a valid
+ * dequeue result.
+ *
+ * Only used for user-provided storage of dequeue results, not DQRR. For
+ * efficiency purposes, the driver will perform any required endianness
+ * conversion to ensure that the user's dequeue result storage is in host-endian
+ * format. As such, once the user has called qbman_result_has_new_result() and
+ * been returned a valid dequeue result, they should not call it again on
+ * the same memory location (except of course if another dequeue command has
+ * been executed to produce a new result to that location).
+ */
+int qbman_result_has_new_result(struct qbman_swp *s, const struct dpaa2_dq *dq)
+{
+	if (dq->dq.tok != QMAN_DQ_TOKEN_VALID)
+		return 0;
+
+	/*
+	 * Set token to be 0 so we will detect change back to 1
+	 * next time the looping is traversed. Const is cast away here
+	 * as we want users to treat the dequeue responses as read only.
+	 */
+	((struct dpaa2_dq *)dq)->dq.tok = 0;
+
+	/*
+	 * Determine whether VDQCR is available based on whether the
+	 * current result is sitting in the first storage location of
+	 * the busy command.
+	 */
+	if (s->vdq.storage == dq) {
+		s->vdq.storage = NULL;
+		atomic_inc(&s->vdq.available);
+	}
+
+	return 1;
+}
+
+/**
+ * qbman_release_desc_clear() - Clear the contents of a descriptor to
+ *                              default/starting state.
+ */
+void qbman_release_desc_clear(struct qbman_release_desc *d)
+{
+	memset(d, 0, sizeof(*d));
+	d->verb = 1 << 5; /* Release Command Valid */
+}
+
+/**
+ * qbman_release_desc_set_bpid() - Set the ID of the buffer pool to release to
+ */
+void qbman_release_desc_set_bpid(struct qbman_release_desc *d, u16 bpid)
+{
+	d->bpid = cpu_to_le16(bpid);
+}
+
+/**
+ * qbman_release_desc_set_rcdi() - Determines whether or not the portal's RCDI
+ * interrupt source should be asserted after the release command is completed.
+ */
+void qbman_release_desc_set_rcdi(struct qbman_release_desc *d, int enable)
+{
+	if (enable)
+		d->verb |= 1 << 6;
+	else
+		d->verb &= ~(1 << 6);
+}
+
+#define RAR_IDX(rar)     ((rar) & 0x7)
+#define RAR_VB(rar)      ((rar) & 0x80)
+#define RAR_SUCCESS(rar) ((rar) & 0x100)
+
+/**
+ * qbman_swp_release() - Issue a buffer release command
+ * @s:           the software portal object
+ * @d:           the release descriptor
+ * @buffers:     a pointer pointing to the buffer address to be released
+ * @num_buffers: number of buffers to be released,  must be less than 8
+ *
+ * Return 0 for success, -EBUSY if the release command ring is not ready.
+ */
+int qbman_swp_release(struct qbman_swp *s, const struct qbman_release_desc *d,
+		      const u64 *buffers, unsigned int num_buffers)
+{
+	int i;
+	struct qbman_release_desc *p;
+	u32 rar;
+
+	if (!num_buffers || (num_buffers > 7))
+		return -EINVAL;
+
+	rar = qbman_read_register(s, QBMAN_CINH_SWP_RAR);
+	if (!RAR_SUCCESS(rar))
+		return -EBUSY;
+
+	/* Start the release command */
+	p = qbman_get_cmd(s, QBMAN_CENA_SWP_RCR(RAR_IDX(rar)));
+	/* Copy the caller's buffer pointers to the command */
+	for (i = 0; i < num_buffers; i++)
+		p->buf[i] = cpu_to_le64(buffers[i]);
+	p->bpid = d->bpid;
+
+	/*
+	 * Set the verb byte, have to substitute in the valid-bit and the number
+	 * of buffers.
+	 */
+	dma_wmb();
+	p->verb = d->verb | RAR_VB(rar) | num_buffers;
+
+	return 0;
+}
+
+struct qbman_acquire_desc {
+	u8 verb;
+	u8 reserved;
+	u16 bpid;
+	u8 num;
+	u8 reserved2[59];
+};
+
+struct qbman_acquire_rslt {
+	u8 verb;
+	u8 rslt;
+	u16 reserved;
+	u8 num;
+	u8 reserved2[3];
+	u64 buf[7];
+};
+
+/**
+ * qbman_swp_acquire() - Issue a buffer acquire command
+ * @s:           the software portal object
+ * @bpid:        the buffer pool index
+ * @buffers:     a pointer pointing to the acquired buffer addresses
+ * @num_buffers: number of buffers to be acquired, must be less than 8
+ *
+ * Return 0 for success, or negative error code if the acquire command
+ * fails.
+ */
+int qbman_swp_acquire(struct qbman_swp *s, u16 bpid, u64 *buffers,
+		      unsigned int num_buffers)
+{
+	struct qbman_acquire_desc *p;
+	struct qbman_acquire_rslt *r;
+	int i;
+
+	if (!num_buffers || (num_buffers > 7))
+		return -EINVAL;
+
+	/* Start the management command */
+	p = qbman_swp_mc_start(s);
+
+	if (!p)
+		return -EBUSY;
+
+	/* Encode the caller-provided attributes */
+	p->bpid = cpu_to_le16(bpid);
+	p->num = num_buffers;
+
+	/* Complete the management command */
+	r = qbman_swp_mc_complete(s, p, QBMAN_MC_ACQUIRE);
+	if (unlikely(!r)) {
+		pr_err("qbman: acquire from BPID %d failed, no response\n",
+		       bpid);
+		return -EIO;
+	}
+
+	/* Decode the outcome */
+	WARN_ON((r->verb & 0x7f) != QBMAN_MC_ACQUIRE);
+
+	/* Determine success or failure */
+	if (unlikely(r->rslt != QBMAN_MC_RSLT_OK)) {
+		pr_err("qbman: acquire from BPID 0x%x failed, code=0x%02x\n",
+		       bpid, r->rslt);
+		return -EIO;
+	}
+
+	WARN_ON(r->num > num_buffers);
+
+	/* Copy the acquired buffers to the caller's array */
+	for (i = 0; i < r->num; i++)
+		buffers[i] = le64_to_cpu(r->buf[i]);
+
+	return (int)r->num;
+}
+
+struct qbman_alt_fq_state_desc {
+	u8 verb;
+	u8 reserved[3];
+	u32 fqid;
+	u8 reserved2[56];
+};
+
+struct qbman_alt_fq_state_rslt {
+	u8 verb;
+	u8 rslt;
+	u8 reserved[62];
+};
+
+#define ALT_FQ_FQID_MASK 0x00FFFFFF
+
+int qbman_swp_alt_fq_state(struct qbman_swp *s, u32 fqid,
+			   u8 alt_fq_verb)
+{
+	struct qbman_alt_fq_state_desc *p;
+	struct qbman_alt_fq_state_rslt *r;
+
+	/* Start the management command */
+	p = qbman_swp_mc_start(s);
+	if (!p)
+		return -EBUSY;
+
+	p->fqid = cpu_to_le32(fqid) & ALT_FQ_FQID_MASK;
+
+	/* Complete the management command */
+	r = qbman_swp_mc_complete(s, p, alt_fq_verb);
+	if (unlikely(!r)) {
+		pr_err("qbman: mgmt cmd failed, no response (verb=0x%x)\n",
+		       alt_fq_verb);
+		return -EIO;
+	}
+
+	/* Decode the outcome */
+	WARN_ON((r->verb & QBMAN_RESULT_MASK) != alt_fq_verb);
+
+	/* Determine success or failure */
+	if (unlikely(r->rslt != QBMAN_MC_RSLT_OK)) {
+		pr_err("qbman: ALT FQID %d failed: verb = 0x%08x code = 0x%02x\n",
+		       fqid, r->verb, r->rslt);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+struct qbman_cdan_ctrl_desc {
+	u8 verb;
+	u8 reserved;
+	u16 ch;
+	u8 we;
+	u8 ctrl;
+	u16 reserved2;
+	u64 cdan_ctx;
+	u8 reserved3[48];
+
+};
+
+struct qbman_cdan_ctrl_rslt {
+	u8 verb;
+	u8 rslt;
+	u16 ch;
+	u8 reserved[60];
+};
+
+int qbman_swp_CDAN_set(struct qbman_swp *s, u16 channelid,
+		       u8 we_mask, u8 cdan_en,
+		       u64 ctx)
+{
+	struct qbman_cdan_ctrl_desc *p = NULL;
+	struct qbman_cdan_ctrl_rslt *r = NULL;
+
+	/* Start the management command */
+	p = qbman_swp_mc_start(s);
+	if (!p)
+		return -EBUSY;
+
+	/* Encode the caller-provided attributes */
+	p->ch = cpu_to_le16(channelid);
+	p->we = we_mask;
+	if (cdan_en)
+		p->ctrl = 1;
+	else
+		p->ctrl = 0;
+	p->cdan_ctx = cpu_to_le64(ctx);
+
+	/* Complete the management command */
+	r = qbman_swp_mc_complete(s, p, QBMAN_WQCHAN_CONFIGURE);
+	if (unlikely(!r)) {
+		pr_err("qbman: wqchan config failed, no response\n");
+		return -EIO;
+	}
+
+	WARN_ON((r->verb & 0x7f) != QBMAN_WQCHAN_CONFIGURE);
+
+	/* Determine success or failure */
+	if (unlikely(r->rslt != QBMAN_MC_RSLT_OK)) {
+		pr_err("qbman: CDAN cQID %d failed: code = 0x%02x\n",
+		       channelid, r->rslt);
+		return -EIO;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/fsl-mc/bus/dpio/qbman-portal.h b/drivers/staging/fsl-mc/bus/dpio/qbman-portal.h
new file mode 100644
index 000000000000..842855971f34
--- /dev/null
+++ b/drivers/staging/fsl-mc/bus/dpio/qbman-portal.h
@@ -0,0 +1,469 @@
+/*
+ * Copyright (C) 2014-2016 Freescale Semiconductor, Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#ifndef __FSL_QBMAN_PORTAL_H
+#define __FSL_QBMAN_PORTAL_H
+
+#include "../../include/dpaa2-fd.h"
+
+struct dpaa2_dq;
+struct qbman_swp;
+
+/* qbman software portal descriptor structure */
+struct qbman_swp_desc {
+	void *cena_bar; /* Cache-enabled portal base address */
+	void *cinh_bar; /* Cache-inhibited portal base address */
+	u32 qman_version;
+};
+
+#define QBMAN_SWP_INTERRUPT_EQRI 0x01
+#define QBMAN_SWP_INTERRUPT_EQDI 0x02
+#define QBMAN_SWP_INTERRUPT_DQRI 0x04
+#define QBMAN_SWP_INTERRUPT_RCRI 0x08
+#define QBMAN_SWP_INTERRUPT_RCDI 0x10
+#define QBMAN_SWP_INTERRUPT_VDCI 0x20
+
+/* the structure for pull dequeue descriptor */
+struct qbman_pull_desc {
+	u8 verb;
+	u8 numf;
+	u8 tok;
+	u8 reserved;
+	u32 dq_src;
+	u64 rsp_addr;
+	u64 rsp_addr_virt;
+	u8 padding[40];
+};
+
+enum qbman_pull_type_e {
+	/* dequeue with priority precedence, respect intra-class scheduling */
+	qbman_pull_type_prio = 1,
+	/* dequeue with active FQ precedence, respect ICS */
+	qbman_pull_type_active,
+	/* dequeue with active FQ precedence, no ICS */
+	qbman_pull_type_active_noics
+};
+
+/* Definitions for parsing dequeue entries */
+#define QBMAN_RESULT_MASK      0x7f
+#define QBMAN_RESULT_DQ        0x60
+#define QBMAN_RESULT_FQRN      0x21
+#define QBMAN_RESULT_FQRNI     0x22
+#define QBMAN_RESULT_FQPN      0x24
+#define QBMAN_RESULT_FQDAN     0x25
+#define QBMAN_RESULT_CDAN      0x26
+#define QBMAN_RESULT_CSCN_MEM  0x27
+#define QBMAN_RESULT_CGCU      0x28
+#define QBMAN_RESULT_BPSCN     0x29
+#define QBMAN_RESULT_CSCN_WQ   0x2a
+
+/* QBMan FQ management command codes */
+#define QBMAN_FQ_SCHEDULE	0x48
+#define QBMAN_FQ_FORCE		0x49
+#define QBMAN_FQ_XON		0x4d
+#define QBMAN_FQ_XOFF		0x4e
+
+/* structure of enqueue descriptor */
+struct qbman_eq_desc {
+	u8 verb;
+	u8 dca;
+	u16 seqnum;
+	u16 orpid;
+	u16 reserved1;
+	u32 tgtid;
+	u32 tag;
+	u16 qdbin;
+	u8 qpri;
+	u8 reserved[3];
+	u8 wae;
+	u8 rspid;
+	u64 rsp_addr;
+	u8 fd[32];
+};
+
+/* buffer release descriptor */
+struct qbman_release_desc {
+	u8 verb;
+	u8 reserved;
+	u16 bpid;
+	u32 reserved2;
+	u64 buf[7];
+};
+
+/* Management command result codes */
+#define QBMAN_MC_RSLT_OK      0xf0
+
+#define CODE_CDAN_WE_EN    0x1
+#define CODE_CDAN_WE_CTX   0x4
+
+/* portal data structure */
+struct qbman_swp {
+	const struct qbman_swp_desc *desc;
+	void __iomem *addr_cena;
+	void __iomem *addr_cinh;
+
+	/* Management commands */
+	struct {
+		u32 valid_bit; /* 0x00 or 0x80 */
+	} mc;
+
+	/* Push dequeues */
+	u32 sdq;
+
+	/* Volatile dequeues */
+	struct {
+		atomic_t available; /* indicates if a command can be sent */
+		u32 valid_bit; /* 0x00 or 0x80 */
+		struct dpaa2_dq *storage; /* NULL if DQRR */
+	} vdq;
+
+	/* DQRR */
+	struct {
+		u32 next_idx;
+		u32 valid_bit;
+		u8 dqrr_size;
+		int reset_bug; /* indicates dqrr reset workaround is needed */
+	} dqrr;
+};
+
+struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d);
+void qbman_swp_finish(struct qbman_swp *p);
+u32 qbman_swp_interrupt_read_status(struct qbman_swp *p);
+void qbman_swp_interrupt_clear_status(struct qbman_swp *p, u32 mask);
+u32 qbman_swp_interrupt_get_trigger(struct qbman_swp *p);
+void qbman_swp_interrupt_set_trigger(struct qbman_swp *p, u32 mask);
+int qbman_swp_interrupt_get_inhibit(struct qbman_swp *p);
+void qbman_swp_interrupt_set_inhibit(struct qbman_swp *p, int inhibit);
+
+void qbman_swp_push_get(struct qbman_swp *p, u8 channel_idx, int *enabled);
+void qbman_swp_push_set(struct qbman_swp *p, u8 channel_idx, int enable);
+
+void qbman_pull_desc_clear(struct qbman_pull_desc *d);
+void qbman_pull_desc_set_storage(struct qbman_pull_desc *d,
+				 struct dpaa2_dq *storage,
+				 dma_addr_t storage_phys,
+				 int stash);
+void qbman_pull_desc_set_numframes(struct qbman_pull_desc *d, u8 numframes);
+void qbman_pull_desc_set_fq(struct qbman_pull_desc *d, u32 fqid);
+void qbman_pull_desc_set_wq(struct qbman_pull_desc *d, u32 wqid,
+			    enum qbman_pull_type_e dct);
+void qbman_pull_desc_set_channel(struct qbman_pull_desc *d, u32 chid,
+				 enum qbman_pull_type_e dct);
+
+int qbman_swp_pull(struct qbman_swp *p, struct qbman_pull_desc *d);
+
+const struct dpaa2_dq *qbman_swp_dqrr_next(struct qbman_swp *s);
+void qbman_swp_dqrr_consume(struct qbman_swp *s, const struct dpaa2_dq *dq);
+
+int qbman_result_has_new_result(struct qbman_swp *p, const struct dpaa2_dq *dq);
+
+void qbman_eq_desc_clear(struct qbman_eq_desc *d);
+void qbman_eq_desc_set_no_orp(struct qbman_eq_desc *d, int respond_success);
+void qbman_eq_desc_set_token(struct qbman_eq_desc *d, u8 token);
+void qbman_eq_desc_set_fq(struct qbman_eq_desc *d, u32 fqid);
+void qbman_eq_desc_set_qd(struct qbman_eq_desc *d, u32 qdid,
+			  u32 qd_bin, u32 qd_prio);
+
+int qbman_swp_enqueue(struct qbman_swp *p, const struct qbman_eq_desc *d,
+		      const struct dpaa2_fd *fd);
+
+void qbman_release_desc_clear(struct qbman_release_desc *d);
+void qbman_release_desc_set_bpid(struct qbman_release_desc *d, u16 bpid);
+void qbman_release_desc_set_rcdi(struct qbman_release_desc *d, int enable);
+
+int qbman_swp_release(struct qbman_swp *s, const struct qbman_release_desc *d,
+		      const u64 *buffers, unsigned int num_buffers);
+int qbman_swp_acquire(struct qbman_swp *s, u16 bpid, u64 *buffers,
+		      unsigned int num_buffers);
+int qbman_swp_alt_fq_state(struct qbman_swp *s, u32 fqid,
+			   u8 alt_fq_verb);
+int qbman_swp_CDAN_set(struct qbman_swp *s, u16 channelid,
+		       u8 we_mask, u8 cdan_en,
+		       u64 ctx);
+
+void *qbman_swp_mc_start(struct qbman_swp *p);
+void qbman_swp_mc_submit(struct qbman_swp *p, void *cmd, u8 cmd_verb);
+void *qbman_swp_mc_result(struct qbman_swp *p);
+
+/**
+ * qbman_result_is_DQ() - check if the dequeue result is a dequeue response
+ * @dq: the dequeue result to be checked
+ *
+ * DQRR entries may contain non-dequeue results, ie. notifications
+ */
+static inline int qbman_result_is_DQ(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_DQ);
+}
+
+/**
+ * qbman_result_is_SCN() - Check the dequeue result is notification or not
+ * @dq: the dequeue result to be checked
+ *
+ */
+static inline int qbman_result_is_SCN(const struct dpaa2_dq *dq)
+{
+	return !qbman_result_is_DQ(dq);
+}
+
+/* FQ Data Availability */
+static inline int qbman_result_is_FQDAN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_FQDAN);
+}
+
+/* Channel Data Availability */
+static inline int qbman_result_is_CDAN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_CDAN);
+}
+
+/* Congestion State Change */
+static inline int qbman_result_is_CSCN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_CSCN_WQ);
+}
+
+/* Buffer Pool State Change */
+static inline int qbman_result_is_BPSCN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_BPSCN);
+}
+
+/* Congestion Group Count Update */
+static inline int qbman_result_is_CGCU(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_CGCU);
+}
+
+/* Retirement */
+static inline int qbman_result_is_FQRN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_FQRN);
+}
+
+/* Retirement Immediate */
+static inline int qbman_result_is_FQRNI(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_FQRNI);
+}
+
+ /* Park */
+static inline int qbman_result_is_FQPN(const struct dpaa2_dq *dq)
+{
+	return ((dq->dq.verb & QBMAN_RESULT_MASK) == QBMAN_RESULT_FQPN);
+}
+
+/**
+ * qbman_result_SCN_state() - Get the state field in State-change notification
+ */
+static inline u8 qbman_result_SCN_state(const struct dpaa2_dq *scn)
+{
+	return scn->scn.state;
+}
+
+#define SCN_RID_MASK 0x00FFFFFF
+
+/**
+ * qbman_result_SCN_rid() - Get the resource id in State-change notification
+ */
+static inline u32 qbman_result_SCN_rid(const struct dpaa2_dq *scn)
+{
+	return le32_to_cpu(scn->scn.rid_tok) & SCN_RID_MASK;
+}
+
+/**
+ * qbman_result_SCN_ctx() - Get the context data in State-change notification
+ */
+static inline u64 qbman_result_SCN_ctx(const struct dpaa2_dq *scn)
+{
+	return le64_to_cpu(scn->scn.ctx);
+}
+
+/**
+ * qbman_swp_fq_schedule() - Move the fq to the scheduled state
+ * @s:    the software portal object
+ * @fqid: the index of frame queue to be scheduled
+ *
+ * There are a couple of different ways that a FQ can end up parked state,
+ * This schedules it.
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_fq_schedule(struct qbman_swp *s, u32 fqid)
+{
+	return qbman_swp_alt_fq_state(s, fqid, QBMAN_FQ_SCHEDULE);
+}
+
+/**
+ * qbman_swp_fq_force() - Force the FQ to fully scheduled state
+ * @s:    the software portal object
+ * @fqid: the index of frame queue to be forced
+ *
+ * Force eligible will force a tentatively-scheduled FQ to be fully-scheduled
+ * and thus be available for selection by any channel-dequeuing behaviour (push
+ * or pull). If the FQ is subsequently "dequeued" from the channel and is still
+ * empty at the time this happens, the resulting dq_entry will have no FD.
+ * (qbman_result_DQ_fd() will return NULL.)
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_fq_force(struct qbman_swp *s, u32 fqid)
+{
+	return qbman_swp_alt_fq_state(s, fqid, QBMAN_FQ_FORCE);
+}
+
+/**
+ * qbman_swp_fq_xon() - sets FQ flow-control to XON
+ * @s:    the software portal object
+ * @fqid: the index of frame queue
+ *
+ * This setting doesn't affect enqueues to the FQ, just dequeues.
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_fq_xon(struct qbman_swp *s, u32 fqid)
+{
+	return qbman_swp_alt_fq_state(s, fqid, QBMAN_FQ_XON);
+}
+
+/**
+ * qbman_swp_fq_xoff() - sets FQ flow-control to XOFF
+ * @s:    the software portal object
+ * @fqid: the index of frame queue
+ *
+ * This setting doesn't affect enqueues to the FQ, just dequeues.
+ * XOFF FQs will remain in the tenatively-scheduled state, even when
+ * non-empty, meaning they won't be selected for scheduled dequeuing.
+ * If a FQ is changed to XOFF after it had already become truly-scheduled
+ * to a channel, and a pull dequeue of that channel occurs that selects
+ * that FQ for dequeuing, then the resulting dq_entry will have no FD.
+ * (qbman_result_DQ_fd() will return NULL.)
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_fq_xoff(struct qbman_swp *s, u32 fqid)
+{
+	return qbman_swp_alt_fq_state(s, fqid, QBMAN_FQ_XOFF);
+}
+
+/* If the user has been allocated a channel object that is going to generate
+ * CDANs to another channel, then the qbman_swp_CDAN* functions will be
+ * necessary.
+ *
+ * CDAN-enabled channels only generate a single CDAN notification, after which
+ * they need to be reenabled before they'll generate another. The idea is
+ * that pull dequeuing will occur in reaction to the CDAN, followed by a
+ * reenable step. Each function generates a distinct command to hardware, so a
+ * combination function is provided if the user wishes to modify the "context"
+ * (which shows up in each CDAN message) each time they reenable, as a single
+ * command to hardware.
+ */
+
+/**
+ * qbman_swp_CDAN_set_context() - Set CDAN context
+ * @s:         the software portal object
+ * @channelid: the channel index
+ * @ctx:       the context to be set in CDAN
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_CDAN_set_context(struct qbman_swp *s, u16 channelid,
+					     u64 ctx)
+{
+	return qbman_swp_CDAN_set(s, channelid,
+				  CODE_CDAN_WE_CTX,
+				  0, ctx);
+}
+
+/**
+ * qbman_swp_CDAN_enable() - Enable CDAN for the channel
+ * @s:         the software portal object
+ * @channelid: the index of the channel to generate CDAN
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_CDAN_enable(struct qbman_swp *s, u16 channelid)
+{
+	return qbman_swp_CDAN_set(s, channelid,
+				  CODE_CDAN_WE_EN,
+				  1, 0);
+}
+
+/**
+ * qbman_swp_CDAN_disable() - disable CDAN for the channel
+ * @s:         the software portal object
+ * @channelid: the index of the channel to generate CDAN
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_CDAN_disable(struct qbman_swp *s, u16 channelid)
+{
+	return qbman_swp_CDAN_set(s, channelid,
+				  CODE_CDAN_WE_EN,
+				  0, 0);
+}
+
+/**
+ * qbman_swp_CDAN_set_context_enable() - Set CDAN contest and enable CDAN
+ * @s:         the software portal object
+ * @channelid: the index of the channel to generate CDAN
+ * @ctx:i      the context set in CDAN
+ *
+ * Return 0 for success, or negative error code for failure.
+ */
+static inline int qbman_swp_CDAN_set_context_enable(struct qbman_swp *s,
+						    u16 channelid,
+						    u64 ctx)
+{
+	return qbman_swp_CDAN_set(s, channelid,
+				  CODE_CDAN_WE_EN | CODE_CDAN_WE_CTX,
+				  1, ctx);
+}
+
+/* Wraps up submit + poll-for-result */
+static inline void *qbman_swp_mc_complete(struct qbman_swp *swp, void *cmd,
+					  u8 cmd_verb)
+{
+	int loopvar = 1000;
+
+	qbman_swp_mc_submit(swp, cmd, cmd_verb);
+
+	do {
+		cmd = qbman_swp_mc_result(swp);
+	} while (!cmd && loopvar--);
+
+	WARN_ON(!loopvar);
+
+	return cmd;
+}
+
+#endif /* __FSL_QBMAN_PORTAL_H */
diff --git a/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c b/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
index 87e44712b56c..49127acb85b2 100644
--- a/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
+++ b/drivers/staging/fsl-mc/bus/irq-gic-v3-its-fsl-mc-msi.c
@@ -95,8 +95,8 @@ int __init its_fsl_mc_msi_init(void)
 			continue;
 		}
 
-		WARN_ON(mc_msi_domain->
-				host_data != &its_fsl_mc_msi_domain_info);
+		WARN_ON(mc_msi_domain->host_data !=
+			&its_fsl_mc_msi_domain_info);
 
 		pr_info("fsl-mc MSI: %s domain created\n", np->full_name);
 	}
diff --git a/drivers/staging/fsl-mc/include/dpaa2-fd.h b/drivers/staging/fsl-mc/include/dpaa2-fd.h
new file mode 100644
index 000000000000..cf7857f00a5c
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpaa2-fd.h
@@ -0,0 +1,451 @@
+/*
+ * Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#ifndef __FSL_DPAA2_FD_H
+#define __FSL_DPAA2_FD_H
+
+#include <linux/kernel.h>
+
+/**
+ * DOC: DPAA2 FD - Frame Descriptor APIs for DPAA2
+ *
+ * Frame Descriptors (FDs) are used to describe frame data in the DPAA2.
+ * Frames can be enqueued and dequeued to Frame Queues (FQs) which are consumed
+ * by the various DPAA accelerators (WRIOP, SEC, PME, DCE)
+ *
+ * There are three types of frames: single, scatter gather, and frame lists.
+ *
+ * The set of APIs in this file must be used to create, manipulate and
+ * query Frame Descriptors.
+ */
+
+/**
+ * struct dpaa2_fd - Struct describing FDs
+ * @words:         for easier/faster copying the whole FD structure
+ * @addr:          address in the FD
+ * @len:           length in the FD
+ * @bpid:          buffer pool ID
+ * @format_offset: format, offset, and short-length fields
+ * @frc:           frame context
+ * @ctrl:          control bits...including dd, sc, va, err, etc
+ * @flc:           flow context address
+ *
+ * This structure represents the basic Frame Descriptor used in the system.
+ */
+struct dpaa2_fd {
+	union {
+		u32 words[8];
+		struct dpaa2_fd_simple {
+			__le64 addr;
+			__le32 len;
+			__le16 bpid;
+			__le16 format_offset;
+			__le32 frc;
+			__le32 ctrl;
+			__le64 flc;
+		} simple;
+	};
+};
+
+#define FD_SHORT_LEN_FLAG_MASK	0x1
+#define FD_SHORT_LEN_FLAG_SHIFT	14
+#define FD_SHORT_LEN_MASK	0x3FFFF
+#define FD_OFFSET_MASK		0x0FFF
+#define FD_FORMAT_MASK		0x3
+#define FD_FORMAT_SHIFT		12
+#define FD_BPID_MASK		0x3FFF
+#define SG_SHORT_LEN_FLAG_MASK	0x1
+#define SG_SHORT_LEN_FLAG_SHIFT	14
+#define SG_SHORT_LEN_MASK	0x1FFFF
+#define SG_OFFSET_MASK		0x0FFF
+#define SG_FORMAT_MASK		0x3
+#define SG_FORMAT_SHIFT		12
+#define SG_BPID_MASK		0x3FFF
+#define SG_FINAL_FLAG_MASK	0x1
+#define SG_FINAL_FLAG_SHIFT	15
+
+enum dpaa2_fd_format {
+	dpaa2_fd_single = 0,
+	dpaa2_fd_list,
+	dpaa2_fd_sg
+};
+
+/**
+ * dpaa2_fd_get_addr() - get the addr field of frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the address in the frame descriptor.
+ */
+static inline dma_addr_t dpaa2_fd_get_addr(const struct dpaa2_fd *fd)
+{
+	return (dma_addr_t)le64_to_cpu(fd->simple.addr);
+}
+
+/**
+ * dpaa2_fd_set_addr() - Set the addr field of frame descriptor
+ * @fd: the given frame descriptor
+ * @addr: the address needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_addr(struct dpaa2_fd *fd, dma_addr_t addr)
+{
+	fd->simple.addr = cpu_to_le64(addr);
+}
+
+/**
+ * dpaa2_fd_get_frc() - Get the frame context in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the frame context field in the frame descriptor.
+ */
+static inline u32 dpaa2_fd_get_frc(const struct dpaa2_fd *fd)
+{
+	return le32_to_cpu(fd->simple.frc);
+}
+
+/**
+ * dpaa2_fd_set_frc() - Set the frame context in the frame descriptor
+ * @fd: the given frame descriptor
+ * @frc: the frame context needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_frc(struct dpaa2_fd *fd, u32 frc)
+{
+	fd->simple.frc = cpu_to_le32(frc);
+}
+
+/**
+ * dpaa2_fd_get_ctrl() - Get the control bits in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the control bits field in the frame descriptor.
+ */
+static inline u32 dpaa2_fd_get_ctrl(const struct dpaa2_fd *fd)
+{
+	return le32_to_cpu(fd->simple.ctrl);
+}
+
+/**
+ * dpaa2_fd_set_ctrl() - Set the control bits in the frame descriptor
+ * @fd: the given frame descriptor
+ * @ctrl: the control bits to be set in the frame descriptor
+ */
+static inline void dpaa2_fd_set_ctrl(struct dpaa2_fd *fd, u32 ctrl)
+{
+	fd->simple.ctrl = cpu_to_le32(ctrl);
+}
+
+/**
+ * dpaa2_fd_get_flc() - Get the flow context in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the flow context in the frame descriptor.
+ */
+static inline dma_addr_t dpaa2_fd_get_flc(const struct dpaa2_fd *fd)
+{
+	return (dma_addr_t)le64_to_cpu(fd->simple.flc);
+}
+
+/**
+ * dpaa2_fd_set_flc() - Set the flow context field of frame descriptor
+ * @fd: the given frame descriptor
+ * @flc_addr: the flow context needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_flc(struct dpaa2_fd *fd,  dma_addr_t flc_addr)
+{
+	fd->simple.flc = cpu_to_le64(flc_addr);
+}
+
+static inline bool dpaa2_fd_short_len(const struct dpaa2_fd *fd)
+{
+	return !!((le16_to_cpu(fd->simple.format_offset) >>
+		  FD_SHORT_LEN_FLAG_SHIFT) & FD_SHORT_LEN_FLAG_MASK);
+}
+
+/**
+ * dpaa2_fd_get_len() - Get the length in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the length field in the frame descriptor.
+ */
+static inline u32 dpaa2_fd_get_len(const struct dpaa2_fd *fd)
+{
+	if (dpaa2_fd_short_len(fd))
+		return le32_to_cpu(fd->simple.len) & FD_SHORT_LEN_MASK;
+
+	return le32_to_cpu(fd->simple.len);
+}
+
+/**
+ * dpaa2_fd_set_len() - Set the length field of frame descriptor
+ * @fd: the given frame descriptor
+ * @len: the length needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_len(struct dpaa2_fd *fd, u32 len)
+{
+	fd->simple.len = cpu_to_le32(len);
+}
+
+/**
+ * dpaa2_fd_get_offset() - Get the offset field in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the offset.
+ */
+static inline uint16_t dpaa2_fd_get_offset(const struct dpaa2_fd *fd)
+{
+	return le16_to_cpu(fd->simple.format_offset) & FD_OFFSET_MASK;
+}
+
+/**
+ * dpaa2_fd_set_offset() - Set the offset field of frame descriptor
+ * @fd: the given frame descriptor
+ * @offset: the offset needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_offset(struct dpaa2_fd *fd, uint16_t offset)
+{
+	fd->simple.format_offset &= cpu_to_le16(~FD_OFFSET_MASK);
+	fd->simple.format_offset |= cpu_to_le16(offset);
+}
+
+/**
+ * dpaa2_fd_get_format() - Get the format field in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the format.
+ */
+static inline enum dpaa2_fd_format dpaa2_fd_get_format(
+						const struct dpaa2_fd *fd)
+{
+	return (enum dpaa2_fd_format)((le16_to_cpu(fd->simple.format_offset)
+				      >> FD_FORMAT_SHIFT) & FD_FORMAT_MASK);
+}
+
+/**
+ * dpaa2_fd_set_format() - Set the format field of frame descriptor
+ * @fd: the given frame descriptor
+ * @format: the format needs to be set in frame descriptor
+ */
+static inline void dpaa2_fd_set_format(struct dpaa2_fd *fd,
+				       enum dpaa2_fd_format format)
+{
+	fd->simple.format_offset &=
+		cpu_to_le16(~(FD_FORMAT_MASK << FD_FORMAT_SHIFT));
+	fd->simple.format_offset |= cpu_to_le16(format << FD_FORMAT_SHIFT);
+}
+
+/**
+ * dpaa2_fd_get_bpid() - Get the bpid field in the frame descriptor
+ * @fd: the given frame descriptor
+ *
+ * Return the buffer pool id.
+ */
+static inline uint16_t dpaa2_fd_get_bpid(const struct dpaa2_fd *fd)
+{
+	return le16_to_cpu(fd->simple.bpid) & FD_BPID_MASK;
+}
+
+/**
+ * dpaa2_fd_set_bpid() - Set the bpid field of frame descriptor
+ * @fd: the given frame descriptor
+ * @bpid: buffer pool id to be set
+ */
+static inline void dpaa2_fd_set_bpid(struct dpaa2_fd *fd, uint16_t bpid)
+{
+	fd->simple.bpid &= cpu_to_le16(~(FD_BPID_MASK));
+	fd->simple.bpid |= cpu_to_le16(bpid);
+}
+
+/**
+ * struct dpaa2_sg_entry - the scatter-gathering structure
+ * @addr: address of the sg entry
+ * @len: length in this sg entry
+ * @bpid: buffer pool id
+ * @format_offset: format and offset fields
+ */
+struct dpaa2_sg_entry {
+	__le64 addr;
+	__le32 len;
+	__le16 bpid;
+	__le16 format_offset;
+};
+
+enum dpaa2_sg_format {
+	dpaa2_sg_single = 0,
+	dpaa2_sg_frame_data,
+	dpaa2_sg_sgt_ext
+};
+
+/* Accessors for SG entry fields */
+
+/**
+ * dpaa2_sg_get_addr() - Get the address from SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return the address.
+ */
+static inline dma_addr_t dpaa2_sg_get_addr(const struct dpaa2_sg_entry *sg)
+{
+	return le64_to_cpu((dma_addr_t)sg->addr);
+}
+
+/**
+ * dpaa2_sg_set_addr() - Set the address in SG entry
+ * @sg: the given scatter-gathering object
+ * @addr: the address to be set
+ */
+static inline void dpaa2_sg_set_addr(struct dpaa2_sg_entry *sg, dma_addr_t addr)
+{
+	sg->addr = cpu_to_le64(addr);
+}
+
+static inline bool dpaa2_sg_short_len(const struct dpaa2_sg_entry *sg)
+{
+	return !!((le16_to_cpu(sg->format_offset) >> SG_SHORT_LEN_FLAG_SHIFT)
+		& SG_SHORT_LEN_FLAG_MASK);
+}
+
+/**
+ * dpaa2_sg_get_len() - Get the length in SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return the length.
+ */
+static inline u32 dpaa2_sg_get_len(const struct dpaa2_sg_entry *sg)
+{
+	if (dpaa2_sg_short_len(sg))
+		return le32_to_cpu(sg->len) & SG_SHORT_LEN_MASK;
+
+	return le32_to_cpu(sg->len);
+}
+
+/**
+ * dpaa2_sg_set_len() - Set the length in SG entry
+ * @sg: the given scatter-gathering object
+ * @len: the length to be set
+ */
+static inline void dpaa2_sg_set_len(struct dpaa2_sg_entry *sg, u32 len)
+{
+	sg->len = cpu_to_le32(len);
+}
+
+/**
+ * dpaa2_sg_get_offset() - Get the offset in SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return the offset.
+ */
+static inline u16 dpaa2_sg_get_offset(const struct dpaa2_sg_entry *sg)
+{
+	return le16_to_cpu(sg->format_offset) & SG_OFFSET_MASK;
+}
+
+/**
+ * dpaa2_sg_set_offset() - Set the offset in SG entry
+ * @sg: the given scatter-gathering object
+ * @offset: the offset to be set
+ */
+static inline void dpaa2_sg_set_offset(struct dpaa2_sg_entry *sg,
+				       u16 offset)
+{
+	sg->format_offset &= cpu_to_le16(~SG_OFFSET_MASK);
+	sg->format_offset |= cpu_to_le16(offset);
+}
+
+/**
+ * dpaa2_sg_get_format() - Get the SG format in SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return the format.
+ */
+static inline enum dpaa2_sg_format
+	dpaa2_sg_get_format(const struct dpaa2_sg_entry *sg)
+{
+	return (enum dpaa2_sg_format)((le16_to_cpu(sg->format_offset)
+				       >> SG_FORMAT_SHIFT) & SG_FORMAT_MASK);
+}
+
+/**
+ * dpaa2_sg_set_format() - Set the SG format in SG entry
+ * @sg: the given scatter-gathering object
+ * @format: the format to be set
+ */
+static inline void dpaa2_sg_set_format(struct dpaa2_sg_entry *sg,
+				       enum dpaa2_sg_format format)
+{
+	sg->format_offset &= cpu_to_le16(~(SG_FORMAT_MASK << SG_FORMAT_SHIFT));
+	sg->format_offset |= cpu_to_le16(format << SG_FORMAT_SHIFT);
+}
+
+/**
+ * dpaa2_sg_get_bpid() - Get the buffer pool id in SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return the bpid.
+ */
+static inline u16 dpaa2_sg_get_bpid(const struct dpaa2_sg_entry *sg)
+{
+	return le16_to_cpu(sg->bpid) & SG_BPID_MASK;
+}
+
+/**
+ * dpaa2_sg_set_bpid() - Set the buffer pool id in SG entry
+ * @sg: the given scatter-gathering object
+ * @bpid: the bpid to be set
+ */
+static inline void dpaa2_sg_set_bpid(struct dpaa2_sg_entry *sg, u16 bpid)
+{
+	sg->bpid &= cpu_to_le16(~(SG_BPID_MASK));
+	sg->bpid |= cpu_to_le16(bpid);
+}
+
+/**
+ * dpaa2_sg_is_final() - Check final bit in SG entry
+ * @sg: the given scatter-gathering object
+ *
+ * Return bool.
+ */
+static inline bool dpaa2_sg_is_final(const struct dpaa2_sg_entry *sg)
+{
+	return !!(le16_to_cpu(sg->format_offset) >> SG_FINAL_FLAG_SHIFT);
+}
+
+/**
+ * dpaa2_sg_set_final() - Set the final bit in SG entry
+ * @sg: the given scatter-gathering object
+ * @final: the final boolean to be set
+ */
+static inline void dpaa2_sg_set_final(struct dpaa2_sg_entry *sg, bool final)
+{
+	sg->format_offset &= cpu_to_le16(~(SG_FINAL_FLAG_MASK
+					 << SG_FINAL_FLAG_SHIFT));
+	sg->format_offset |= cpu_to_le16(final << SG_FINAL_FLAG_SHIFT);
+}
+
+#endif /* __FSL_DPAA2_FD_H */
diff --git a/drivers/staging/fsl-mc/include/dpaa2-global.h b/drivers/staging/fsl-mc/include/dpaa2-global.h
new file mode 100644
index 000000000000..0326447fde4e
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpaa2-global.h
@@ -0,0 +1,202 @@
+/*
+ * Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright 2016 NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#ifndef __FSL_DPAA2_GLOBAL_H
+#define __FSL_DPAA2_GLOBAL_H
+
+#include <linux/types.h>
+#include <linux/cpumask.h>
+#include "dpaa2-fd.h"
+
+struct dpaa2_dq {
+	union {
+		struct common {
+			u8 verb;
+			u8 reserved[63];
+		} common;
+		struct dq {
+			u8 verb;
+			u8 stat;
+			__le16 seqnum;
+			__le16 oprid;
+			u8 reserved;
+			u8 tok;
+			__le32 fqid;
+			u32 reserved2;
+			__le32 fq_byte_cnt;
+			__le32 fq_frm_cnt;
+			__le64 fqd_ctx;
+			u8 fd[32];
+		} dq;
+		struct scn {
+			u8 verb;
+			u8 stat;
+			u8 state;
+			u8 reserved;
+			__le32 rid_tok;
+			__le64 ctx;
+		} scn;
+	};
+};
+
+/* Parsing frame dequeue results */
+/* FQ empty */
+#define DPAA2_DQ_STAT_FQEMPTY       0x80
+/* FQ held active */
+#define DPAA2_DQ_STAT_HELDACTIVE    0x40
+/* FQ force eligible */
+#define DPAA2_DQ_STAT_FORCEELIGIBLE 0x20
+/* valid frame */
+#define DPAA2_DQ_STAT_VALIDFRAME    0x10
+/* FQ ODP enable */
+#define DPAA2_DQ_STAT_ODPVALID      0x04
+/* volatile dequeue */
+#define DPAA2_DQ_STAT_VOLATILE      0x02
+/* volatile dequeue command is expired */
+#define DPAA2_DQ_STAT_EXPIRED       0x01
+
+#define DQ_FQID_MASK		0x00FFFFFF
+#define DQ_FRAME_COUNT_MASK	0x00FFFFFF
+
+/**
+ * dpaa2_dq_flags() - Get the stat field of dequeue response
+ * @dq: the dequeue result.
+ */
+static inline u32 dpaa2_dq_flags(const struct dpaa2_dq *dq)
+{
+	return dq->dq.stat;
+}
+
+/**
+ * dpaa2_dq_is_pull() - Check whether the dq response is from a pull
+ *                      command.
+ * @dq: the dequeue result
+ *
+ * Return 1 for volatile(pull) dequeue, 0 for static dequeue.
+ */
+static inline int dpaa2_dq_is_pull(const struct dpaa2_dq *dq)
+{
+	return (int)(dpaa2_dq_flags(dq) & DPAA2_DQ_STAT_VOLATILE);
+}
+
+/**
+ * dpaa2_dq_is_pull_complete() - Check whether the pull command is completed.
+ * @dq: the dequeue result
+ *
+ * Return boolean.
+ */
+static inline bool dpaa2_dq_is_pull_complete(const struct dpaa2_dq *dq)
+{
+	return !!(dpaa2_dq_flags(dq) & DPAA2_DQ_STAT_EXPIRED);
+}
+
+/**
+ * dpaa2_dq_seqnum() - Get the seqnum field in dequeue response
+ * @dq: the dequeue result
+ *
+ * seqnum is valid only if VALIDFRAME flag is TRUE
+ *
+ * Return seqnum.
+ */
+static inline u16 dpaa2_dq_seqnum(const struct dpaa2_dq *dq)
+{
+	return le16_to_cpu(dq->dq.seqnum);
+}
+
+/**
+ * dpaa2_dq_odpid() - Get the odpid field in dequeue response
+ * @dq: the dequeue result
+ *
+ * odpid is valid only if ODPVALID flag is TRUE.
+ *
+ * Return odpid.
+ */
+static inline u16 dpaa2_dq_odpid(const struct dpaa2_dq *dq)
+{
+	return le16_to_cpu(dq->dq.oprid);
+}
+
+/**
+ * dpaa2_dq_fqid() - Get the fqid in dequeue response
+ * @dq: the dequeue result
+ *
+ * Return fqid.
+ */
+static inline u32 dpaa2_dq_fqid(const struct dpaa2_dq *dq)
+{
+	return le32_to_cpu(dq->dq.fqid) & DQ_FQID_MASK;
+}
+
+/**
+ * dpaa2_dq_byte_count() - Get the byte count in dequeue response
+ * @dq: the dequeue result
+ *
+ * Return the byte count remaining in the FQ.
+ */
+static inline u32 dpaa2_dq_byte_count(const struct dpaa2_dq *dq)
+{
+	return le32_to_cpu(dq->dq.fq_byte_cnt);
+}
+
+/**
+ * dpaa2_dq_frame_count() - Get the frame count in dequeue response
+ * @dq: the dequeue result
+ *
+ * Return the frame count remaining in the FQ.
+ */
+static inline u32 dpaa2_dq_frame_count(const struct dpaa2_dq *dq)
+{
+	return le32_to_cpu(dq->dq.fq_frm_cnt) & DQ_FRAME_COUNT_MASK;
+}
+
+/**
+ * dpaa2_dq_fd_ctx() - Get the frame queue context in dequeue response
+ * @dq: the dequeue result
+ *
+ * Return the frame queue context.
+ */
+static inline u64 dpaa2_dq_fqd_ctx(const struct dpaa2_dq *dq)
+{
+	return le64_to_cpu(dq->dq.fqd_ctx);
+}
+
+/**
+ * dpaa2_dq_fd() - Get the frame descriptor in dequeue response
+ * @dq: the dequeue result
+ *
+ * Return the frame descriptor.
+ */
+static inline const struct dpaa2_fd *dpaa2_dq_fd(const struct dpaa2_dq *dq)
+{
+	return (const struct dpaa2_fd *)&dq->dq.fd[0];
+}
+
+#endif /* __FSL_DPAA2_GLOBAL_H */
diff --git a/drivers/staging/fsl-mc/include/dpaa2-io.h b/drivers/staging/fsl-mc/include/dpaa2-io.h
new file mode 100644
index 000000000000..002829cecd75
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpaa2-io.h
@@ -0,0 +1,139 @@
+/*
+ * Copyright 2014-2016 Freescale Semiconductor Inc.
+ * Copyright NXP
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * 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.
+ *     * Neither the name of Freescale Semiconductor nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+#ifndef __FSL_DPAA2_IO_H
+#define __FSL_DPAA2_IO_H
+
+#include <linux/types.h>
+#include <linux/cpumask.h>
+
+#include "dpaa2-fd.h"
+#include "dpaa2-global.h"
+
+struct dpaa2_io;
+struct dpaa2_io_store;
+struct device;
+
+/**
+ * DOC: DPIO Service
+ *
+ * The DPIO service provides APIs for users to interact with the datapath
+ * by enqueueing and dequeing frame descriptors.
+ *
+ * The following set of APIs can be used to enqueue and dequeue frames
+ * as well as producing notification callbacks when data is available
+ * for dequeue.
+ */
+
+/**
+ * struct dpaa2_io_desc - The DPIO descriptor
+ * @receives_notifications: Use notificaton mode. Non-zero if the DPIO
+ *                  has a channel.
+ * @has_8prio:      Set to non-zero for channel with 8 priority WQs.  Ignored
+ *                  unless receives_notification is TRUE.
+ * @cpu:            The cpu index that at least interrupt handlers will
+ *                  execute on.
+ * @stash_affinity: The stash affinity for this portal favour 'cpu'
+ * @regs_cena:      The cache enabled regs.
+ * @regs_cinh:      The cache inhibited regs
+ * @dpio_id:        The dpio index
+ * @qman_version:   The qman version
+ *
+ * Describes the attributes and features of the DPIO object.
+ */
+struct dpaa2_io_desc {
+	int receives_notifications;
+	int has_8prio;
+	int cpu;
+	void *regs_cena;
+	void *regs_cinh;
+	int dpio_id;
+	u32 qman_version;
+};
+
+struct dpaa2_io *dpaa2_io_create(const struct dpaa2_io_desc *desc);
+
+void dpaa2_io_down(struct dpaa2_io *d);
+
+irqreturn_t dpaa2_io_irq(struct dpaa2_io *obj);
+
+/**
+ * struct dpaa2_io_notification_ctx - The DPIO notification context structure
+ * @cb:           The callback to be invoked when the notification arrives
+ * @is_cdan:      Zero for FQDAN, non-zero for CDAN
+ * @id:           FQID or channel ID, needed for rearm
+ * @desired_cpu:  The cpu on which the notifications will show up. -1 means
+ *                any CPU.
+ * @dpio_id:      The dpio index
+ * @qman64:       The 64-bit context value shows up in the FQDAN/CDAN.
+ * @node:         The list node
+ * @dpio_private: The dpio object internal to dpio_service
+ *
+ * Used when a FQDAN/CDAN registration is made by drivers.
+ */
+struct dpaa2_io_notification_ctx {
+	void (*cb)(struct dpaa2_io_notification_ctx *);
+	int is_cdan;
+	u32 id;
+	int desired_cpu;
+	int dpio_id;
+	u64 qman64;
+	struct list_head node;
+	void *dpio_private;
+};
+
+int dpaa2_io_service_register(struct dpaa2_io *service,
+			      struct dpaa2_io_notification_ctx *ctx);
+void dpaa2_io_service_deregister(struct dpaa2_io *service,
+				 struct dpaa2_io_notification_ctx *ctx);
+int dpaa2_io_service_rearm(struct dpaa2_io *service,
+			   struct dpaa2_io_notification_ctx *ctx);
+
+int dpaa2_io_service_pull_fq(struct dpaa2_io *d, u32 fqid,
+			     struct dpaa2_io_store *s);
+int dpaa2_io_service_pull_channel(struct dpaa2_io *d, u32 channelid,
+				  struct dpaa2_io_store *s);
+
+int dpaa2_io_service_enqueue_fq(struct dpaa2_io *d, u32 fqid,
+				const struct dpaa2_fd *fd);
+int dpaa2_io_service_enqueue_qd(struct dpaa2_io *d, u32 qdid, u8 prio,
+				u16 qdbin, const struct dpaa2_fd *fd);
+int dpaa2_io_service_release(struct dpaa2_io *d, u32 bpid,
+			     const u64 *buffers, unsigned int num_buffers);
+int dpaa2_io_service_acquire(struct dpaa2_io *d, u32 bpid,
+			     u64 *buffers, unsigned int num_buffers);
+
+struct dpaa2_io_store *dpaa2_io_store_create(unsigned int max_frames,
+					     struct device *dev);
+void dpaa2_io_store_destroy(struct dpaa2_io_store *s);
+struct dpaa2_dq *dpaa2_io_store_next(struct dpaa2_io_store *s, int *is_last);
+
+#endif /* __FSL_DPAA2_IO_H */
diff --git a/drivers/staging/fsl-mc/include/dpcon.h b/drivers/staging/fsl-mc/include/dpcon.h
new file mode 100644
index 000000000000..efa23906b364
--- /dev/null
+++ b/drivers/staging/fsl-mc/include/dpcon.h
@@ -0,0 +1,115 @@
+/* Copyright 2013-2016 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * 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.
+ * * Neither the name of the above-listed copyright holders nor the
+ * names of any contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ *
+ * ALTERNATIVELY, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * 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 HOLDERS 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.
+ */
+#ifndef __FSL_DPCON_H
+#define __FSL_DPCON_H
+
+/* Data Path Concentrator API
+ * Contains initialization APIs and runtime control APIs for DPCON
+ */
+
+struct fsl_mc_io;
+
+/** General DPCON macros */
+
+/**
+ * Use it to disable notifications; see dpcon_set_notification()
+ */
+#define DPCON_INVALID_DPIO_ID		(int)(-1)
+
+int dpcon_open(struct fsl_mc_io *mc_io,
+	       u32 cmd_flags,
+	       int dpcon_id,
+	       u16 *token);
+
+int dpcon_close(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token);
+
+int dpcon_enable(struct fsl_mc_io *mc_io,
+		 u32 cmd_flags,
+		 u16 token);
+
+int dpcon_disable(struct fsl_mc_io *mc_io,
+		  u32 cmd_flags,
+		  u16 token);
+
+int dpcon_is_enabled(struct fsl_mc_io *mc_io,
+		     u32 cmd_flags,
+		     u16 token,
+		     int *en);
+
+int dpcon_reset(struct fsl_mc_io *mc_io,
+		u32 cmd_flags,
+		u16 token);
+
+/**
+ * struct dpcon_attr - Structure representing DPCON attributes
+ * @id: DPCON object ID
+ * @qbman_ch_id: Channel ID to be used by dequeue operation
+ * @num_priorities: Number of priorities for the DPCON channel (1-8)
+ */
+struct dpcon_attr {
+	int id;
+	u16 qbman_ch_id;
+	u8 num_priorities;
+};
+
+int dpcon_get_attributes(struct fsl_mc_io *mc_io,
+			 u32 cmd_flags,
+			 u16 token,
+			 struct dpcon_attr *attr);
+
+/**
+ * struct dpcon_notification_cfg - Structure representing notification params
+ * @dpio_id:	DPIO object ID; must be configured with a notification channel;
+ *	to disable notifications set it to 'DPCON_INVALID_DPIO_ID';
+ * @priority:	Priority selection within the DPIO channel; valid values
+ *		are 0-7, depending on the number of priorities in that channel
+ * @user_ctx:	User context value provided with each CDAN message
+ */
+struct dpcon_notification_cfg {
+	int dpio_id;
+	u8 priority;
+	u64 user_ctx;
+};
+
+int dpcon_set_notification(struct fsl_mc_io *mc_io,
+			   u32 cmd_flags,
+			   u16 token,
+			   struct dpcon_notification_cfg *cfg);
+
+int dpcon_get_api_version(struct fsl_mc_io *mc_io,
+			  u32 cmd_flags,
+			  u16 *major_ver,
+			  u16 *minor_ver);
+
+#endif /* __FSL_DPCON_H */
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index a3e046c3f65c..cf809987f79f 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -178,10 +178,10 @@ static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type)
 	return 0;
 }
 
-static int icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len)
+static __sum16 icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len)
 {
 	unsigned short *w = ptr;
-	int sum = 0;
+	__wsum sum = 0;
 	int i;
 
 	union {
@@ -203,19 +203,16 @@ static int icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len)
 
 	w = (u16 *)&pseudo_header;
 	for (i = 0; i < ARRAY_SIZE(pseudo_header.pa); i++)
-		sum += pseudo_header.pa[i];
+		sum = csum_add(sum, csum_unfold(
+					(__force __sum16)pseudo_header.pa[i]));
 
 	w = ptr;
 	while (len > 1) {
-		sum += *w++;
+		sum = csum_add(sum, csum_unfold((__force __sum16)*w++));
 		len -= 2;
 	}
 
-	sum = (sum >> 16) + (sum & 0xFFFF);
-	sum += (sum >> 16);
-	sum = ~sum & 0xffff;
-
-	return sum;
+	return csum_fold(sum);
 }
 
 static int gdm_lte_emulate_ndp(struct sk_buff *skb_in, u32 nic_type)
@@ -353,7 +350,7 @@ static s32 gdm_lte_tx_nic_type(struct net_device *dev, struct sk_buff *skb)
 	struct ipv6hdr *ipv6;
 	int mac_proto;
 	void *network_data;
-	u32 nic_type = 0;
+	u32 nic_type;
 
 	/* NIC TYPE is based on the nic_id of this net_device */
 	nic_type = 0x00000010 | nic->nic_id;
diff --git a/drivers/staging/gdm724x/gdm_lte.h b/drivers/staging/gdm724x/gdm_lte.h
index 7ddeabc0e50a..3ecaff1a40cb 100644
--- a/drivers/staging/gdm724x/gdm_lte.h
+++ b/drivers/staging/gdm724x/gdm_lte.h
@@ -49,7 +49,7 @@ struct phy_dev {
 	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 dftEpsId, unsigned int epsId,
+				 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,
diff --git a/drivers/staging/gdm724x/gdm_mux.c b/drivers/staging/gdm724x/gdm_mux.c
index 400969170d1c..996b1f538aae 100644
--- a/drivers/staging/gdm724x/gdm_mux.c
+++ b/drivers/staging/gdm724x/gdm_mux.c
@@ -62,7 +62,7 @@ static int packet_type_to_index(u16 packetType)
 
 static struct mux_tx *alloc_mux_tx(int len)
 {
-	struct mux_tx *t = NULL;
+	struct mux_tx *t;
 
 	t = kzalloc(sizeof(*t), GFP_ATOMIC);
 	if (!t)
@@ -91,7 +91,7 @@ static void free_mux_tx(struct mux_tx *t)
 
 static struct mux_rx *alloc_mux_rx(void)
 {
-	struct mux_rx *r = NULL;
+	struct mux_rx *r;
 
 	r = kzalloc(sizeof(*r), GFP_KERNEL);
 	if (!r)
@@ -664,9 +664,8 @@ static int __init gdm_usb_mux_init(void)
 
 static void __exit gdm_usb_mux_exit(void)
 {
-	unregister_lte_tty_driver();
-
 	usb_deregister(&gdm_mux_driver);
+	unregister_lte_tty_driver();
 }
 
 module_init(gdm_usb_mux_init);
diff --git a/drivers/staging/gdm724x/gdm_tty.c b/drivers/staging/gdm724x/gdm_tty.c
index ae396638f897..fc7682c18f20 100644
--- a/drivers/staging/gdm724x/gdm_tty.c
+++ b/drivers/staging/gdm724x/gdm_tty.c
@@ -190,8 +190,7 @@ static int gdm_tty_write(struct tty_struct *tty, const unsigned char *buf,
 		return 0;
 
 	while (1) {
-		sending_len = remain > MUX_TX_MAX_SIZE ? MUX_TX_MAX_SIZE :
-							 remain;
+		sending_len = min(MUX_TX_MAX_SIZE, remain);
 		gdm_tty_send(gdm,
 			     (void *)(buf + sent_len),
 			     sending_len,
diff --git a/drivers/staging/goldfish/goldfish_nand.c b/drivers/staging/goldfish/goldfish_nand.c
index 76d60eed1490..8f92ff4ba4b8 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -114,8 +114,8 @@ static int goldfish_nand_erase(struct mtd_info *mtd, struct erase_info *instr)
 	len = len / mtd->writesize * (mtd->writesize + mtd->oobsize);
 
 	if (goldfish_nand_cmd(mtd, NAND_CMD_ERASE, ofs, len, NULL) != len) {
-		pr_err("goldfish_nand_erase: erase failed, start %llx, len %x, dev_size %llx, erase_size %x\n",
-		       ofs, len, mtd->size, mtd->erasesize);
+		pr_err("%s: erase failed, start %llx, len %x, dev_size %llx, erase_size %x\n",
+		       __func__, ofs, len, mtd->size, mtd->erasesize);
 		return -EIO;
 	}
 
@@ -125,8 +125,8 @@ static int goldfish_nand_erase(struct mtd_info *mtd, struct erase_info *instr)
 	return 0;
 
 invalid_arg:
-	pr_err("goldfish_nand_erase: invalid erase, start %llx, len %x, dev_size %llx, erase_size %x\n",
-	       ofs, len, mtd->size, mtd->erasesize);
+	pr_err("%s: invalid erase, start %llx, len %x, dev_size %llx, erase_size %x\n",
+	       __func__, ofs, len, mtd->size, mtd->erasesize);
 	return -EINVAL;
 }
 
@@ -254,8 +254,8 @@ static int goldfish_nand_block_isbad(struct mtd_info *mtd, loff_t ofs)
 	return goldfish_nand_cmd(mtd, NAND_CMD_BLOCK_BAD_GET, ofs, 0, NULL);
 
 invalid_arg:
-	pr_err("goldfish_nand_block_isbad: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
-	       ofs, mtd->size, mtd->writesize);
+	pr_err("%s: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
+	       __func__, ofs, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
@@ -277,8 +277,8 @@ static int goldfish_nand_block_markbad(struct mtd_info *mtd, loff_t ofs)
 	return 0;
 
 invalid_arg:
-	pr_err("goldfish_nand_block_markbad: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
-	       ofs, mtd->size, mtd->writesize);
+	pr_err("%s: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
+	       __func__, ofs, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
diff --git a/drivers/staging/greybus/Documentation/firmware/authenticate.c b/drivers/staging/greybus/Documentation/firmware/authenticate.c
index ab0688ad1e37..b836f0a20c36 100644
--- a/drivers/staging/greybus/Documentation/firmware/authenticate.c
+++ b/drivers/staging/greybus/Documentation/firmware/authenticate.c
@@ -103,7 +103,7 @@ int main(int argc, char *argv[])
 		goto close_fd;
 	}
 
-	printf("UID received: 0x%llx\n", *(long long unsigned int *)(uid.uid));
+	printf("UID received: 0x%llx\n", *(unsigned long long int *)(uid.uid));
 
 	/* Get certificate */
 	printf("Get IMS certificate\n");
diff --git a/drivers/staging/greybus/Documentation/firmware/firmware.c b/drivers/staging/greybus/Documentation/firmware/firmware.c
index ff9382401030..c73dee9d13c1 100644
--- a/drivers/staging/greybus/Documentation/firmware/firmware.c
+++ b/drivers/staging/greybus/Documentation/firmware/firmware.c
@@ -52,6 +52,7 @@
  */
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
 #include <sys/ioctl.h>
@@ -64,12 +65,12 @@
 #define FW_TAG_INT_DEFAULT	"s3f"
 #define FW_TAG_BCND_DEFAULT	"bf_01"
 #define FW_UPDATE_TYPE_DEFAULT	0
-#define FW_TIMEOUT_DEFAULT	10000;
+#define FW_TIMEOUT_DEFAULT	10000
 
 static const char *firmware_tag;
 static const char *fwdev = FW_DEV_DEFAULT;
-static int fw_update_type = FW_UPDATE_TYPE_DEFAULT;
-static int fw_timeout = FW_TIMEOUT_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;
@@ -204,6 +205,7 @@ retry_fw_update:
 int main(int argc, char *argv[])
 {
 	int fd, ret;
+	char *endptr;
 
 	if (argc > 1 &&
 	    (!strcmp(argv[1], "-h") || !strcmp(argv[1], "--help"))) {
@@ -215,20 +217,19 @@ int main(int argc, char *argv[])
 		fwdev = argv[1];
 
 	if (argc > 2)
-		sscanf(argv[2], "%u", &fw_update_type);
+		fw_update_type = strtoul(argv[2], &endptr, 10);
 
-	if (argc > 3) {
+	if (argc > 3)
 		firmware_tag = argv[3];
-	} else if (!fw_update_type) {
+	else if (!fw_update_type)
 		firmware_tag = FW_TAG_INT_DEFAULT;
-	} else {
+	else
 		firmware_tag = FW_TAG_BCND_DEFAULT;
-	}
 
 	if (argc > 4)
-		sscanf(argv[4], "%u", &fw_timeout);
+		fw_timeout = strtoul(argv[4], &endptr, 10);
 
-	printf("Trying Firmware update: fwdev: %s, type: %s, tag: %s, timeout: %d\n",
+	printf("Trying Firmware update: fwdev: %s, type: %s, tag: %s, timeout: %u\n",
 		fwdev, fw_update_type == 0 ? "interface" : "backend",
 		firmware_tag, fw_timeout);
 
diff --git a/drivers/staging/greybus/connection.c b/drivers/staging/greybus/connection.c
index 1bf0ee403106..2cf64640e8ec 100644
--- a/drivers/staging/greybus/connection.c
+++ b/drivers/staging/greybus/connection.c
@@ -366,6 +366,9 @@ static int gb_connection_hd_cport_quiesce(struct gb_connection *connection)
 	if (connection->mode_switch)
 		peer_space += sizeof(struct gb_operation_msg_hdr);
 
+	if (!hd->driver->cport_quiesce)
+		return 0;
+
 	ret = hd->driver->cport_quiesce(hd, connection->hd_cport_id,
 					peer_space,
 					GB_CONNECTION_CPORT_QUIESCE_TIMEOUT);
diff --git a/drivers/staging/greybus/gbphy.c b/drivers/staging/greybus/gbphy.c
index 64a1eb93ec96..603de6f21fd1 100644
--- a/drivers/staging/greybus/gbphy.c
+++ b/drivers/staging/greybus/gbphy.c
@@ -11,7 +11,6 @@
 
 #include <linux/types.h>
 #include <linux/module.h>
-#include <linux/moduleparam.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/device.h>
diff --git a/drivers/staging/greybus/light.c b/drivers/staging/greybus/light.c
index 8dffd8a7e762..16813628eda1 100644
--- a/drivers/staging/greybus/light.c
+++ b/drivers/staging/greybus/light.c
@@ -12,7 +12,6 @@
 #include <linux/led-class-flash.h>
 #include <linux/module.h>
 #include <linux/slab.h>
-#include <linux/version.h>
 #include <media/v4l2-flash-led-class.h>
 
 #include "greybus.h"
diff --git a/drivers/staging/greybus/loopback.c b/drivers/staging/greybus/loopback.c
index aaf29a5fac83..08e255884206 100644
--- a/drivers/staging/greybus/loopback.c
+++ b/drivers/staging/greybus/loopback.c
@@ -365,11 +365,8 @@ static void gb_loopback_calculate_stats(struct gb_loopback *gb, bool error);
 
 static u32 gb_loopback_nsec_to_usec_latency(u64 elapsed_nsecs)
 {
-	u32 lat;
-
 	do_div(elapsed_nsecs, NSEC_PER_USEC);
-	lat = elapsed_nsecs;
-	return lat;
+	return elapsed_nsecs;
 }
 
 static u64 __gb_loopback_calc_latency(u64 t1, u64 t2)
diff --git a/drivers/staging/greybus/tools/loopback_test.c b/drivers/staging/greybus/tools/loopback_test.c
index 18d7a3d1f3c7..32a43693181c 100644
--- a/drivers/staging/greybus/tools/loopback_test.c
+++ b/drivers/staging/greybus/tools/loopback_test.c
@@ -476,7 +476,7 @@ int format_output(struct loopback_test *t,
 			r->gbphy_firmware_latency_jitter);
 
 	} else {
-		len += snprintf(&buf[len], buf_len- len, ",%s,%s,%u,%u,%u",
+		len += snprintf(&buf[len], buf_len - len, ",%s,%s,%u,%u,%u",
 			t->test_name, dev_name, t->size, t->iteration_max,
 			r->error);
 
@@ -636,7 +636,7 @@ int find_loopback_devices(struct loopback_test *t)
 	ret = 0;
 done:
 	for (i = 0; i < n; i++)
-		free(namelist[n]);
+		free(namelist[i]);
 	free(namelist);
 baddir:
 	return ret;
@@ -674,7 +674,7 @@ static int open_poll_files(struct loopback_test *t)
 
 err:
 	for (i = 0; i < fds_idx; i++)
-		close(t->fds[fds_idx].fd);
+		close(t->fds[i].fd);
 
 	return -1;
 }
diff --git a/drivers/staging/greybus/uart.c b/drivers/staging/greybus/uart.c
index 43255e2e9276..c6d01b800d3c 100644
--- a/drivers/staging/greybus/uart.c
+++ b/drivers/staging/greybus/uart.c
@@ -23,7 +23,6 @@
 #include <linux/serial.h>
 #include <linux/tty_driver.h>
 #include <linux/tty_flip.h>
-#include <linux/serial.h>
 #include <linux/idr.h>
 #include <linux/fs.h>
 #include <linux/kdev_t.h>
@@ -34,7 +33,7 @@
 #include "greybus.h"
 #include "gbphy.h"
 
-#define GB_NUM_MINORS	16	/* 16 is is more than enough */
+#define GB_NUM_MINORS	16	/* 16 is more than enough */
 #define GB_NAME		"ttyGB"
 
 #define GB_UART_WRITE_FIFO_SIZE		PAGE_SIZE
diff --git a/drivers/staging/iio/accel/Makefile b/drivers/staging/iio/accel/Makefile
index febb137b60c4..5d8ad21a0dae 100644
--- a/drivers/staging/iio/accel/Makefile
+++ b/drivers/staging/iio/accel/Makefile
@@ -2,14 +2,7 @@
 # Makefile for industrial I/O accelerometer drivers
 #
 
-adis16201-y             := adis16201_core.o
 obj-$(CONFIG_ADIS16201) += adis16201.o
-
-adis16203-y             := adis16203_core.o
 obj-$(CONFIG_ADIS16203) += adis16203.o
-
-adis16209-y             := adis16209_core.o
 obj-$(CONFIG_ADIS16209) += adis16209.o
-
-adis16240-y             := adis16240_core.o
 obj-$(CONFIG_ADIS16240) += adis16240.o
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201.c
index 7963d4a83f84..fbc240663621 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201.c
@@ -20,7 +20,145 @@
 #include <linux/iio/buffer.h>
 #include <linux/iio/imu/adis.h>
 
-#include "adis16201.h"
+#define ADIS16201_STARTUP_DELAY	220 /* ms */
+
+/* Flash memory write count */
+#define ADIS16201_FLASH_CNT      0x00
+
+/* Output, power supply */
+#define ADIS16201_SUPPLY_OUT     0x02
+
+/* Output, x-axis accelerometer */
+#define ADIS16201_XACCL_OUT      0x04
+
+/* Output, y-axis accelerometer */
+#define ADIS16201_YACCL_OUT      0x06
+
+/* Output, auxiliary ADC input */
+#define ADIS16201_AUX_ADC        0x08
+
+/* Output, temperature */
+#define ADIS16201_TEMP_OUT       0x0A
+
+/* Output, x-axis inclination */
+#define ADIS16201_XINCL_OUT      0x0C
+
+/* Output, y-axis inclination */
+#define ADIS16201_YINCL_OUT      0x0E
+
+/* Calibration, x-axis acceleration offset */
+#define ADIS16201_XACCL_OFFS     0x10
+
+/* Calibration, y-axis acceleration offset */
+#define ADIS16201_YACCL_OFFS     0x12
+
+/* x-axis acceleration scale factor */
+#define ADIS16201_XACCL_SCALE    0x14
+
+/* y-axis acceleration scale factor */
+#define ADIS16201_YACCL_SCALE    0x16
+
+/* Calibration, x-axis inclination offset */
+#define ADIS16201_XINCL_OFFS     0x18
+
+/* Calibration, y-axis inclination offset */
+#define ADIS16201_YINCL_OFFS     0x1A
+
+/* x-axis inclination scale factor */
+#define ADIS16201_XINCL_SCALE    0x1C
+
+/* y-axis inclination scale factor */
+#define ADIS16201_YINCL_SCALE    0x1E
+
+/* Alarm 1 amplitude threshold */
+#define ADIS16201_ALM_MAG1       0x20
+
+/* Alarm 2 amplitude threshold */
+#define ADIS16201_ALM_MAG2       0x22
+
+/* Alarm 1, sample period */
+#define ADIS16201_ALM_SMPL1      0x24
+
+/* Alarm 2, sample period */
+#define ADIS16201_ALM_SMPL2      0x26
+
+/* Alarm control */
+#define ADIS16201_ALM_CTRL       0x28
+
+/* Auxiliary DAC data */
+#define ADIS16201_AUX_DAC        0x30
+
+/* General-purpose digital input/output control */
+#define ADIS16201_GPIO_CTRL      0x32
+
+/* Miscellaneous control */
+#define ADIS16201_MSC_CTRL       0x34
+
+/* Internal sample period (rate) control */
+#define ADIS16201_SMPL_PRD       0x36
+
+/* Operation, filter configuration */
+#define ADIS16201_AVG_CNT        0x38
+
+/* Operation, sleep mode control */
+#define ADIS16201_SLP_CNT        0x3A
+
+/* Diagnostics, system status register */
+#define ADIS16201_DIAG_STAT      0x3C
+
+/* Operation, system command register */
+#define ADIS16201_GLOB_CMD       0x3E
+
+/* MSC_CTRL */
+
+/* Self-test enable */
+#define ADIS16201_MSC_CTRL_SELF_TEST_EN	        BIT(8)
+
+/* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16201_MSC_CTRL_DATA_RDY_EN	        BIT(2)
+
+/* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16201_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
+
+/* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
+#define ADIS16201_MSC_CTRL_DATA_RDY_DIO1	BIT(0)
+
+/* DIAG_STAT */
+
+/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16201_DIAG_STAT_ALARM2        BIT(9)
+
+/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16201_DIAG_STAT_ALARM1        BIT(8)
+
+/* SPI communications failure */
+#define ADIS16201_DIAG_STAT_SPI_FAIL_BIT   3
+
+/* Flash update failure */
+#define ADIS16201_DIAG_STAT_FLASH_UPT_BIT  2
+
+/* Power supply above 3.625 V */
+#define ADIS16201_DIAG_STAT_POWER_HIGH_BIT 1
+
+/* Power supply below 3.15 V */
+#define ADIS16201_DIAG_STAT_POWER_LOW_BIT  0
+
+/* GLOB_CMD */
+
+#define ADIS16201_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16201_GLOB_CMD_FACTORY_CAL	BIT(1)
+
+#define ADIS16201_ERROR_ACTIVE          BIT(14)
+
+enum adis16201_scan {
+	ADIS16201_SCAN_ACC_X,
+	ADIS16201_SCAN_ACC_Y,
+	ADIS16201_SCAN_INCLI_X,
+	ADIS16201_SCAN_INCLI_Y,
+	ADIS16201_SCAN_SUPPLY,
+	ADIS16201_SCAN_AUX_ADC,
+	ADIS16201_SCAN_TEMP,
+};
 
 static const u8 adis16201_addresses[] = {
 	[ADIS16201_SCAN_ACC_X] = ADIS16201_XACCL_OFFS,
@@ -85,17 +223,13 @@ static int adis16201_read_raw(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
-		mutex_lock(&indio_dev->mlock);
 		addr = adis16201_addresses[chan->scan_index];
 		ret = adis_read_reg_16(st, addr, &val16);
-		if (ret) {
-			mutex_unlock(&indio_dev->mlock);
+		if (ret)
 			return ret;
-		}
 		val16 &= (1 << bits) - 1;
 		val16 = (s16)(val16 << (16 - bits)) >> (16 - bits);
 		*val = val16;
-		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 	}
 	return -EINVAL;
@@ -147,8 +281,8 @@ static const struct iio_chan_spec adis16201_channels[] = {
 };
 
 static const struct iio_info adis16201_info = {
-	.read_raw = &adis16201_read_raw,
-	.write_raw = &adis16201_write_raw,
+	.read_raw = adis16201_read_raw,
+	.write_raw = adis16201_write_raw,
 	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
diff --git a/drivers/staging/iio/accel/adis16201.h b/drivers/staging/iio/accel/adis16201.h
deleted file mode 100644
index 64844adcaacd..000000000000
--- a/drivers/staging/iio/accel/adis16201.h
+++ /dev/null
@@ -1,144 +0,0 @@
-#ifndef SPI_ADIS16201_H_
-#define SPI_ADIS16201_H_
-
-#define ADIS16201_STARTUP_DELAY	220 /* ms */
-
-/* Flash memory write count */
-#define ADIS16201_FLASH_CNT      0x00
-
-/* Output, power supply */
-#define ADIS16201_SUPPLY_OUT     0x02
-
-/* Output, x-axis accelerometer */
-#define ADIS16201_XACCL_OUT      0x04
-
-/* Output, y-axis accelerometer */
-#define ADIS16201_YACCL_OUT      0x06
-
-/* Output, auxiliary ADC input */
-#define ADIS16201_AUX_ADC        0x08
-
-/* Output, temperature */
-#define ADIS16201_TEMP_OUT       0x0A
-
-/* Output, x-axis inclination */
-#define ADIS16201_XINCL_OUT      0x0C
-
-/* Output, y-axis inclination */
-#define ADIS16201_YINCL_OUT      0x0E
-
-/* Calibration, x-axis acceleration offset */
-#define ADIS16201_XACCL_OFFS     0x10
-
-/* Calibration, y-axis acceleration offset */
-#define ADIS16201_YACCL_OFFS     0x12
-
-/* x-axis acceleration scale factor */
-#define ADIS16201_XACCL_SCALE    0x14
-
-/* y-axis acceleration scale factor */
-#define ADIS16201_YACCL_SCALE    0x16
-
-/* Calibration, x-axis inclination offset */
-#define ADIS16201_XINCL_OFFS     0x18
-
-/* Calibration, y-axis inclination offset */
-#define ADIS16201_YINCL_OFFS     0x1A
-
-/* x-axis inclination scale factor */
-#define ADIS16201_XINCL_SCALE    0x1C
-
-/* y-axis inclination scale factor */
-#define ADIS16201_YINCL_SCALE    0x1E
-
-/* Alarm 1 amplitude threshold */
-#define ADIS16201_ALM_MAG1       0x20
-
-/* Alarm 2 amplitude threshold */
-#define ADIS16201_ALM_MAG2       0x22
-
-/* Alarm 1, sample period */
-#define ADIS16201_ALM_SMPL1      0x24
-
-/* Alarm 2, sample period */
-#define ADIS16201_ALM_SMPL2      0x26
-
-/* Alarm control */
-#define ADIS16201_ALM_CTRL       0x28
-
-/* Auxiliary DAC data */
-#define ADIS16201_AUX_DAC        0x30
-
-/* General-purpose digital input/output control */
-#define ADIS16201_GPIO_CTRL      0x32
-
-/* Miscellaneous control */
-#define ADIS16201_MSC_CTRL       0x34
-
-/* Internal sample period (rate) control */
-#define ADIS16201_SMPL_PRD       0x36
-
-/* Operation, filter configuration */
-#define ADIS16201_AVG_CNT        0x38
-
-/* Operation, sleep mode control */
-#define ADIS16201_SLP_CNT        0x3A
-
-/* Diagnostics, system status register */
-#define ADIS16201_DIAG_STAT      0x3C
-
-/* Operation, system command register */
-#define ADIS16201_GLOB_CMD       0x3E
-
-/* MSC_CTRL */
-
-/* Self-test enable */
-#define ADIS16201_MSC_CTRL_SELF_TEST_EN	        BIT(8)
-
-/* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16201_MSC_CTRL_DATA_RDY_EN	        BIT(2)
-
-/* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16201_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
-
-/* Data-ready line selection: 1 = DIO1, 0 = DIO0 */
-#define ADIS16201_MSC_CTRL_DATA_RDY_DIO1	BIT(0)
-
-/* DIAG_STAT */
-
-/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16201_DIAG_STAT_ALARM2        BIT(9)
-
-/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16201_DIAG_STAT_ALARM1        BIT(8)
-
-/* SPI communications failure */
-#define ADIS16201_DIAG_STAT_SPI_FAIL_BIT   3
-
-/* Flash update failure */
-#define ADIS16201_DIAG_STAT_FLASH_UPT_BIT  2
-
-/* Power supply above 3.625 V */
-#define ADIS16201_DIAG_STAT_POWER_HIGH_BIT 1
-
-/* Power supply below 3.15 V */
-#define ADIS16201_DIAG_STAT_POWER_LOW_BIT  0
-
-/* GLOB_CMD */
-
-#define ADIS16201_GLOB_CMD_SW_RESET	BIT(7)
-#define ADIS16201_GLOB_CMD_FACTORY_CAL	BIT(1)
-
-#define ADIS16201_ERROR_ACTIVE          BIT(14)
-
-enum adis16201_scan {
-	ADIS16201_SCAN_ACC_X,
-	ADIS16201_SCAN_ACC_Y,
-	ADIS16201_SCAN_INCLI_X,
-	ADIS16201_SCAN_INCLI_Y,
-	ADIS16201_SCAN_SUPPLY,
-	ADIS16201_SCAN_AUX_ADC,
-	ADIS16201_SCAN_TEMP,
-};
-
-#endif /* SPI_ADIS16201_H_ */
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203.c
index bd8119a23339..4e3fa7592d3f 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203.c
@@ -7,20 +7,140 @@
  */
 
 #include <linux/delay.h>
-#include <linux/mutex.h>
 #include <linux/device.h>
+
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/imu/adis.h>
+#include <linux/iio/sysfs.h>
+
 #include <linux/kernel.h>
-#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/slab.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/buffer.h>
-#include <linux/iio/imu/adis.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
 
-#include "adis16203.h"
+/* 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 3.15 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"
 
@@ -83,17 +203,13 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_CALIBBIAS:
 		bits = 14;
-		mutex_lock(&indio_dev->mlock);
 		addr = adis16203_addresses[chan->scan_index];
 		ret = adis_read_reg_16(st, addr, &val16);
-		if (ret) {
-			mutex_unlock(&indio_dev->mlock);
+		if (ret)
 			return ret;
-		}
 		val16 &= (1 << bits) - 1;
 		val16 = (s16)(val16 << (16 - bits)) >> (16 - bits);
 		*val = val16;
-		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 	default:
 		return -EINVAL;
@@ -113,8 +229,8 @@ static const struct iio_chan_spec adis16203_channels[] = {
 };
 
 static const struct iio_info adis16203_info = {
-	.read_raw = &adis16203_read_raw,
-	.write_raw = &adis16203_write_raw,
+	.read_raw = adis16203_read_raw,
+	.write_raw = adis16203_write_raw,
 	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
diff --git a/drivers/staging/iio/accel/adis16203.h b/drivers/staging/iio/accel/adis16203.h
deleted file mode 100644
index b483e4e6475b..000000000000
--- a/drivers/staging/iio/accel/adis16203.h
+++ /dev/null
@@ -1,125 +0,0 @@
-#ifndef SPI_ADIS16203_H_
-#define SPI_ADIS16203_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 3.15 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,
-};
-
-#endif /* SPI_ADIS16203_H_ */
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209.c
index a599e19303d3..8485c024e3f5 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209.c
@@ -7,7 +7,6 @@
  */
 
 #include <linux/delay.h>
-#include <linux/mutex.h>
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/spi/spi.h>
@@ -21,7 +20,147 @@
 #include <linux/iio/buffer.h>
 #include <linux/iio/imu/adis.h>
 
-#include "adis16209.h"
+#define ADIS16209_STARTUP_DELAY	220 /* ms */
+
+/* Flash memory write count */
+#define ADIS16209_FLASH_CNT      0x00
+
+/* Output, power supply */
+#define ADIS16209_SUPPLY_OUT     0x02
+
+/* Output, x-axis accelerometer */
+#define ADIS16209_XACCL_OUT      0x04
+
+/* Output, y-axis accelerometer */
+#define ADIS16209_YACCL_OUT      0x06
+
+/* Output, auxiliary ADC input */
+#define ADIS16209_AUX_ADC        0x08
+
+/* Output, temperature */
+#define ADIS16209_TEMP_OUT       0x0A
+
+/* Output, x-axis inclination */
+#define ADIS16209_XINCL_OUT      0x0C
+
+/* Output, y-axis inclination */
+#define ADIS16209_YINCL_OUT      0x0E
+
+/* Output, +/-180 vertical rotational position */
+#define ADIS16209_ROT_OUT        0x10
+
+/* Calibration, x-axis acceleration offset null */
+#define ADIS16209_XACCL_NULL     0x12
+
+/* Calibration, y-axis acceleration offset null */
+#define ADIS16209_YACCL_NULL     0x14
+
+/* Calibration, x-axis inclination offset null */
+#define ADIS16209_XINCL_NULL     0x16
+
+/* Calibration, y-axis inclination offset null */
+#define ADIS16209_YINCL_NULL     0x18
+
+/* Calibration, vertical rotation offset null */
+#define ADIS16209_ROT_NULL       0x1A
+
+/* Alarm 1 amplitude threshold */
+#define ADIS16209_ALM_MAG1       0x20
+
+/* Alarm 2 amplitude threshold */
+#define ADIS16209_ALM_MAG2       0x22
+
+/* Alarm 1, sample period */
+#define ADIS16209_ALM_SMPL1      0x24
+
+/* Alarm 2, sample period */
+#define ADIS16209_ALM_SMPL2      0x26
+
+/* Alarm control */
+#define ADIS16209_ALM_CTRL       0x28
+
+/* Auxiliary DAC data */
+#define ADIS16209_AUX_DAC        0x30
+
+/* General-purpose digital input/output control */
+#define ADIS16209_GPIO_CTRL      0x32
+
+/* Miscellaneous control */
+#define ADIS16209_MSC_CTRL       0x34
+
+/* Internal sample period (rate) control */
+#define ADIS16209_SMPL_PRD       0x36
+
+/* Operation, filter configuration */
+#define ADIS16209_AVG_CNT        0x38
+
+/* Operation, sleep mode control */
+#define ADIS16209_SLP_CNT        0x3A
+
+/* Diagnostics, system status register */
+#define ADIS16209_DIAG_STAT      0x3C
+
+/* Operation, system command register */
+#define ADIS16209_GLOB_CMD       0x3E
+
+/* MSC_CTRL */
+
+/* Self-test at power-on: 1 = disabled, 0 = enabled */
+#define ADIS16209_MSC_CTRL_PWRUP_SELF_TEST	BIT(10)
+
+/* Self-test enable */
+#define ADIS16209_MSC_CTRL_SELF_TEST_EN	        BIT(8)
+
+/* Data-ready enable: 1 = enabled, 0 = disabled */
+#define ADIS16209_MSC_CTRL_DATA_RDY_EN	        BIT(2)
+
+/* Data-ready polarity: 1 = active high, 0 = active low */
+#define ADIS16209_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
+
+/* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
+#define ADIS16209_MSC_CTRL_DATA_RDY_DIO2	BIT(0)
+
+/* DIAG_STAT */
+
+/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16209_DIAG_STAT_ALARM2        BIT(9)
+
+/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
+#define ADIS16209_DIAG_STAT_ALARM1        BIT(8)
+
+/* Self-test diagnostic error flag: 1 = error condition, 0 = normal operation */
+#define ADIS16209_DIAG_STAT_SELFTEST_FAIL_BIT	5
+
+/* SPI communications failure */
+#define ADIS16209_DIAG_STAT_SPI_FAIL_BIT	3
+
+/* Flash update failure */
+#define ADIS16209_DIAG_STAT_FLASH_UPT_BIT	2
+
+/* Power supply above 3.625 V */
+#define ADIS16209_DIAG_STAT_POWER_HIGH_BIT	1
+
+/* Power supply below 3.15 V */
+#define ADIS16209_DIAG_STAT_POWER_LOW_BIT	0
+
+/* GLOB_CMD */
+
+#define ADIS16209_GLOB_CMD_SW_RESET	BIT(7)
+#define ADIS16209_GLOB_CMD_CLEAR_STAT	BIT(4)
+#define ADIS16209_GLOB_CMD_FACTORY_CAL	BIT(1)
+
+#define ADIS16209_ERROR_ACTIVE          BIT(14)
+
+enum adis16209_scan {
+	ADIS16209_SCAN_SUPPLY,
+	ADIS16209_SCAN_ACC_X,
+	ADIS16209_SCAN_ACC_Y,
+	ADIS16209_SCAN_AUX_ADC,
+	ADIS16209_SCAN_TEMP,
+	ADIS16209_SCAN_INCLI_X,
+	ADIS16209_SCAN_INCLI_Y,
+	ADIS16209_SCAN_ROT,
+};
 
 static const u8 adis16209_addresses[8][1] = {
 	[ADIS16209_SCAN_SUPPLY] = { },
@@ -114,17 +253,13 @@ static int adis16209_read_raw(struct iio_dev *indio_dev,
 		default:
 			return -EINVAL;
 		}
-		mutex_lock(&indio_dev->mlock);
 		addr = adis16209_addresses[chan->scan_index][0];
 		ret = adis_read_reg_16(st, addr, &val16);
-		if (ret) {
-			mutex_unlock(&indio_dev->mlock);
+		if (ret)
 			return ret;
-		}
 		val16 &= (1 << bits) - 1;
 		val16 = (s16)(val16 << (16 - bits)) >> (16 - bits);
 		*val = val16;
-		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 	}
 	return -EINVAL;
@@ -147,8 +282,8 @@ static const struct iio_chan_spec adis16209_channels[] = {
 };
 
 static const struct iio_info adis16209_info = {
-	.read_raw = &adis16209_read_raw,
-	.write_raw = &adis16209_write_raw,
+	.read_raw = adis16209_read_raw,
+	.write_raw = adis16209_write_raw,
 	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
diff --git a/drivers/staging/iio/accel/adis16209.h b/drivers/staging/iio/accel/adis16209.h
deleted file mode 100644
index 315f1c0c46e8..000000000000
--- a/drivers/staging/iio/accel/adis16209.h
+++ /dev/null
@@ -1,144 +0,0 @@
-#ifndef SPI_ADIS16209_H_
-#define SPI_ADIS16209_H_
-
-#define ADIS16209_STARTUP_DELAY	220 /* ms */
-
-/* Flash memory write count */
-#define ADIS16209_FLASH_CNT      0x00
-
-/* Output, power supply */
-#define ADIS16209_SUPPLY_OUT     0x02
-
-/* Output, x-axis accelerometer */
-#define ADIS16209_XACCL_OUT      0x04
-
-/* Output, y-axis accelerometer */
-#define ADIS16209_YACCL_OUT      0x06
-
-/* Output, auxiliary ADC input */
-#define ADIS16209_AUX_ADC        0x08
-
-/* Output, temperature */
-#define ADIS16209_TEMP_OUT       0x0A
-
-/* Output, x-axis inclination */
-#define ADIS16209_XINCL_OUT      0x0C
-
-/* Output, y-axis inclination */
-#define ADIS16209_YINCL_OUT      0x0E
-
-/* Output, +/-180 vertical rotational position */
-#define ADIS16209_ROT_OUT        0x10
-
-/* Calibration, x-axis acceleration offset null */
-#define ADIS16209_XACCL_NULL     0x12
-
-/* Calibration, y-axis acceleration offset null */
-#define ADIS16209_YACCL_NULL     0x14
-
-/* Calibration, x-axis inclination offset null */
-#define ADIS16209_XINCL_NULL     0x16
-
-/* Calibration, y-axis inclination offset null */
-#define ADIS16209_YINCL_NULL     0x18
-
-/* Calibration, vertical rotation offset null */
-#define ADIS16209_ROT_NULL       0x1A
-
-/* Alarm 1 amplitude threshold */
-#define ADIS16209_ALM_MAG1       0x20
-
-/* Alarm 2 amplitude threshold */
-#define ADIS16209_ALM_MAG2       0x22
-
-/* Alarm 1, sample period */
-#define ADIS16209_ALM_SMPL1      0x24
-
-/* Alarm 2, sample period */
-#define ADIS16209_ALM_SMPL2      0x26
-
-/* Alarm control */
-#define ADIS16209_ALM_CTRL       0x28
-
-/* Auxiliary DAC data */
-#define ADIS16209_AUX_DAC        0x30
-
-/* General-purpose digital input/output control */
-#define ADIS16209_GPIO_CTRL      0x32
-
-/* Miscellaneous control */
-#define ADIS16209_MSC_CTRL       0x34
-
-/* Internal sample period (rate) control */
-#define ADIS16209_SMPL_PRD       0x36
-
-/* Operation, filter configuration */
-#define ADIS16209_AVG_CNT        0x38
-
-/* Operation, sleep mode control */
-#define ADIS16209_SLP_CNT        0x3A
-
-/* Diagnostics, system status register */
-#define ADIS16209_DIAG_STAT      0x3C
-
-/* Operation, system command register */
-#define ADIS16209_GLOB_CMD       0x3E
-
-/* MSC_CTRL */
-
-/* Self-test at power-on: 1 = disabled, 0 = enabled */
-#define ADIS16209_MSC_CTRL_PWRUP_SELF_TEST	BIT(10)
-
-/* Self-test enable */
-#define ADIS16209_MSC_CTRL_SELF_TEST_EN	        BIT(8)
-
-/* Data-ready enable: 1 = enabled, 0 = disabled */
-#define ADIS16209_MSC_CTRL_DATA_RDY_EN	        BIT(2)
-
-/* Data-ready polarity: 1 = active high, 0 = active low */
-#define ADIS16209_MSC_CTRL_ACTIVE_HIGH	        BIT(1)
-
-/* Data-ready line selection: 1 = DIO2, 0 = DIO1 */
-#define ADIS16209_MSC_CTRL_DATA_RDY_DIO2	BIT(0)
-
-/* DIAG_STAT */
-
-/* Alarm 2 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16209_DIAG_STAT_ALARM2        BIT(9)
-
-/* Alarm 1 status: 1 = alarm active, 0 = alarm inactive */
-#define ADIS16209_DIAG_STAT_ALARM1        BIT(8)
-
-/* Self-test diagnostic error flag: 1 = error condition, 0 = normal operation */
-#define ADIS16209_DIAG_STAT_SELFTEST_FAIL_BIT	5
-
-/* SPI communications failure */
-#define ADIS16209_DIAG_STAT_SPI_FAIL_BIT	3
-
-/* Flash update failure */
-#define ADIS16209_DIAG_STAT_FLASH_UPT_BIT	2
-
-/* Power supply above 3.625 V */
-#define ADIS16209_DIAG_STAT_POWER_HIGH_BIT	1
-
-/* Power supply below 3.15 V */
-#define ADIS16209_DIAG_STAT_POWER_LOW_BIT	0
-
-/* GLOB_CMD */
-
-#define ADIS16209_GLOB_CMD_SW_RESET	BIT(7)
-#define ADIS16209_GLOB_CMD_CLEAR_STAT	BIT(4)
-#define ADIS16209_GLOB_CMD_FACTORY_CAL	BIT(1)
-
-#define ADIS16209_ERROR_ACTIVE          BIT(14)
-
-#define ADIS16209_SCAN_SUPPLY	0
-#define ADIS16209_SCAN_ACC_X	1
-#define ADIS16209_SCAN_ACC_Y	2
-#define ADIS16209_SCAN_AUX_ADC	3
-#define ADIS16209_SCAN_TEMP	4
-#define ADIS16209_SCAN_INCLI_X	5
-#define ADIS16209_SCAN_INCLI_Y	6
-#define ADIS16209_SCAN_ROT	7
-
-#endif /* SPI_ADIS16209_H_ */
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240.c
index d5b99e610d08..109cd94b5ac3 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240.c
@@ -10,7 +10,6 @@
 #include <linux/irq.h>
 #include <linux/gpio.h>
 #include <linux/delay.h>
-#include <linux/mutex.h>
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/spi/spi.h>
@@ -24,7 +23,182 @@
 #include <linux/iio/buffer.h>
 #include <linux/iio/imu/adis.h>
 
-#include "adis16240.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 3.15 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,
@@ -54,18 +228,10 @@ static ssize_t adis16240_read_12bit_signed(struct device *dev,
 					   struct device_attribute *attr,
 					   char *buf)
 {
-	ssize_t ret;
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-
-	/* Take the iio_dev status lock */
-	mutex_lock(&indio_dev->mlock);
-	ret =  adis16240_spi_read_signed(dev, attr, buf, 12);
-	mutex_unlock(&indio_dev->mlock);
-
-	return ret;
+	return adis16240_spi_read_signed(dev, attr, buf, 12);
 }
 
-static IIO_DEVICE_ATTR(in_accel_xyz_squared_peak_raw, S_IRUGO,
+static IIO_DEVICE_ATTR(in_accel_xyz_squared_peak_raw, 0444,
 		       adis16240_read_12bit_signed, NULL,
 		       ADIS16240_XYZPEAK_OUT);
 
@@ -122,31 +288,23 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_CALIBBIAS:
 		bits = 10;
-		mutex_lock(&indio_dev->mlock);
 		addr = adis16240_addresses[chan->scan_index][0];
 		ret = adis_read_reg_16(st, addr, &val16);
-		if (ret) {
-			mutex_unlock(&indio_dev->mlock);
+		if (ret)
 			return ret;
-		}
 		val16 &= (1 << bits) - 1;
 		val16 = (s16)(val16 << (16 - bits)) >> (16 - bits);
 		*val = val16;
-		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_PEAK:
 		bits = 10;
-		mutex_lock(&indio_dev->mlock);
 		addr = adis16240_addresses[chan->scan_index][1];
 		ret = adis_read_reg_16(st, addr, &val16);
-		if (ret) {
-			mutex_unlock(&indio_dev->mlock);
+		if (ret)
 			return ret;
-		}
 		val16 &= (1 << bits) - 1;
 		val16 = (s16)(val16 << (16 - bits)) >> (16 - bits);
 		*val = val16;
-		mutex_unlock(&indio_dev->mlock);
 		return IIO_VAL_INT;
 	}
 	return -EINVAL;
@@ -200,8 +358,8 @@ static const struct attribute_group adis16240_attribute_group = {
 
 static const struct iio_info adis16240_info = {
 	.attrs = &adis16240_attribute_group,
-	.read_raw = &adis16240_read_raw,
-	.write_raw = &adis16240_write_raw,
+	.read_raw = adis16240_read_raw,
+	.write_raw = adis16240_write_raw,
 	.update_scan_mode = adis_update_scan_mode,
 	.driver_module = THIS_MODULE,
 };
diff --git a/drivers/staging/iio/accel/adis16240.h b/drivers/staging/iio/accel/adis16240.h
deleted file mode 100644
index b2cb37b95913..000000000000
--- a/drivers/staging/iio/accel/adis16240.h
+++ /dev/null
@@ -1,179 +0,0 @@
-#ifndef SPI_ADIS16240_H_
-#define SPI_ADIS16240_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 3.15 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!
- */
-
-#define ADIS16240_SCAN_ACC_X	0
-#define ADIS16240_SCAN_ACC_Y	1
-#define ADIS16240_SCAN_ACC_Z	2
-#define ADIS16240_SCAN_SUPPLY	3
-#define ADIS16240_SCAN_AUX_ADC	4
-#define ADIS16240_SCAN_TEMP	5
-
-#endif /* SPI_ADIS16240_H_ */
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index deff89973d53..e17efb03bac0 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -80,26 +80,4 @@ config AD7280
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7280a
 
-config LPC32XX_ADC
-	tristate "NXP LPC32XX ADC"
-	depends on ARCH_LPC32XX || COMPILE_TEST
-	depends on HAS_IOMEM
-	help
-	  Say yes here to build support for the integrated ADC inside the
-	  LPC32XX SoC. Note that this feature uses the same hardware as the
-	  touchscreen driver, so you should either select only one of the two
-	  drivers (lpc32xx_adc or lpc32xx_ts) or, in the OpenFirmware case,
-	  activate only one via device tree selection.  Provides direct access
-	  via sysfs.
-
-config SPEAR_ADC
-	tristate "ST SPEAr ADC"
-	depends on PLAT_SPEAR || COMPILE_TEST
-	depends on HAS_IOMEM
-	help
-	  Say yes here to build support for the integrated ADC inside the
-	  ST SPEAr SoC. Provides direct access via sysfs.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called spear_adc.
 endmenu
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index ac09485923b6..bf18bdd7c99d 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -10,5 +10,3 @@ obj-$(CONFIG_AD7780) += ad7780.o
 obj-$(CONFIG_AD7816) += ad7816.o
 obj-$(CONFIG_AD7192) += ad7192.o
 obj-$(CONFIG_AD7280) += ad7280a.o
-obj-$(CONFIG_LPC32XX_ADC) += lpc32xx_adc.o
-obj-$(CONFIG_SPEAR_ADC) += spear_adc.o
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 1fb68c01abd5..d11c6de9c777 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -342,9 +342,9 @@ ad7192_show_scale_available(struct device *dev,
 
 static IIO_DEVICE_ATTR_NAMED(in_v_m_v_scale_available,
 			     in_voltage-voltage_scale_available,
-			     S_IRUGO, ad7192_show_scale_available, NULL, 0);
+			     0444, ad7192_show_scale_available, NULL, 0);
 
-static IIO_DEVICE_ATTR(in_voltage_scale_available, S_IRUGO,
+static IIO_DEVICE_ATTR(in_voltage_scale_available, 0444,
 		       ad7192_show_scale_available, NULL, 0);
 
 static ssize_t ad7192_show_ac_excitation(struct device *dev,
@@ -412,11 +412,11 @@ static ssize_t ad7192_set(struct device *dev,
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(bridge_switch_en, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(bridge_switch_en, 0644,
 		       ad7192_show_bridge_switch, ad7192_set,
 		       AD7192_REG_GPOCON);
 
-static IIO_DEVICE_ATTR(ac_excitation_en, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(ac_excitation_en, 0644,
 		       ad7192_show_ac_excitation, ad7192_set,
 		       AD7192_REG_MODE);
 
@@ -564,18 +564,18 @@ static int ad7192_write_raw_get_fmt(struct iio_dev *indio_dev,
 }
 
 static const struct iio_info ad7192_info = {
-	.read_raw = &ad7192_read_raw,
-	.write_raw = &ad7192_write_raw,
-	.write_raw_get_fmt = &ad7192_write_raw_get_fmt,
+	.read_raw = ad7192_read_raw,
+	.write_raw = ad7192_write_raw,
+	.write_raw_get_fmt = ad7192_write_raw_get_fmt,
 	.attrs = &ad7192_attribute_group,
 	.validate_trigger = ad_sd_validate_trigger,
 	.driver_module = THIS_MODULE,
 };
 
 static const struct iio_info ad7195_info = {
-	.read_raw = &ad7192_read_raw,
-	.write_raw = &ad7192_write_raw,
-	.write_raw_get_fmt = &ad7192_write_raw_get_fmt,
+	.read_raw = ad7192_read_raw,
+	.write_raw = ad7192_write_raw,
+	.write_raw_get_fmt = ad7192_write_raw_get_fmt,
 	.attrs = &ad7195_attribute_group,
 	.validate_trigger = ad_sd_validate_trigger,
 	.driver_module = THIS_MODULE,
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index ee679ac0368f..d5ab83f0236d 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -134,6 +134,7 @@ struct ad7280_state {
 	unsigned char			aux_threshhigh;
 	unsigned char			aux_threshlow;
 	unsigned char			cb_mask[AD7280A_MAX_CHAIN];
+	struct mutex			lock; /* protect sensor state */
 
 	__be32				buf[2] ____cacheline_aligned;
 };
@@ -410,7 +411,7 @@ static ssize_t ad7280_store_balance_sw(struct device *dev,
 	devaddr = this_attr->address >> 8;
 	ch = this_attr->address & 0xFF;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	if (readin)
 		st->cb_mask[devaddr] |= 1 << (ch + 2);
 	else
@@ -418,7 +419,7 @@ static ssize_t ad7280_store_balance_sw(struct device *dev,
 
 	ret = ad7280_write(st, devaddr, AD7280A_CELL_BALANCE,
 			   0, st->cb_mask[devaddr]);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -433,10 +434,10 @@ static ssize_t ad7280_show_balance_timer(struct device *dev,
 	int ret;
 	unsigned int msecs;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad7280_read(st, this_attr->address >> 8,
 			  this_attr->address & 0xFF);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	if (ret < 0)
 		return ret;
@@ -466,11 +467,11 @@ static ssize_t ad7280_store_balance_timer(struct device *dev,
 	if (val > 31)
 		return -EINVAL;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad7280_write(st, this_attr->address >> 8,
 			   this_attr->address & 0xFF,
 			   0, (val & 0x1F) << 3);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -559,7 +560,7 @@ static int ad7280_attr_init(struct ad7280_state *st)
 			st->iio_attr[cnt].address =
 				AD7280A_DEVADDR(dev) << 8 | ch;
 			st->iio_attr[cnt].dev_attr.attr.mode =
-				S_IWUSR | S_IRUGO;
+				0644;
 			st->iio_attr[cnt].dev_attr.show =
 				ad7280_show_balance_sw;
 			st->iio_attr[cnt].dev_attr.store =
@@ -576,7 +577,7 @@ static int ad7280_attr_init(struct ad7280_state *st)
 				AD7280A_DEVADDR(dev) << 8 |
 				(AD7280A_CB1_TIMER + ch);
 			st->iio_attr[cnt].dev_attr.attr.mode =
-				S_IWUSR | S_IRUGO;
+				0644;
 			st->iio_attr[cnt].dev_attr.show =
 				ad7280_show_balance_timer;
 			st->iio_attr[cnt].dev_attr.store =
@@ -655,7 +656,7 @@ static ssize_t ad7280_write_channel_config(struct device *dev,
 
 	val = clamp(val, 0L, 0xFFL);
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	switch ((u32)this_attr->address) {
 	case AD7280A_CELL_OVERVOLTAGE:
 		st->cell_threshhigh = val;
@@ -674,7 +675,7 @@ static ssize_t ad7280_write_channel_config(struct device *dev,
 	ret = ad7280_write(st, AD7280A_DEVADDR_MASTER,
 			   this_attr->address, 1, val);
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -745,26 +746,26 @@ out:
 
 static IIO_DEVICE_ATTR_NAMED(in_thresh_low_value,
 		in_voltage-voltage_thresh_low_value,
-		S_IRUGO | S_IWUSR,
+		0644,
 		ad7280_read_channel_config,
 		ad7280_write_channel_config,
 		AD7280A_CELL_UNDERVOLTAGE);
 
 static IIO_DEVICE_ATTR_NAMED(in_thresh_high_value,
 		in_voltage-voltage_thresh_high_value,
-		S_IRUGO | S_IWUSR,
+		0644,
 		ad7280_read_channel_config,
 		ad7280_write_channel_config,
 		AD7280A_CELL_OVERVOLTAGE);
 
 static IIO_DEVICE_ATTR(in_temp_thresh_low_value,
-		S_IRUGO | S_IWUSR,
+		0644,
 		ad7280_read_channel_config,
 		ad7280_write_channel_config,
 		AD7280A_AUX_ADC_UNDERVOLTAGE);
 
 static IIO_DEVICE_ATTR(in_temp_thresh_high_value,
-		S_IRUGO | S_IWUSR,
+		0644,
 		ad7280_read_channel_config,
 		ad7280_write_channel_config,
 		AD7280A_AUX_ADC_OVERVOLTAGE);
@@ -792,13 +793,13 @@ static int ad7280_read_raw(struct iio_dev *indio_dev,
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		if (chan->address == AD7280A_ALL_CELLS)
 			ret = ad7280_read_all_channels(st, st->scan_cnt, NULL);
 		else
 			ret = ad7280_read_channel(st, chan->address >> 8,
 						  chan->address & 0xFF);
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 
 		if (ret < 0)
 			return ret;
@@ -847,6 +848,7 @@ static int ad7280_probe(struct spi_device *spi)
 	st = iio_priv(indio_dev);
 	spi_set_drvdata(spi, indio_dev);
 	st->spi = spi;
+	mutex_init(&st->lock);
 
 	if (!pdata)
 		pdata = &ad7793_default_pdata;
diff --git a/drivers/staging/iio/adc/ad7606.c b/drivers/staging/iio/adc/ad7606.c
index 9dbfa64b1e53..18f5f139117e 100644
--- a/drivers/staging/iio/adc/ad7606.c
+++ b/drivers/staging/iio/adc/ad7606.c
@@ -208,7 +208,7 @@ static int ad7606_write_raw(struct iio_dev *indio_dev,
 	switch (mask) {
 	case IIO_CHAN_INFO_SCALE:
 		ret = -EINVAL;
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		for (i = 0; i < ARRAY_SIZE(scale_avail); i++)
 			if (val2 == scale_avail[i][1]) {
 				gpiod_set_value(st->gpio_range, i);
@@ -217,7 +217,7 @@ static int ad7606_write_raw(struct iio_dev *indio_dev,
 				ret = 0;
 				break;
 			}
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 
 		return ret;
 	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
@@ -231,11 +231,11 @@ static int ad7606_write_raw(struct iio_dev *indio_dev,
 		values[1] = (ret >> 1) & 1;
 		values[2] = (ret >> 2) & 1;
 
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		gpiod_set_array_value(ARRAY_SIZE(values), st->gpio_os->desc,
 				      values);
 		st->oversampling = val;
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 
 		return 0;
 	default:
@@ -413,6 +413,7 @@ int ad7606_probe(struct device *dev, int irq, void __iomem *base_address,
 	st = iio_priv(indio_dev);
 
 	st->dev = dev;
+	mutex_init(&st->lock);
 	st->bops = bops;
 	st->base_address = base_address;
 	/* tied to logic low, analog input range is +/- 5V */
diff --git a/drivers/staging/iio/adc/ad7606.h b/drivers/staging/iio/adc/ad7606.h
index 746f9553d2ba..acaed8d5379c 100644
--- a/drivers/staging/iio/adc/ad7606.h
+++ b/drivers/staging/iio/adc/ad7606.h
@@ -14,6 +14,7 @@
  * @name:		identification string for chip
  * @channels:		channel specification
  * @num_channels:	number of channels
+ * @lock		protect sensor state
  */
 
 struct ad7606_chip_info {
@@ -23,6 +24,7 @@ struct ad7606_chip_info {
 
 /**
  * struct ad7606_state - driver instance specific data
+ * @lock		protect sensor state
  */
 
 struct ad7606_state {
@@ -37,6 +39,7 @@ struct ad7606_state {
 	bool				done;
 	void __iomem			*base_address;
 
+	struct mutex			lock; /* protect sensor state */
 	struct gpio_desc		*gpio_convst;
 	struct gpio_desc		*gpio_reset;
 	struct gpio_desc		*gpio_range;
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index e14960038d3e..dec3ba6eba8a 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -154,7 +154,7 @@ static const struct ad7780_chip_info ad7780_chip_info_tbl[] = {
 };
 
 static const struct iio_info ad7780_info = {
-	.read_raw = &ad7780_read_raw,
+	.read_raw = ad7780_read_raw,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 6054c7298fce..b2bce26499f5 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -244,7 +244,6 @@ static ssize_t _adt7316_store_enabled(struct adt7316_chip_info *chip,
 	chip->config1 = config1;
 
 	return ret;
-
 }
 
 static ssize_t adt7316_store_enabled(struct device *dev,
@@ -267,7 +266,7 @@ static ssize_t adt7316_store_enabled(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(enabled, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(enabled, 0644,
 		adt7316_show_enabled,
 		adt7316_store_enabled,
 		0);
@@ -311,7 +310,7 @@ static ssize_t adt7316_store_select_ex_temp(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(select_ex_temp, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(select_ex_temp, 0644,
 		adt7316_show_select_ex_temp,
 		adt7316_store_select_ex_temp,
 		0);
@@ -352,7 +351,7 @@ static ssize_t adt7316_store_mode(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(mode, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(mode, 0644,
 		adt7316_show_mode,
 		adt7316_store_mode,
 		0);
@@ -364,7 +363,7 @@ static ssize_t adt7316_show_all_modes(struct device *dev,
 	return sprintf(buf, "single_channel\nround_robin\n");
 }
 
-static IIO_DEVICE_ATTR(all_modes, S_IRUGO, adt7316_show_all_modes, NULL, 0);
+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,
@@ -434,7 +433,6 @@ static ssize_t adt7316_store_ad_channel(struct device *dev,
 		config2 = chip->config2 & (~ADT7316_AD_SINGLE_CH_MASK);
 	}
 
-
 	config2 |= data;
 
 	ret = chip->bus.write(chip->bus.client, ADT7316_CONFIG2, config2);
@@ -446,7 +444,7 @@ static ssize_t adt7316_store_ad_channel(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(ad_channel, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(ad_channel, 0644,
 		adt7316_show_ad_channel,
 		adt7316_store_ad_channel,
 		0);
@@ -469,7 +467,7 @@ static ssize_t adt7316_show_all_ad_channels(struct device *dev,
 			"2 - External Temperature\n");
 }
 
-static IIO_DEVICE_ATTR(all_ad_channels, S_IRUGO,
+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,
@@ -506,7 +504,7 @@ static ssize_t adt7316_store_disable_averaging(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(disable_averaging, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(disable_averaging, 0644,
 		adt7316_show_disable_averaging,
 		adt7316_store_disable_averaging,
 		0);
@@ -545,7 +543,7 @@ static ssize_t adt7316_store_enable_smbus_timeout(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(enable_smbus_timeout, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(enable_smbus_timeout, 0644,
 		adt7316_show_enable_smbus_timeout,
 		adt7316_store_enable_smbus_timeout,
 		0);
@@ -583,7 +581,7 @@ static ssize_t adt7316_store_powerdown(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(powerdown, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(powerdown, 0644,
 		adt7316_show_powerdown,
 		adt7316_store_powerdown,
 		0);
@@ -621,7 +619,7 @@ static ssize_t adt7316_store_fast_ad_clock(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(fast_ad_clock, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(fast_ad_clock, 0644,
 		adt7316_show_fast_ad_clock,
 		adt7316_store_fast_ad_clock,
 		0);
@@ -674,7 +672,7 @@ static ssize_t adt7316_store_da_high_resolution(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(da_high_resolution, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(da_high_resolution, 0644,
 		adt7316_show_da_high_resolution,
 		adt7316_store_da_high_resolution,
 		0);
@@ -720,12 +718,11 @@ static ssize_t adt7316_store_AIN_internal_Vref(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(AIN_internal_Vref, S_IRUGO | S_IWUSR,
+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)
@@ -760,7 +757,7 @@ static ssize_t adt7316_store_enable_prop_DACA(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(enable_proportion_DACA, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(enable_proportion_DACA, 0644,
 		adt7316_show_enable_prop_DACA,
 		adt7316_store_enable_prop_DACA,
 		0);
@@ -799,7 +796,7 @@ static ssize_t adt7316_store_enable_prop_DACB(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(enable_proportion_DACB, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(enable_proportion_DACB, 0644,
 		adt7316_show_enable_prop_DACB,
 		adt7316_store_enable_prop_DACB,
 		0);
@@ -842,7 +839,7 @@ static ssize_t adt7316_store_DAC_2Vref_ch_mask(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(DAC_2Vref_channels_mask, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(DAC_2Vref_channels_mask, 0644,
 		adt7316_show_DAC_2Vref_ch_mask,
 		adt7316_store_DAC_2Vref_ch_mask,
 		0);
@@ -902,7 +899,7 @@ static ssize_t adt7316_store_DAC_update_mode(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(DAC_update_mode, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(DAC_update_mode, 0644,
 		adt7316_show_DAC_update_mode,
 		adt7316_store_DAC_update_mode,
 		0);
@@ -922,10 +919,9 @@ static ssize_t adt7316_show_all_DAC_update_modes(struct device *dev,
 	return sprintf(buf, "manual\n");
 }
 
-static IIO_DEVICE_ATTR(all_DAC_update_modes, S_IRUGO,
+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,
@@ -961,7 +957,7 @@ static ssize_t adt7316_store_update_DAC(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(update_DAC, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(update_DAC, 0644,
 		NULL,
 		adt7316_store_update_DAC,
 		0);
@@ -1006,7 +1002,7 @@ static ssize_t adt7316_store_DA_AB_Vref_bypass(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(DA_AB_Vref_bypass, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(DA_AB_Vref_bypass, 0644,
 		adt7316_show_DA_AB_Vref_bypass,
 		adt7316_store_DA_AB_Vref_bypass,
 		0);
@@ -1051,7 +1047,7 @@ static ssize_t adt7316_store_DA_CD_Vref_bypass(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(DA_CD_Vref_bypass, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(DA_CD_Vref_bypass, 0644,
 		adt7316_show_DA_CD_Vref_bypass,
 		adt7316_store_DA_CD_Vref_bypass,
 		0);
@@ -1112,7 +1108,7 @@ static ssize_t adt7316_store_DAC_internal_Vref(struct device *dev,
 	return len;
 }
 
-static IIO_DEVICE_ATTR(DAC_internal_Vref, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(DAC_internal_Vref, 0644,
 		adt7316_show_DAC_internal_Vref,
 		adt7316_store_DAC_internal_Vref,
 		0);
@@ -1201,7 +1197,7 @@ static ssize_t adt7316_show_VDD(struct device *dev,
 
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_VDD, buf);
 }
-static IIO_DEVICE_ATTR(VDD, S_IRUGO, adt7316_show_VDD, NULL, 0);
+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,
@@ -1213,7 +1209,7 @@ static ssize_t adt7316_show_in_temp(struct device *dev,
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_IN, buf);
 }
 
-static IIO_DEVICE_ATTR(in_temp, S_IRUGO, adt7316_show_in_temp, NULL, 0);
+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,
@@ -1225,9 +1221,9 @@ static ssize_t adt7316_show_ex_temp_AIN1(struct device *dev,
 	return adt7316_show_ad(chip, ADT7316_AD_SINGLE_CH_EX, buf);
 }
 
-static IIO_DEVICE_ATTR(ex_temp_AIN1, S_IRUGO, adt7316_show_ex_temp_AIN1,
+static IIO_DEVICE_ATTR(ex_temp_AIN1, 0444, adt7316_show_ex_temp_AIN1,
 		NULL, 0);
-static IIO_DEVICE_ATTR(ex_temp, S_IRUGO, 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,
@@ -1238,7 +1234,7 @@ static ssize_t adt7316_show_AIN2(struct device *dev,
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN2, buf);
 }
-static IIO_DEVICE_ATTR(AIN2, S_IRUGO, adt7316_show_AIN2, NULL, 0);
+static IIO_DEVICE_ATTR(AIN2, 0444, adt7316_show_AIN2, NULL, 0);
 
 static ssize_t adt7316_show_AIN3(struct device *dev,
 		struct device_attribute *attr,
@@ -1249,7 +1245,7 @@ static ssize_t adt7316_show_AIN3(struct device *dev,
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN3, buf);
 }
-static IIO_DEVICE_ATTR(AIN3, S_IRUGO, adt7316_show_AIN3, NULL, 0);
+static IIO_DEVICE_ATTR(AIN3, 0444, adt7316_show_AIN3, NULL, 0);
 
 static ssize_t adt7316_show_AIN4(struct device *dev,
 		struct device_attribute *attr,
@@ -1260,7 +1256,7 @@ static ssize_t adt7316_show_AIN4(struct device *dev,
 
 	return adt7316_show_ad(chip, ADT7516_AD_SINGLE_CH_AIN4, buf);
 }
-static IIO_DEVICE_ATTR(AIN4, S_IRUGO, adt7316_show_AIN4, NULL, 0);
+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)
@@ -1325,7 +1321,7 @@ static ssize_t adt7316_store_in_temp_offset(struct device *dev,
 			len);
 }
 
-static IIO_DEVICE_ATTR(in_temp_offset, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(in_temp_offset, 0644,
 		adt7316_show_in_temp_offset,
 		adt7316_store_in_temp_offset, 0);
 
@@ -1351,7 +1347,7 @@ static ssize_t adt7316_store_ex_temp_offset(struct device *dev,
 			len);
 }
 
-static IIO_DEVICE_ATTR(ex_temp_offset, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(ex_temp_offset, 0644,
 		adt7316_show_ex_temp_offset,
 		adt7316_store_ex_temp_offset, 0);
 
@@ -1378,7 +1374,7 @@ static ssize_t adt7316_store_in_analog_temp_offset(struct device *dev,
 			ADT7316_IN_ANALOG_TEMP_OFFSET, buf, len);
 }
 
-static IIO_DEVICE_ATTR(in_analog_temp_offset, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(in_analog_temp_offset, 0644,
 		adt7316_show_in_analog_temp_offset,
 		adt7316_store_in_analog_temp_offset, 0);
 
@@ -1405,7 +1401,7 @@ static ssize_t adt7316_store_ex_analog_temp_offset(struct device *dev,
 			ADT7316_EX_ANALOG_TEMP_OFFSET, buf, len);
 }
 
-static IIO_DEVICE_ATTR(ex_analog_temp_offset, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(ex_analog_temp_offset, 0644,
 		adt7316_show_ex_analog_temp_offset,
 		adt7316_store_ex_analog_temp_offset, 0);
 
@@ -1500,7 +1496,7 @@ static ssize_t adt7316_store_DAC_A(struct device *dev,
 	return adt7316_store_DAC(chip, 0, buf, len);
 }
 
-static IIO_DEVICE_ATTR(DAC_A, S_IRUGO | S_IWUSR, adt7316_show_DAC_A,
+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,
@@ -1524,7 +1520,7 @@ static ssize_t adt7316_store_DAC_B(struct device *dev,
 	return adt7316_store_DAC(chip, 1, buf, len);
 }
 
-static IIO_DEVICE_ATTR(DAC_B, S_IRUGO | S_IWUSR, adt7316_show_DAC_B,
+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,
@@ -1548,7 +1544,7 @@ static ssize_t adt7316_store_DAC_C(struct device *dev,
 	return adt7316_store_DAC(chip, 2, buf, len);
 }
 
-static IIO_DEVICE_ATTR(DAC_C, S_IRUGO | S_IWUSR, adt7316_show_DAC_C,
+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,
@@ -1572,7 +1568,7 @@ static ssize_t adt7316_store_DAC_D(struct device *dev,
 	return adt7316_store_DAC(chip, 3, buf, len);
 }
 
-static IIO_DEVICE_ATTR(DAC_D, S_IRUGO | S_IWUSR, adt7316_show_DAC_D,
+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,
@@ -1591,7 +1587,7 @@ static ssize_t adt7316_show_device_id(struct device *dev,
 	return sprintf(buf, "%d\n", id);
 }
 
-static IIO_DEVICE_ATTR(device_id, S_IRUGO, adt7316_show_device_id, NULL, 0);
+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,
@@ -1609,7 +1605,7 @@ static ssize_t adt7316_show_manufactorer_id(struct device *dev,
 	return sprintf(buf, "%d\n", id);
 }
 
-static IIO_DEVICE_ATTR(manufactorer_id, S_IRUGO,
+static IIO_DEVICE_ATTR(manufactorer_id, 0444,
 		adt7316_show_manufactorer_id, NULL, 0);
 
 static ssize_t adt7316_show_device_rev(struct device *dev,
@@ -1628,7 +1624,7 @@ static ssize_t adt7316_show_device_rev(struct device *dev,
 	return sprintf(buf, "%d\n", rev);
 }
 
-static IIO_DEVICE_ATTR(device_rev, S_IRUGO, adt7316_show_device_rev, NULL, 0);
+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,
@@ -1649,7 +1645,7 @@ static ssize_t adt7316_show_bus_type(struct device *dev,
 	return sprintf(buf, "i2c\n");
 }
 
-static IIO_DEVICE_ATTR(bus_type, S_IRUGO, adt7316_show_bus_type, NULL, 0);
+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,
@@ -1867,6 +1863,7 @@ static ssize_t adt7316_set_int_mask(struct device *dev,
 
 	return len;
 }
+
 static inline ssize_t adt7316_show_ad_bound(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
@@ -1972,61 +1969,61 @@ static ssize_t adt7316_set_int_enabled(struct device *dev,
 }
 
 static IIO_DEVICE_ATTR(int_mask,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_int_mask, adt7316_set_int_mask,
 		       0);
 static IIO_DEVICE_ATTR(in_temp_high_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7316_IN_TEMP_HIGH);
 static IIO_DEVICE_ATTR(in_temp_low_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7316_IN_TEMP_LOW);
 static IIO_DEVICE_ATTR(ex_temp_high_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7316_EX_TEMP_HIGH);
 static IIO_DEVICE_ATTR(ex_temp_low_value,
-		       S_IRUGO | S_IWUSR,
+		       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,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7316_EX_TEMP_HIGH);
 static IIO_DEVICE_ATTR(ex_temp_ain1_low_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7316_EX_TEMP_LOW);
 static IIO_DEVICE_ATTR(ain2_high_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN2_HIGH);
 static IIO_DEVICE_ATTR(ain2_low_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN2_LOW);
 static IIO_DEVICE_ATTR(ain3_high_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN3_HIGH);
 static IIO_DEVICE_ATTR(ain3_low_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN3_LOW);
 static IIO_DEVICE_ATTR(ain4_high_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN4_HIGH);
 static IIO_DEVICE_ATTR(ain4_low_value,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_ad_bound, adt7316_set_ad_bound,
 		       ADT7516_AIN4_LOW);
 static IIO_DEVICE_ATTR(int_enabled,
-		       S_IRUGO | S_IWUSR,
+		       0644,
 		       adt7316_show_int_enabled,
 		       adt7316_set_int_enabled, 0);
 
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index ca72af3e9d4b..a6f249e9c1e1 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -232,7 +232,7 @@ static int ad7150_write_event_config(struct iio_dev *indio_dev,
 	if (ret < 0)
 		goto error_ret;
 
-	cfg = ret & ~((0x03 << 5) | (0x1 << 7));
+	cfg = ret & ~((0x03 << 5) | BIT(7));
 
 	switch (type) {
 	case IIO_EV_TYPE_MAG_ADAPTIVE:
diff --git a/drivers/staging/iio/cdc/ad7152.c b/drivers/staging/iio/cdc/ad7152.c
index b91b50f345bd..dc6ecd824365 100644
--- a/drivers/staging/iio/cdc/ad7152.c
+++ b/drivers/staging/iio/cdc/ad7152.c
@@ -41,10 +41,10 @@
 #define AD7152_REG_CFG2			26
 
 /* Status Register Bit Designations (AD7152_REG_STATUS) */
-#define AD7152_STATUS_RDY1		(1 << 0)
-#define AD7152_STATUS_RDY2		(1 << 1)
-#define AD7152_STATUS_C1C2		(1 << 2)
-#define AD7152_STATUS_PWDN		(1 << 7)
+#define AD7152_STATUS_RDY1		BIT(0)
+#define AD7152_STATUS_RDY2		BIT(1)
+#define AD7152_STATUS_C1C2		BIT(2)
+#define AD7152_STATUS_PWDN		BIT(7)
 
 /* Setup Register Bit Designations (AD7152_REG_CHx_SETUP) */
 #define AD7152_SETUP_CAPDIFF		(1 << 5)
@@ -155,13 +155,13 @@ static ssize_t ad7152_start_gain_calib(struct device *dev,
 }
 
 static IIO_DEVICE_ATTR(in_capacitance0_calibbias_calibration,
-		       S_IWUSR, NULL, ad7152_start_offset_calib, 0);
+		       0200, NULL, ad7152_start_offset_calib, 0);
 static IIO_DEVICE_ATTR(in_capacitance1_calibbias_calibration,
-		       S_IWUSR, NULL, ad7152_start_offset_calib, 1);
+		       0200, NULL, ad7152_start_offset_calib, 1);
 static IIO_DEVICE_ATTR(in_capacitance0_calibscale_calibration,
-		       S_IWUSR, NULL, ad7152_start_gain_calib, 0);
+		       0200, NULL, ad7152_start_gain_calib, 0);
 static IIO_DEVICE_ATTR(in_capacitance1_calibscale_calibration,
-		       S_IWUSR, NULL, ad7152_start_gain_calib, 1);
+		       0200, NULL, ad7152_start_gain_calib, 1);
 
 /* Values are Update Rate (Hz), Conversion Time (ms) + 1*/
 static const unsigned char ad7152_filter_rate_table[][2] = {
@@ -244,6 +244,7 @@ static int ad7152_write_raw_samp_freq(struct device *dev, int val)
 
 	return ret;
 }
+
 static int ad7152_write_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan,
 			    int val,
@@ -441,9 +442,9 @@ static int ad7152_write_raw_get_fmt(struct iio_dev *indio_dev,
 
 static const struct iio_info ad7152_info = {
 	.attrs = &ad7152_attribute_group,
-	.read_raw = &ad7152_read_raw,
-	.write_raw = &ad7152_write_raw,
-	.write_raw_get_fmt = &ad7152_write_raw_get_fmt,
+	.read_raw = ad7152_read_raw,
+	.write_raw = ad7152_write_raw,
+	.write_raw_get_fmt = ad7152_write_raw_get_fmt,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/cdc/ad7746.c b/drivers/staging/iio/cdc/ad7746.c
index 81f8b9ee1120..cdcb4fccf3fe 100644
--- a/drivers/staging/iio/cdc/ad7746.c
+++ b/drivers/staging/iio/cdc/ad7746.c
@@ -45,10 +45,10 @@
 #define AD7746_STATUS_RDYCAP		BIT(0)
 
 /* Capacitive Channel Setup Register Bit Designations (AD7746_REG_CAP_SETUP) */
-#define AD7746_CAPSETUP_CAPEN		(1 << 7)
-#define AD7746_CAPSETUP_CIN2		(1 << 6) /* AD7746 only */
-#define AD7746_CAPSETUP_CAPDIFF		(1 << 5)
-#define AD7746_CAPSETUP_CACHOP		(1 << 0)
+#define AD7746_CAPSETUP_CAPEN		BIT(7)
+#define AD7746_CAPSETUP_CIN2		BIT(6) /* AD7746 only */
+#define AD7746_CAPSETUP_CAPDIFF		BIT(5)
+#define AD7746_CAPSETUP_CACHOP		BIT(0)
 
 /* Voltage/Temperature Setup Register Bit Designations (AD7746_REG_VT_SETUP) */
 #define AD7746_VTSETUP_VTEN		(1 << 7)
@@ -56,9 +56,9 @@
 #define AD7746_VTSETUP_VTMD_EXT_TEMP	(1 << 5)
 #define AD7746_VTSETUP_VTMD_VDD_MON	(2 << 5)
 #define AD7746_VTSETUP_VTMD_EXT_VIN	(3 << 5)
-#define AD7746_VTSETUP_EXTREF		(1 << 4)
-#define AD7746_VTSETUP_VTSHORT		(1 << 1)
-#define AD7746_VTSETUP_VTCHOP		(1 << 0)
+#define AD7746_VTSETUP_EXTREF		BIT(4)
+#define AD7746_VTSETUP_VTSHORT		BIT(1)
+#define AD7746_VTSETUP_VTCHOP		BIT(0)
 
 /* Excitation Setup Register Bit Designations (AD7746_REG_EXC_SETUP) */
 #define AD7746_EXCSETUP_CLKCTRL		BIT(7)
@@ -82,7 +82,7 @@
 #define AD7746_CONF_MODE_GAIN_CAL	(6 << 0)
 
 /* CAPDAC Register Bit Designations (AD7746_REG_CAPDACx) */
-#define AD7746_CAPDAC_DACEN		(1 << 7)
+#define AD7746_CAPDAC_DACEN		BIT(7)
 #define AD7746_CAPDAC_DACP(x)		((x) & 0x7F)
 
 /*
@@ -91,6 +91,7 @@
 
 struct ad7746_chip_info {
 	struct i2c_client *client;
+	struct mutex lock; /* protect sensor state */
 	/*
 	 * Capacitive channel digital filter setup;
 	 * conversion time/update rate setup per channel
@@ -298,11 +299,11 @@ static inline ssize_t ad7746_start_calib(struct device *dev,
 	if (!doit)
 		return 0;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&chip->lock);
 	regval |= chip->config;
 	ret = i2c_smbus_write_byte_data(chip->client, AD7746_REG_CFG, regval);
 	if (ret < 0) {
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&chip->lock);
 		return ret;
 	}
 
@@ -310,12 +311,12 @@ static inline ssize_t ad7746_start_calib(struct device *dev,
 		msleep(20);
 		ret = i2c_smbus_read_byte_data(chip->client, AD7746_REG_CFG);
 		if (ret < 0) {
-			mutex_unlock(&indio_dev->mlock);
+			mutex_unlock(&chip->lock);
 			return ret;
 		}
 	} while ((ret == regval) && timeout--);
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&chip->lock);
 
 	return len;
 }
@@ -351,15 +352,15 @@ static ssize_t ad7746_start_gain_calib(struct device *dev,
 }
 
 static IIO_DEVICE_ATTR(in_capacitance0_calibbias_calibration,
-		       S_IWUSR, NULL, ad7746_start_offset_calib, CIN1);
+		       0200, NULL, ad7746_start_offset_calib, CIN1);
 static IIO_DEVICE_ATTR(in_capacitance1_calibbias_calibration,
-		       S_IWUSR, NULL, ad7746_start_offset_calib, CIN2);
+		       0200, NULL, ad7746_start_offset_calib, CIN2);
 static IIO_DEVICE_ATTR(in_capacitance0_calibscale_calibration,
-		       S_IWUSR, NULL, ad7746_start_gain_calib, CIN1);
+		       0200, NULL, ad7746_start_gain_calib, CIN1);
 static IIO_DEVICE_ATTR(in_capacitance1_calibscale_calibration,
-		       S_IWUSR, NULL, ad7746_start_gain_calib, CIN2);
+		       0200, NULL, ad7746_start_gain_calib, CIN2);
 static IIO_DEVICE_ATTR(in_voltage0_calibscale_calibration,
-		       S_IWUSR, NULL, ad7746_start_gain_calib, VIN);
+		       0200, NULL, ad7746_start_gain_calib, VIN);
 
 static int ad7746_store_cap_filter_rate_setup(struct ad7746_chip_info *chip,
 					      int val)
@@ -426,7 +427,7 @@ static int ad7746_write_raw(struct iio_dev *indio_dev,
 	struct ad7746_chip_info *chip = iio_priv(indio_dev);
 	int ret, reg;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&chip->lock);
 
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBSCALE:
@@ -521,7 +522,7 @@ static int ad7746_write_raw(struct iio_dev *indio_dev,
 	}
 
 out:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&chip->lock);
 	return ret;
 }
 
@@ -534,7 +535,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 	int ret, delay, idx;
 	u8 regval, reg;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&chip->lock);
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
@@ -546,7 +547,7 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 
 		regval = chip->config | AD7746_CONF_MODE_SINGLE_CONV;
 		ret = i2c_smbus_write_byte_data(chip->client, AD7746_REG_CFG,
-				regval);
+						regval);
 		if (ret < 0)
 			goto out;
 
@@ -658,14 +659,14 @@ static int ad7746_read_raw(struct iio_dev *indio_dev,
 		ret = -EINVAL;
 	}
 out:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&chip->lock);
 	return ret;
 }
 
 static const struct iio_info ad7746_info = {
 	.attrs = &ad7746_attribute_group,
-	.read_raw = &ad7746_read_raw,
-	.write_raw = &ad7746_write_raw,
+	.read_raw = ad7746_read_raw,
+	.write_raw = ad7746_write_raw,
 	.driver_module = THIS_MODULE,
 };
 
@@ -686,6 +687,7 @@ static int ad7746_probe(struct i2c_client *client,
 	if (!indio_dev)
 		return -ENOMEM;
 	chip = iio_priv(indio_dev);
+	mutex_init(&chip->lock);
 	/* this is only used for device removal purposes */
 	i2c_set_clientdata(client, indio_dev);
 
diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c
index a5b2f068168d..6da46ede7ee0 100644
--- a/drivers/staging/iio/frequency/ad9832.c
+++ b/drivers/staging/iio/frequency/ad9832.c
@@ -22,6 +22,100 @@
 
 #include "ad9832.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;
+	unsigned long			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]____cacheline_aligned;
+		__be16			phase_data[2];
+		__be16			data;
+	};
+};
+
 static unsigned long ad9832_calc_freqreg(unsigned long mclk, unsigned long fout)
 {
 	unsigned long long freqreg = (u64)fout *
@@ -85,7 +179,7 @@ static ssize_t ad9832_write(struct device *dev, struct device_attribute *attr,
 	if (ret)
 		goto error_ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	switch ((u32)this_attr->address) {
 	case AD9832_FREQ0HM:
 	case AD9832_FREQ1HM:
@@ -146,7 +240,7 @@ static ssize_t ad9832_write(struct device *dev, struct device_attribute *attr,
 	default:
 		ret = -ENODEV;
 	}
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 error_ret:
 	return ret ? ret : len;
@@ -156,22 +250,22 @@ error_ret:
  * see dds.h for further information
  */
 
-static IIO_DEV_ATTR_FREQ(0, 0, S_IWUSR, NULL, ad9832_write, AD9832_FREQ0HM);
-static IIO_DEV_ATTR_FREQ(0, 1, S_IWUSR, NULL, ad9832_write, AD9832_FREQ1HM);
-static IIO_DEV_ATTR_FREQSYMBOL(0, S_IWUSR, NULL, ad9832_write, AD9832_FREQ_SYM);
+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, S_IWUSR, NULL, ad9832_write, AD9832_PHASE0H);
-static IIO_DEV_ATTR_PHASE(0, 1, S_IWUSR, NULL, ad9832_write, AD9832_PHASE1H);
-static IIO_DEV_ATTR_PHASE(0, 2, S_IWUSR, NULL, ad9832_write, AD9832_PHASE2H);
-static IIO_DEV_ATTR_PHASE(0, 3, S_IWUSR, NULL, ad9832_write, AD9832_PHASE3H);
-static IIO_DEV_ATTR_PHASESYMBOL(0, S_IWUSR, NULL,
+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, S_IWUSR, NULL,
+static IIO_DEV_ATTR_PINCONTROL_EN(0, 0200, NULL,
 				ad9832_write, AD9832_PINCTRL_EN);
-static IIO_DEV_ATTR_OUT_ENABLE(0, S_IWUSR, NULL,
+static IIO_DEV_ATTR_OUT_ENABLE(0, 0200, NULL,
 				ad9832_write, AD9832_OUTPUT_EN);
 
 static struct attribute *ad9832_attributes[] = {
@@ -242,6 +336,7 @@ static int ad9832_probe(struct spi_device *spi)
 
 	st->mclk = pdata->mclk;
 	st->spi = spi;
+	mutex_init(&st->lock);
 
 	indio_dev->dev.parent = &spi->dev;
 	indio_dev->name = spi_get_device_id(spi)->name;
diff --git a/drivers/staging/iio/frequency/ad9832.h b/drivers/staging/iio/frequency/ad9832.h
index 1b08b04482a4..39d326cc1af9 100644
--- a/drivers/staging/iio/frequency/ad9832.h
+++ b/drivers/staging/iio/frequency/ad9832.h
@@ -8,98 +8,6 @@
 #ifndef IIO_DDS_AD9832_H_
 #define IIO_DDS_AD9832_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
- * @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;
-	unsigned long			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;
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	union {
-		__be16			freq_data[4]____cacheline_aligned;
-		__be16			phase_data[2];
-		__be16			data;
-	};
-};
-
 /*
  * TODO: struct ad9832_platform_data needs to go into include/linux/iio
  */
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index 19216af1dfc9..af108e96b3ec 100644
--- a/drivers/staging/iio/frequency/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -25,6 +25,80 @@
 
 #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
+ * @reg:		supply regulator
+ * @mclk:		external master clock
+ * @control:		cached control word
+ * @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 regulator		*reg;
+	unsigned int			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 ____cacheline_aligned;
+	__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);
@@ -75,9 +149,9 @@ static ssize_t ad9834_write(struct device *dev,
 
 	ret = kstrtoul(buf, 10, &val);
 	if (ret)
-		goto error_ret;
+		return ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	switch ((u32)this_attr->address) {
 	case AD9834_REG_FREQ0:
 	case AD9834_REG_FREQ1:
@@ -135,9 +209,8 @@ static ssize_t ad9834_write(struct device *dev,
 	default:
 		ret = -ENODEV;
 	}
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
-error_ret:
 	return ret ? ret : len;
 }
 
@@ -152,7 +225,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 	int ret = 0;
 	bool is_ad9833_7 = (st->devid == ID_AD9833) || (st->devid == ID_AD9837);
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	switch ((u32)this_attr->address) {
 	case 0:
@@ -195,7 +268,7 @@ static ssize_t ad9834_store_wavetype(struct device *dev,
 		st->data = cpu_to_be16(AD9834_REG_CMD | st->control);
 		ret = spi_sync(st->spi, &st->msg);
 	}
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -346,6 +419,7 @@ static int ad9834_probe(struct spi_device *spi)
 	}
 	spi_set_drvdata(spi, indio_dev);
 	st = iio_priv(indio_dev);
+	mutex_init(&st->lock);
 	st->mclk = pdata->mclk;
 	st->spi = spi;
 	st->devid = spi_get_device_id(spi)->driver_data;
diff --git a/drivers/staging/iio/frequency/ad9834.h b/drivers/staging/iio/frequency/ad9834.h
index 40fdd5da7bd0..ae620f38eb49 100644
--- a/drivers/staging/iio/frequency/ad9834.h
+++ b/drivers/staging/iio/frequency/ad9834.h
@@ -8,67 +8,6 @@
 #ifndef IIO_DDS_AD9834_H_
 #define IIO_DDS_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
- * @reg:		supply regulator
- * @mclk:		external master clock
- * @control:		cached control word
- * @xfer:		default spi transfer
- * @msg:		default spi message
- * @freq_xfer:		tuning word spi transfer
- * @freq_msg:		tuning word spi message
- * @data:		spi transmit buffer
- * @freq_data:		tuning word spi transmit buffer
- */
-
-struct ad9834_state {
-	struct spi_device		*spi;
-	struct regulator		*reg;
-	unsigned int			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;
-
-	/*
-	 * DMA (thus cache coherency maintenance) requires the
-	 * transfer buffers to live in their own cache lines.
-	 */
-	__be16				data ____cacheline_aligned;
-	__be16				freq_data[2];
-};
-
 /*
  * TODO: struct ad7887_platform_data needs to go into include/linux/iio
  */
@@ -97,15 +36,4 @@ struct ad9834_platform_data {
 	bool			en_signbit_msb_out;
 };
 
-/**
- * ad9834_supported_device_ids:
- */
-
-enum ad9834_supported_device_ids {
-	ID_AD9833,
-	ID_AD9834,
-	ID_AD9837,
-	ID_AD9838,
-};
-
 #endif /* IIO_DDS_AD9834_H_ */
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index ab816a215eb8..967524583d8a 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -40,25 +40,20 @@ struct adis16060_state {
 
 static struct iio_dev *adis16060_iio_dev;
 
-static int adis16060_spi_write(struct iio_dev *indio_dev, u8 val)
+static int adis16060_spi_write_then_read(struct iio_dev *indio_dev,
+					 u8 conf, u16 *val)
 {
 	int ret;
 	struct adis16060_state *st = iio_priv(indio_dev);
 
 	mutex_lock(&st->buf_lock);
-	st->buf[2] = val; /* The last 8 bits clocked in are latched */
+	st->buf[2] = conf; /* The last 8 bits clocked in are latched */
 	ret = spi_write(st->us_w, st->buf, 3);
-	mutex_unlock(&st->buf_lock);
-
-	return ret;
-}
-
-static int adis16060_spi_read(struct iio_dev *indio_dev, u16 *val)
-{
-	int ret;
-	struct adis16060_state *st = iio_priv(indio_dev);
 
-	mutex_lock(&st->buf_lock);
+	if (ret < 0) {
+		mutex_unlock(&st->buf_lock);
+		return ret;
+	}
 
 	ret = spi_read(st->us_r, st->buf, 3);
 
@@ -86,17 +81,11 @@ static int adis16060_read_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		/* Take the iio_dev status lock */
-		mutex_lock(&indio_dev->mlock);
-		ret = adis16060_spi_write(indio_dev, chan->address);
+		ret = adis16060_spi_write_then_read(indio_dev,
+						    chan->address, &tval);
 		if (ret < 0)
-			goto out_unlock;
+			return ret;
 
-		ret = adis16060_spi_read(indio_dev, &tval);
-		if (ret < 0)
-			goto out_unlock;
-
-		mutex_unlock(&indio_dev->mlock);
 		*val = tval;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_OFFSET:
@@ -110,14 +99,10 @@ static int adis16060_read_raw(struct iio_dev *indio_dev,
 	}
 
 	return -EINVAL;
-
-out_unlock:
-	mutex_unlock(&indio_dev->mlock);
-	return ret;
 }
 
 static const struct iio_info adis16060_info = {
-	.read_raw = &adis16060_read_raw,
+	.read_raw = adis16060_read_raw,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index 5e96352fa4ac..3d539eeb0e26 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -98,6 +98,7 @@ struct ad5933_state {
 	struct i2c_client		*client;
 	struct regulator		*reg;
 	struct delayed_work		work;
+	struct mutex			lock; /* Protect sensor state */
 	unsigned long			mclk_hz;
 	unsigned char			ctrl_hb;
 	unsigned char			ctrl_lb;
@@ -306,9 +307,11 @@ static ssize_t ad5933_show_frequency(struct device *dev,
 		u8 d8[4];
 	} dat;
 
-	mutex_lock(&indio_dev->mlock);
+	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]);
-	mutex_unlock(&indio_dev->mlock);
+	iio_device_release_direct_mode(indio_dev);
 	if (ret < 0)
 		return ret;
 
@@ -338,19 +341,21 @@ static ssize_t ad5933_store_frequency(struct device *dev,
 	if (val > AD5933_MAX_OUTPUT_FREQ_Hz)
 		return -EINVAL;
 
-	mutex_lock(&indio_dev->mlock);
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
 	ret = ad5933_set_freq(st, this_attr->address, val);
-	mutex_unlock(&indio_dev->mlock);
+	iio_device_release_direct_mode(indio_dev);
 
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage0_freq_start, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(out_voltage0_freq_start, 0644,
 			ad5933_show_frequency,
 			ad5933_store_frequency,
 			AD5933_REG_FREQ_START);
 
-static IIO_DEVICE_ATTR(out_voltage0_freq_increment, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(out_voltage0_freq_increment, 0644,
 			ad5933_show_frequency,
 			ad5933_store_frequency,
 			AD5933_REG_FREQ_INC);
@@ -364,7 +369,7 @@ static ssize_t ad5933_show(struct device *dev,
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	int ret = 0, len = 0;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	switch ((u32)this_attr->address) {
 	case AD5933_OUT_RANGE:
 		len = sprintf(buf, "%u\n",
@@ -393,7 +398,7 @@ static ssize_t ad5933_show(struct device *dev,
 		ret = -EINVAL;
 	}
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 	return ret ? ret : len;
 }
 
@@ -415,7 +420,10 @@ static ssize_t ad5933_store(struct device *dev,
 			return ret;
 	}
 
-	mutex_lock(&indio_dev->mlock);
+	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;
@@ -465,36 +473,37 @@ static ssize_t ad5933_store(struct device *dev,
 		ret = -EINVAL;
 	}
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
+	iio_device_release_direct_mode(indio_dev);
 	return ret ? ret : len;
 }
 
-static IIO_DEVICE_ATTR(out_voltage0_scale, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(out_voltage0_scale, 0644,
 			ad5933_show,
 			ad5933_store,
 			AD5933_OUT_RANGE);
 
-static IIO_DEVICE_ATTR(out_voltage0_scale_available, S_IRUGO,
+static IIO_DEVICE_ATTR(out_voltage0_scale_available, 0444,
 			ad5933_show,
 			NULL,
 			AD5933_OUT_RANGE_AVAIL);
 
-static IIO_DEVICE_ATTR(in_voltage0_scale, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(in_voltage0_scale, 0644,
 			ad5933_show,
 			ad5933_store,
 			AD5933_IN_PGA_GAIN);
 
-static IIO_DEVICE_ATTR(in_voltage0_scale_available, S_IRUGO,
+static IIO_DEVICE_ATTR(in_voltage0_scale_available, 0444,
 			ad5933_show,
 			NULL,
 			AD5933_IN_PGA_GAIN_AVAIL);
 
-static IIO_DEVICE_ATTR(out_voltage0_freq_points, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(out_voltage0_freq_points, 0644,
 			ad5933_show,
 			ad5933_store,
 			AD5933_FREQ_POINTS);
 
-static IIO_DEVICE_ATTR(out_voltage0_settling_cycles, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(out_voltage0_settling_cycles, 0644,
 			ad5933_show,
 			ad5933_store,
 			AD5933_OUT_SETTLING_CYCLES);
@@ -532,11 +541,9 @@ static int ad5933_read_raw(struct iio_dev *indio_dev,
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
-		if (iio_buffer_enabled(indio_dev)) {
-			ret = -EBUSY;
-			goto out;
-		}
+		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;
@@ -549,7 +556,7 @@ static int ad5933_read_raw(struct iio_dev *indio_dev,
 				      2, (u8 *)&dat);
 		if (ret < 0)
 			goto out;
-		mutex_unlock(&indio_dev->mlock);
+		iio_device_release_direct_mode(indio_dev);
 		*val = sign_extend32(be16_to_cpu(dat), 13);
 
 		return IIO_VAL_INT;
@@ -561,7 +568,7 @@ static int ad5933_read_raw(struct iio_dev *indio_dev,
 
 	return -EINVAL;
 out:
-	mutex_unlock(&indio_dev->mlock);
+	iio_device_release_direct_mode(indio_dev);
 	return ret;
 }
 
@@ -657,18 +664,17 @@ static void ad5933_work(struct work_struct *work)
 	unsigned char status;
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
 	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);
-		goto out;
+		return;
 	}
 
 	ret = ad5933_i2c_read(st->client, AD5933_REG_STATUS, 1, &status);
 	if (ret)
-		goto out;
+		return;
 
 	if (status & AD5933_STAT_DATA_VALID) {
 		int scan_count = bitmap_weight(indio_dev->active_scan_mask,
@@ -678,7 +684,7 @@ static void ad5933_work(struct work_struct *work)
 				AD5933_REG_REAL_DATA : AD5933_REG_IMAG_DATA,
 				scan_count * 2, (u8 *)buf);
 		if (ret)
-			goto out;
+			return;
 
 		if (scan_count == 2) {
 			val[0] = be16_to_cpu(buf[0]);
@@ -690,7 +696,7 @@ static void ad5933_work(struct work_struct *work)
 	} else {
 		/* no data available - try again later */
 		schedule_delayed_work(&st->work, st->poll_time_jiffies);
-		goto out;
+		return;
 	}
 
 	if (status & AD5933_STAT_SWEEP_DONE) {
@@ -703,8 +709,6 @@ static void ad5933_work(struct work_struct *work)
 		ad5933_cmd(st, AD5933_CTRL_INC_FREQ);
 		schedule_delayed_work(&st->work, st->poll_time_jiffies);
 	}
-out:
-	mutex_unlock(&indio_dev->mlock);
 }
 
 static int ad5933_probe(struct i2c_client *client,
@@ -723,6 +727,8 @@ static int ad5933_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, indio_dev);
 	st->client = client;
 
+	mutex_init(&st->lock);
+
 	if (!pdata)
 		pdata = &ad5933_default_pdata;
 
diff --git a/drivers/staging/iio/light/isl29028.c b/drivers/staging/iio/light/isl29028.c
index 6bb6d37cc7d1..5375e7a81205 100644
--- a/drivers/staging/iio/light/isl29028.c
+++ b/drivers/staging/iio/light/isl29028.c
@@ -3,6 +3,7 @@
  * ISL29028 is Concurrent Ambient Light and Proximity Sensor
  *
  * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2016-2017 Brian Masney <masneyb@onstation.org>
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -63,6 +64,9 @@
 
 #define ISL29028_POWER_OFF_DELAY_MS		2000
 
+static const unsigned int isl29028_prox_sleep_time[] = {800, 400, 200, 100, 75,
+							50, 12, 0};
+
 enum isl29028_als_ir_mode {
 	ISL29028_MODE_NONE = 0,
 	ISL29028_MODE_ALS,
@@ -78,22 +82,29 @@ struct isl29028_chip {
 	enum isl29028_als_ir_mode	als_ir_mode;
 };
 
-static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
-					unsigned int sampling)
+static int isl29028_find_prox_sleep_time_index(int sampling)
 {
-	struct device *dev = regmap_get_device(chip->regmap);
-	static unsigned int prox_period[] = {800, 400, 200, 100, 75, 50, 12, 0};
 	unsigned int period = DIV_ROUND_UP(1000, sampling);
-	int sel, ret;
+	int i;
 
-	for (sel = 0; sel < ARRAY_SIZE(prox_period); ++sel) {
-		if (period >= prox_period[sel])
+	for (i = 0; i < ARRAY_SIZE(isl29028_prox_sleep_time); ++i) {
+		if (period >= isl29028_prox_sleep_time[i])
 			break;
 	}
 
+	return i;
+}
+
+static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
+					unsigned int sampling)
+{
+	struct device *dev = regmap_get_device(chip->regmap);
+	int sleep_index, ret;
+
+	sleep_index = isl29028_find_prox_sleep_time_index(sampling);
 	ret = regmap_update_bits(chip->regmap, ISL29028_REG_CONFIGURE,
 				 ISL29028_CONF_PROX_SLP_MASK,
-				 sel << ISL29028_CONF_PROX_SLP_SH);
+				 sleep_index << ISL29028_CONF_PROX_SLP_SH);
 
 	if (ret < 0) {
 		dev_err(dev, "%s(): Error %d setting the proximity sampling\n",
@@ -108,7 +119,7 @@ static int isl29028_set_proxim_sampling(struct isl29028_chip *chip,
 
 static int isl29028_enable_proximity(struct isl29028_chip *chip)
 {
-	int ret;
+	int sleep_index, ret;
 
 	ret = isl29028_set_proxim_sampling(chip, chip->prox_sampling);
 	if (ret < 0)
@@ -121,7 +132,8 @@ static int isl29028_enable_proximity(struct isl29028_chip *chip)
 		return ret;
 
 	/* Wait for conversion to be complete for first sample */
-	mdelay(DIV_ROUND_UP(1000, chip->prox_sampling));
+	sleep_index = isl29028_find_prox_sleep_time_index(chip->prox_sampling);
+	msleep(isl29028_prox_sleep_time[sleep_index]);
 
 	return 0;
 }
@@ -192,7 +204,7 @@ static int isl29028_set_als_ir_mode(struct isl29028_chip *chip,
 		return ret;
 
 	/* Need to wait for conversion time if ALS/IR mode enabled */
-	mdelay(ISL29028_CONV_TIME_MS);
+	msleep(ISL29028_CONV_TIME_MS);
 
 	chip->als_ir_mode = mode;
 
@@ -645,7 +657,8 @@ static int __maybe_unused isl29028_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops isl29028_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(isl29028_suspend, isl29028_resume)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
 	SET_RUNTIME_PM_OPS(isl29028_suspend, isl29028_resume, NULL)
 };
 
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index ea15bc1c300c..af3910bc1b4f 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -854,7 +854,7 @@ void tsl2x7x_prox_calculate(int *data, int length,
 		tmp = data[i] - statP->mean;
 		sample_sum += tmp * tmp;
 	}
-	statP->stddev = int_sqrt((long)sample_sum) / length;
+	statP->stddev = int_sqrt((long)sample_sum / length);
 }
 
 /**
@@ -1676,7 +1676,7 @@ static const struct attribute_group tsl2X7X_device_attr_group_tbl[] = {
 	},
 };
 
-static struct attribute_group tsl2X7X_event_attr_group_tbl[] = {
+static const struct attribute_group tsl2X7X_event_attr_group_tbl[] = {
 	[ALS] = {
 		.attrs = tsl2X7X_ALS_event_attrs,
 		.name = "events",
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 671dc9971610..b71fbd313778 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -6,22 +6,88 @@
  * Licensed under the GPL-2 or later.
  */
 
-#include <linux/interrupt.h>
-#include <linux/irq.h>
 #include <linux/delay.h>
-#include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/kernel.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
 #include <linux/list.h>
 #include <linux/module.h>
-
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/spi/spi.h>
 #include "meter.h"
-#include "ade7753.h"
+
+#define ADE7753_WAVEFORM   0x01
+#define ADE7753_AENERGY    0x02
+#define ADE7753_RAENERGY   0x03
+#define ADE7753_LAENERGY   0x04
+#define ADE7753_VAENERGY   0x05
+#define ADE7753_RVAENERGY  0x06
+#define ADE7753_LVAENERGY  0x07
+#define ADE7753_LVARENERGY 0x08
+#define ADE7753_MODE       0x09
+#define ADE7753_IRQEN      0x0A
+#define ADE7753_STATUS     0x0B
+#define ADE7753_RSTSTATUS  0x0C
+#define ADE7753_CH1OS      0x0D
+#define ADE7753_CH2OS      0x0E
+#define ADE7753_GAIN       0x0F
+#define ADE7753_PHCAL      0x10
+#define ADE7753_APOS       0x11
+#define ADE7753_WGAIN      0x12
+#define ADE7753_WDIV       0x13
+#define ADE7753_CFNUM      0x14
+#define ADE7753_CFDEN      0x15
+#define ADE7753_IRMS       0x16
+#define ADE7753_VRMS       0x17
+#define ADE7753_IRMSOS     0x18
+#define ADE7753_VRMSOS     0x19
+#define ADE7753_VAGAIN     0x1A
+#define ADE7753_VADIV      0x1B
+#define ADE7753_LINECYC    0x1C
+#define ADE7753_ZXTOUT     0x1D
+#define ADE7753_SAGCYC     0x1E
+#define ADE7753_SAGLVL     0x1F
+#define ADE7753_IPKLVL     0x20
+#define ADE7753_VPKLVL     0x21
+#define ADE7753_IPEAK      0x22
+#define ADE7753_RSTIPEAK   0x23
+#define ADE7753_VPEAK      0x24
+#define ADE7753_RSTVPEAK   0x25
+#define ADE7753_TEMP       0x26
+#define ADE7753_PERIOD     0x27
+#define ADE7753_TMODE      0x3D
+#define ADE7753_CHKSUM     0x3E
+#define ADE7753_DIEREV     0x3F
+
+#define ADE7753_READ_REG(a)    a
+#define ADE7753_WRITE_REG(a) ((a) | 0x80)
+
+#define ADE7753_MAX_TX    4
+#define ADE7753_MAX_RX    4
+#define ADE7753_STARTUP_DELAY 1000
+
+#define ADE7753_SPI_SLOW    (u32)(300 * 1000)
+#define ADE7753_SPI_BURST   (u32)(1000 * 1000)
+#define ADE7753_SPI_FAST    (u32)(2000 * 1000)
+
+/**
+ * struct ade7753_state - device instance specific data
+ * @us:         actual spi_device
+ * @tx:         transmit buffer
+ * @rx:         receive buffer
+ * @buf_lock:       mutex to protect tx and rx
+ **/
+struct ade7753_state {
+	struct spi_device   *us;
+	struct mutex        buf_lock;
+	u8          tx[ADE7753_MAX_TX] ____cacheline_aligned;
+	u8          rx[ADE7753_MAX_RX];
+};
 
 static int ade7753_spi_write_reg_8(struct device *dev,
 				   u8 reg_address,
diff --git a/drivers/staging/iio/meter/ade7753.h b/drivers/staging/iio/meter/ade7753.h
deleted file mode 100644
index bfe749156bce..000000000000
--- a/drivers/staging/iio/meter/ade7753.h
+++ /dev/null
@@ -1,72 +0,0 @@
-#ifndef _ADE7753_H
-#define _ADE7753_H
-
-#define ADE7753_WAVEFORM   0x01
-#define ADE7753_AENERGY    0x02
-#define ADE7753_RAENERGY   0x03
-#define ADE7753_LAENERGY   0x04
-#define ADE7753_VAENERGY   0x05
-#define ADE7753_RVAENERGY  0x06
-#define ADE7753_LVAENERGY  0x07
-#define ADE7753_LVARENERGY 0x08
-#define ADE7753_MODE       0x09
-#define ADE7753_IRQEN      0x0A
-#define ADE7753_STATUS     0x0B
-#define ADE7753_RSTSTATUS  0x0C
-#define ADE7753_CH1OS      0x0D
-#define ADE7753_CH2OS      0x0E
-#define ADE7753_GAIN       0x0F
-#define ADE7753_PHCAL      0x10
-#define ADE7753_APOS       0x11
-#define ADE7753_WGAIN      0x12
-#define ADE7753_WDIV       0x13
-#define ADE7753_CFNUM      0x14
-#define ADE7753_CFDEN      0x15
-#define ADE7753_IRMS       0x16
-#define ADE7753_VRMS       0x17
-#define ADE7753_IRMSOS     0x18
-#define ADE7753_VRMSOS     0x19
-#define ADE7753_VAGAIN     0x1A
-#define ADE7753_VADIV      0x1B
-#define ADE7753_LINECYC    0x1C
-#define ADE7753_ZXTOUT     0x1D
-#define ADE7753_SAGCYC     0x1E
-#define ADE7753_SAGLVL     0x1F
-#define ADE7753_IPKLVL     0x20
-#define ADE7753_VPKLVL     0x21
-#define ADE7753_IPEAK      0x22
-#define ADE7753_RSTIPEAK   0x23
-#define ADE7753_VPEAK      0x24
-#define ADE7753_RSTVPEAK   0x25
-#define ADE7753_TEMP       0x26
-#define ADE7753_PERIOD     0x27
-#define ADE7753_TMODE      0x3D
-#define ADE7753_CHKSUM     0x3E
-#define ADE7753_DIEREV     0x3F
-
-#define ADE7753_READ_REG(a)    a
-#define ADE7753_WRITE_REG(a) ((a) | 0x80)
-
-#define ADE7753_MAX_TX    4
-#define ADE7753_MAX_RX    4
-#define ADE7753_STARTUP_DELAY 1000
-
-#define ADE7753_SPI_SLOW	(u32)(300 * 1000)
-#define ADE7753_SPI_BURST	(u32)(1000 * 1000)
-#define ADE7753_SPI_FAST	(u32)(2000 * 1000)
-
-/**
- * struct ade7753_state - device instance specific data
- * @us:			actual spi_device
- * @tx:			transmit buffer
- * @rx:			receive buffer
- * @buf_lock:		mutex to protect tx and rx
- **/
-struct ade7753_state {
-	struct spi_device	*us;
-	struct mutex		buf_lock;
-	u8			tx[ADE7753_MAX_TX] ____cacheline_aligned;
-	u8			rx[ADE7753_MAX_RX];
-};
-
-#endif
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index 024463a11c47..32dc50341746 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -6,34 +6,126 @@
  * Licensed under the GPL-2 or later.
  */
 
-#include <linux/interrupt.h>
-#include <linux/irq.h>
 #include <linux/delay.h>
-#include <linux/mutex.h>
 #include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/spi/spi.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 "ade7754.h"
 
-static int ade7754_spi_write_reg_8(struct device *dev, u8 reg_address, u8 val)
+#define ADE7754_AENERGY   0x01
+#define ADE7754_RAENERGY  0x02
+#define ADE7754_LAENERGY  0x03
+#define ADE7754_VAENERGY  0x04
+#define ADE7754_RVAENERGY 0x05
+#define ADE7754_LVAENERGY 0x06
+#define ADE7754_PERIOD    0x07
+#define ADE7754_TEMP      0x08
+#define ADE7754_WFORM     0x09
+#define ADE7754_OPMODE    0x0A
+#define ADE7754_MMODE     0x0B
+#define ADE7754_WAVMODE   0x0C
+#define ADE7754_WATMODE   0x0D
+#define ADE7754_VAMODE    0x0E
+#define ADE7754_IRQEN     0x0F
+#define ADE7754_STATUS    0x10
+#define ADE7754_RSTATUS   0x11
+#define ADE7754_ZXTOUT    0x12
+#define ADE7754_LINCYC    0x13
+#define ADE7754_SAGCYC    0x14
+#define ADE7754_SAGLVL    0x15
+#define ADE7754_VPEAK     0x16
+#define ADE7754_IPEAK     0x17
+#define ADE7754_GAIN      0x18
+#define ADE7754_AWG       0x19
+#define ADE7754_BWG       0x1A
+#define ADE7754_CWG       0x1B
+#define ADE7754_AVAG      0x1C
+#define ADE7754_BVAG      0x1D
+#define ADE7754_CVAG      0x1E
+#define ADE7754_APHCAL    0x1F
+#define ADE7754_BPHCAL    0x20
+#define ADE7754_CPHCAL    0x21
+#define ADE7754_AAPOS     0x22
+#define ADE7754_BAPOS     0x23
+#define ADE7754_CAPOS     0x24
+#define ADE7754_CFNUM     0x25
+#define ADE7754_CFDEN     0x26
+#define ADE7754_WDIV      0x27
+#define ADE7754_VADIV     0x28
+#define ADE7754_AIRMS     0x29
+#define ADE7754_BIRMS     0x2A
+#define ADE7754_CIRMS     0x2B
+#define ADE7754_AVRMS     0x2C
+#define ADE7754_BVRMS     0x2D
+#define ADE7754_CVRMS     0x2E
+#define ADE7754_AIRMSOS   0x2F
+#define ADE7754_BIRMSOS   0x30
+#define ADE7754_CIRMSOS   0x31
+#define ADE7754_AVRMSOS   0x32
+#define ADE7754_BVRMSOS   0x33
+#define ADE7754_CVRMSOS   0x34
+#define ADE7754_AAPGAIN   0x35
+#define ADE7754_BAPGAIN   0x36
+#define ADE7754_CAPGAIN   0x37
+#define ADE7754_AVGAIN    0x38
+#define ADE7754_BVGAIN    0x39
+#define ADE7754_CVGAIN    0x3A
+#define ADE7754_CHKSUM    0x3E
+#define ADE7754_VERSION   0x3F
+
+#define ADE7754_READ_REG(a)    a
+#define ADE7754_WRITE_REG(a) ((a) | 0x80)
+
+#define ADE7754_MAX_TX    4
+#define ADE7754_MAX_RX    4
+#define ADE7754_STARTUP_DELAY 1000
+
+#define ADE7754_SPI_SLOW	(u32)(300 * 1000)
+#define ADE7754_SPI_BURST	(u32)(1000 * 1000)
+#define ADE7754_SPI_FAST	(u32)(2000 * 1000)
+
+/**
+ * struct ade7754_state - device instance specific data
+ * @us:			actual spi_device
+ * @buf_lock:		mutex to protect tx, rx and write frequency
+ * @tx:			transmit buffer
+ * @rx:			receive buffer
+ **/
+struct ade7754_state {
+	struct spi_device	*us;
+	struct mutex		buf_lock;
+	u8			tx[ADE7754_MAX_TX] ____cacheline_aligned;
+	u8			rx[ADE7754_MAX_RX];
+};
+
+/* Unlocked version of ade7754_spi_write_reg_8 function */
+static int __ade7754_spi_write_reg_8(struct device *dev, u8 reg_address, u8 val)
 {
-	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ade7754_state *st = iio_priv(indio_dev);
 
-	mutex_lock(&st->buf_lock);
 	st->tx[0] = ADE7754_WRITE_REG(reg_address);
 	st->tx[1] = val;
+	return spi_write(st->us, st->tx, 2);
+}
+
+static int ade7754_spi_write_reg_8(struct device *dev, u8 reg_address, u8 val)
+{
+	int ret;
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct ade7754_state *st = iio_priv(indio_dev);
 
-	ret = spi_write(st->us, st->tx, 2);
+	mutex_lock(&st->buf_lock);
+	ret = __ade7754_spi_write_reg_8(dev, reg_address, val);
 	mutex_unlock(&st->buf_lock);
 
 	return ret;
@@ -349,9 +441,7 @@ static int ade7754_set_irq(struct device *dev, bool enable)
 	else
 		irqen &= ~BIT(14);
 
-	ret = ade7754_spi_write_reg_16(dev, ADE7754_IRQEN, irqen);
-
-	return ret;
+	return ade7754_spi_write_reg_16(dev, ADE7754_IRQEN, irqen);
 }
 
 /* Power down the device */
@@ -430,7 +520,7 @@ static ssize_t ade7754_write_frequency(struct device *dev,
 	if (!val)
 		return -EINVAL;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->buf_lock);
 
 	t = 26000 / val;
 	if (t > 0)
@@ -448,10 +538,10 @@ static ssize_t ade7754_write_frequency(struct device *dev,
 	reg &= ~(3 << 3);
 	reg |= t << 3;
 
-	ret = ade7754_spi_write_reg_8(dev, ADE7754_WAVMODE, reg);
+	ret = __ade7754_spi_write_reg_8(dev, ADE7754_WAVMODE, reg);
 
 out:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->buf_lock);
 
 	return ret ? ret : len;
 }
diff --git a/drivers/staging/iio/meter/ade7754.h b/drivers/staging/iio/meter/ade7754.h
deleted file mode 100644
index 28f71c2cde0c..000000000000
--- a/drivers/staging/iio/meter/ade7754.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#ifndef _ADE7754_H
-#define _ADE7754_H
-
-#define ADE7754_AENERGY   0x01
-#define ADE7754_RAENERGY  0x02
-#define ADE7754_LAENERGY  0x03
-#define ADE7754_VAENERGY  0x04
-#define ADE7754_RVAENERGY 0x05
-#define ADE7754_LVAENERGY 0x06
-#define ADE7754_PERIOD    0x07
-#define ADE7754_TEMP      0x08
-#define ADE7754_WFORM     0x09
-#define ADE7754_OPMODE    0x0A
-#define ADE7754_MMODE     0x0B
-#define ADE7754_WAVMODE   0x0C
-#define ADE7754_WATMODE   0x0D
-#define ADE7754_VAMODE    0x0E
-#define ADE7754_IRQEN     0x0F
-#define ADE7754_STATUS    0x10
-#define ADE7754_RSTATUS   0x11
-#define ADE7754_ZXTOUT    0x12
-#define ADE7754_LINCYC    0x13
-#define ADE7754_SAGCYC    0x14
-#define ADE7754_SAGLVL    0x15
-#define ADE7754_VPEAK     0x16
-#define ADE7754_IPEAK     0x17
-#define ADE7754_GAIN      0x18
-#define ADE7754_AWG       0x19
-#define ADE7754_BWG       0x1A
-#define ADE7754_CWG       0x1B
-#define ADE7754_AVAG      0x1C
-#define ADE7754_BVAG      0x1D
-#define ADE7754_CVAG      0x1E
-#define ADE7754_APHCAL    0x1F
-#define ADE7754_BPHCAL    0x20
-#define ADE7754_CPHCAL    0x21
-#define ADE7754_AAPOS     0x22
-#define ADE7754_BAPOS     0x23
-#define ADE7754_CAPOS     0x24
-#define ADE7754_CFNUM     0x25
-#define ADE7754_CFDEN     0x26
-#define ADE7754_WDIV      0x27
-#define ADE7754_VADIV     0x28
-#define ADE7754_AIRMS     0x29
-#define ADE7754_BIRMS     0x2A
-#define ADE7754_CIRMS     0x2B
-#define ADE7754_AVRMS     0x2C
-#define ADE7754_BVRMS     0x2D
-#define ADE7754_CVRMS     0x2E
-#define ADE7754_AIRMSOS   0x2F
-#define ADE7754_BIRMSOS   0x30
-#define ADE7754_CIRMSOS   0x31
-#define ADE7754_AVRMSOS   0x32
-#define ADE7754_BVRMSOS   0x33
-#define ADE7754_CVRMSOS   0x34
-#define ADE7754_AAPGAIN   0x35
-#define ADE7754_BAPGAIN   0x36
-#define ADE7754_CAPGAIN   0x37
-#define ADE7754_AVGAIN    0x38
-#define ADE7754_BVGAIN    0x39
-#define ADE7754_CVGAIN    0x3A
-#define ADE7754_CHKSUM    0x3E
-#define ADE7754_VERSION   0x3F
-
-#define ADE7754_READ_REG(a)    a
-#define ADE7754_WRITE_REG(a) ((a) | 0x80)
-
-#define ADE7754_MAX_TX    4
-#define ADE7754_MAX_RX    4
-#define ADE7754_STARTUP_DELAY 1000
-
-#define ADE7754_SPI_SLOW	(u32)(300 * 1000)
-#define ADE7754_SPI_BURST	(u32)(1000 * 1000)
-#define ADE7754_SPI_FAST	(u32)(2000 * 1000)
-
-/**
- * struct ade7754_state - device instance specific data
- * @us:			actual spi_device
- * @buf_lock:		mutex to protect tx and rx
- * @tx:			transmit buffer
- * @rx:			receive buffer
- **/
-struct ade7754_state {
-	struct spi_device	*us;
-	struct mutex		buf_lock;
-	u8			tx[ADE7754_MAX_TX] ____cacheline_aligned;
-	u8			rx[ADE7754_MAX_RX];
-};
-
-#endif
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index 944ee3401029..1691760339da 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -21,7 +21,55 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include "meter.h"
-#include "ade7759.h"
+
+#define ADE7759_WAVEFORM  0x01
+#define ADE7759_AENERGY   0x02
+#define ADE7759_RSTENERGY 0x03
+#define ADE7759_STATUS    0x04
+#define ADE7759_RSTSTATUS 0x05
+#define ADE7759_MODE      0x06
+#define ADE7759_CFDEN     0x07
+#define ADE7759_CH1OS     0x08
+#define ADE7759_CH2OS     0x09
+#define ADE7759_GAIN      0x0A
+#define ADE7759_APGAIN    0x0B
+#define ADE7759_PHCAL     0x0C
+#define ADE7759_APOS      0x0D
+#define ADE7759_ZXTOUT    0x0E
+#define ADE7759_SAGCYC    0x0F
+#define ADE7759_IRQEN     0x10
+#define ADE7759_SAGLVL    0x11
+#define ADE7759_TEMP      0x12
+#define ADE7759_LINECYC   0x13
+#define ADE7759_LENERGY   0x14
+#define ADE7759_CFNUM     0x15
+#define ADE7759_CHKSUM    0x1E
+#define ADE7759_DIEREV    0x1F
+
+#define ADE7759_READ_REG(a)    a
+#define ADE7759_WRITE_REG(a) ((a) | 0x80)
+
+#define ADE7759_MAX_TX    6
+#define ADE7759_MAX_RX    6
+#define ADE7759_STARTUP_DELAY 1000
+
+#define ADE7759_SPI_SLOW	(u32)(300 * 1000)
+#define ADE7759_SPI_BURST	(u32)(1000 * 1000)
+#define ADE7759_SPI_FAST	(u32)(2000 * 1000)
+
+/**
+ * struct ade7759_state - device instance specific data
+ * @us:			actual spi_device
+ * @buf_lock:		mutex to protect tx and rx
+ * @tx:			transmit buffer
+ * @rx:			receive buffer
+ **/
+struct ade7759_state {
+	struct spi_device	*us;
+	struct mutex		buf_lock;
+	u8			tx[ADE7759_MAX_TX] ____cacheline_aligned;
+	u8			rx[ADE7759_MAX_RX];
+};
 
 static int ade7759_spi_write_reg_8(struct device *dev,
 		u8 reg_address,
@@ -231,49 +279,49 @@ static int ade7759_reset(struct device *dev)
 }
 
 static IIO_DEV_ATTR_AENERGY(ade7759_read_40bit, ADE7759_AENERGY);
-static IIO_DEV_ATTR_CFDEN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFDEN(0644,
 		ade7759_read_16bit,
 		ade7759_write_16bit,
 		ADE7759_CFDEN);
-static IIO_DEV_ATTR_CFNUM(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CFNUM(0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_CFNUM);
 static IIO_DEV_ATTR_CHKSUM(ade7759_read_8bit, ADE7759_CHKSUM);
-static IIO_DEV_ATTR_PHCAL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_PHCAL(0644,
 		ade7759_read_16bit,
 		ade7759_write_16bit,
 		ADE7759_PHCAL);
-static IIO_DEV_ATTR_APOS(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_APOS(0644,
 		ade7759_read_16bit,
 		ade7759_write_16bit,
 		ADE7759_APOS);
-static IIO_DEV_ATTR_SAGCYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAGCYC(0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_SAGCYC);
-static IIO_DEV_ATTR_SAGLVL(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAGLVL(0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_SAGLVL);
-static IIO_DEV_ATTR_LINECYC(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_LINECYC(0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_LINECYC);
 static IIO_DEV_ATTR_LENERGY(ade7759_read_40bit, ADE7759_LENERGY);
-static IIO_DEV_ATTR_PGA_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_PGA_GAIN(0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_GAIN);
-static IIO_DEV_ATTR_ACTIVE_POWER_GAIN(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_ACTIVE_POWER_GAIN(0644,
 		ade7759_read_16bit,
 		ade7759_write_16bit,
 		ADE7759_APGAIN);
-static IIO_DEV_ATTR_CH_OFF(1, S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CH_OFF(1, 0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_CH1OS);
-static IIO_DEV_ATTR_CH_OFF(2, S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_CH_OFF(2, 0644,
 		ade7759_read_8bit,
 		ade7759_write_8bit,
 		ADE7759_CH2OS);
@@ -410,7 +458,7 @@ static IIO_DEV_ATTR_TEMP_RAW(ade7759_read_8bit);
 static IIO_CONST_ATTR(in_temp_offset, "70 C");
 static IIO_CONST_ATTR(in_temp_scale, "1 C");
 
-static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+static IIO_DEV_ATTR_SAMP_FREQ(0644,
 		ade7759_read_frequency,
 		ade7759_write_frequency);
 
diff --git a/drivers/staging/iio/meter/ade7759.h b/drivers/staging/iio/meter/ade7759.h
deleted file mode 100644
index f0716d2fdf8e..000000000000
--- a/drivers/staging/iio/meter/ade7759.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef _ADE7759_H
-#define _ADE7759_H
-
-#define ADE7759_WAVEFORM  0x01
-#define ADE7759_AENERGY   0x02
-#define ADE7759_RSTENERGY 0x03
-#define ADE7759_STATUS    0x04
-#define ADE7759_RSTSTATUS 0x05
-#define ADE7759_MODE      0x06
-#define ADE7759_CFDEN     0x07
-#define ADE7759_CH1OS     0x08
-#define ADE7759_CH2OS     0x09
-#define ADE7759_GAIN      0x0A
-#define ADE7759_APGAIN    0x0B
-#define ADE7759_PHCAL     0x0C
-#define ADE7759_APOS      0x0D
-#define ADE7759_ZXTOUT    0x0E
-#define ADE7759_SAGCYC    0x0F
-#define ADE7759_IRQEN     0x10
-#define ADE7759_SAGLVL    0x11
-#define ADE7759_TEMP      0x12
-#define ADE7759_LINECYC   0x13
-#define ADE7759_LENERGY   0x14
-#define ADE7759_CFNUM     0x15
-#define ADE7759_CHKSUM    0x1E
-#define ADE7759_DIEREV    0x1F
-
-#define ADE7759_READ_REG(a)    a
-#define ADE7759_WRITE_REG(a) ((a) | 0x80)
-
-#define ADE7759_MAX_TX    6
-#define ADE7759_MAX_RX    6
-#define ADE7759_STARTUP_DELAY 1000
-
-#define ADE7759_SPI_SLOW	(u32)(300 * 1000)
-#define ADE7759_SPI_BURST	(u32)(1000 * 1000)
-#define ADE7759_SPI_FAST	(u32)(2000 * 1000)
-
-/**
- * struct ade7759_state - device instance specific data
- * @us:			actual spi_device
- * @buf_lock:		mutex to protect tx and rx
- * @tx:			transmit buffer
- * @rx:			receive buffer
- **/
-struct ade7759_state {
-	struct spi_device	*us;
-	struct mutex		buf_lock;
-	u8			tx[ADE7759_MAX_TX] ____cacheline_aligned;
-	u8			rx[ADE7759_MAX_RX];
-};
-
-#endif
diff --git a/drivers/staging/iio/meter/ade7854.c b/drivers/staging/iio/meter/ade7854.c
index e8007f0c5186..c6cffc11b0ba 100644
--- a/drivers/staging/iio/meter/ade7854.c
+++ b/drivers/staging/iio/meter/ade7854.c
@@ -426,9 +426,7 @@ static int ade7854_set_irq(struct device *dev, bool enable)
 	else
 		irqen &= ~BIT(17);
 
-	ret = st->write_reg_32(dev, ADE7854_MASK0, irqen);
-
-	return ret;
+	return st->write_reg_32(dev, ADE7854_MASK0, irqen);
 }
 
 static int ade7854_initial_setup(struct iio_dev *indio_dev)
diff --git a/drivers/staging/iio/meter/meter.h b/drivers/staging/iio/meter/meter.h
index dfba510f29be..0e37f23853f1 100644
--- a/drivers/staging/iio/meter/meter.h
+++ b/drivers/staging/iio/meter/meter.h
@@ -81,94 +81,94 @@
 	IIO_DEVICE_ATTR(reactive_power_c_gain, _mode, _show, _store, _addr)
 
 #define IIO_DEV_ATTR_CURRENT_A(_show, _addr)			\
-	IIO_DEVICE_ATTR(current_a, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(current_a, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CURRENT_B(_show, _addr)			\
-	IIO_DEVICE_ATTR(current_b, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(current_b, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CURRENT_C(_show, _addr)			\
-	IIO_DEVICE_ATTR(current_c, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(current_c, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_VOLT_A(_show, _addr)			\
-	IIO_DEVICE_ATTR(volt_a, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(volt_a, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_VOLT_B(_show, _addr)			\
-	IIO_DEVICE_ATTR(volt_b, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(volt_b, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_VOLT_C(_show, _addr)			\
-	IIO_DEVICE_ATTR(volt_c, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(volt_c, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_AENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(aenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(aenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_LENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(lenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(lenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_RAENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(raenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(raenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_LAENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(laenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(laenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_VAENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(vaenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(vaenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_LVAENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(lvaenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(lvaenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_RVAENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(rvaenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(rvaenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_LVARENERGY(_show, _addr)			\
-	IIO_DEVICE_ATTR(lvarenergy, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(lvarenergy, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CHKSUM(_show, _addr)                       \
-	IIO_DEVICE_ATTR(chksum, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(chksum, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_ANGLE0(_show, _addr)                       \
-	IIO_DEVICE_ATTR(angle0, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(angle0, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_ANGLE1(_show, _addr)                       \
-	IIO_DEVICE_ATTR(angle1, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(angle1, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_ANGLE2(_show, _addr)                       \
-	IIO_DEVICE_ATTR(angle2, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(angle2, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_AWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(awatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(awatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_BWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(bwatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(bwatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(cwatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(cwatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_AFWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(afwatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(afwatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_BFWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(bfwatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(bfwatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CFWATTHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(cfwatthr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(cfwatthr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_AVARHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(avarhr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(avarhr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_BVARHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(bvarhr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(bvarhr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CVARHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(cvarhr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(cvarhr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_AVAHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(avahr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(avahr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_BVAHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(bvahr, S_IRUGO, _show, NULL, _addr)
+	IIO_DEVICE_ATTR(bvahr, 0444, _show, NULL, _addr)
 
 #define IIO_DEV_ATTR_CVAHR(_show, _addr)			\
-	IIO_DEVICE_ATTR(cvahr, S_IRUGO, _show, NULL, _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)
diff --git a/drivers/staging/iio/resolver/ad2s1200.c b/drivers/staging/iio/resolver/ad2s1200.c
index 82b2d88ca942..a37e199225f4 100644
--- a/drivers/staging/iio/resolver/ad2s1200.c
+++ b/drivers/staging/iio/resolver/ad2s1200.c
@@ -97,7 +97,7 @@ static const struct iio_chan_spec ad2s1200_channels[] = {
 };
 
 static const struct iio_info ad2s1200_info = {
-	.read_raw = &ad2s1200_read_raw,
+	.read_raw = ad2s1200_read_raw,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index 6b992634f009..a6a8393d6664 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -490,8 +490,8 @@ static int ad2s1210_read_raw(struct iio_dev *indio_dev,
 		ad2s1210_set_mode(MOD_VEL, st);
 		break;
 	default:
-	       ret = -EINVAL;
-	       break;
+		ret = -EINVAL;
+		break;
 	}
 	if (ret < 0)
 		goto error_ret;
@@ -531,36 +531,36 @@ error_ret:
 	return ret;
 }
 
-static IIO_DEVICE_ATTR(fclkin, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(fclkin, 0644,
 		       ad2s1210_show_fclkin, ad2s1210_store_fclkin, 0);
-static IIO_DEVICE_ATTR(fexcit, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(fexcit, 0644,
 		       ad2s1210_show_fexcit,	ad2s1210_store_fexcit, 0);
-static IIO_DEVICE_ATTR(control, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(control, 0644,
 		       ad2s1210_show_control, ad2s1210_store_control, 0);
-static IIO_DEVICE_ATTR(bits, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(bits, 0644,
 		       ad2s1210_show_resolution, ad2s1210_store_resolution, 0);
-static IIO_DEVICE_ATTR(fault, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(fault, 0644,
 		       ad2s1210_show_fault, ad2s1210_clear_fault, 0);
 
-static IIO_DEVICE_ATTR(los_thrd, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(los_thrd, 0644,
 		       ad2s1210_show_reg, ad2s1210_store_reg,
 		       AD2S1210_REG_LOS_THRD);
-static IIO_DEVICE_ATTR(dos_ovr_thrd, S_IRUGO | S_IWUSR,
+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, S_IRUGO | S_IWUSR,
+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, S_IRUGO | S_IWUSR,
+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, S_IRUGO | S_IWUSR,
+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, S_IRUGO | S_IWUSR,
+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, S_IRUGO | S_IWUSR,
+static IIO_DEVICE_ATTR(lot_low_thrd, 0644,
 		       ad2s1210_show_reg, ad2s1210_store_reg,
 		       AD2S1210_REG_LOT_LOW_THRD);
 
diff --git a/drivers/staging/iio/resolver/ad2s90.c b/drivers/staging/iio/resolver/ad2s90.c
index 5b1c0db33e7f..b2270908f26f 100644
--- a/drivers/staging/iio/resolver/ad2s90.c
+++ b/drivers/staging/iio/resolver/ad2s90.c
@@ -47,7 +47,7 @@ error_ret:
 }
 
 static const struct iio_info ad2s90_info = {
-	.read_raw = &ad2s90_read_raw,
+	.read_raw = ad2s90_read_raw,
 	.driver_module = THIS_MODULE,
 };
 
diff --git a/drivers/staging/ks7010/TODO b/drivers/staging/ks7010/TODO
index 2938d35be5bb..d393ca58e231 100644
--- a/drivers/staging/ks7010/TODO
+++ b/drivers/staging/ks7010/TODO
@@ -18,8 +18,6 @@ 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
-- No '#if 0/1' removal unless the surrounding code is understood and removal is
-  really OK. There might be some hints hidden there.
 
 Now the TODOs:
 
@@ -29,6 +27,8 @@ Now the TODOs:
 - 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
+- replace custom Michael MIC implementation with the kernel
+  implementation. This task is only required for a *clean* WEXT interface.
 
 Please send any patches to:
 Greg Kroah-Hartman <gregkh@linuxfoundation.org>
diff --git a/drivers/staging/ks7010/eap_packet.h b/drivers/staging/ks7010/eap_packet.h
index df7f760e4110..b2d25ef1cd6b 100644
--- a/drivers/staging/ks7010/eap_packet.h
+++ b/drivers/staging/ks7010/eap_packet.h
@@ -9,6 +9,8 @@
 #define ETH_ALEN 6
 #endif
 
+#define ETHER_HDR_SIZE 20
+
 struct ether_hdr {
 	unsigned char h_dest[ETH_ALEN];	/* destination eth addr */
 	unsigned char h_source[ETH_ALEN];	/* source ether addr    */
@@ -58,12 +60,15 @@ struct ieee802_1x_eapol_key {
 	 * encrypt the Key field; 64-bit NTP timestamp MAY be used here
 	 */
 	unsigned char replay_counter[IEEE8021X_REPLAY_COUNTER_LEN];
-	unsigned char key_iv[IEEE8021X_KEY_IV_LEN];	/* cryptographically random number */
+	unsigned char key_iv[IEEE8021X_KEY_IV_LEN]; /* cryptographically random
+						     * number
+						     */
 	unsigned char key_index;	/*
 					 * key flag in the most significant bit:
 					 * 0 = broadcast (default key),
-					 * 1 = unicast (key mapping key); key index is in the
-					 * 7 least significant bits
+					 * 1 = unicast (key mapping key);
+					 * key index is in the 7 least
+					 * significant bits
 					 */
 	/*
 	 * HMAC-MD5 message integrity check computed with MS-MPPE-Send-Key as
diff --git a/drivers/staging/ks7010/ks7010_sdio.c b/drivers/staging/ks7010/ks7010_sdio.c
index 6f9f746a3a61..c325f4846209 100644
--- a/drivers/staging/ks7010/ks7010_sdio.c
+++ b/drivers/staging/ks7010/ks7010_sdio.c
@@ -32,8 +32,6 @@ static const struct sdio_device_id ks7010_sdio_ids[] = {
 };
 MODULE_DEVICE_TABLE(sdio, ks7010_sdio_ids);
 
-/* macro */
-
 #define inc_txqhead(priv) \
 	(priv->tx_dev.qhead = (priv->tx_dev.qhead + 1) % TX_DEVICE_BUFF_SIZE)
 #define inc_txqtail(priv) \
@@ -48,48 +46,51 @@ MODULE_DEVICE_TABLE(sdio, ks7010_sdio_ids);
 #define cnt_rxqbody(priv) \
 	(((priv->rx_dev.qtail + RX_DEVICE_BUFF_SIZE) - (priv->rx_dev.qhead)) % RX_DEVICE_BUFF_SIZE)
 
+/* Read single byte from device address into byte (CMD52) */
+static int ks7010_sdio_readb(struct ks_wlan_private *priv, unsigned int address,
+			     unsigned char *byte)
+{
+	struct sdio_func *func = priv->ks_sdio_card->func;
+	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, unsigned int address,
 			    unsigned char *buffer, int length)
 {
-	struct ks_sdio_card *card;
-	int rc;
+	struct sdio_func *func = priv->ks_sdio_card->func;
 
-	card = priv->ks_wlan_hw.sdio_card;
+	return sdio_memcpy_fromio(func, buffer, address, length);
+}
 
-	if (length == 1)	/* CMD52 */
-		*buffer = sdio_readb(card->func, address, &rc);
-	else	/* CMD53 multi-block transfer */
-		rc = sdio_memcpy_fromio(card->func, buffer, address, length);
+/* Write single byte to device address (CMD52) */
+static int ks7010_sdio_writeb(struct ks_wlan_private *priv,
+			      unsigned int address, unsigned char byte)
+{
+	struct sdio_func *func = priv->ks_sdio_card->func;
+	int ret;
 
-	if (rc != 0)
-		DPRINTK(1, "sdio error=%d size=%d\n", rc, length);
+	sdio_writeb(func, byte, address, &ret);
 
-	return rc;
+	return ret;
 }
 
+/* Write length bytes to device address from buffer (CMD53) */
 static int ks7010_sdio_write(struct ks_wlan_private *priv, unsigned int address,
 			     unsigned char *buffer, int length)
 {
-	struct ks_sdio_card *card;
-	int rc;
-
-	card = priv->ks_wlan_hw.sdio_card;
-
-	if (length == 1)	/* CMD52 */
-		sdio_writeb(card->func, *buffer, address, &rc);
-	else	/* CMD53 */
-		rc = sdio_memcpy_toio(card->func, address, buffer, length);
-
-	if (rc != 0)
-		DPRINTK(1, "sdio error=%d size=%d\n", rc, length);
+	struct sdio_func *func = priv->ks_sdio_card->func;
 
-	return rc;
+	return sdio_memcpy_toio(func, address, buffer, length);
 }
 
 static void ks_wlan_hw_sleep_doze_request(struct ks_wlan_private *priv)
 {
-	unsigned char rw_data;
-	int retval;
+	int ret;
 
 	DPRINTK(4, "\n");
 
@@ -97,14 +98,11 @@ static void ks_wlan_hw_sleep_doze_request(struct ks_wlan_private *priv)
 	atomic_set(&priv->sleepstatus.doze_request, 0);
 
 	if (atomic_read(&priv->sleepstatus.status) == 0) {
-		rw_data = GCR_B_DOZE;
-		retval =
-		    ks7010_sdio_write(priv, GCR_B, &rw_data, sizeof(rw_data));
-		if (retval) {
-			DPRINTK(1, " error : GCR_B=%02X\n", rw_data);
-			goto out;
+		ret = ks7010_sdio_writeb(priv, GCR_B, GCR_B_DOZE);
+		if (ret) {
+			DPRINTK(1, " error : GCR_B\n");
+			goto set_sleep_mode;
 		}
-		DPRINTK(4, "PMG SET!! : GCR_B=%02X\n", rw_data);
 		DPRINTK(3, "sleep_mode=SLP_SLEEP\n");
 		atomic_set(&priv->sleepstatus.status, 1);
 		priv->last_doze = jiffies;
@@ -112,14 +110,13 @@ static void ks_wlan_hw_sleep_doze_request(struct ks_wlan_private *priv)
 		DPRINTK(1, "sleep_mode=%d\n", priv->sleep_mode);
 	}
 
- out:
+set_sleep_mode:
 	priv->sleep_mode = atomic_read(&priv->sleepstatus.status);
 }
 
 static void ks_wlan_hw_sleep_wakeup_request(struct ks_wlan_private *priv)
 {
-	unsigned char rw_data;
-	int retval;
+	int ret;
 
 	DPRINTK(4, "\n");
 
@@ -127,14 +124,12 @@ static void ks_wlan_hw_sleep_wakeup_request(struct ks_wlan_private *priv)
 	atomic_set(&priv->sleepstatus.wakeup_request, 0);
 
 	if (atomic_read(&priv->sleepstatus.status) == 1) {
-		rw_data = WAKEUP_REQ;
-		retval =
-		    ks7010_sdio_write(priv, WAKEUP, &rw_data, sizeof(rw_data));
-		if (retval) {
-			DPRINTK(1, " error : WAKEUP=%02X\n", rw_data);
-			goto out;
+		ret = ks7010_sdio_writeb(priv, WAKEUP, WAKEUP_REQ);
+		if (ret) {
+			DPRINTK(1, " error : WAKEUP\n");
+			goto set_sleep_mode;
 		}
-		DPRINTK(4, "wake up : WAKEUP=%02X\n", rw_data);
+		DPRINTK(4, "wake up : WAKEUP\n");
 		atomic_set(&priv->sleepstatus.status, 0);
 		priv->last_wakeup = jiffies;
 		++priv->wakeup_count;
@@ -142,24 +137,22 @@ static void ks_wlan_hw_sleep_wakeup_request(struct ks_wlan_private *priv)
 		DPRINTK(1, "sleep_mode=%d\n", priv->sleep_mode);
 	}
 
- out:
+set_sleep_mode:
 	priv->sleep_mode = atomic_read(&priv->sleepstatus.status);
 }
 
 void ks_wlan_hw_wakeup_request(struct ks_wlan_private *priv)
 {
-	unsigned char rw_data;
-	int retval;
+	int ret;
 
 	DPRINTK(4, "\n");
 	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
-		rw_data = WAKEUP_REQ;
-		retval =
-		    ks7010_sdio_write(priv, WAKEUP, &rw_data, sizeof(rw_data));
-		if (retval)
-			DPRINTK(1, " error : WAKEUP=%02X\n", rw_data);
+		ret = ks7010_sdio_writeb(priv, WAKEUP, WAKEUP_REQ);
+		if (ret)
+			DPRINTK(1, " error : WAKEUP\n");
+		else
+			DPRINTK(4, "wake up : WAKEUP\n");
 
-		DPRINTK(4, "wake up : WAKEUP=%02X\n", rw_data);
 		priv->last_wakeup = jiffies;
 		++priv->wakeup_count;
 	} else {
@@ -168,131 +161,110 @@ void ks_wlan_hw_wakeup_request(struct ks_wlan_private *priv)
 	}
 }
 
-static int _ks_wlan_hw_power_save(struct ks_wlan_private *priv)
+static void _ks_wlan_hw_power_save(struct ks_wlan_private *priv)
 {
-	unsigned char rw_data;
-	int retval;
+	unsigned char byte;
+	int ret;
 
-	if (priv->reg.powermgt == POWMGT_ACTIVE_MODE)
-		return 0;
+	if (priv->reg.power_mgmt == POWER_MGMT_ACTIVE)
+		return;
 
-	if (priv->reg.operation_mode == MODE_INFRASTRUCTURE &&
-	    (priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
-		if (priv->dev_state == DEVICE_STATE_SLEEP) {
-			switch (atomic_read(&priv->psstatus.status)) {
-			case PS_SNOOZE:	/* 4 */
-				break;
-			default:
-				DPRINTK(5, "\npsstatus.status=%d\npsstatus.confirm_wait=%d\npsstatus.snooze_guard=%d\ncnt_txqbody=%d\n",
-					atomic_read(&priv->psstatus.status),
-					atomic_read(&priv->psstatus.confirm_wait),
-					atomic_read(&priv->psstatus.snooze_guard),
-					cnt_txqbody(priv));
-
-				if (!atomic_read(&priv->psstatus.confirm_wait)
-				    && !atomic_read(&priv->psstatus.snooze_guard)
-				    && !cnt_txqbody(priv)) {
-					retval =
-					    ks7010_sdio_read(priv, INT_PENDING,
-							     &rw_data,
-							     sizeof(rw_data));
-					if (retval) {
-						DPRINTK(1,
-							" error : INT_PENDING=%02X\n",
-							rw_data);
-						queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-								   &priv->ks_wlan_hw.rw_wq, 1);
-						break;
-					}
-					if (!rw_data) {
-						rw_data = GCR_B_DOZE;
-						retval =
-						    ks7010_sdio_write(priv,
-								      GCR_B,
-								      &rw_data,
-								      sizeof(rw_data));
-						if (retval) {
-							DPRINTK(1,
-								" error : GCR_B=%02X\n",
-								rw_data);
-							queue_delayed_work
-							    (priv->ks_wlan_hw.ks7010sdio_wq,
-							     &priv->ks_wlan_hw.rw_wq, 1);
-							break;
-						}
-						DPRINTK(4,
-							"PMG SET!! : GCR_B=%02X\n",
-							rw_data);
-						atomic_set(&priv->psstatus.
-							   status, PS_SNOOZE);
-						DPRINTK(3,
-							"psstatus.status=PS_SNOOZE\n");
-					} else {
-						queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-								   &priv->ks_wlan_hw.rw_wq, 1);
-					}
-				} else {
-					queue_delayed_work(priv->ks_wlan_hw.
-							   ks7010sdio_wq,
-							   &priv->ks_wlan_hw.rw_wq,
-							   0);
-				}
-				break;
-			}
-		}
+	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;
+
+	DPRINTK(5, "\npsstatus.status=%d\npsstatus.confirm_wait=%d\npsstatus.snooze_guard=%d\ncnt_txqbody=%d\n",
+		atomic_read(&priv->psstatus.status),
+		atomic_read(&priv->psstatus.confirm_wait),
+		atomic_read(&priv->psstatus.snooze_guard),
+		cnt_txqbody(priv));
+
+	if (atomic_read(&priv->psstatus.confirm_wait) ||
+	    atomic_read(&priv->psstatus.snooze_guard) ||
+	    cnt_txqbody(priv)) {
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+		return;
 	}
 
-	return 0;
+	ret = ks7010_sdio_readb(priv, INT_PENDING, &byte);
+	if (ret) {
+		DPRINTK(1, " error : INT_PENDING\n");
+		goto queue_delayed_work;
+	}
+	if (byte)
+		goto queue_delayed_work;
+
+	ret = ks7010_sdio_writeb(priv, GCR_B, GCR_B_DOZE);
+	if (ret) {
+		DPRINTK(1, " error : GCR_B\n");
+		goto queue_delayed_work;
+	}
+	atomic_set(&priv->psstatus.status, PS_SNOOZE);
+	DPRINTK(3, "psstatus.status=PS_SNOOZE\n");
+
+	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->ks_wlan_hw.ks7010sdio_wq,
-			   &priv->ks_wlan_hw.rw_wq, 1);
+	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)(void *arg1, void *arg2),
-			 void *arg1, void *arg2)
+			 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) {
-		kfree(p);
-		if (complete_handler)
-			(*complete_handler) (arg1, arg2);
-		return 1;
+		ret = -EPERM;
+		goto err_complete;
 	}
 
 	if ((TX_DEVICE_BUFF_SIZE - 1) <= cnt_txqbody(priv)) {
-		/* in case of buffer overflow */
 		DPRINTK(1, "tx buffer overflow\n");
-		kfree(p);
-		if (complete_handler)
-			(*complete_handler) (arg1, arg2);
-		return 1;
+		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->arg1 = arg1;
-	sp->arg2 = arg2;
+	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, unsigned char *buffer,
 			   unsigned long size)
 {
-	int retval;
-	unsigned char rw_data;
 	struct hostif_hdr *hdr;
+	int ret;
 
 	hdr = (struct hostif_hdr *)buffer;
 
@@ -302,59 +274,53 @@ static int write_to_device(struct ks_wlan_private *priv, unsigned char *buffer,
 		return 0;
 	}
 
-	retval = ks7010_sdio_write(priv, DATA_WINDOW, buffer, size);
-	if (retval) {
-		DPRINTK(1, " write error : retval=%d\n", retval);
-		return -4;
+	ret = ks7010_sdio_write(priv, DATA_WINDOW, buffer, size);
+	if (ret) {
+		DPRINTK(1, " write error : retval=%d\n", ret);
+		return ret;
 	}
 
-	rw_data = WRITE_STATUS_BUSY;
-	retval =
-	    ks7010_sdio_write(priv, WRITE_STATUS, &rw_data, sizeof(rw_data));
-	if (retval) {
-		DPRINTK(1, " error : WRITE_STATUS=%02X\n", rw_data);
-		return -3;
+	ret = ks7010_sdio_writeb(priv, WRITE_STATUS, REG_STATUS_BUSY);
+	if (ret) {
+		DPRINTK(1, " error : WRITE_STATUS\n");
+		return ret;
 	}
 
 	return 0;
 }
 
-static void tx_device_task(void *dev)
+static void tx_device_task(struct ks_wlan_private *priv)
 {
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev;
 	struct tx_device_buffer *sp;
-	int rc = 0;
+	int ret;
 
 	DPRINTK(4, "\n");
-	if (cnt_txqbody(priv) > 0
-	    && atomic_read(&priv->psstatus.status) != PS_SNOOZE) {
-		sp = &priv->tx_dev.tx_dev_buff[priv->tx_dev.qhead];
-		if (priv->dev_state >= DEVICE_STATE_BOOT) {
-			rc = write_to_device(priv, sp->sendp, sp->size);
-			if (rc) {
-				DPRINTK(1, "write_to_device error !!(%d)\n",
-					rc);
-				queue_delayed_work(priv->ks_wlan_hw.
-						   ks7010sdio_wq,
-						   &priv->ks_wlan_hw.rw_wq, 1);
-				return;
-			}
-		}
-		kfree(sp->sendp);	/* allocated memory free */
-		if (sp->complete_handler)	/* TX Complete */
-			(*sp->complete_handler) (sp->arg1, sp->arg2);
-		inc_txqhead(priv);
+	if (cnt_txqbody(priv) <= 0 ||
+	    atomic_read(&priv->psstatus.status) == PS_SNOOZE)
+		return;
 
-		if (cnt_txqbody(priv) > 0) {
-			queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-					   &priv->ks_wlan_hw.rw_wq, 0);
+	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) {
+			DPRINTK(1, "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 (cnt_txqbody(priv) > 0)
+		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)(void *arg1, void *arg2),
-		  void *arg1, void *arg2)
+		  void (*complete_handler)(struct ks_wlan_private *priv,
+					   struct sk_buff *skb),
+		  struct sk_buff *skb)
 {
 	int result = 0;
 	struct hostif_hdr *hdr;
@@ -372,13 +338,12 @@ int ks_wlan_hw_tx(struct ks_wlan_private *priv, void *p, unsigned long size,
 
 	DPRINTK(4, "event=%04X\n", hdr->event);
 	spin_lock(&priv->tx_dev.tx_dev_lock);
-	result = enqueue_txdev(priv, p, size, complete_handler, arg1, arg2);
+	result = enqueue_txdev(priv, p, size, complete_handler, skb);
 	spin_unlock(&priv->tx_dev.tx_dev_lock);
 
-	if (cnt_txqbody(priv) > 0) {
-		queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-				   &priv->ks_wlan_hw.rw_wq, 0);
-	}
+	if (cnt_txqbody(priv) > 0)
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
+
 	return result;
 }
 
@@ -395,34 +360,30 @@ static void rx_event_task(unsigned long dev)
 		inc_rxqhead(priv);
 
 		if (cnt_rxqbody(priv) > 0)
-			tasklet_schedule(&priv->ks_wlan_hw.rx_bh_task);
+			tasklet_schedule(&priv->rx_bh_task);
 	}
 }
 
-static void ks_wlan_hw_rx(void *dev, uint16_t size)
+static void ks_wlan_hw_rx(struct ks_wlan_private *priv, uint16_t size)
 {
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev;
-	int retval;
+	int ret;
 	struct rx_device_buffer *rx_buffer;
 	struct hostif_hdr *hdr;
-	unsigned char read_status;
 	unsigned short event = 0;
 
 	DPRINTK(4, "\n");
 
 	/* receive data */
 	if (cnt_rxqbody(priv) >= (RX_DEVICE_BUFF_SIZE - 1)) {
-		/* in case of buffer overflow */
 		DPRINTK(1, "rx buffer overflow\n");
-		goto error_out;
+		return;
 	}
 	rx_buffer = &priv->rx_dev.rx_dev_buff[priv->rx_dev.qtail];
 
-	retval =
-	    ks7010_sdio_read(priv, DATA_WINDOW, &rx_buffer->data[0],
-			     hif_align_size(size));
-	if (retval)
-		goto error_out;
+	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) {
@@ -432,15 +393,12 @@ static void ks_wlan_hw_rx(void *dev, uint16_t size)
 					     DUMP_PREFIX_OFFSET,
 					     rx_buffer->data, 32);
 #endif
-		/* rx_status update */
-		read_status = READ_STATUS_IDLE;
-		retval =
-		    ks7010_sdio_write(priv, READ_STATUS, &read_status,
-				      sizeof(read_status));
-		if (retval)
-			DPRINTK(1, " error : READ_STATUS=%02X\n", read_status);
-
-		goto error_out;
+		ret = ks7010_sdio_writeb(priv, READ_STATUS, REG_STATUS_IDLE);
+		if (ret)
+			DPRINTK(1, " error : READ_STATUS\n");
+
+		/* length check fail */
+		return;
 	}
 
 	hdr = (struct hostif_hdr *)&rx_buffer->data[0];
@@ -448,15 +406,9 @@ static void ks_wlan_hw_rx(void *dev, uint16_t size)
 	event = hdr->event;
 	inc_rxqtail(priv);
 
-	/* read status update */
-	read_status = READ_STATUS_IDLE;
-	retval =
-	    ks7010_sdio_write(priv, READ_STATUS, &read_status,
-			      sizeof(read_status));
-	if (retval)
-		DPRINTK(1, " error : READ_STATUS=%02X\n", read_status);
-
-	DPRINTK(4, "READ_STATUS=%02X\n", read_status);
+	ret = ks7010_sdio_writeb(priv, READ_STATUS, REG_STATUS_IDLE);
+	if (ret)
+		DPRINTK(1, " error : READ_STATUS\n");
 
 	if (atomic_read(&priv->psstatus.confirm_wait)) {
 		if (IS_HIF_CONF(event)) {
@@ -465,213 +417,162 @@ static void ks_wlan_hw_rx(void *dev, uint16_t size)
 		}
 	}
 
-	/* rx_event_task((void *)priv); */
-	tasklet_schedule(&priv->ks_wlan_hw.rx_bh_task);
-
- error_out:
-	return;
+	tasklet_schedule(&priv->rx_bh_task);
 }
 
 static void ks7010_rw_function(struct work_struct *work)
 {
-	struct hw_info_t *hw;
 	struct ks_wlan_private *priv;
-	unsigned char rw_data;
-	int retval;
+	unsigned char byte;
+	int ret;
 
-	hw = container_of(work, struct hw_info_t, rw_wq.work);
-	priv = container_of(hw, struct ks_wlan_private, ks_wlan_hw);
+	priv = container_of(work, struct ks_wlan_private, rw_dwork.work);
 
 	DPRINTK(4, "\n");
 
-	/* wiat after DOZE */
+	/* wait after DOZE */
 	if (time_after(priv->last_doze + ((30 * HZ) / 1000), jiffies)) {
 		DPRINTK(4, "wait after DOZE\n");
-		queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-				   &priv->ks_wlan_hw.rw_wq, 1);
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
 		return;
 	}
 
-	/* wiat after WAKEUP */
+	/* wait after WAKEUP */
 	while (time_after(priv->last_wakeup + ((30 * HZ) / 1000), jiffies)) {
 		DPRINTK(4, "wait after WAKEUP\n");
-/*		queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,&priv->ks_wlan_hw.rw_wq,
-		(priv->last_wakeup + ((30*HZ)/1000) - jiffies));*/
-		dev_info(&priv->ks_wlan_hw.sdio_card->func->dev,
+		dev_info(&priv->ks_sdio_card->func->dev,
 			 "wake: %lu %lu\n",
 			 priv->last_wakeup + (30 * HZ) / 1000,
 				jiffies);
 		msleep(30);
 	}
 
-	sdio_claim_host(priv->ks_wlan_hw.sdio_card->func);
+	sdio_claim_host(priv->ks_sdio_card->func);
 
 	/* power save wakeup */
 	if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
 		if (cnt_txqbody(priv) > 0) {
 			ks_wlan_hw_wakeup_request(priv);
-			queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-					   &priv->ks_wlan_hw.rw_wq, 1);
+			queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
 		}
-		goto err_out;
+		goto release_host;
 	}
 
 	/* sleep mode doze */
 	if (atomic_read(&priv->sleepstatus.doze_request) == 1) {
 		ks_wlan_hw_sleep_doze_request(priv);
-		goto err_out;
+		goto release_host;
 	}
 	/* sleep mode wakeup */
 	if (atomic_read(&priv->sleepstatus.wakeup_request) == 1) {
 		ks_wlan_hw_sleep_wakeup_request(priv);
-		goto err_out;
+		goto release_host;
 	}
 
 	/* read (WriteStatus/ReadDataSize FN1:00_0014) */
-	retval =
-	    ks7010_sdio_read(priv, WSTATUS_RSIZE, &rw_data, sizeof(rw_data));
-	if (retval) {
-		DPRINTK(1, " error : WSTATUS_RSIZE=%02X psstatus=%d\n", rw_data,
+	ret = ks7010_sdio_readb(priv, WSTATUS_RSIZE, &byte);
+	if (ret) {
+		DPRINTK(1, " error : WSTATUS_RSIZE psstatus=%d\n",
 			atomic_read(&priv->psstatus.status));
-		goto err_out;
+		goto release_host;
 	}
-	DPRINTK(4, "WSTATUS_RSIZE=%02X\n", rw_data);
+	DPRINTK(4, "WSTATUS_RSIZE=%02X\n", byte);
 
-	if (rw_data & RSIZE_MASK) {	/* Read schedule */
-		ks_wlan_hw_rx((void *)priv,
-			      (uint16_t)((rw_data & RSIZE_MASK) << 4));
+	if (byte & RSIZE_MASK) {	/* Read schedule */
+		ks_wlan_hw_rx(priv, (uint16_t)((byte & RSIZE_MASK) << 4));
 	}
-	if ((rw_data & WSTATUS_MASK))
-		tx_device_task((void *)priv);
+	if ((byte & WSTATUS_MASK))
+		tx_device_task(priv);
 
 	_ks_wlan_hw_power_save(priv);
 
- err_out:
-	sdio_release_host(priv->ks_wlan_hw.sdio_card->func);
+release_host:
+	sdio_release_host(priv->ks_sdio_card->func);
 }
 
 static void ks_sdio_interrupt(struct sdio_func *func)
 {
-	int retval;
+	int ret;
 	struct ks_sdio_card *card;
 	struct ks_wlan_private *priv;
-	unsigned char status, rsize, rw_data;
+	unsigned char status, rsize, byte;
 
 	card = sdio_get_drvdata(func);
 	priv = card->priv;
 	DPRINTK(4, "\n");
 
-	if (priv->dev_state >= DEVICE_STATE_BOOT) {
-		retval =
-		    ks7010_sdio_read(priv, INT_PENDING, &status,
-				     sizeof(status));
-		if (retval) {
-			DPRINTK(1, "read INT_PENDING Failed!!(%d)\n", retval);
-			goto intr_out;
+	if (priv->dev_state < DEVICE_STATE_BOOT)
+		goto queue_delayed_work;
+
+	ret = ks7010_sdio_readb(priv, INT_PENDING, &status);
+	if (ret) {
+		DPRINTK(1, "error : INT_PENDING\n");
+		goto queue_delayed_work;
+	}
+	DPRINTK(4, "INT_PENDING=%02X\n", status);
+
+	/* 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, &byte);
+		if (ret) {
+			DPRINTK(1, " error : GCR_B\n");
+			goto queue_delayed_work;
 		}
-		DPRINTK(4, "INT_PENDING=%02X\n", rw_data);
-
-		/* 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) {
-			retval =
-			    ks7010_sdio_read(priv, GCR_B, &rw_data,
-					     sizeof(rw_data));
-			if (retval) {
-				DPRINTK(1, " error : GCR_B=%02X\n", rw_data);
-				goto intr_out;
-			}
-			/* DPRINTK(1, "GCR_B=%02X\n", rw_data); */
-			if (rw_data == 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);
+		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) */
-			retval =
-			    ks7010_sdio_read(priv, WSTATUS_RSIZE, &rw_data,
-					     sizeof(rw_data));
-			if (retval) {
-				DPRINTK(1, " error : WSTATUS_RSIZE=%02X\n",
-					rw_data);
-				goto intr_out;
-			}
-			DPRINTK(4, "WSTATUS_RSIZE=%02X\n", rw_data);
-			rsize = rw_data & RSIZE_MASK;
-			if (rsize) {	/* Read schedule */
-				ks_wlan_hw_rx((void *)priv,
-					      (uint16_t)(rsize << 4));
-			}
-			if (rw_data & WSTATUS_MASK) {
-#if 0
-				if (status & INT_WRITE_STATUS
-				    && !cnt_txqbody(priv)) {
-					/* dummy write for interrupt clear */
-					rw_data = 0;
-					retval =
-					    ks7010_sdio_write(priv, DATA_WINDOW,
-							      &rw_data,
-							      sizeof(rw_data));
-					if (retval) {
-						DPRINTK(1,
-							"write DATA_WINDOW Failed!!(%d)\n",
-							retval);
-					}
-					status &= ~INT_WRITE_STATUS;
-				} else {
-#endif
-					if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
-						if (cnt_txqbody(priv)) {
-							ks_wlan_hw_wakeup_request(priv);
-							queue_delayed_work
-							    (priv->ks_wlan_hw.
-							     ks7010sdio_wq,
-							     &priv->ks_wlan_hw.
-							     rw_wq, 1);
-							return;
-						}
-					} else {
-						tx_device_task((void *)priv);
-					}
-#if 0
+	do {
+		/* read (WriteStatus/ReadDataSize FN1:00_0014) */
+		ret = ks7010_sdio_readb(priv, WSTATUS_RSIZE, &byte);
+		if (ret) {
+			DPRINTK(1, " error : WSTATUS_RSIZE\n");
+			goto queue_delayed_work;
+		}
+		DPRINTK(4, "WSTATUS_RSIZE=%02X\n", byte);
+		rsize = byte & RSIZE_MASK;
+		if (rsize != 0)		/* Read schedule */
+			ks_wlan_hw_rx(priv, (uint16_t)(rsize << 4));
+
+		if (byte & WSTATUS_MASK) {
+			if (atomic_read(&priv->psstatus.status) == PS_SNOOZE) {
+				if (cnt_txqbody(priv)) {
+					ks_wlan_hw_wakeup_request(priv);
+					queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
+					return;
 				}
-#endif
+			} else {
+				tx_device_task(priv);
 			}
-		} while (rsize);
-	}
+		}
+	} while (rsize);
 
- intr_out:
-	queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-			   &priv->ks_wlan_hw.rw_wq, 0);
+queue_delayed_work:
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 0);
 }
 
 static int trx_device_init(struct ks_wlan_private *priv)
 {
-	/* initialize values (tx) */
 	priv->tx_dev.qhead = 0;
 	priv->tx_dev.qtail = 0;
 
-	/* initialize values (rx) */
 	priv->rx_dev.qhead = 0;
 	priv->rx_dev.qtail = 0;
 
-	/* initialize spinLock (tx,rx) */
 	spin_lock_init(&priv->tx_dev.tx_dev_lock);
 	spin_lock_init(&priv->rx_dev.rx_dev_lock);
 
-	tasklet_init(&priv->ks_wlan_hw.rx_bh_task, rx_event_task,
-		     (unsigned long)priv);
+	tasklet_init(&priv->rx_bh_task, rx_event_task, (unsigned long)priv);
 
 	return 0;
 }
@@ -683,106 +584,100 @@ static void trx_device_exit(struct ks_wlan_private *priv)
 	/* tx buffer clear */
 	while (cnt_txqbody(priv) > 0) {
 		sp = &priv->tx_dev.tx_dev_buff[priv->tx_dev.qhead];
-		kfree(sp->sendp);	/* allocated memory free */
+		kfree(sp->sendp);
 		if (sp->complete_handler)	/* TX Complete */
-			(*sp->complete_handler) (sp->arg1, sp->arg2);
+			(*sp->complete_handler)(priv, sp->skb);
 		inc_txqhead(priv);
 	}
 
-	tasklet_kill(&priv->ks_wlan_hw.rx_bh_task);
+	tasklet_kill(&priv->rx_bh_task);
 }
 
 static int ks7010_sdio_update_index(struct ks_wlan_private *priv, u32 index)
 {
-	int rc = 0;
-	int retval;
+	int ret;
 	unsigned char *data_buf;
 
 	data_buf = kmalloc(sizeof(u32), GFP_KERNEL);
-	if (!data_buf) {
-		rc = 1;
-		goto error_out;
-	}
+	if (!data_buf)
+		return -ENOMEM;
 
 	memcpy(data_buf, &index, sizeof(index));
-	retval = ks7010_sdio_write(priv, WRITE_INDEX, data_buf, sizeof(index));
-	if (retval) {
-		rc = 2;
-		goto error_out;
-	}
+	ret = ks7010_sdio_write(priv, WRITE_INDEX, data_buf, sizeof(index));
+	if (ret)
+		goto err_free_data_buf;
 
-	retval = ks7010_sdio_write(priv, READ_INDEX, data_buf, sizeof(index));
-	if (retval) {
-		rc = 3;
-		goto error_out;
-	}
- error_out:
+	ret = ks7010_sdio_write(priv, READ_INDEX, data_buf, sizeof(index));
+	if (ret)
+		goto err_free_data_buf;
+
+	return 0;
+
+err_free_data_buf:
 	kfree(data_buf);
-	return rc;
+
+	return ret;
 }
 
 #define ROM_BUFF_SIZE (64 * 1024)
 static int ks7010_sdio_data_compare(struct ks_wlan_private *priv, u32 address,
 				    unsigned char *data, unsigned int size)
 {
-	int rc = 0;
-	int retval;
+	int ret;
 	unsigned char *read_buf;
 
 	read_buf = kmalloc(ROM_BUFF_SIZE, GFP_KERNEL);
-	if (!read_buf) {
-		rc = 1;
-		goto error_out;
-	}
-	retval = ks7010_sdio_read(priv, address, read_buf, size);
-	if (retval) {
-		rc = 2;
-		goto error_out;
-	}
-	retval = memcmp(data, read_buf, size);
+	if (!read_buf)
+		return -ENOMEM;
 
-	if (retval) {
-		DPRINTK(0, "data compare error (%d)\n", retval);
-		rc = 3;
-		goto error_out;
+	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;
+		DPRINTK(0, "data compare error (%d)\n", ret);
+		goto err_free_read_buf;
 	}
- error_out:
+
+	return 0;
+
+err_free_read_buf:
 	kfree(read_buf);
-	return rc;
+
+	return ret;
 }
 
-static int ks7010_upload_firmware(struct ks_wlan_private *priv,
-				  struct ks_sdio_card *card)
+static int ks7010_upload_firmware(struct ks_sdio_card *card)
 {
+	struct ks_wlan_private *priv = card->priv;
 	unsigned int size, offset, n = 0;
 	unsigned char *rom_buf;
-	unsigned char rw_data = 0;
-	int retval, rc = 0;
-	int length;
+	unsigned char byte = 0;
+	int ret;
+	unsigned int length;
 	const struct firmware *fw_entry = NULL;
 
-	/* buffer allocate */
 	rom_buf = kmalloc(ROM_BUFF_SIZE, GFP_KERNEL);
 	if (!rom_buf)
-		return 3;
+		return -ENOMEM;
 
 	sdio_claim_host(card->func);
 
 	/* Firmware running ? */
-	retval = ks7010_sdio_read(priv, GCR_A, &rw_data, sizeof(rw_data));
-	if (rw_data == GCR_A_RUN) {
+	ret = ks7010_sdio_readb(priv, GCR_A, &byte);
+	if (byte == GCR_A_RUN) {
 		DPRINTK(0, "MAC firmware running ...\n");
-		rc = 0;
-		goto error_out0;
+		goto release_host_and_free;
 	}
 
-	retval = request_firmware(&fw_entry, ROM_FILE, &priv->ks_wlan_hw.sdio_card->func->dev);
-	if (retval)
-		goto error_out0;
+	ret = request_firmware(&fw_entry, ROM_FILE,
+			       &priv->ks_sdio_card->func->dev);
+	if (ret)
+		goto release_host_and_free;
 
 	length = fw_entry->size;
 
-	/* Load Program */
 	n = 0;
 	do {
 		if (length >= ROM_BUFF_SIZE) {
@@ -796,77 +691,62 @@ static int ks7010_upload_firmware(struct ks_wlan_private *priv,
 		if (size == 0)
 			break;
 		memcpy(rom_buf, fw_entry->data + n, size);
-		/* Update write index */
+
 		offset = n;
-		retval =
-		    ks7010_sdio_update_index(priv,
-					     KS7010_IRAM_ADDRESS + offset);
-		if (retval) {
-			rc = 6;
-			goto error_out1;
-		}
+		ret = ks7010_sdio_update_index(priv, KS7010_IRAM_ADDRESS + offset);
+		if (ret)
+			goto release_firmware;
 
-		/* Write data */
-		retval = ks7010_sdio_write(priv, DATA_WINDOW, rom_buf, size);
-		if (retval) {
-			rc = 8;
-			goto error_out1;
-		}
+		ret = ks7010_sdio_write(priv, DATA_WINDOW, rom_buf, size);
+		if (ret)
+			goto release_firmware;
+
+		ret = ks7010_sdio_data_compare(priv, DATA_WINDOW, rom_buf, size);
+		if (ret)
+			goto release_firmware;
 
-		/* compare */
-		retval =
-		    ks7010_sdio_data_compare(priv, DATA_WINDOW, rom_buf, size);
-		if (retval) {
-			rc = 9;
-			goto error_out1;
-		}
 		n += size;
 
 	} while (size);
 
-	/* Remap request */
-	rw_data = GCR_A_REMAP;
-	retval = ks7010_sdio_write(priv, GCR_A, &rw_data, sizeof(rw_data));
-	if (retval) {
-		rc = 11;
-		goto error_out1;
-	}
-	DPRINTK(4, " REMAP Request : GCR_A=%02X\n", rw_data);
+	ret = ks7010_sdio_writeb(priv, GCR_A, GCR_A_REMAP);
+	if (ret)
+		goto release_firmware;
+
+	DPRINTK(4, " REMAP Request : GCR_A\n");
 
 	/* Firmware running check */
 	for (n = 0; n < 50; ++n) {
 		mdelay(10);	/* wait_ms(10); */
-		retval =
-		    ks7010_sdio_read(priv, GCR_A, &rw_data, sizeof(rw_data));
-		if (retval) {
-			rc = 11;
-			goto error_out1;
-		}
-		if (rw_data == GCR_A_RUN)
+		ret = ks7010_sdio_readb(priv, GCR_A, &byte);
+		if (ret)
+			goto release_firmware;
+
+		if (byte == GCR_A_RUN)
 			break;
 	}
 	DPRINTK(4, "firmware wakeup (%d)!!!!\n", n);
 	if ((50) <= n) {
 		DPRINTK(1, "firmware can't start\n");
-		rc = 12;
-		goto error_out1;
+		ret = -EIO;
+		goto release_firmware;
 	}
 
-	rc = 0;
+	ret = 0;
 
- error_out1:
+ release_firmware:
 	release_firmware(fw_entry);
- error_out0:
+ release_host_and_free:
 	sdio_release_host(card->func);
 	kfree(rom_buf);
-	return rc;
+
+	return ret;
 }
 
 static void ks7010_card_init(struct ks_wlan_private *priv)
 {
 	DPRINTK(5, "\ncard_init_task()\n");
 
-	/* init_waitqueue_head(&priv->confirm_wait); */
 	init_completion(&priv->confirm_wait);
 
 	DPRINTK(5, "init_completion()\n");
@@ -881,7 +761,7 @@ static void ks7010_card_init(struct ks_wlan_private *priv)
 		DPRINTK(1, "wait time out!! SME_START\n");
 	}
 
-	if (priv->mac_address_valid && priv->version_size)
+	if (priv->mac_address_valid && priv->version_size != 0)
 		priv->dev_state = DEVICE_STATE_PREINIT;
 
 	hostif_sme_enqueue(priv, SME_GET_EEPROM_CKSUM);
@@ -920,7 +800,7 @@ static void ks7010_init_defaults(struct ks_wlan_private *priv)
 {
 	priv->reg.tx_rate = TX_RATE_AUTO;
 	priv->reg.preamble = LONG_PREAMBLE;
-	priv->reg.powermgt = POWMGT_ACTIVE_MODE;
+	priv->reg.power_mgmt = POWER_MGMT_ACTIVE;
 	priv->reg.scan_type = ACTIVE_SCAN;
 	priv->reg.beacon_lost_count = 20;
 	priv->reg.rts = 2347UL;
@@ -949,7 +829,7 @@ static int ks7010_sdio_probe(struct sdio_func *func,
 	struct ks_wlan_private *priv;
 	struct ks_sdio_card *card;
 	struct net_device *netdev;
-	unsigned char rw_data;
+	unsigned char byte;
 	int ret;
 
 	DPRINTK(5, "ks7010_sdio_probe()\n");
@@ -957,63 +837,53 @@ static int ks7010_sdio_probe(struct sdio_func *func,
 	priv = NULL;
 	netdev = NULL;
 
-	/* initialize ks_sdio_card */
 	card = kzalloc(sizeof(*card), GFP_KERNEL);
 	if (!card)
 		return -ENOMEM;
 
 	card->func = func;
-	spin_lock_init(&card->lock);
 
-	/*** Initialize  SDIO ***/
 	sdio_claim_host(func);
 
-	/* bus setting  */
-	/* Issue config request to override clock rate */
-
-	/* function blocksize set */
 	ret = sdio_set_block_size(func, KS7010_IO_BLOCK_SIZE);
 	DPRINTK(5, "multi_block=%d sdio_set_block_size()=%d %d\n",
 		func->card->cccr.multi_block, func->cur_blksize, ret);
 
-	/* Allocate the slot current */
-
-	/* function enable */
 	ret = sdio_enable_func(func);
 	DPRINTK(5, "sdio_enable_func() %d\n", ret);
 	if (ret)
-		goto error_free_card;
+		goto err_free_card;
 
 	/* interrupt disable */
 	sdio_writeb(func, 0, INT_ENABLE, &ret);
 	if (ret)
-		goto error_free_card;
+		goto err_free_card;
 	sdio_writeb(func, 0xff, INT_PENDING, &ret);
 	if (ret)
-		goto error_disable_func;
+		goto err_disable_func;
 
 	/* setup interrupt handler */
 	ret = sdio_claim_irq(func, ks_sdio_interrupt);
 	if (ret)
-		goto error_disable_func;
+		goto err_disable_func;
 
 	sdio_release_host(func);
 
 	sdio_set_drvdata(func, card);
 
-	DPRINTK(5, "class = 0x%X, vendor = 0x%X, "
-		"device = 0x%X\n", func->class, func->vendor, func->device);
+	DPRINTK(5, "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, "ks7010 : Unable to alloc new net device\n");
-		goto error_release_irq;
+		goto err_release_irq;
 	}
 	if (dev_alloc_name(netdev, "wlan%d") < 0) {
 		dev_err(&card->func->dev,
 			"ks7010 :  Couldn't get name!\n");
-		goto error_free_netdev;
+		goto err_free_netdev;
 	}
 
 	priv = netdev_priv(netdev);
@@ -1022,18 +892,13 @@ static int ks7010_sdio_probe(struct sdio_func *func,
 	SET_NETDEV_DEV(netdev, &card->func->dev);	/* for create sysfs symlinks */
 
 	/* private memory initialize */
-	priv->ks_wlan_hw.sdio_card = card;
-	init_completion(&priv->ks_wlan_hw.ks7010_sdio_wait);
-	priv->ks_wlan_hw.read_buf = NULL;
-	priv->ks_wlan_hw.read_buf = kmalloc(RX_DATA_SIZE, GFP_KERNEL);
-	if (!priv->ks_wlan_hw.read_buf)
-		goto error_free_netdev;
+	priv->ks_sdio_card = card;
 
 	priv->dev_state = DEVICE_STATE_PREBOOT;
 	priv->net_dev = netdev;
 	priv->firmware_version[0] = '\0';
 	priv->version_size = 0;
-	priv->last_doze = jiffies;	/* set current jiffies */
+	priv->last_doze = jiffies;
 	priv->last_wakeup = jiffies;
 	memset(&priv->nstats, 0, sizeof(priv->nstats));
 	memset(&priv->wstats, 0, sizeof(priv->wstats));
@@ -1049,64 +914,57 @@ static int ks7010_sdio_probe(struct sdio_func *func,
 
 	ks7010_init_defaults(priv);
 
-	/* Upload firmware */
-	ret = ks7010_upload_firmware(priv, card);	/* firmware load */
+	ret = ks7010_upload_firmware(card);
 	if (ret) {
 		dev_err(&card->func->dev,
 			"ks7010: firmware load failed !! return code = %d\n",
 			 ret);
-		goto error_free_read_buf;
+		goto err_free_netdev;
 	}
 
 	/* interrupt setting */
 	/* clear Interrupt status write (ARMtoSD_InterruptPending FN1:00_0024) */
-	rw_data = 0xff;
 	sdio_claim_host(func);
-	ret = ks7010_sdio_write(priv, INT_PENDING, &rw_data, sizeof(rw_data));
+	ret = ks7010_sdio_writeb(priv, INT_PENDING, 0xff);
 	sdio_release_host(func);
 	if (ret)
-		DPRINTK(1, " error : INT_PENDING=%02X\n", rw_data);
+		DPRINTK(1, " error : INT_PENDING\n");
 
-	DPRINTK(4, " clear Interrupt : INT_PENDING=%02X\n", rw_data);
-
-	/* enable ks7010sdio interrupt (INT_GCR_B|INT_READ_STATUS|INT_WRITE_STATUS) */
-	rw_data = (INT_GCR_B | INT_READ_STATUS | INT_WRITE_STATUS);
+	/* enable ks7010sdio interrupt */
+	byte = (INT_GCR_B | INT_READ_STATUS | INT_WRITE_STATUS);
 	sdio_claim_host(func);
-	ret = ks7010_sdio_write(priv, INT_ENABLE, &rw_data, sizeof(rw_data));
+	ret = ks7010_sdio_writeb(priv, INT_ENABLE, byte);
 	sdio_release_host(func);
 	if (ret)
-		DPRINTK(1, " error : INT_ENABLE=%02X\n", rw_data);
+		DPRINTK(1, " err : INT_ENABLE\n");
 
-	DPRINTK(4, " enable Interrupt : INT_ENABLE=%02X\n", rw_data);
+	DPRINTK(4, " enable Interrupt : INT_ENABLE=%02X\n", byte);
 	priv->dev_state = DEVICE_STATE_BOOT;
 
-	priv->ks_wlan_hw.ks7010sdio_wq = create_workqueue("ks7010sdio_wq");
-	if (!priv->ks_wlan_hw.ks7010sdio_wq) {
+	priv->wq = create_workqueue("wq");
+	if (!priv->wq) {
 		DPRINTK(1, "create_workqueue failed !!\n");
-		goto error_free_read_buf;
+		goto err_free_netdev;
 	}
 
-	INIT_DELAYED_WORK(&priv->ks_wlan_hw.rw_wq, ks7010_rw_function);
+	INIT_DELAYED_WORK(&priv->rw_dwork, ks7010_rw_function);
 	ks7010_card_init(priv);
 
 	ret = register_netdev(priv->net_dev);
 	if (ret)
-		goto error_free_read_buf;
+		goto err_free_netdev;
 
 	return 0;
 
- error_free_read_buf:
-	kfree(priv->ks_wlan_hw.read_buf);
-	priv->ks_wlan_hw.read_buf = NULL;
- error_free_netdev:
+ err_free_netdev:
 	free_netdev(priv->net_dev);
 	card->priv = NULL;
- error_release_irq:
+ err_release_irq:
 	sdio_claim_host(func);
 	sdio_release_irq(func);
- error_disable_func:
+ err_disable_func:
 	sdio_disable_func(func);
- error_free_card:
+ err_free_card:
 	sdio_release_host(func);
 	sdio_set_drvdata(func, NULL);
 	kfree(card);
@@ -1114,6 +972,34 @@ static int ks7010_sdio_probe(struct sdio_func *func,
 	return -ENODEV;
 }
 
+/* send stop request to MAC */
+static int send_stop_request(struct sdio_func *func)
+{
+	struct hostif_stop_request_t *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) {
+		DPRINTK(3, "allocate memory failed..\n");
+		return -ENOMEM;
+	}
+
+	size = sizeof(*pp) - sizeof(pp->header.size);
+	pp->header.size = cpu_to_le16((uint16_t)size);
+	pp->header.event = cpu_to_le16((uint16_t)HIF_STOP_REQ);
+
+	sdio_claim_host(func);
+	write_to_device(card->priv, (unsigned char *)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;
@@ -1142,35 +1028,17 @@ static void ks7010_sdio_remove(struct sdio_func *func)
 		sdio_release_host(func);
 		DPRINTK(1, "interrupt disable\n");
 
-		/* send stop request to MAC */
-		{
-			struct hostif_stop_request_t *pp;
+		ret = send_stop_request(func);
+		if (ret)	/* memory allocation failure */
+			return;
 
-			pp = kzalloc(hif_align_size(sizeof(*pp)), GFP_KERNEL);
-			if (!pp) {
-				DPRINTK(3, "allocate memory failed..\n");
-				return;	/* to do goto ni suru */
-			}
-			pp->header.size =
-			    cpu_to_le16((uint16_t)
-					(sizeof(*pp) -
-					 sizeof(pp->header.size)));
-			pp->header.event = cpu_to_le16((uint16_t)HIF_STOP_REQ);
-
-			sdio_claim_host(func);
-			write_to_device(priv, (unsigned char *)pp,
-					hif_align_size(sizeof(*pp)));
-			sdio_release_host(func);
-			kfree(pp);
-		}
 		DPRINTK(1, "STOP Req\n");
 
-		if (priv->ks_wlan_hw.ks7010sdio_wq) {
-			flush_workqueue(priv->ks_wlan_hw.ks7010sdio_wq);
-			destroy_workqueue(priv->ks_wlan_hw.ks7010sdio_wq);
+		if (priv->wq) {
+			flush_workqueue(priv->wq);
+			destroy_workqueue(priv->wq);
 		}
-		DPRINTK(1,
-			"destroy_workqueue(priv->ks_wlan_hw.ks7010sdio_wq);\n");
+		DPRINTK(1, "destroy_workqueue(priv->wq);\n");
 
 		hostif_exit(priv);
 		DPRINTK(1, "hostif_exit\n");
@@ -1178,7 +1046,6 @@ static void ks7010_sdio_remove(struct sdio_func *func)
 		unregister_netdev(netdev);
 
 		trx_device_exit(priv);
-		kfree(priv->ks_wlan_hw.read_buf);
 		free_netdev(priv->net_dev);
 		card->priv = NULL;
 	}
diff --git a/drivers/staging/ks7010/ks7010_sdio.h b/drivers/staging/ks7010/ks7010_sdio.h
index d7e1523a222f..e4f56a11c888 100644
--- a/drivers/staging/ks7010/ks7010_sdio.h
+++ b/drivers/staging/ks7010/ks7010_sdio.h
@@ -22,10 +22,13 @@
 /* Older sources suggest earlier versions were named 7910 or 79xx */
 #define SDIO_DEVICE_ID_KS_7010		0x7910
 
-/* Read Status Register */
+/* Read/Write Status Register */
 #define READ_STATUS		0x000000
-#define READ_STATUS_BUSY	0
-#define READ_STATUS_IDLE	1
+#define WRITE_STATUS		0x00000C
+enum reg_status_type {
+	REG_STATUS_BUSY,
+	REG_STATUS_IDLE
+};
 
 /* Read Index Register */
 #define READ_INDEX		0x000004
@@ -33,11 +36,6 @@
 /* Read Data Size Register */
 #define READ_DATA_SIZE		0x000008
 
-/* Write Status Register */
-#define WRITE_STATUS		0x00000C
-#define WRITE_STATUS_BUSY	0
-#define WRITE_STATUS_IDLE	1
-
 /* Write Index Register */
 #define WRITE_INDEX		0x000010
 
@@ -64,18 +62,20 @@
 
 /* General Communication Register A */
 #define GCR_A			0x000028
-#define GCR_A_INIT		0
-#define GCR_A_REMAP		1
-#define GCR_A_RUN		2
+enum gen_com_reg_a {
+	GCR_A_INIT,
+	GCR_A_REMAP,
+	GCR_A_RUN
+};
 
 /* General Communication Register B */
 #define GCR_B			0x00002C
-#define GCR_B_ACTIVE		0
-#define GCR_B_DOZE		1
+enum gen_com_reg_b {
+	GCR_B_ACTIVE,
+	GCR_B_DOZE
+};
 
 /* Wakeup Register */
-/* #define WAKEUP			0x008104 */
-/* #define WAKEUP_REQ		0x00 */
 #define WAKEUP			0x008018
 #define WAKEUP_REQ		0x5a
 
@@ -85,62 +85,78 @@
 
 #define KS7010_IRAM_ADDRESS	0x06000000
 
-/*
- * struct define
+/**
+ * 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 hw_info_t {
-	struct ks_sdio_card *sdio_card;
-	struct completion ks7010_sdio_wait;
-	struct workqueue_struct *ks7010sdio_wq;
-	struct delayed_work rw_wq;
-	unsigned char *read_buf;
-	struct tasklet_struct rx_bh_task;
-};
-
-struct ks_sdio_packet {
-	struct ks_sdio_packet *next;
-	u16 nb;
-	u8 buffer[0] __aligned(4);
-};
-
 struct ks_sdio_card {
 	struct sdio_func *func;
 	struct ks_wlan_private *priv;
-	spinlock_t lock;
 };
 
 /* Tx Device struct */
 #define	TX_DEVICE_BUFF_SIZE	1024
 
+/**
+ * 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;	/* pointer of send req data */
+	unsigned char *sendp;
 	unsigned int size;
-	void (*complete_handler) (void *arg1, void *arg2);
-	void *arg1;
-	void *arg2;
+	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;	/* tx buffer queue first pointer */
-	unsigned int qtail;	/* tx buffer queue last pointer */
-	spinlock_t tx_dev_lock;
+	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;	/* rx buffer queue first pointer */
-	unsigned int qtail;	/* rx buffer queue last pointer */
-	spinlock_t rx_dev_lock;
+	unsigned int qhead;
+	unsigned int qtail;
+	spinlock_t rx_dev_lock;	/* protect access to the queue */
 };
 
 #define	ROM_FILE "ks7010sd.rom"
diff --git a/drivers/staging/ks7010/ks_hostif.c b/drivers/staging/ks7010/ks_hostif.c
index da7c42ef05f5..49e95426ac30 100644
--- a/drivers/staging/ks7010/ks_hostif.c
+++ b/drivers/staging/ks7010/ks_hostif.c
@@ -66,11 +66,13 @@ inline u32 get_DWORD(struct ks_wlan_private *priv)
 
 static void ks_wlan_hw_wakeup_task(struct work_struct *work)
 {
-	struct ks_wlan_private *priv =
-	    container_of(work, struct ks_wlan_private, ks_wlan_wakeup_task);
-	int ps_status = atomic_read(&priv->psstatus.status);
+	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(
@@ -78,7 +80,7 @@ static void ks_wlan_hw_wakeup_task(struct work_struct *work)
 				msecs_to_jiffies(20));
 		if (time_left <= 0) {
 			DPRINTK(1, "wake up timeout or interrupted !!!\n");
-			schedule_work(&priv->ks_wlan_wakeup_task);
+			schedule_work(&priv->wakeup_work);
 			return;
 		}
 	} else {
@@ -97,7 +99,7 @@ int ks_wlan_do_power_save(struct ks_wlan_private *priv)
 {
 	DPRINTK(4, "psstatus.status=%d\n", atomic_read(&priv->psstatus.status));
 
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS)
+	if (is_connect_status(priv->connect_status))
 		hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
 	else
 		priv->dev_state = DEVICE_STATE_READY;
@@ -110,30 +112,29 @@ int get_current_ap(struct ks_wlan_private *priv, struct link_ap_info_t *ap_info)
 	struct local_ap_t *ap;
 	union iwreq_data wrqu;
 	struct net_device *netdev = priv->net_dev;
-	int rc = 0;
 
 	DPRINTK(3, "\n");
-	ap = &(priv->current_ap);
+	ap = &priv->current_ap;
 
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == DISCONNECT_STATUS) {
+	if (is_disconnect_status(priv->connect_status)) {
 		memset(ap, 0, sizeof(struct local_ap_t));
-		return 1;
+		return -EPERM;
 	}
 
 	/* bssid */
-	memcpy(&(ap->bssid[0]), &(ap_info->bssid[0]), ETH_ALEN);
+	memcpy(ap->bssid, ap_info->bssid, ETH_ALEN);
 	/* essid */
-	memcpy(&(ap->ssid.body[0]), &(priv->reg.ssid.body[0]),
+	memcpy(ap->ssid.body, priv->reg.ssid.body,
 	       priv->reg.ssid.size);
 	ap->ssid.size = priv->reg.ssid.size;
 	/* rate_set */
-	memcpy(&(ap->rate_set.body[0]), &(ap_info->rate_set.body[0]),
+	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) {
+	if (ap_info->ext_rate_set.size != 0) {
 		/* rate_set */
-		memcpy(&(ap->rate_set.body[ap->rate_set.size]),
-		       &(ap_info->ext_rate_set.body[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;
 	}
@@ -148,28 +149,28 @@ int get_current_ap(struct ks_wlan_private *priv, struct link_ap_info_t *ap_info)
 	/* capability */
 	ap->capability = ap_info->capability;
 	/* rsn */
-	if ((ap_info->rsn_mode & RSN_MODE_WPA2)
-	    && (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)) {
+	if ((ap_info->rsn_mode & RSN_MODE_WPA2) &&
+	    (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)) {
 		ap->rsn_ie.id = 0x30;
 		if (ap_info->rsn.size <= RSN_IE_BODY_MAX) {
 			ap->rsn_ie.size = ap_info->rsn.size;
-			memcpy(&(ap->rsn_ie.body[0]), &(ap_info->rsn.body[0]),
+			memcpy(ap->rsn_ie.body, ap_info->rsn.body,
 			       ap_info->rsn.size);
 		} else {
 			ap->rsn_ie.size = RSN_IE_BODY_MAX;
-			memcpy(&(ap->rsn_ie.body[0]), &(ap_info->rsn.body[0]),
+			memcpy(ap->rsn_ie.body, ap_info->rsn.body,
 			       RSN_IE_BODY_MAX);
 		}
-	} else if ((ap_info->rsn_mode & RSN_MODE_WPA)
-		   && (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA)) {
+	} else if ((ap_info->rsn_mode & RSN_MODE_WPA) &&
+		   (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA)) {
 		ap->wpa_ie.id = 0xdd;
 		if (ap_info->rsn.size <= RSN_IE_BODY_MAX) {
 			ap->wpa_ie.size = ap_info->rsn.size;
-			memcpy(&(ap->wpa_ie.body[0]), &(ap_info->rsn.body[0]),
+			memcpy(ap->wpa_ie.body, ap_info->rsn.body,
 			       ap_info->rsn.size);
 		} else {
 			ap->wpa_ie.size = RSN_IE_BODY_MAX;
-			memcpy(&(ap->wpa_ie.body[0]), &(ap_info->rsn.body[0]),
+			memcpy(ap->wpa_ie.body, ap_info->rsn.body,
 			       RSN_IE_BODY_MAX);
 		}
 	} else {
@@ -182,23 +183,35 @@ int get_current_ap(struct ks_wlan_private *priv, struct link_ap_info_t *ap_info)
 	wrqu.data.length = 0;
 	wrqu.data.flags = 0;
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
+	if (is_connect_status(priv->connect_status)) {
 		memcpy(wrqu.ap_addr.sa_data,
-		       &(priv->current_ap.bssid[0]), ETH_ALEN);
+		       priv->current_ap.bssid, ETH_ALEN);
 		DPRINTK(3,
 			"IWEVENT: connect bssid=%pM\n", wrqu.ap_addr.sa_data);
 		wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
 	}
 	DPRINTK(4, "\n    Link AP\n");
-	DPRINTK(4, "    bssid=%02X:%02X:%02X:%02X:%02X:%02X\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", ap->bssid[0], ap->bssid[1], ap->bssid[2], ap->bssid[3], ap->bssid[4], ap->bssid[5], &(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);
+	DPRINTK(4, "    bssid=%02X:%02X:%02X:%02X:%02X:%02X\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",
+		ap->bssid[0], ap->bssid[1], ap->bssid[2],
+		ap->bssid[3], ap->bssid[4], ap->bssid[5],
+		&(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);
 	DPRINTK(4, "\n    Link AP\n    rsn.mode=%d\n    rsn.size=%d\n",
 		ap_info->rsn_mode, ap_info->rsn.size);
 	DPRINTK(4, "\n    ext_rate_set_size=%d\n    rate_set_size=%d\n",
 		ap_info->ext_rate_set.size, ap_info->rate_set.size);
 
-	return rc;
+	return 0;
 }
 
 static
@@ -212,7 +225,7 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 	memset(ap, 0, sizeof(struct local_ap_t));
 
 	/* bssid */
-	memcpy(&(ap->bssid[0]), &(ap_info->bssid[0]), ETH_ALEN);
+	memcpy(ap->bssid, ap_info->bssid, ETH_ALEN);
 	/* rssi */
 	ap->rssi = ap_info->rssi;
 	/* sq */
@@ -224,7 +237,7 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 	/* channel */
 	ap->channel = ap_info->ch_info;
 
-	bp = &(ap_info->body[0]);
+	bp = ap_info->body;
 	bsize = ap_info->body_size;
 	offset = 0;
 
@@ -239,19 +252,19 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 					*(bp + 1));
 				ap->ssid.size = SSID_MAX_SIZE;
 			}
-			memcpy(&(ap->ssid.body[0]), bp + 2, ap->ssid.size);
+			memcpy(ap->ssid.body, bp + 2, ap->ssid.size);
 			break;
 		case 1:	/* rate */
 		case 50:	/* ext rate */
 			if ((*(bp + 1) + ap->rate_set.size) <=
 			    RATE_SET_MAX_SIZE) {
-				memcpy(&(ap->rate_set.body[ap->rate_set.size]),
+				memcpy(&ap->rate_set.body[ap->rate_set.size],
 				       bp + 2, *(bp + 1));
 				ap->rate_set.size += *(bp + 1);
 			} else {
 				DPRINTK(1, "size over :: rate size=%d\n",
 					(*(bp + 1) + ap->rate_set.size));
-				memcpy(&(ap->rate_set.body[ap->rate_set.size]),
+				memcpy(&ap->rate_set.body[ap->rate_set.size],
 				       bp + 2,
 				       RATE_SET_MAX_SIZE - ap->rate_set.size);
 				ap->rate_set.size +=
@@ -269,10 +282,10 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 					*(bp + 1));
 				ap->rsn_ie.size = RSN_IE_BODY_MAX;
 			}
-			memcpy(&(ap->rsn_ie.body[0]), bp + 2, ap->rsn_ie.size);
+			memcpy(ap->rsn_ie.body, bp + 2, ap->rsn_ie.size);
 			break;
 		case 221:	/* WPA */
-			if (!memcmp(bp + 2, "\x00\x50\xf2\x01", 4)) {	/* WPA OUI check */
+			if (memcmp(bp + 2, "\x00\x50\xf2\x01", 4) == 0) {	/* WPA OUI check */
 				ap->wpa_ie.id = *bp;
 				if (*(bp + 1) <= RSN_IE_BODY_MAX) {
 					ap->wpa_ie.size = *(bp + 1);
@@ -282,7 +295,7 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 						*(bp + 1));
 					ap->wpa_ie.size = RSN_IE_BODY_MAX;
 				}
-				memcpy(&(ap->wpa_ie.body[0]), bp + 2,
+				memcpy(ap->wpa_ie.body, bp + 2,
 				       ap->wpa_ie.size);
 			}
 			break;
@@ -308,23 +321,98 @@ int get_ap_information(struct ks_wlan_private *priv, struct ap_info_t *ap_info,
 }
 
 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[8];
+	char buf[128];
+	unsigned long now;
+	struct mic_failure_t *mic_failure;
+	struct michael_mic_t michael_mic;
+	union iwreq_data wrqu;
+	unsigned int key_index = auth_type - 1;
+	struct wpa_key_t *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) {
+		DPRINTK(1, "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) {
+		DPRINTK(4, "TKIP: protocol=%04X: size=%u\n",
+			eth_proto, priv->rx_size);
+		/* MIC save */
+		memcpy(&recv_mic[0], (priv->rxp) + ((priv->rx_size) - 8), 8);
+		priv->rx_size = priv->rx_size - 8;
+		if (auth_type > 0 && auth_type < 4) {	/* auth_type check */
+			MichaelMICFunction(&michael_mic,
+					   (uint8_t *)key->rx_mic_key,
+					   (uint8_t *)priv->rxp,
+					   (int)priv->rx_size,
+					   (uint8_t)0,	/* priority */
+					   (uint8_t *)michael_mic.Result);
+		}
+		if (memcmp(michael_mic.Result, recv_mic, 8) != 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;
+			}
+			DPRINTK(4, "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 =
+					(uint16_t)((now - mic_failure->last_failure_time) / HZ);
+				if (!mic_failure->counter)	/*  range 1-60 */
+					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);
+			DPRINTK(4, "IWEVENT:MICHAELMICFAILURE\n");
+			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;
 	unsigned short auth_type;
 	unsigned char temp[256];
-
-	unsigned char RecvMIC[8];
-	char buf[128];
 	struct ether_hdr *eth_hdr;
 	unsigned short eth_proto;
-	unsigned long now;
-	struct mic_failure_t *mic_failure;
 	struct ieee802_1x_hdr *aa1x_hdr;
-	struct wpa_eapol_key *eap_key;
-	struct michel_mic_t michel_mic;
-	union iwreq_data wrqu;
+	size_t size;
+	int ret;
 
 	DPRINTK(3, "\n");
 
@@ -343,7 +431,7 @@ void hostif_data_indication(struct ks_wlan_private *priv)
 	DPRINTK(3, "ether protocol = %04X\n", eth_proto);
 
 	/* source address check */
-	if (!memcmp(&priv->eth_addr[0], eth_hdr->h_source, ETH_ALEN)) {
+	if (memcmp(&priv->eth_addr[0], eth_hdr->h_source, ETH_ALEN) == 0) {
 		DPRINTK(1, "invalid : source is own mac address !!\n");
 		DPRINTK(1,
 			"eth_hdrernet->h_dest=%02X:%02X:%02X:%02X:%02X:%02X\n",
@@ -356,79 +444,9 @@ void hostif_data_indication(struct ks_wlan_private *priv)
 
 	/*  for WPA */
 	if (auth_type != TYPE_DATA && priv->wpa.rsn_enabled) {
-		if (memcmp(&eth_hdr->h_source[0], &priv->eth_addr[0], ETH_ALEN)) {	/* source address check */
-			if (eth_hdr->h_dest_snap != eth_hdr->h_source_snap) {
-				DPRINTK(1, "invalid data format\n");
-				priv->nstats.rx_errors++;
-				return;
-			}
-			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))
-			    && priv->wpa.key[auth_type - 1].key_len) {
-				DPRINTK(4, "TKIP: protocol=%04X: size=%u\n",
-					eth_proto, priv->rx_size);
-				/* MIC save */
-				memcpy(&RecvMIC[0],
-				       (priv->rxp) + ((priv->rx_size) - 8), 8);
-				priv->rx_size = priv->rx_size - 8;
-				if (auth_type > 0 && auth_type < 4) {	/* auth_type check */
-					MichaelMICFunction(&michel_mic, (uint8_t *) priv->wpa.key[auth_type - 1].rx_mic_key, (uint8_t *) priv->rxp, (int)priv->rx_size, (uint8_t) 0,	/* priority */
-							   (uint8_t *)
-							   michel_mic.Result);
-				}
-				if (memcmp(michel_mic.Result, RecvMIC, 8)) {
-					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;
-					}
-					DPRINTK(4, "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 =
-						    (uint16_t) ((now -
-								 mic_failure->
-								 last_failure_time)
-								/ HZ);
-						if (!mic_failure->counter)	/* mic_failure counter value range 1-60 */
-							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)",
-						auth_type - 1,
-						eth_hdr->
-						h_dest[0] & 0x01 ? "broad" :
-						"uni", eth_hdr->h_source);
-					memset(&wrqu, 0, sizeof(wrqu));
-					wrqu.data.length = strlen(buf);
-					DPRINTK(4,
-						"IWEVENT:MICHAELMICFAILURE\n");
-					wireless_send_event(priv->net_dev,
-							    IWEVCUSTOM, &wrqu,
-							    buf);
-					return;
-				}
-			}
-		}
+		ret = hostif_data_indication_wpa(priv, auth_type);
+		if (ret)
+			return;
 	}
 
 	if ((priv->connect_status & FORCE_DISCONNECT) ||
@@ -441,77 +459,68 @@ void hostif_data_indication(struct ks_wlan_private *priv)
 	case 0xAA:	/* SNAP */
 		rx_ind_size = priv->rx_size - 6;
 		skb = dev_alloc_skb(rx_ind_size);
+		if (!skb) {
+			priv->nstats.rx_dropped++;
+			return;
+		}
 		DPRINTK(4, "SNAP, rx_ind_size = %d\n", rx_ind_size);
 
-		if (skb) {
-			memcpy(skb_put(skb, 12), priv->rxp, 12);	/* 8802/FDDI MAC copy */
-			/* (SNAP+UI..) skip */
-			memcpy(skb_put(skb, rx_ind_size - 12), priv->rxp + 18, rx_ind_size - 12);	/* copy after Type */
-
-			aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + 20);
-			if (aa1x_hdr->type == IEEE802_1X_TYPE_EAPOL_KEY
-			    && priv->wpa.rsn_enabled) {
-				eap_key =
-				    (struct wpa_eapol_key *)(aa1x_hdr + 1);
-				atomic_set(&priv->psstatus.snooze_guard, 1);
-			}
+		size = ETH_ALEN * 2;
+		memcpy(skb_put(skb, size), priv->rxp, size);
 
-			/* 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);
-		} else {
-			priv->nstats.rx_dropped++;
-		}
+		/* (SNAP+UI..) skip */
+
+		size = rx_ind_size - (ETH_ALEN * 2);
+		memcpy(skb_put(skb, size), &eth_hdr->h_proto, size);
+
+		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + ETHER_HDR_SIZE);
 		break;
 	case 0xF0:	/* NETBEUI/NetBIOS */
 		rx_ind_size = (priv->rx_size + 2);
 		skb = dev_alloc_skb(rx_ind_size);
+		if (!skb) {
+			priv->nstats.rx_dropped++;
+			return;
+		}
 		DPRINTK(3, "NETBEUI/NetBIOS rx_ind_size=%d\n", rx_ind_size);
 
-		if (skb) {
-			memcpy(skb_put(skb, 12), priv->rxp, 12);	/* 8802/FDDI MAC copy */
+		memcpy(skb_put(skb, 12), priv->rxp, 12);	/* 8802/FDDI MAC copy */
 
-			temp[0] = (((rx_ind_size - 12) >> 8) & 0xff);	/* NETBEUI size add */
-			temp[1] = ((rx_ind_size - 12) & 0xff);
-			memcpy(skb_put(skb, 2), temp, 2);
+		temp[0] = (((rx_ind_size - 12) >> 8) & 0xff);	/* NETBEUI size add */
+		temp[1] = ((rx_ind_size - 12) & 0xff);
+		memcpy(skb_put(skb, 2), temp, 2);
 
-			memcpy(skb_put(skb, rx_ind_size - 14), priv->rxp + 12, rx_ind_size - 14);	/* copy after Type */
+		memcpy(skb_put(skb, rx_ind_size - 14), priv->rxp + 12,
+		       rx_ind_size - 14);	/* copy after Type */
 
-			aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + 14);
-			if (aa1x_hdr->type == IEEE802_1X_TYPE_EAPOL_KEY
-			    && priv->wpa.rsn_enabled) {
-				eap_key =
-				    (struct wpa_eapol_key *)(aa1x_hdr + 1);
-				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);
-		} else {
-			priv->nstats.rx_dropped++;
-		}
+		aa1x_hdr = (struct ieee802_1x_hdr *)(priv->rxp + 14);
 		break;
 	default:	/* other rx data */
 		DPRINTK(2, "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;
-	uint32_t mib_status;
-	uint32_t mib_attribute;
-	uint16_t mib_val_size;
-	uint16_t mib_val_type;
+	u32 mib_status;
+	u32 mib_attribute;
+	u16 mib_val_size;
+	u16 mib_val_type;
 
 	DPRINTK(3, "\n");
 
@@ -520,7 +529,7 @@ void hostif_mib_get_confirm(struct ks_wlan_private *priv)
 	mib_val_size = get_WORD(priv);	/* MIB value size */
 	mib_val_type = get_WORD(priv);	/* MIB value type */
 
-	if (mib_status != 0) {
+	if (mib_status) {
 		/* in case of error */
 		DPRINTK(1, "attribute=%08X, status=%08X\n", mib_attribute,
 			mib_status);
@@ -588,15 +597,15 @@ void hostif_mib_get_confirm(struct ks_wlan_private *priv)
 static
 void hostif_mib_set_confirm(struct ks_wlan_private *priv)
 {
-	uint32_t mib_status;	/* +04 MIB Status */
-	uint32_t mib_attribute;	/* +08 MIB attribute */
+	u32 mib_status;	/* +04 MIB Status */
+	u32 mib_attribute;	/* +08 MIB attribute */
 
 	DPRINTK(3, "\n");
 
 	mib_status = get_DWORD(priv);	/* MIB Status */
 	mib_attribute = get_DWORD(priv);	/* MIB attribute */
 
-	if (mib_status != 0) {
+	if (mib_status) {
 		/* in case of error */
 		DPRINTK(1, "error :: attribute=%08X, status=%08X\n",
 			mib_attribute, mib_status);
@@ -715,11 +724,11 @@ void hostif_mib_set_confirm(struct ks_wlan_private *priv)
 }
 
 static
-void hostif_power_mngmt_confirm(struct ks_wlan_private *priv)
+void hostif_power_mgmt_confirm(struct ks_wlan_private *priv)
 {
 	DPRINTK(3, "\n");
 
-	if (priv->reg.powermgt > POWMGT_ACTIVE_MODE &&
+	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;
@@ -735,8 +744,7 @@ void hostif_sleep_confirm(struct ks_wlan_private *priv)
 	DPRINTK(3, "\n");
 
 	atomic_set(&priv->sleepstatus.doze_request, 1);
-	queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-			   &priv->ks_wlan_hw.rw_wq, 1);
+	queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
 }
 
 static
@@ -748,7 +756,7 @@ void hostif_start_confirm(struct ks_wlan_private *priv)
 	wrqu.data.length = 0;
 	wrqu.data.flags = 0;
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
+	if (is_connect_status(priv->connect_status)) {
 		eth_zero_addr(wrqu.ap_addr.sa_data);
 		DPRINTK(3, "IWEVENT: disconnect\n");
 		wireless_send_event(priv->net_dev, SIOCGIWAP, &wrqu, NULL);
@@ -795,8 +803,8 @@ void hostif_connect_indication(struct ks_wlan_private *priv)
 	}
 
 	get_current_ap(priv, (struct link_ap_info_t *)priv->rxp);
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS &&
-	    (old_status & CONNECT_STATUS_MASK) == DISCONNECT_STATUS) {
+	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);
@@ -806,8 +814,8 @@ void hostif_connect_indication(struct ks_wlan_private *priv)
 	wrqu0.data.length = 0;
 	wrqu0.data.flags = 0;
 	wrqu0.ap_addr.sa_family = ARPHRD_ETHER;
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == DISCONNECT_STATUS &&
-	    (old_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
+	if (is_disconnect_status(priv->connect_status) &&
+	    is_connect_status(old_status)) {
 		eth_zero_addr(wrqu0.ap_addr.sa_data);
 		DPRINTK(3, "IWEVENT: disconnect\n");
 		DPRINTK(3, "disconnect :: scan_ind_count=%d\n",
@@ -826,18 +834,16 @@ void hostif_scan_indication(struct ks_wlan_private *priv)
 	DPRINTK(3, "scan_ind_count = %d\n", priv->scan_ind_count);
 	ap_info = (struct ap_info_t *)(priv->rxp);
 
-	if (priv->scan_ind_count != 0) {
+	if (priv->scan_ind_count) {
 		for (i = 0; i < priv->aplist.size; i++) {	/* bssid check */
-			if (!memcmp
-			    (&(ap_info->bssid[0]),
-			     &(priv->aplist.ap[i].bssid[0]), ETH_ALEN)) {
-				if (ap_info->frame_type ==
-				    FRAME_TYPE_PROBE_RESP)
-					get_ap_information(priv, ap_info,
-							   &(priv->aplist.
-							     ap[i]));
-				return;
-			}
+			if (memcmp(ap_info->bssid,
+				   priv->aplist.ap[i].bssid, ETH_ALEN) != 0)
+				continue;
+
+			if (ap_info->frame_type == FRAME_TYPE_PROBE_RESP)
+				get_ap_information(priv, ap_info,
+						   &priv->aplist.ap[i]);
+			return;
 		}
 	}
 	priv->scan_ind_count++;
@@ -845,8 +851,7 @@ void hostif_scan_indication(struct ks_wlan_private *priv)
 		DPRINTK(4, " scan_ind_count=%d :: aplist.size=%d\n",
 			priv->scan_ind_count, priv->aplist.size);
 		get_ap_information(priv, (struct ap_info_t *)(priv->rxp),
-				   &(priv->aplist.
-				     ap[priv->scan_ind_count - 1]));
+				   &(priv->aplist.ap[priv->scan_ind_count - 1]));
 		priv->aplist.size = priv->scan_ind_count;
 	} else {
 		DPRINTK(4, " count over :: scan_ind_count=%d\n",
@@ -867,7 +872,7 @@ void hostif_stop_confirm(struct ks_wlan_private *priv)
 		priv->dev_state = DEVICE_STATE_READY;
 
 	/* disconnect indication */
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
+	if (is_connect_status(priv->connect_status)) {
 		netif_carrier_off(netdev);
 		tmp = FORCE_DISCONNECT & priv->connect_status;
 		priv->connect_status = tmp | DISCONNECT_STATUS;
@@ -876,9 +881,8 @@ void hostif_stop_confirm(struct ks_wlan_private *priv)
 		wrqu0.data.length = 0;
 		wrqu0.data.flags = 0;
 		wrqu0.ap_addr.sa_family = ARPHRD_ETHER;
-		if ((priv->connect_status & CONNECT_STATUS_MASK) ==
-		    DISCONNECT_STATUS
-		    && (old_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
+		if (is_disconnect_status(priv->connect_status) &&
+		    is_connect_status(old_status)) {
 			eth_zero_addr(wrqu0.ap_addr.sa_data);
 			DPRINTK(3, "IWEVENT: disconnect\n");
 			netdev_info(netdev, "IWEVENT: disconnect\n");
@@ -903,7 +907,7 @@ void hostif_ps_adhoc_set_confirm(struct ks_wlan_private *priv)
 static
 void hostif_infrastructure_set_confirm(struct ks_wlan_private *priv)
 {
-	uint16_t result_code;
+	u16 result_code;
 
 	DPRINTK(3, "\n");
 	result_code = get_WORD(priv);
@@ -1011,9 +1015,15 @@ void hostif_phy_information_confirm(struct ks_wlan_private *priv)
 	wstats->qual.noise = 0;	/* invalid noise value */
 	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
 
-	DPRINTK(3, "\n    rssi=%u\n    signal=%u\n    LinkSpeed=%ux500Kbps\n \
-   TransmittedFrameCount=%u\n    ReceivedFragmentCount=%u\n    FailedCount=%u\n \
-   FCSErrorCount=%u\n", rssi, signal, LinkSpeed, TransmittedFrameCount, ReceivedFragmentCount, FailedCount, FCSErrorCount);
+	DPRINTK(3, "\n    rssi=%u\n"
+		   "    signal=%u\n"
+		   "    LinkSpeed=%ux500Kbps\n"
+		   "    TransmittedFrameCount=%u\n"
+		   "    ReceivedFragmentCount=%u\n"
+		   "    FailedCount=%u\n"
+		   "    FCSErrorCount=%u\n",
+		rssi, signal, LinkSpeed, TransmittedFrameCount,
+		ReceivedFragmentCount, FailedCount, FCSErrorCount);
 
 	/* wake_up_interruptible_all(&priv->confirm_wait); */
 	complete(&priv->confirm_wait);
@@ -1043,8 +1053,8 @@ void hostif_event_check(struct ks_wlan_private *priv)
 	case HIF_MIB_SET_CONF:
 		hostif_mib_set_confirm(priv);
 		break;
-	case HIF_POWERMGT_CONF:
-		hostif_power_mngmt_confirm(priv);
+	case HIF_POWER_MGMT_CONF:
+		hostif_power_mgmt_confirm(priv);
 		break;
 	case HIF_SLEEP_CONF:
 		hostif_sleep_confirm(priv);
@@ -1099,11 +1109,24 @@ void hostif_event_check(struct ks_wlan_private *priv)
 	priv->hostt.qtail = (priv->hostt.qtail + 1) % SME_EVENT_BUFF_SIZE;
 }
 
-#define CHECK_ALINE(size) (size % 4 ? (size + (4 - (size % 4))) : 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), KS_WLAN_MEM_FLAG);
+	if (!p)
+		return NULL;
 
-int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
+	p->size = cpu_to_le16((u16)(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 packet_len = 0;
+	unsigned int skb_len = 0;
 
 	unsigned char *buffer = NULL;
 	unsigned int length = 0;
@@ -1112,27 +1135,29 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 	int result = 0;
 	unsigned short eth_proto;
 	struct ether_hdr *eth_hdr;
-	struct michel_mic_t michel_mic;
+	struct michael_mic_t michael_mic;
 	unsigned short keyinfo = 0;
 	struct ieee802_1x_hdr *aa1x_hdr;
 	struct wpa_eapol_key *eap_key;
 	struct ethhdr *eth;
+	size_t size;
+	int ret;
 
-	packet_len = packet->len;
-	if (packet_len > ETH_FRAME_LEN) {
-		DPRINTK(1, "bad length packet_len=%d\n", packet_len);
-		dev_kfree_skb(packet);
-		return -1;
+	skb_len = skb->len;
+	if (skb_len > ETH_FRAME_LEN) {
+		DPRINTK(1, "bad length skb_len=%d\n", skb_len);
+		ret = -EOVERFLOW;
+		goto err_kfree_skb;
 	}
 
-	if (((priv->connect_status & CONNECT_STATUS_MASK) == DISCONNECT_STATUS)
-	    || (priv->connect_status & FORCE_DISCONNECT)
-	    || priv->wpa.mic_failure.stop) {
+	if (is_disconnect_status(priv->connect_status) ||
+	    (priv->connect_status & FORCE_DISCONNECT) ||
+	    priv->wpa.mic_failure.stop) {
 		DPRINTK(3, " DISCONNECT\n");
 		if (netif_queue_stopped(priv->net_dev))
 			netif_wake_queue(priv->net_dev);
-		if (packet)
-			dev_kfree_skb(packet);
+		if (skb)
+			dev_kfree_skb(skb);
 
 		return 0;
 	}
@@ -1143,36 +1168,34 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 			netif_stop_queue(priv->net_dev);
 	}
 
-	DPRINTK(4, "skb_buff length=%d\n", packet_len);
-	pp = kmalloc(hif_align_size(sizeof(*pp) + 6 + packet_len + 8),
-		     KS_WLAN_MEM_FLAG);
-
+	size = sizeof(*pp) + 6 + skb_len + 8;
+	pp = kmalloc(hif_align_size(size), KS_WLAN_MEM_FLAG);
 	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
-		dev_kfree_skb(packet);
-		return -2;
+		ret = -ENOMEM;
+		goto err_kfree_skb;
 	}
 
 	p = (unsigned char *)pp->data;
 
-	buffer = packet->data;
-	length = packet->len;
+	buffer = skb->data;
+	length = skb->len;
 
-	/* packet check */
-	eth = (struct ethhdr *)packet->data;
-	if (memcmp(&priv->eth_addr[0], eth->h_source, ETH_ALEN)) {
+	/* skb check */
+	eth = (struct ethhdr *)skb->data;
+	if (memcmp(&priv->eth_addr[0], eth->h_source, ETH_ALEN) != 0) {
 		DPRINTK(1, "invalid mac address !!\n");
 		DPRINTK(1, "ethernet->h_source=%pM\n", eth->h_source);
-		dev_kfree_skb(packet);
-		kfree(pp);
-		return -3;
+		ret = -ENXIO;
+		goto err_kfree;
 	}
 
-	/* MAC address copy */
-	memcpy(p, buffer, 12);	/* DST/SRC MAC address */
-	p += 12;
-	buffer += 12;
-	length -= 12;
+	/* 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); */
@@ -1184,13 +1207,13 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 		*p++ = 0x00;	/* OUI ("000000") */
 		*p++ = 0x00;	/* OUI ("000000") */
 		*p++ = 0x00;	/* OUI ("000000") */
-		packet_len += 6;
+		skb_len += 6;
 	} else {
 		DPRINTK(4, "DIX\n");
 		/* Length(2 byte) delete */
 		buffer += 2;
 		length -= 2;
-		packet_len -= 2;
+		skb_len -= 2;
 	}
 
 	/* pp->data copy */
@@ -1203,8 +1226,8 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 	eth_proto = ntohs(eth_hdr->h_proto);
 
 	/* for MIC FAILURE REPORT check */
-	if (eth_proto == ETHER_PROTOCOL_TYPE_EAP
-	    && priv->wpa.mic_failure.failure > 0) {
+	if (eth_proto == ETHER_PROTOCOL_TYPE_EAP &&
+	    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);
@@ -1213,53 +1236,54 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 	}
 
 	if (priv->wpa.rsn_enabled && priv->wpa.key[0].key_len) {
-		if (eth_proto == ETHER_PROTOCOL_TYPE_EAP
-		    && !(priv->wpa.key[1].key_len)
-		    && !(priv->wpa.key[2].key_len)
-		    && !(priv->wpa.key[3].key_len)) {
-			pp->auth_type = cpu_to_le16((uint16_t) TYPE_AUTH);	/* no encryption */
+		if (eth_proto == ETHER_PROTOCOL_TYPE_EAP &&
+		    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((uint16_t)TYPE_AUTH);	/* no encryption */
 		} else {
 			if (priv->wpa.pairwise_suite == IW_AUTH_CIPHER_TKIP) {
-				MichaelMICFunction(&michel_mic, (uint8_t *) priv->wpa.key[0].tx_mic_key, (uint8_t *) &pp->data[0], (int)packet_len, (uint8_t) 0,	/* priority */
-						   (uint8_t *) michel_mic.
-						   Result);
-				memcpy(p, michel_mic.Result, 8);
+				MichaelMICFunction(&michael_mic,
+						   (uint8_t *)priv->wpa.key[0].tx_mic_key,
+						   (uint8_t *)&pp->data[0],
+						   (int)skb_len,
+						   (uint8_t)0,	/* priority */
+						   (uint8_t *)michael_mic.Result);
+				memcpy(p, michael_mic.Result, 8);
 				length += 8;
-				packet_len += 8;
+				skb_len += 8;
 				p += 8;
 				pp->auth_type =
-				    cpu_to_le16((uint16_t) TYPE_DATA);
+				    cpu_to_le16((uint16_t)TYPE_DATA);
 
 			} else if (priv->wpa.pairwise_suite ==
 				   IW_AUTH_CIPHER_CCMP) {
 				pp->auth_type =
-				    cpu_to_le16((uint16_t) TYPE_DATA);
+				    cpu_to_le16((uint16_t)TYPE_DATA);
 			}
 		}
 	} else {
 		if (eth_proto == ETHER_PROTOCOL_TYPE_EAP)
-			pp->auth_type = cpu_to_le16((uint16_t) TYPE_AUTH);
+			pp->auth_type = cpu_to_le16((uint16_t)TYPE_AUTH);
 		else
-			pp->auth_type = cpu_to_le16((uint16_t) TYPE_DATA);
+			pp->auth_type = cpu_to_le16((uint16_t)TYPE_DATA);
 	}
 
 	/* header value set */
 	pp->header.size =
 	    cpu_to_le16((uint16_t)
-			(sizeof(*pp) - sizeof(pp->header.size) + packet_len));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_DATA_REQ);
+			(sizeof(*pp) - sizeof(pp->header.size) + skb_len));
+	pp->header.event = cpu_to_le16((uint16_t)HIF_DATA_REQ);
 
 	/* tx request */
-	result =
-	    ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp) + packet_len),
-			  (void *)send_packet_complete, (void *)priv,
-			  (void *)packet);
+	result = ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp) + skb_len),
+			       send_packet_complete, skb);
 
 	/* MIC FAILURE REPORT check */
-	if (eth_proto == ETHER_PROTOCOL_TYPE_EAP
-	    && priv->wpa.mic_failure.failure > 0) {
-		if (keyinfo & WPA_KEY_INFO_ERROR
-		    && keyinfo & WPA_KEY_INFO_REQUEST) {
+	if (eth_proto == ETHER_PROTOCOL_TYPE_EAP &&
+	    priv->wpa.mic_failure.failure > 0) {
+		if (keyinfo & WPA_KEY_INFO_ERROR &&
+		    keyinfo & WPA_KEY_INFO_REQUEST) {
 			DPRINTK(3, " MIC ERROR Report SET : %04X\n", keyinfo);
 			hostif_sme_enqueue(priv, SME_MIC_FAILURE_REQUEST);
 		}
@@ -1268,13 +1292,20 @@ int hostif_data_request(struct ks_wlan_private *priv, struct sk_buff *packet)
 	}
 
 	return result;
+
+err_kfree:
+	kfree(pp);
+err_kfree_skb:
+	dev_kfree_skb(skb);
+
+	return ret;
 }
 
-#define ps_confirm_wait_inc(priv) do { \
-	if (atomic_read(&priv->psstatus.status) > PS_ACTIVE_SET) { \
-		atomic_inc(&priv->psstatus.confirm_wait); \
-		/* atomic_set(&priv->psstatus.status, PS_CONF_WAIT);*/ \
-	} } while (0)
+#define ps_confirm_wait_inc(priv)					 \
+	do {								 \
+		if (atomic_read(&priv->psstatus.status) > PS_ACTIVE_SET) \
+			atomic_inc(&priv->psstatus.confirm_wait);	 \
+	} while (0)
 
 static
 void hostif_mib_get_request(struct ks_wlan_private *priv,
@@ -1284,20 +1315,15 @@ void hostif_mib_get_request(struct ks_wlan_private *priv,
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIB_GET_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_MIB_GET_REQ);
-	pp->mib_attribute = cpu_to_le32((uint32_t) mib_attribute);
+
+	pp->mib_attribute = cpu_to_le32((uint32_t)mib_attribute);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
@@ -1314,26 +1340,18 @@ void hostif_mib_set_request(struct ks_wlan_private *priv,
 		return;
 	}
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp) + size), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIB_SET_REQ);
+	if (!pp)
 		return;
-	}
 
-	pp->header.size =
-	    cpu_to_le16((uint16_t)
-			(sizeof(*pp) - sizeof(pp->header.size) + size));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_MIB_SET_REQ);
-	pp->mib_attribute = cpu_to_le32((uint32_t) mib_attribute);
-	pp->mib_value.size = cpu_to_le16((uint16_t) size);
-	pp->mib_value.type = cpu_to_le16((uint16_t) type);
+	pp->mib_attribute = cpu_to_le32((uint32_t)mib_attribute);
+	pp->mib_value.size = cpu_to_le16((uint16_t)size);
+	pp->mib_value.type = cpu_to_le16((uint16_t)type);
 	memcpy(&pp->mib_value.body, vp, size);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp) + size), NULL, NULL,
-		      NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp) + size), NULL, NULL);
 }
 
 static
@@ -1343,20 +1361,15 @@ void hostif_start_request(struct ks_wlan_private *priv, unsigned char mode)
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_START_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_START_REQ);
-	pp->mode = cpu_to_le16((uint16_t) mode);
+
+	pp->mode = cpu_to_le16((uint16_t)mode);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 
 	priv->aplist.size = 0;
 	priv->scan_ind_count = 0;
@@ -1366,24 +1379,18 @@ static
 void hostif_ps_adhoc_set_request(struct ks_wlan_private *priv)
 {
 	struct hostif_ps_adhoc_set_request_t *pp;
-	uint16_t capability;
+	u16 capability;
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_PS_ADH_SET_REQ);
+	if (!pp)
 		return;
-	}
-	memset(pp, 0, sizeof(*pp));
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_PS_ADH_SET_REQ);
-	pp->phy_type = cpu_to_le16((uint16_t) (priv->reg.phy_type));
-	pp->cts_mode = cpu_to_le16((uint16_t) (priv->reg.cts_mode));
-	pp->scan_type = cpu_to_le16((uint16_t) (priv->reg.scan_type));
-	pp->channel = cpu_to_le16((uint16_t) (priv->reg.channel));
+
+	pp->phy_type = cpu_to_le16((uint16_t)(priv->reg.phy_type));
+	pp->cts_mode = cpu_to_le16((uint16_t)(priv->reg.cts_mode));
+	pp->scan_type = cpu_to_le16((uint16_t)(priv->reg.scan_type));
+	pp->channel = cpu_to_le16((uint16_t)(priv->reg.channel));
 	pp->rate_set.size = priv->reg.rate_set.size;
 	memcpy(&pp->rate_set.body[0], &priv->reg.rate_set.body[0],
 	       priv->reg.rate_set.size);
@@ -1398,33 +1405,28 @@ void hostif_ps_adhoc_set_request(struct ks_wlan_private *priv)
 		capability |= BSS_CAP_SHORT_SLOT_TIME;	/* ShortSlotTime support */
 		capability &= ~(BSS_CAP_DSSS_OFDM);	/* DSSS OFDM */
 	}
-	pp->capability = cpu_to_le16((uint16_t) capability);
+	pp->capability = cpu_to_le16((uint16_t)capability);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
 void hostif_infrastructure_set_request(struct ks_wlan_private *priv)
 {
 	struct hostif_infrastructure_set_request_t *pp;
-	uint16_t capability;
+	u16 capability;
 
 	DPRINTK(3, "ssid.size=%d\n", priv->reg.ssid.size);
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_INFRA_SET_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_INFRA_SET_REQ);
-	pp->phy_type = cpu_to_le16((uint16_t) (priv->reg.phy_type));
-	pp->cts_mode = cpu_to_le16((uint16_t) (priv->reg.cts_mode));
-	pp->scan_type = cpu_to_le16((uint16_t) (priv->reg.scan_type));
+
+	pp->phy_type = cpu_to_le16((uint16_t)(priv->reg.phy_type));
+	pp->cts_mode = cpu_to_le16((uint16_t)(priv->reg.cts_mode));
+	pp->scan_type = cpu_to_le16((uint16_t)(priv->reg.scan_type));
 
 	pp->rate_set.size = priv->reg.rate_set.size;
 	memcpy(&pp->rate_set.body[0], &priv->reg.rate_set.body[0],
@@ -1442,10 +1444,10 @@ void hostif_infrastructure_set_request(struct ks_wlan_private *priv)
 		capability |= BSS_CAP_SHORT_SLOT_TIME;	/* ShortSlotTime support */
 		capability &= ~(BSS_CAP_DSSS_OFDM);	/* DSSS OFDM not support */
 	}
-	pp->capability = cpu_to_le16((uint16_t) capability);
+	pp->capability = cpu_to_le16((uint16_t)capability);
 	pp->beacon_lost_count =
-	    cpu_to_le16((uint16_t) (priv->reg.beacon_lost_count));
-	pp->auth_type = cpu_to_le16((uint16_t) (priv->reg.authenticate_type));
+	    cpu_to_le16((uint16_t)(priv->reg.beacon_lost_count));
+	pp->auth_type = cpu_to_le16((uint16_t)(priv->reg.authenticate_type));
 
 	pp->channel_list.body[0] = 1;
 	pp->channel_list.body[1] = 8;
@@ -1469,28 +1471,23 @@ void hostif_infrastructure_set_request(struct ks_wlan_private *priv)
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static void hostif_infrastructure_set2_request(struct ks_wlan_private *priv)
 {
 	struct hostif_infrastructure_set2_request_t *pp;
-	uint16_t capability;
+	u16 capability;
 
 	DPRINTK(2, "ssid.size=%d\n", priv->reg.ssid.size);
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_INFRA_SET2_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_INFRA_SET2_REQ);
-	pp->phy_type = cpu_to_le16((uint16_t) (priv->reg.phy_type));
-	pp->cts_mode = cpu_to_le16((uint16_t) (priv->reg.cts_mode));
-	pp->scan_type = cpu_to_le16((uint16_t) (priv->reg.scan_type));
+
+	pp->phy_type = cpu_to_le16((uint16_t)(priv->reg.phy_type));
+	pp->cts_mode = cpu_to_le16((uint16_t)(priv->reg.cts_mode));
+	pp->scan_type = cpu_to_le16((uint16_t)(priv->reg.scan_type));
 
 	pp->rate_set.size = priv->reg.rate_set.size;
 	memcpy(&pp->rate_set.body[0], &priv->reg.rate_set.body[0],
@@ -1508,10 +1505,10 @@ static void hostif_infrastructure_set2_request(struct ks_wlan_private *priv)
 		capability |= BSS_CAP_SHORT_SLOT_TIME;	/* ShortSlotTime support */
 		capability &= ~(BSS_CAP_DSSS_OFDM);	/* DSSS OFDM not support */
 	}
-	pp->capability = cpu_to_le16((uint16_t) capability);
+	pp->capability = cpu_to_le16((uint16_t)capability);
 	pp->beacon_lost_count =
-	    cpu_to_le16((uint16_t) (priv->reg.beacon_lost_count));
-	pp->auth_type = cpu_to_le16((uint16_t) (priv->reg.authenticate_type));
+	    cpu_to_le16((uint16_t)(priv->reg.beacon_lost_count));
+	pp->auth_type = cpu_to_le16((uint16_t)(priv->reg.authenticate_type));
 
 	pp->channel_list.body[0] = 1;
 	pp->channel_list.body[1] = 8;
@@ -1537,31 +1534,25 @@ static void hostif_infrastructure_set2_request(struct ks_wlan_private *priv)
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
 void hostif_adhoc_set_request(struct ks_wlan_private *priv)
 {
 	struct hostif_adhoc_set_request_t *pp;
-	uint16_t capability;
+	u16 capability;
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_ADH_SET_REQ);
+	if (!pp)
 		return;
-	}
-	memset(pp, 0, sizeof(*pp));
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_ADH_SET_REQ);
-	pp->phy_type = cpu_to_le16((uint16_t) (priv->reg.phy_type));
-	pp->cts_mode = cpu_to_le16((uint16_t) (priv->reg.cts_mode));
-	pp->scan_type = cpu_to_le16((uint16_t) (priv->reg.scan_type));
-	pp->channel = cpu_to_le16((uint16_t) (priv->reg.channel));
+
+	pp->phy_type = cpu_to_le16((uint16_t)(priv->reg.phy_type));
+	pp->cts_mode = cpu_to_le16((uint16_t)(priv->reg.cts_mode));
+	pp->scan_type = cpu_to_le16((uint16_t)(priv->reg.scan_type));
+	pp->channel = cpu_to_le16((uint16_t)(priv->reg.channel));
 	pp->rate_set.size = priv->reg.rate_set.size;
 	memcpy(&pp->rate_set.body[0], &priv->reg.rate_set.body[0],
 	       priv->reg.rate_set.size);
@@ -1578,34 +1569,28 @@ void hostif_adhoc_set_request(struct ks_wlan_private *priv)
 		capability |= BSS_CAP_SHORT_SLOT_TIME;	/* ShortSlotTime support */
 		capability &= ~(BSS_CAP_DSSS_OFDM);	/* DSSS OFDM not support */
 	}
-	pp->capability = cpu_to_le16((uint16_t) capability);
+	pp->capability = cpu_to_le16((uint16_t)capability);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
 void hostif_adhoc_set2_request(struct ks_wlan_private *priv)
 {
 	struct hostif_adhoc_set2_request_t *pp;
-	uint16_t capability;
+	u16 capability;
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_ADH_SET_REQ);
+	if (!pp)
 		return;
-	}
-	memset(pp, 0, sizeof(*pp));
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_ADH_SET_REQ);
-	pp->phy_type = cpu_to_le16((uint16_t) (priv->reg.phy_type));
-	pp->cts_mode = cpu_to_le16((uint16_t) (priv->reg.cts_mode));
-	pp->scan_type = cpu_to_le16((uint16_t) (priv->reg.scan_type));
+
+	pp->phy_type = cpu_to_le16((uint16_t)(priv->reg.phy_type));
+	pp->cts_mode = cpu_to_le16((uint16_t)(priv->reg.cts_mode));
+	pp->scan_type = cpu_to_le16((uint16_t)(priv->reg.scan_type));
 	pp->rate_set.size = priv->reg.rate_set.size;
 	memcpy(&pp->rate_set.body[0], &priv->reg.rate_set.body[0],
 	       priv->reg.rate_set.size);
@@ -1622,7 +1607,7 @@ void hostif_adhoc_set2_request(struct ks_wlan_private *priv)
 		capability |= BSS_CAP_SHORT_SLOT_TIME;	/* ShortSlotTime support */
 		capability &= ~(BSS_CAP_DSSS_OFDM);	/* DSSS OFDM not support */
 	}
-	pp->capability = cpu_to_le16((uint16_t) capability);
+	pp->capability = cpu_to_le16((uint16_t)capability);
 
 	pp->channel_list.body[0] = priv->reg.channel;
 	pp->channel_list.size = 1;
@@ -1630,7 +1615,7 @@ void hostif_adhoc_set2_request(struct ks_wlan_private *priv)
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
@@ -1640,19 +1625,13 @@ void hostif_stop_request(struct ks_wlan_private *priv)
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_STOP_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_STOP_REQ);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
@@ -1662,84 +1641,68 @@ void hostif_phy_information_request(struct ks_wlan_private *priv)
 
 	DPRINTK(3, "\n");
 
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+	pp = hostif_generic_request(sizeof(*pp), HIF_PHY_INFO_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_PHY_INFO_REQ);
+
 	if (priv->reg.phy_info_timer) {
-		pp->type = cpu_to_le16((uint16_t) TIME_TYPE);
-		pp->time = cpu_to_le16((uint16_t) (priv->reg.phy_info_timer));
+		pp->type = cpu_to_le16((uint16_t)TIME_TYPE);
+		pp->time = cpu_to_le16((uint16_t)(priv->reg.phy_info_timer));
 	} else {
-		pp->type = cpu_to_le16((uint16_t) NORMAL_TYPE);
-		pp->time = cpu_to_le16((uint16_t) 0);
+		pp->type = cpu_to_le16((uint16_t)NORMAL_TYPE);
+		pp->time = cpu_to_le16((uint16_t)0);
 	}
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
-void hostif_power_mngmt_request(struct ks_wlan_private *priv,
-				unsigned long mode, unsigned long wake_up,
-				unsigned long receiveDTIMs)
+void hostif_power_mgmt_request(struct ks_wlan_private *priv,
+			       unsigned long mode, unsigned long wake_up,
+			       unsigned long receiveDTIMs)
 {
-	struct hostif_power_mngmt_request_t *pp;
+	struct hostif_power_mgmt_request_t *pp;
 
 	DPRINTK(3, "mode=%lu wake_up=%lu receiveDTIMs=%lu\n", mode, wake_up,
 		receiveDTIMs);
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_POWER_MGMT_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_POWERMGT_REQ);
-	pp->mode = cpu_to_le32((uint32_t) mode);
-	pp->wake_up = cpu_to_le32((uint32_t) wake_up);
-	pp->receiveDTIMs = cpu_to_le32((uint32_t) receiveDTIMs);
+
+	pp->mode = cpu_to_le32((uint32_t)mode);
+	pp->wake_up = cpu_to_le32((uint32_t)wake_up);
+	pp->receiveDTIMs = cpu_to_le32((uint32_t)receiveDTIMs);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 static
-void hostif_sleep_request(struct ks_wlan_private *priv, unsigned long mode)
+void hostif_sleep_request(struct ks_wlan_private *priv,
+			  enum sleep_mode_type mode)
 {
 	struct hostif_sleep_request_t *pp;
 
-	DPRINTK(3, "mode=%lu\n", mode);
+	DPRINTK(3, "mode=%lu\n", (long)mode);
 
 	if (mode == SLP_SLEEP) {
-		/* make primitive */
-		pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-		if (!pp) {
-			DPRINTK(3, "allocate memory failed..\n");
+		pp = hostif_generic_request(sizeof(*pp), HIF_SLEEP_REQ);
+		if (!pp)
 			return;
-		}
-		pp->header.size =
-		    cpu_to_le16((uint16_t)
-				(sizeof(*pp) - sizeof(pp->header.size)));
-		pp->header.event = cpu_to_le16((uint16_t) HIF_SLEEP_REQ);
 
 		/* send to device request */
 		ps_confirm_wait_inc(priv);
-		ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL,
+		ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL,
 			      NULL);
 	} else if (mode == SLP_ACTIVE) {
 		atomic_set(&priv->sleepstatus.wakeup_request, 1);
-		queue_delayed_work(priv->ks_wlan_hw.ks7010sdio_wq,
-				   &priv->ks_wlan_hw.rw_wq, 1);
+		queue_delayed_work(priv->wq, &priv->rw_dwork, 1);
 	} else {
-		DPRINTK(3, "invalid mode %ld\n", mode);
+		DPRINTK(3, "invalid mode %ld\n", (long)mode);
 		return;
 	}
 }
@@ -1752,19 +1715,15 @@ void hostif_bss_scan_request(struct ks_wlan_private *priv,
 	struct hostif_bss_scan_request_t *pp;
 
 	DPRINTK(2, "\n");
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_SCAN_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_SCAN_REQ);
+
 	pp->scan_type = scan_type;
 
-	pp->ch_time_min = cpu_to_le32((uint32_t) 110);	/* default value */
-	pp->ch_time_max = cpu_to_le32((uint32_t) 130);	/* default value */
+	pp->ch_time_min = cpu_to_le32((uint32_t)110);	/* default value */
+	pp->ch_time_max = cpu_to_le32((uint32_t)130);	/* default value */
 	pp->channel_list.body[0] = 1;
 	pp->channel_list.body[1] = 8;
 	pp->channel_list.body[2] = 2;
@@ -1794,7 +1753,7 @@ void hostif_bss_scan_request(struct ks_wlan_private *priv,
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 
 	priv->aplist.size = 0;
 	priv->scan_ind_count = 0;
@@ -1808,21 +1767,17 @@ void hostif_mic_failure_request(struct ks_wlan_private *priv,
 	struct hostif_mic_failure_request_t *pp;
 
 	DPRINTK(3, "count=%d :: timer=%d\n", failure_count, timer);
-	/* make primitive */
-	pp = kmalloc(hif_align_size(sizeof(*pp)), KS_WLAN_MEM_FLAG);
-	if (!pp) {
-		DPRINTK(3, "allocate memory failed..\n");
+
+	pp = hostif_generic_request(sizeof(*pp), HIF_MIC_FAILURE_REQ);
+	if (!pp)
 		return;
-	}
-	pp->header.size =
-	    cpu_to_le16((uint16_t) (sizeof(*pp) - sizeof(pp->header.size)));
-	pp->header.event = cpu_to_le16((uint16_t) HIF_MIC_FAILURE_REQ);
-	pp->failure_count = cpu_to_le16((uint16_t) failure_count);
-	pp->timer = cpu_to_le16((uint16_t) timer);
+
+	pp->failure_count = cpu_to_le16((uint16_t)failure_count);
+	pp->timer = cpu_to_le16((uint16_t)timer);
 
 	/* send to device request */
 	ps_confirm_wait_inc(priv);
-	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL, NULL);
+	ks_wlan_hw_tx(priv, pp, hif_align_size(sizeof(*pp)), NULL, NULL);
 }
 
 /* Device I/O Receive indicate */
@@ -1867,11 +1822,11 @@ void hostif_receive(struct ks_wlan_private *priv, unsigned char *p,
 static
 void hostif_sme_set_wep(struct ks_wlan_private *priv, int type)
 {
-	uint32_t val;
+	u32 val;
 
 	switch (type) {
 	case SME_WEP_INDEX_REQUEST:
-		val = cpu_to_le32((uint32_t) (priv->reg.wep_index));
+		val = cpu_to_le32((uint32_t)(priv->reg.wep_index));
 		hostif_mib_set_request(priv, DOT11_WEP_DEFAULT_KEY_ID,
 				       sizeof(val), MIB_VALUE_TYPE_INT, &val);
 		break;
@@ -1908,7 +1863,7 @@ void hostif_sme_set_wep(struct ks_wlan_private *priv, int type)
 					       &priv->reg.wep_key[3].val[0]);
 		break;
 	case SME_WEP_FLAG_REQUEST:
-		val = cpu_to_le32((uint32_t) (priv->reg.privacy_invoked));
+		val = cpu_to_le32((uint32_t)(priv->reg.privacy_invoked));
 		hostif_mib_set_request(priv, DOT11_PRIVACY_INVOKED,
 				       sizeof(val), MIB_VALUE_TYPE_BOOL, &val);
 		break;
@@ -1921,8 +1876,8 @@ struct wpa_suite_t {
 } __packed;
 
 struct rsn_mode_t {
-	uint32_t rsn_mode;
-	uint16_t rsn_capability;
+	u32 rsn_mode;
+	u16 rsn_capability;
 } __packed;
 
 static
@@ -1930,13 +1885,13 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 {
 	struct wpa_suite_t wpa_suite;
 	struct rsn_mode_t rsn_mode;
-	uint32_t val;
+	u32 val;
 
 	memset(&wpa_suite, 0, sizeof(wpa_suite));
 
 	switch (type) {
 	case SME_RSN_UCAST_REQUEST:
-		wpa_suite.size = cpu_to_le16((uint16_t) 1);
+		wpa_suite.size = cpu_to_le16((uint16_t)1);
 		switch (priv->wpa.pairwise_suite) {
 		case IW_AUTH_CIPHER_NONE:
 			if (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)
@@ -2034,7 +1989,7 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 				       &wpa_suite.suite[0][0]);
 		break;
 	case SME_RSN_AUTH_REQUEST:
-		wpa_suite.size = cpu_to_le16((uint16_t) 1);
+		wpa_suite.size = cpu_to_le16((uint16_t)1);
 		switch (priv->wpa.key_mgmt_suite) {
 		case IW_AUTH_KEY_MGMT_802_1X:
 			if (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2)
@@ -2078,23 +2033,23 @@ void hostif_sme_set_rsn(struct ks_wlan_private *priv, int type)
 				       MIB_VALUE_TYPE_OSTRING, &wpa_suite);
 		break;
 	case SME_RSN_ENABLED_REQUEST:
-		val = cpu_to_le32((uint32_t) (priv->wpa.rsn_enabled));
+		val = cpu_to_le32((uint32_t)(priv->wpa.rsn_enabled));
 		hostif_mib_set_request(priv, DOT11_RSN_ENABLED,
 				       sizeof(val), MIB_VALUE_TYPE_BOOL, &val);
 		break;
 	case SME_RSN_MODE_REQUEST:
 		if (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA2) {
 			rsn_mode.rsn_mode =
-			    cpu_to_le32((uint32_t) RSN_MODE_WPA2);
-			rsn_mode.rsn_capability = cpu_to_le16((uint16_t) 0);
+			    cpu_to_le32((uint32_t)RSN_MODE_WPA2);
+			rsn_mode.rsn_capability = cpu_to_le16((uint16_t)0);
 		} else if (priv->wpa.version == IW_AUTH_WPA_VERSION_WPA) {
 			rsn_mode.rsn_mode =
-			    cpu_to_le32((uint32_t) RSN_MODE_WPA);
-			rsn_mode.rsn_capability = cpu_to_le16((uint16_t) 0);
+			    cpu_to_le32((uint32_t)RSN_MODE_WPA);
+			rsn_mode.rsn_capability = cpu_to_le16((uint16_t)0);
 		} else {
 			rsn_mode.rsn_mode =
-			    cpu_to_le32((uint32_t) RSN_MODE_NONE);
-			rsn_mode.rsn_capability = cpu_to_le16((uint16_t) 0);
+			    cpu_to_le32((uint32_t)RSN_MODE_NONE);
+			rsn_mode.rsn_capability = cpu_to_le16((uint16_t)0);
 		}
 		hostif_mib_set_request(priv, LOCAL_RSN_MODE, sizeof(rsn_mode),
 				       MIB_VALUE_TYPE_OSTRING, &rsn_mode);
@@ -2137,32 +2092,28 @@ void hostif_sme_mode_setup(struct ks_wlan_private *priv)
 	/* 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])) {
-				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
+			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])) {
-				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];
-			} else
+			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;
@@ -2185,7 +2136,7 @@ void hostif_sme_mode_setup(struct ks_wlan_private *priv)
 		break;
 	case MODE_INFRASTRUCTURE:
 		/* Infrastructure mode */
-		if (!is_valid_ether_addr((u8 *) priv->reg.bssid)) {
+		if (!is_valid_ether_addr((u8 *)priv->reg.bssid)) {
 			hostif_infrastructure_set_request(priv);
 		} else {
 			hostif_infrastructure_set2_request(priv);
@@ -2195,7 +2146,7 @@ void hostif_sme_mode_setup(struct ks_wlan_private *priv)
 		break;
 	case MODE_ADHOC:
 		/* IEEE802.11 Ad-Hoc mode */
-		if (!is_valid_ether_addr((u8 *) priv->reg.bssid)) {
+		if (!is_valid_ether_addr((u8 *)priv->reg.bssid)) {
 			hostif_adhoc_set_request(priv);
 		} else {
 			hostif_adhoc_set2_request(priv);
@@ -2225,56 +2176,58 @@ void hostif_sme_multicast_set(struct ks_wlan_private *priv)
 	memset(set_address, 0, NIC_MAX_MCAST_LIST * ETH_ALEN);
 
 	if (dev->flags & IFF_PROMISC) {
-		filter_type = cpu_to_le32((uint32_t) MCAST_FILTER_PROMISC);
+		filter_type = cpu_to_le32((uint32_t)MCAST_FILTER_PROMISC);
 		hostif_mib_set_request(priv, LOCAL_MULTICAST_FILTER,
 				       sizeof(filter_type), MIB_VALUE_TYPE_BOOL,
 				       &filter_type);
-	} else if ((netdev_mc_count(dev) > NIC_MAX_MCAST_LIST)
-		   || (dev->flags & IFF_ALLMULTI)) {
-		filter_type = cpu_to_le32((uint32_t) MCAST_FILTER_MCASTALL);
+		goto spin_unlock;
+	}
+
+	if ((netdev_mc_count(dev) > NIC_MAX_MCAST_LIST) ||
+	    (dev->flags & IFF_ALLMULTI)) {
+		filter_type = cpu_to_le32((uint32_t)MCAST_FILTER_MCASTALL);
 		hostif_mib_set_request(priv, LOCAL_MULTICAST_FILTER,
 				       sizeof(filter_type), MIB_VALUE_TYPE_BOOL,
 				       &filter_type);
-	} else {
-		if (priv->sme_i.sme_flag & SME_MULTICAST) {
-			mc_count = netdev_mc_count(dev);
-			netdev_for_each_mc_addr(ha, dev) {
-				memcpy(&set_address[i * ETH_ALEN], ha->addr,
-				       ETH_ALEN);
-				i++;
-			}
-			priv->sme_i.sme_flag &= ~SME_MULTICAST;
-			hostif_mib_set_request(priv, LOCAL_MULTICAST_ADDRESS,
-					       (ETH_ALEN * mc_count),
-					       MIB_VALUE_TYPE_OSTRING,
-					       &set_address[0]);
-		} else {
-			filter_type =
-			    cpu_to_le32((uint32_t) MCAST_FILTER_MCAST);
-			priv->sme_i.sme_flag |= SME_MULTICAST;
-			hostif_mib_set_request(priv, LOCAL_MULTICAST_FILTER,
-					       sizeof(filter_type),
-					       MIB_VALUE_TYPE_BOOL,
-					       &filter_type);
+		goto spin_unlock;
+	}
+
+	if (priv->sme_i.sme_flag & SME_MULTICAST) {
+		mc_count = netdev_mc_count(dev);
+		netdev_for_each_mc_addr(ha, dev) {
+			memcpy(&set_address[i * ETH_ALEN], ha->addr, ETH_ALEN);
+			i++;
 		}
+		priv->sme_i.sme_flag &= ~SME_MULTICAST;
+		hostif_mib_set_request(priv, LOCAL_MULTICAST_ADDRESS,
+				       ETH_ALEN * mc_count,
+				       MIB_VALUE_TYPE_OSTRING,
+				       &set_address[0]);
+	} else {
+		filter_type = cpu_to_le32((uint32_t)MCAST_FILTER_MCAST);
+		priv->sme_i.sme_flag |= SME_MULTICAST;
+		hostif_mib_set_request(priv, LOCAL_MULTICAST_FILTER,
+				       sizeof(filter_type), MIB_VALUE_TYPE_BOOL,
+				       &filter_type);
 	}
 
+spin_unlock:
 	spin_unlock(&priv->multicast_spin);
 }
 
 static
-void hostif_sme_powermgt_set(struct ks_wlan_private *priv)
+void hostif_sme_power_mgmt_set(struct ks_wlan_private *priv)
 {
 	unsigned long mode, wake_up, receiveDTIMs;
 
 	DPRINTK(3, "\n");
-	switch (priv->reg.powermgt) {
-	case POWMGT_ACTIVE_MODE:
+	switch (priv->reg.power_mgmt) {
+	case POWER_MGMT_ACTIVE:
 		mode = POWER_ACTIVE;
 		wake_up = 0;
 		receiveDTIMs = 0;
 		break;
-	case POWMGT_SAVE1_MODE:
+	case POWER_MGMT_SAVE1:
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
 			mode = POWER_SAVE;
 			wake_up = 0;
@@ -2285,7 +2238,7 @@ void hostif_sme_powermgt_set(struct ks_wlan_private *priv)
 			receiveDTIMs = 0;
 		}
 		break;
-	case POWMGT_SAVE2_MODE:
+	case POWER_MGMT_SAVE2:
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
 			mode = POWER_SAVE;
 			wake_up = 0;
@@ -2302,7 +2255,7 @@ void hostif_sme_powermgt_set(struct ks_wlan_private *priv)
 		receiveDTIMs = 0;
 		break;
 	}
-	hostif_power_mngmt_request(priv, mode, wake_up, receiveDTIMs);
+	hostif_power_mgmt_request(priv, mode, wake_up, receiveDTIMs);
 }
 
 static
@@ -2324,16 +2277,16 @@ void hostif_sme_sleep_set(struct ks_wlan_private *priv)
 static
 void hostif_sme_set_key(struct ks_wlan_private *priv, int type)
 {
-	uint32_t val;
+	u32 val;
 
 	switch (type) {
 	case SME_SET_FLAG:
-		val = cpu_to_le32((uint32_t) (priv->reg.privacy_invoked));
+		val = cpu_to_le32((uint32_t)(priv->reg.privacy_invoked));
 		hostif_mib_set_request(priv, DOT11_PRIVACY_INVOKED,
 				       sizeof(val), MIB_VALUE_TYPE_BOOL, &val);
 		break;
 	case SME_SET_TXKEY:
-		val = cpu_to_le32((uint32_t) (priv->wpa.txkey));
+		val = cpu_to_le32((uint32_t)(priv->wpa.txkey));
 		hostif_mib_set_request(priv, DOT11_WEP_DEFAULT_KEY_ID,
 				       sizeof(val), MIB_VALUE_TYPE_INT, &val);
 		break;
@@ -2383,10 +2336,10 @@ static
 void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
 {
 	struct pmk_cache_t {
-		uint16_t size;
+		u16 size;
 		struct {
-			uint8_t bssid[ETH_ALEN];
-			uint8_t pmkid[IW_PMKID_LEN];
+			u8 bssid[ETH_ALEN];
+			u8 pmkid[IW_PMKID_LEN];
 		} __packed list[PMK_LIST_MAX];
 	} __packed pmkcache;
 	struct pmk_t *pmk;
@@ -2402,7 +2355,7 @@ void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
 			i++;
 		}
 	}
-	pmkcache.size = cpu_to_le16((uint16_t) (priv->pmklist.size));
+	pmkcache.size = cpu_to_le16((uint16_t)(priv->pmklist.size));
 	hostif_mib_set_request(priv, LOCAL_PMK,
 			       sizeof(priv->pmklist.size) + (ETH_ALEN +
 							     IW_PMKID_LEN) *
@@ -2414,7 +2367,7 @@ void hostif_sme_set_pmksa(struct ks_wlan_private *priv)
 static
 void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 {
-	uint32_t val;
+	u32 val;
 
 	DPRINTK(3, "event=%d\n", event);
 	switch (event) {
@@ -2435,7 +2388,7 @@ void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 					priv->scan_ssid, priv->scan_ssid_len);
 		break;
 	case SME_POW_MNGMT_REQUEST:
-		hostif_sme_powermgt_set(priv);
+		hostif_sme_power_mgmt_set(priv);
 		break;
 	case SME_PHY_INFO_REQUEST:
 		hostif_phy_information_request(priv);
@@ -2443,18 +2396,16 @@ void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 	case SME_MIC_FAILURE_REQUEST:
 		if (priv->wpa.mic_failure.failure == 1) {
 			hostif_mic_failure_request(priv,
-						   priv->wpa.mic_failure.
-						   failure - 1, 0);
+						   priv->wpa.mic_failure.failure - 1,
+						   0);
 		} else if (priv->wpa.mic_failure.failure == 2) {
 			hostif_mic_failure_request(priv,
-						   priv->wpa.mic_failure.
-						   failure - 1,
-						   priv->wpa.mic_failure.
-						   counter);
-		} else
-			DPRINTK(4,
-				"SME_MIC_FAILURE_REQUEST: failure count=%u error?\n",
+						   priv->wpa.mic_failure.failure - 1,
+						   priv->wpa.mic_failure.counter);
+		} else {
+			DPRINTK(4, "SME_MIC_FAILURE_REQUEST: failure count=%u error?\n",
 				priv->wpa.mic_failure.failure);
+		}
 		break;
 	case SME_MIC_FAILURE_CONFIRM:
 		if (priv->wpa.mic_failure.failure == 2) {
@@ -2476,12 +2427,12 @@ void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 		hostif_stop_request(priv);
 		break;
 	case SME_RTS_THRESHOLD_REQUEST:
-		val = cpu_to_le32((uint32_t) (priv->reg.rts));
+		val = cpu_to_le32((uint32_t)(priv->reg.rts));
 		hostif_mib_set_request(priv, DOT11_RTS_THRESHOLD,
 				       sizeof(val), MIB_VALUE_TYPE_INT, &val);
 		break;
 	case SME_FRAGMENTATION_THRESHOLD_REQUEST:
-		val = cpu_to_le32((uint32_t) (priv->reg.fragment));
+		val = cpu_to_le32((uint32_t)(priv->reg.fragment));
 		hostif_mib_set_request(priv, DOT11_FRAGMENTATION_THRESHOLD,
 				       sizeof(val), MIB_VALUE_TYPE_INT, &val);
 		break;
@@ -2558,7 +2509,7 @@ void hostif_sme_execute(struct ks_wlan_private *priv, int event)
 		hostif_sme_sleep_set(priv);
 		break;
 	case SME_SET_REGION:
-		val = cpu_to_le32((uint32_t) (priv->region));
+		val = cpu_to_le32((uint32_t)(priv->region));
 		hostif_mib_set_request(priv, LOCAL_REGION,
 				       sizeof(val), MIB_VALUE_TYPE_INT, &val);
 		break;
@@ -2595,17 +2546,16 @@ void hostif_sme_task(unsigned long dev)
 
 	DPRINTK(3, "\n");
 
-	if (priv->dev_state >= DEVICE_STATE_BOOT) {
-		if (0 < cnt_smeqbody(priv)
-		    && priv->dev_state >= DEVICE_STATE_BOOT) {
-			hostif_sme_execute(priv,
-					   priv->sme_i.event_buff[priv->sme_i.
-								  qhead]);
-			inc_smeqhead(priv);
-			if (0 < cnt_smeqbody(priv))
-				tasklet_schedule(&priv->sme_task);
-		}
-	}
+	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)
+		tasklet_schedule(&priv->sme_task);
 }
 
 /* send to Station Management Entity module */
@@ -2617,14 +2567,12 @@ void hostif_sme_enqueue(struct ks_wlan_private *priv, unsigned short event)
 	if (cnt_smeqbody(priv) < (SME_EVENT_BUFF_SIZE - 1)) {
 		priv->sme_i.event_buff[priv->sme_i.qtail] = event;
 		inc_smeqtail(priv);
-		//DPRINTK(3,"inc_smeqtail \n");
 #ifdef KS_WLAN_DEBUG
 		if (priv->sme_i.max_event_count < cnt_smeqbody(priv))
 			priv->sme_i.max_event_count = cnt_smeqbody(priv);
 #endif /* KS_WLAN_DEBUG */
 	} else {
 		/* in case of buffer overflow */
-		//DPRINTK(2,"sme queue buffer overflow\n");
 		netdev_err(priv->net_dev, "sme queue buffer overflow\n");
 	}
 
@@ -2639,7 +2587,7 @@ int hostif_init(struct ks_wlan_private *priv)
 
 	priv->aplist.size = 0;
 	for (i = 0; i < LOCAL_APLIST_MAX; i++)
-		memset(&(priv->aplist.ap[i]), 0, sizeof(struct local_ap_t));
+		memset(&priv->aplist.ap[i], 0, sizeof(struct local_ap_t));
 	priv->infra_status = 0;
 	priv->current_rate = 4;
 	priv->connect_status = DISCONNECT_STATUS;
@@ -2656,24 +2604,23 @@ int hostif_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_waitqueue_head(&priv->psstatus.wakeup_wait); */
 	init_completion(&priv->psstatus.wakeup_wait);
-	//INIT_WORK(&priv->ks_wlan_wakeup_task, ks_wlan_hw_wakeup_task, (void *)priv);
-	INIT_WORK(&priv->ks_wlan_wakeup_task, ks_wlan_hw_wakeup_task);
+	INIT_WORK(&priv->wakeup_work, ks_wlan_hw_wakeup_task);
 
 	/* WPA */
-	memset(&(priv->wpa), 0, sizeof(priv->wpa));
+	memset(&priv->wpa, 0, sizeof(priv->wpa));
 	priv->wpa.rsn_enabled = 0;
 	priv->wpa.mic_failure.failure = 0;
 	priv->wpa.mic_failure.last_failure_time = 0;
 	priv->wpa.mic_failure.stop = 0;
-	memset(&(priv->pmklist), 0, sizeof(priv->pmklist));
+	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);
 
 	priv->sme_i.sme_status = SME_IDLE;
-	priv->sme_i.qhead = priv->sme_i.qtail = 0;
+	priv->sme_i.qhead = 0;
+	priv->sme_i.qtail = 0;
 #ifdef KS_WLAN_DEBUG
 	priv->sme_i.max_event_count = 0;
 #endif
diff --git a/drivers/staging/ks7010/ks_hostif.h b/drivers/staging/ks7010/ks_hostif.h
index 30c49b699d62..d758076d419d 100644
--- a/drivers/staging/ks7010/ks_hostif.h
+++ b/drivers/staging/ks7010/ks_hostif.h
@@ -1,6 +1,6 @@
 /*
  *   Driver for KeyStream wireless LAN
- *   
+ *
  *   Copyright (c) 2005-2008 KeyStream Corp.
  *   Copyright (C) 2009 Renesas Technology Corp.
  *
@@ -23,8 +23,8 @@
 #define HIF_MIB_GET_CONF	0xE802
 #define HIF_MIB_SET_REQ		0xE003
 #define HIF_MIB_SET_CONF	0xE803
-#define HIF_POWERMGT_REQ	0xE004
-#define HIF_POWERMGT_CONF	0xE804
+#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
@@ -62,35 +62,35 @@
  */
 
 struct hostif_hdr {
-	uint16_t size;
-	uint16_t event;
+	u16 size;
+	u16 event;
 } __packed;
 
 struct hostif_data_request_t {
 	struct hostif_hdr header;
-	uint16_t auth_type;
+	u16 auth_type;
 #define TYPE_DATA 0x0000
 #define TYPE_AUTH 0x0001
-	uint16_t reserved;
-	uint8_t data[0];
+	u16 reserved;
+	u8 data[0];
 } __packed;
 
 struct hostif_data_indication_t {
 	struct hostif_hdr header;
-	uint16_t auth_type;
+	u16 auth_type;
 /* #define TYPE_DATA 0x0000 */
 #define TYPE_PMK1 0x0001
 #define TYPE_GMK1 0x0002
 #define TYPE_GMK2 0x0003
-	uint16_t reserved;
-	uint8_t data[0];
+	u16 reserved;
+	u8 data[0];
 } __packed;
 
 #define CHANNEL_LIST_MAX_SIZE 14
 struct channel_list_t {
-	uint8_t size;
-	uint8_t body[CHANNEL_LIST_MAX_SIZE];
-	uint8_t pad;
+	u8 size;
+	u8 body[CHANNEL_LIST_MAX_SIZE];
+	u8 pad;
 } __packed;
 
 /* MIB Attribute */
@@ -110,9 +110,9 @@ struct channel_list_t {
 #define	DOT11_OPERATION_RATE_SET	  0x11110100	/* rate set */
 
 #define LOCAL_AP_SEARCH_INTEAVAL          0xF1010100	/* AP search interval (R/W) */
-#define LOCAL_CURRENTADDRESS              0xF1050100	/* MAC Adress change (W) */
-#define LOCAL_MULTICAST_ADDRESS           0xF1060100	/* Multicast Adress (W) */
-#define LOCAL_MULTICAST_FILTER            0xF1060200	/* Multicast Adress Filter enable/disable (W) */
+#define LOCAL_CURRENTADDRESS              0xF1050100	/* MAC Address change (W) */
+#define LOCAL_MULTICAST_ADDRESS           0xF1060100	/* Multicast Address (W) */
+#define LOCAL_MULTICAST_FILTER            0xF1060200	/* Multicast Address Filter enable/disable (W) */
 #define LOCAL_SEARCHED_AP_LIST            0xF1030100	/* AP list (R) */
 #define LOCAL_LINK_AP_STATUS              0xF1040100	/* Link AP status (R) */
 #define	LOCAL_PACKET_STATISTICS		  0xF1020100	/* tx,rx packets statistics */
@@ -128,7 +128,7 @@ struct channel_list_t {
 #define DOT11_PMK_TSC                     0x55010100	/* PMK_TSC (W) */
 #define DOT11_GMK1_TSC                    0x55010101	/* GMK1_TSC (W) */
 #define DOT11_GMK2_TSC                    0x55010102	/* GMK2_TSC (W) */
-#define DOT11_GMK3_TSC   		  0x55010103	/* GMK3_TSC */
+#define DOT11_GMK3_TSC                    0x55010103	/* GMK3_TSC */
 #define LOCAL_PMK                         0x58010100	/* Pairwise Master Key cache (W) */
 
 #define LOCAL_REGION                      0xF10A0100	/* Region setting */
@@ -143,61 +143,60 @@ struct channel_list_t {
 
 struct hostif_mib_get_request_t {
 	struct hostif_hdr header;
-	uint32_t mib_attribute;
+	u32 mib_attribute;
 } __packed;
 
 struct hostif_mib_value_t {
-	uint16_t size;
-	uint16_t type;
+	u16 size;
+	u16 type;
 #define MIB_VALUE_TYPE_NULL     0
 #define MIB_VALUE_TYPE_INT      1
 #define MIB_VALUE_TYPE_BOOL     2
 #define MIB_VALUE_TYPE_COUNT32  3
 #define MIB_VALUE_TYPE_OSTRING  4
-	uint8_t body[0];
+	u8 body[0];
 } __packed;
 
 struct hostif_mib_get_confirm_t {
 	struct hostif_hdr header;
-	uint32_t mib_status;
+	u32 mib_status;
 #define MIB_SUCCESS    0
 #define MIB_INVALID    1
 #define MIB_READ_ONLY  2
 #define MIB_WRITE_ONLY 3
-	uint32_t mib_attribute;
+	u32 mib_attribute;
 	struct hostif_mib_value_t mib_value;
 } __packed;
 
 struct hostif_mib_set_request_t {
 	struct hostif_hdr header;
-	uint32_t mib_attribute;
+	u32 mib_attribute;
 	struct hostif_mib_value_t mib_value;
 } __packed;
 
 struct hostif_mib_set_confirm_t {
 	struct hostif_hdr header;
-	uint32_t mib_status;
-	uint32_t mib_attribute;
+	u32 mib_status;
+	u32 mib_attribute;
 } __packed;
 
-struct hostif_power_mngmt_request_t {
+struct hostif_power_mgmt_request_t {
 	struct hostif_hdr header;
-	uint32_t mode;
+	u32 mode;
 #define POWER_ACTIVE  1
 #define POWER_SAVE    2
-	uint32_t wake_up;
+	u32 wake_up;
 #define SLEEP_FALSE 0
 #define SLEEP_TRUE  1	/* not used */
-	uint32_t receiveDTIMs;
+	u32 receiveDTIMs;
 #define DTIM_FALSE 0
 #define DTIM_TRUE  1
 } __packed;
 
-/* power management mode */
-enum {
-	POWMGT_ACTIVE_MODE = 0,
-	POWMGT_SAVE1_MODE,
-	POWMGT_SAVE2_MODE
+enum power_mgmt_mode_type {
+	POWER_MGMT_ACTIVE,
+	POWER_MGMT_SAVE1,
+	POWER_MGMT_SAVE2
 };
 
 #define	RESULT_SUCCESS            0
@@ -206,14 +205,14 @@ enum {
 /* #define	RESULT_ALREADY_RUNNING    3 */
 #define	RESULT_ALREADY_RUNNING    7
 
-struct hostif_power_mngmt_confirm_t {
+struct hostif_power_mgmt_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
 struct hostif_start_request_t {
 	struct hostif_hdr header;
-	uint16_t mode;
+	u16 mode;
 #define MODE_PSEUDO_ADHOC   0
 #define MODE_INFRASTRUCTURE 1
 #define MODE_AP             2	/* not used */
@@ -222,118 +221,118 @@ struct hostif_start_request_t {
 
 struct hostif_start_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
 #define SSID_MAX_SIZE 32
 struct ssid_t {
-	uint8_t size;
-	uint8_t body[SSID_MAX_SIZE];
-	uint8_t ssid_pad;
+	u8 size;
+	u8 body[SSID_MAX_SIZE];
+	u8 ssid_pad;
 } __packed;
 
 #define RATE_SET_MAX_SIZE 16
 struct rate_set8_t {
-	uint8_t size;
-	uint8_t body[8];
-	uint8_t rate_pad;
+	u8 size;
+	u8 body[8];
+	u8 rate_pad;
 } __packed;
 
 struct FhParms_t {
-	uint16_t dwellTime;
-	uint8_t hopSet;
-	uint8_t hopPattern;
-	uint8_t hopIndex;
+	u16 dwellTime;
+	u8 hopSet;
+	u8 hopPattern;
+	u8 hopIndex;
 } __packed;
 
 struct DsParms_t {
-	uint8_t channel;
+	u8 channel;
 } __packed;
 
 struct CfParms_t {
-	uint8_t count;
-	uint8_t period;
-	uint16_t maxDuration;
-	uint16_t durRemaining;
+	u8 count;
+	u8 period;
+	u16 maxDuration;
+	u16 durRemaining;
 } __packed;
 
 struct IbssParms_t {
-	uint16_t atimWindow;
+	u16 atimWindow;
 } __packed;
 
 struct rsn_t {
-	uint8_t size;
+	u8 size;
 #define RSN_BODY_SIZE 64
-	uint8_t body[RSN_BODY_SIZE];
+	u8 body[RSN_BODY_SIZE];
 } __packed;
 
 struct ErpParams_t {
-	uint8_t erp_info;
+	u8 erp_info;
 } __packed;
 
 struct rate_set16_t {
-	uint8_t size;
-	uint8_t body[16];
-	uint8_t rate_pad;
+	u8 size;
+	u8 body[16];
+	u8 rate_pad;
 } __packed;
 
 struct ap_info_t {
-	uint8_t bssid[6];	/* +00 */
-	uint8_t rssi;	/* +06 */
-	uint8_t sq;	/* +07 */
-	uint8_t noise;	/* +08 */
-	uint8_t pad0;	/* +09 */
-	uint16_t beacon_period;	/* +10 */
-	uint16_t capability;	/* +12 */
-#define BSS_CAP_ESS             (1<<0)
-#define BSS_CAP_IBSS            (1<<1)
-#define BSS_CAP_CF_POLABLE      (1<<2)
-#define BSS_CAP_CF_POLL_REQ     (1<<3)
-#define BSS_CAP_PRIVACY         (1<<4)
-#define BSS_CAP_SHORT_PREAMBLE  (1<<5)
-#define BSS_CAP_PBCC            (1<<6)
-#define BSS_CAP_CHANNEL_AGILITY (1<<7)
-#define BSS_CAP_SHORT_SLOT_TIME (1<<10)
-#define BSS_CAP_DSSS_OFDM       (1<<13)
-	uint8_t frame_type;	/* +14 */
-	uint8_t ch_info;	/* +15 */
+	u8 bssid[6];	/* +00 */
+	u8 rssi;	/* +06 */
+	u8 sq;	/* +07 */
+	u8 noise;	/* +08 */
+	u8 pad0;	/* +09 */
+	u16 beacon_period;	/* +10 */
+	u16 capability;	/* +12 */
+#define BSS_CAP_ESS             BIT(0)
+#define BSS_CAP_IBSS            BIT(1)
+#define BSS_CAP_CF_POLABLE      BIT(2)
+#define BSS_CAP_CF_POLL_REQ     BIT(3)
+#define BSS_CAP_PRIVACY         BIT(4)
+#define BSS_CAP_SHORT_PREAMBLE  BIT(5)
+#define BSS_CAP_PBCC            BIT(6)
+#define BSS_CAP_CHANNEL_AGILITY BIT(7)
+#define BSS_CAP_SHORT_SLOT_TIME BIT(10)
+#define BSS_CAP_DSSS_OFDM       BIT(13)
+	u8 frame_type;	/* +14 */
+	u8 ch_info;	/* +15 */
 #define FRAME_TYPE_BEACON	0x80
 #define FRAME_TYPE_PROBE_RESP	0x50
-	uint16_t body_size;	/* +16 */
-	uint8_t body[1024];	/* +18 */
+	u16 body_size;	/* +16 */
+	u8 body[1024];	/* +18 */
 	/* +1032 */
 } __packed;
 
 struct link_ap_info_t {
-	uint8_t bssid[6];	/* +00 */
-	uint8_t rssi;	/* +06 */
-	uint8_t sq;	/* +07 */
-	uint8_t noise;	/* +08 */
-	uint8_t pad0;	/* +09 */
-	uint16_t beacon_period;	/* +10 */
-	uint16_t capability;	/* +12 */
+	u8 bssid[6];	/* +00 */
+	u8 rssi;	/* +06 */
+	u8 sq;	/* +07 */
+	u8 noise;	/* +08 */
+	u8 pad0;	/* +09 */
+	u16 beacon_period;	/* +10 */
+	u16 capability;	/* +12 */
 	struct rate_set8_t rate_set;	/* +14 */
 	struct FhParms_t fh_parameter;	/* +24 */
 	struct DsParms_t ds_parameter;	/* +29 */
 	struct CfParms_t cf_parameter;	/* +30 */
 	struct IbssParms_t ibss_parameter;	/* +36 */
 	struct ErpParams_t erp_parameter;	/* +38 */
-	uint8_t pad1;	/* +39 */
+	u8 pad1;	/* +39 */
 	struct rate_set8_t ext_rate_set;	/* +40 */
-	uint8_t DTIM_period;	/* +50 */
-	uint8_t rsn_mode;	/* +51 */
+	u8 DTIM_period;	/* +50 */
+	u8 rsn_mode;	/* +51 */
 #define RSN_MODE_NONE	0
 #define RSN_MODE_WPA	1
 #define RSN_MODE_WPA2	2
 	struct {
-		uint8_t size;	/* +52 */
-		uint8_t body[128];	/* +53 */
+		u8 size;	/* +52 */
+		u8 body[128];	/* +53 */
 	} __packed rsn;
 } __packed;
 
 struct hostif_connect_indication_t {
 	struct hostif_hdr header;
-	uint16_t connect_code;
+	u16 connect_code;
 #define RESULT_CONNECT    0
 #define RESULT_DISCONNECT 1
 	struct link_ap_info_t link_ap_info;
@@ -345,125 +344,155 @@ struct hostif_stop_request_t {
 
 struct hostif_stop_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
+/**
+ * struct hostif_ps_adhoc_set_request_t - 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_t {
 	struct hostif_hdr header;
-	uint16_t phy_type;
+	u16 phy_type;
 #define D_11B_ONLY_MODE		0
 #define D_11G_ONLY_MODE		1
 #define D_11BG_COMPATIBLE_MODE	2
 #define D_11A_ONLY_MODE		3
-	uint16_t cts_mode;
+	u16 cts_mode;
 #define CTS_MODE_FALSE	0
 #define CTS_MODE_TRUE	1
-	uint16_t channel;
+	u16 channel;
 	struct rate_set16_t rate_set;
-	uint16_t capability;	/* bit5:preamble bit6:pbcc pbcc not supported always 0 
-				 * bit10:ShortSlotTime bit13:DSSS-OFDM DSSS-OFDM not supported always 0 */
-	uint16_t scan_type;
+	u16 capability;
+	u16 scan_type;
 } __packed;
 
 struct hostif_ps_adhoc_set_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
+/**
+ * struct hostif_infrastructure_set_request_t
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
 struct hostif_infrastructure_set_request_t {
 	struct hostif_hdr header;
-	uint16_t phy_type;
-	uint16_t cts_mode;
+	u16 phy_type;
+	u16 cts_mode;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	uint16_t capability;	/* bit5:preamble bit6:pbcc pbcc not supported always 0 
-				 * bit10:ShortSlotTime bit13:DSSS-OFDM DSSS-OFDM not supported always 0 */
-	uint16_t beacon_lost_count;
-	uint16_t auth_type;
+	u16 capability;
+	u16 beacon_lost_count;
+	u16 auth_type;
 #define AUTH_TYPE_OPEN_SYSTEM 0
 #define AUTH_TYPE_SHARED_KEY  1
 	struct channel_list_t channel_list;
-	uint16_t scan_type;
+	u16 scan_type;
 } __packed;
 
+/**
+ * struct hostif_infrastructure_set2_request_t
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
 struct hostif_infrastructure_set2_request_t {
 	struct hostif_hdr header;
-	uint16_t phy_type;
-	uint16_t cts_mode;
+	u16 phy_type;
+	u16 cts_mode;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	uint16_t capability;	/* bit5:preamble bit6:pbcc pbcc not supported always 0 
-				 * bit10:ShortSlotTime bit13:DSSS-OFDM DSSS-OFDM not supported always 0 */
-	uint16_t beacon_lost_count;
-	uint16_t auth_type;
+	u16 capability;
+	u16 beacon_lost_count;
+	u16 auth_type;
 #define AUTH_TYPE_OPEN_SYSTEM 0
 #define AUTH_TYPE_SHARED_KEY  1
 	struct channel_list_t channel_list;
-	uint16_t scan_type;
-	uint8_t bssid[ETH_ALEN];
+	u16 scan_type;
+	u8 bssid[ETH_ALEN];
 } __packed;
 
 struct hostif_infrastructure_set_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
+/**
+ * struct hostif_adhoc_set_request_t
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
 struct hostif_adhoc_set_request_t {
 	struct hostif_hdr header;
-	uint16_t phy_type;
-	uint16_t cts_mode;
-	uint16_t channel;
+	u16 phy_type;
+	u16 cts_mode;
+	u16 channel;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	uint16_t capability;	/* bit5:preamble bit6:pbcc pbcc not supported always 0 
-				 * bit10:ShortSlotTime bit13:DSSS-OFDM DSSS-OFDM not supported always 0 */
-	uint16_t scan_type;
+	u16 capability;
+	u16 scan_type;
 } __packed;
 
+/**
+ * struct hostif_adhoc_set2_request_t
+ * @capability: bit5  : preamble
+ *              bit6  : pbcc - Not supported always 0
+ *              bit10 : ShortSlotTime
+ *              bit13 : DSSS-OFDM - Not supported always 0
+ */
 struct hostif_adhoc_set2_request_t {
 	struct hostif_hdr header;
-	uint16_t phy_type;
-	uint16_t cts_mode;
-	uint16_t reserved;
+	u16 phy_type;
+	u16 cts_mode;
+	u16 reserved;
 	struct rate_set16_t rate_set;
 	struct ssid_t ssid;
-	uint16_t capability;	/* bit5:preamble bit6:pbcc pbcc not supported always 0 
-				 * bit10:ShortSlotTime bit13:DSSS-OFDM DSSS-OFDM not supported always 0 */
-	uint16_t scan_type;
+	u16 capability;
+	u16 scan_type;
 	struct channel_list_t channel_list;
-	uint8_t bssid[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
 } __packed;
 
 struct hostif_adhoc_set_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
 struct last_associate_t {
-	uint8_t type;
-	uint8_t status;
+	u8 type;
+	u8 status;
 } __packed;
 
 struct association_request_t {
-	uint8_t type;
+	u8 type;
 #define FRAME_TYPE_ASSOC_REQ	0x00
 #define FRAME_TYPE_REASSOC_REQ	0x20
-	uint8_t pad;
-	uint16_t capability;
-	uint16_t listen_interval;
-	uint8_t ap_address[6];
-	uint16_t reqIEs_size;
+	u8 pad;
+	u16 capability;
+	u16 listen_interval;
+	u8 ap_address[6];
+	u16 reqIEs_size;
 } __packed;
 
 struct association_response_t {
-	uint8_t type;
+	u8 type;
 #define FRAME_TYPE_ASSOC_RESP	0x10
 #define FRAME_TYPE_REASSOC_RESP	0x30
-	uint8_t pad;
-	uint16_t capability;
-	uint16_t status;
-	uint16_t association_id;
-	uint16_t respIEs_size;
+	u8 pad;
+	u16 capability;
+	u16 status;
+	u16 association_id;
+	u16 respIEs_size;
 } __packed;
 
 struct hostif_associate_indication_t {
@@ -476,63 +505,65 @@ struct hostif_associate_indication_t {
 
 struct hostif_bss_scan_request_t {
 	struct hostif_hdr header;
-	uint8_t scan_type;
+	u8 scan_type;
 #define ACTIVE_SCAN  0
 #define PASSIVE_SCAN 1
-	uint8_t pad[3];
-	uint32_t ch_time_min;
-	uint32_t ch_time_max;
+	u8 pad[3];
+	u32 ch_time_min;
+	u32 ch_time_max;
 	struct channel_list_t channel_list;
 	struct ssid_t ssid;
 } __packed;
 
 struct hostif_bss_scan_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
-	uint16_t reserved;
+	u16 result_code;
+	u16 reserved;
 } __packed;
 
 struct hostif_phy_information_request_t {
 	struct hostif_hdr header;
-	uint16_t type;
+	u16 type;
 #define NORMAL_TYPE	0
 #define TIME_TYPE	1
-	uint16_t time;	/* unit 100ms */
+	u16 time;	/* unit 100ms */
 } __packed;
 
 struct hostif_phy_information_confirm_t {
 	struct hostif_hdr header;
-	uint8_t rssi;
-	uint8_t sq;
-	uint8_t noise;
-	uint8_t link_speed;
-	uint32_t tx_frame;
-	uint32_t rx_frame;
-	uint32_t tx_error;
-	uint32_t rx_error;
-} __packed;
-
-/* sleep mode */
-#define SLP_ACTIVE  0
-#define SLP_SLEEP   1
+	u8 rssi;
+	u8 sq;
+	u8 noise;
+	u8 link_speed;
+	u32 tx_frame;
+	u32 rx_frame;
+	u32 tx_error;
+	u32 rx_error;
+} __packed;
+
+enum sleep_mode_type {
+	SLP_ACTIVE,
+	SLP_SLEEP
+};
+
 struct hostif_sleep_request_t {
 	struct hostif_hdr header;
 } __packed;
 
 struct hostif_sleep_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
 struct hostif_mic_failure_request_t {
 	struct hostif_hdr header;
-	uint16_t failure_count;
-	uint16_t timer;
+	u16 failure_count;
+	u16 timer;
 } __packed;
 
 struct hostif_mic_failure_confirm_t {
 	struct hostif_hdr header;
-	uint16_t result_code;
+	u16 result_code;
 } __packed;
 
 #define BASIC_RATE	0x80
@@ -568,69 +599,68 @@ struct hostif_mic_failure_confirm_t {
 #define TX_RATE_48M	(uint8_t)(480 / 5)
 #define TX_RATE_54M	(uint8_t)(540 / 5)
 
-#define IS_11B_RATE(A) (((A & RATE_MASK) == TX_RATE_1M ) || ((A & RATE_MASK) == TX_RATE_2M) || \
-                        ((A & RATE_MASK) == TX_RATE_5M) || ((A & RATE_MASK) == TX_RATE_11M))
+#define IS_11B_RATE(A) (((A & RATE_MASK) == TX_RATE_1M) || ((A & RATE_MASK) == TX_RATE_2M) || \
+			((A & RATE_MASK) == TX_RATE_5M) || ((A & RATE_MASK) == TX_RATE_11M))
 
 #define IS_OFDM_RATE(A) (((A & RATE_MASK) == TX_RATE_6M) || ((A & RATE_MASK) == TX_RATE_12M) || \
-                        ((A & RATE_MASK) == TX_RATE_24M) || ((A & RATE_MASK) == TX_RATE_9M) || \
-                        ((A & RATE_MASK) == TX_RATE_18M) || ((A & RATE_MASK) == TX_RATE_36M) || \
-                        ((A & RATE_MASK) == TX_RATE_48M) || ((A & RATE_MASK) == TX_RATE_54M))
+			 ((A & RATE_MASK) == TX_RATE_24M) || ((A & RATE_MASK) == TX_RATE_9M) || \
+			 ((A & RATE_MASK) == TX_RATE_18M) || ((A & RATE_MASK) == TX_RATE_36M) || \
+			 ((A & RATE_MASK) == TX_RATE_48M) || ((A & RATE_MASK) == TX_RATE_54M))
 
 #define IS_11BG_RATE(A) (IS_11B_RATE(A) || IS_OFDM_RATE(A))
 
-#define IS_OFDM_EXT_RATE(A)  (((A & RATE_MASK) == TX_RATE_9M) || ((A & RATE_MASK) == TX_RATE_18M) || \
-                             ((A & RATE_MASK) == TX_RATE_36M) || ((A & RATE_MASK) == TX_RATE_48M) || \
-                             ((A & RATE_MASK) == TX_RATE_54M))
+#define IS_OFDM_EXT_RATE(A) (((A & RATE_MASK) == TX_RATE_9M) || ((A & RATE_MASK) == TX_RATE_18M) || \
+			     ((A & RATE_MASK) == TX_RATE_36M) || ((A & RATE_MASK) == TX_RATE_48M) || \
+			     ((A & RATE_MASK) == TX_RATE_54M))
 
-enum {
-	CONNECT_STATUS = 0,
+enum connect_status_type {
+	CONNECT_STATUS,
 	DISCONNECT_STATUS
 };
 
-/* preamble type */
-enum {
-	LONG_PREAMBLE = 0,
+enum preamble_type {
+	LONG_PREAMBLE,
 	SHORT_PREAMBLE
 };
 
-/* multicast filter */
-#define MCAST_FILTER_MCAST    0
-#define MCAST_FILTER_MCASTALL 1
-#define MCAST_FILTER_PROMISC  2
+enum multicast_filter_type {
+	MCAST_FILTER_MCAST,
+	MCAST_FILTER_MCASTALL,
+	MCAST_FILTER_PROMISC,
+};
 
 #define NIC_MAX_MCAST_LIST 32
 
 /* macro function */
 #define HIF_EVENT_MASK 0xE800
 #define IS_HIF_IND(_EVENT)  ((_EVENT & HIF_EVENT_MASK) == 0xE800  && \
-                             ((_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))
+			     ((_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))
 
 #define IS_HIF_CONF(_EVENT) ((_EVENT & HIF_EVENT_MASK) == 0xE800  && \
-                             (_EVENT & ~HIF_EVENT_MASK) > 0x0000  && \
-                             (_EVENT & ~HIF_EVENT_MASK) < 0x0012  && \
-                             !IS_HIF_IND(_EVENT) )
+			     (_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 *packet);
+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);
+		    unsigned int size);
 void hostif_sme_enqueue(struct ks_wlan_private *priv, uint16_t 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) (void *arg1, void *arg2),
-		   void *arg1, void *arg2);
-void send_packet_complete(void *, void *);
+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);
diff --git a/drivers/staging/ks7010/ks_wlan.h b/drivers/staging/ks7010/ks_wlan.h
index 9ab80e1f123e..cd4f56ddbea8 100644
--- a/drivers/staging/ks7010/ks_wlan.h
+++ b/drivers/staging/ks7010/ks_wlan.h
@@ -18,10 +18,10 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 
-#include <linux/spinlock.h>	/* spinlock_t                                   */
-#include <linux/sched.h>	/* wait_queue_head_t                            */
-#include <linux/types.h>	/* pid_t                                        */
-#include <linux/netdevice.h>	/* struct net_device_stats,  struct sk_buff     */
+#include <linux/spinlock.h>	/* spinlock_t */
+#include <linux/sched.h>	/* wait_queue_head_t */
+#include <linux/types.h>	/* pid_t */
+#include <linux/netdevice.h>	/* struct net_device_stats,  struct sk_buff */
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
 #include <linux/atomic.h>	/* struct atomic_t */
@@ -36,7 +36,10 @@
 
 #ifdef KS_WLAN_DEBUG
 #define DPRINTK(n, fmt, args...) \
-                 if (KS_WLAN_DEBUG > (n)) printk(KERN_NOTICE "%s: "fmt, __FUNCTION__, ## args)
+	do { \
+		if (KS_WLAN_DEBUG > (n)) \
+			pr_notice("%s: "fmt, __func__, ## args); \
+	} while (0)
 #else
 #define DPRINTK(n, fmt, args...)
 #endif
@@ -55,7 +58,7 @@ struct ks_wlan_parameter {
 		u8 body[32 + 1];
 	} ssid;	/*  SSID */
 	u8 preamble;	/*  Preamble */
-	u8 powermgt;	/*  PowerManagementMode */
+	u8 power_mgmt;
 	u32 scan_type;	/*  AP List Scan Type */
 #define BEACON_LOST_COUNT_MIN 0
 #define BEACON_LOST_COUNT_MAX 65535
@@ -85,23 +88,23 @@ enum {
 };
 
 /* SME flag */
-#define SME_MODE_SET	    (1<<0)
-#define SME_RTS             (1<<1)
-#define SME_FRAG            (1<<2)
-#define SME_WEP_FLAG        (1<<3)
-#define SME_WEP_INDEX       (1<<4)
-#define SME_WEP_VAL1        (1<<5)
-#define SME_WEP_VAL2        (1<<6)
-#define SME_WEP_VAL3        (1<<7)
-#define SME_WEP_VAL4        (1<<8)
+#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    (SME_WEP_VAL1 | SME_WEP_VAL2 | SME_WEP_VAL3 | SME_WEP_VAL4)
-#define SME_RSN             (1<<9)
-#define SME_RSN_MULTICAST   (1<<10)
-#define SME_RSN_UNICAST	    (1<<11)
-#define SME_RSN_AUTH	    (1<<12)
+#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         (1<<13)
-#define SME_MULTICAST       (1<<14)
+#define SME_AP_SCAN         BIT(13)
+#define SME_MULTICAST       BIT(14)
 
 /* SME Event */
 enum {
@@ -356,7 +359,8 @@ struct wpa_key_t {
 	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 */
+				 * individual keys
+				 */
 	u16 alg;
 	u16 key_len;	/* WEP: 5 or 13, TKIP: 32, CCMP: 16 */
 	u8 key_val[IW_ENCODING_TOKEN_MAX];
@@ -409,7 +413,11 @@ struct wps_status_t {
 #endif /* WPS */
 
 struct ks_wlan_private {
-	struct hw_info_t ks_wlan_hw;	/* hardware information */
+	/* hardware information */
+	struct ks_sdio_card *ks_sdio_card;
+	struct workqueue_struct *wq;
+	struct delayed_work rw_dwork;
+	struct tasklet_struct rx_bh_task;
 
 	struct net_device *net_dev;
 	int reg_net;	/* register_netdev */
@@ -425,7 +433,7 @@ struct ks_wlan_private {
 	u8 *rxp;
 	unsigned int rx_size;
 	struct tasklet_struct sme_task;
-	struct work_struct ks_wlan_wakeup_task;
+	struct work_struct wakeup_work;
 	int scan_ind_count;
 
 	unsigned char eth_addr[ETH_ALEN];
@@ -500,5 +508,7 @@ struct ks_wlan_private {
 
 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
index 8e62b10effd6..28b381c24b42 100644
--- a/drivers/staging/ks7010/ks_wlan_ioctl.h
+++ b/drivers/staging/ks7010/ks_wlan_ioctl.h
@@ -1,6 +1,6 @@
 /*
  *   Driver for KeyStream 11b/g wireless LAN
- *   
+ *
  *   Copyright (c) 2005-2008 KeyStream Corp.
  *   Copyright (C) 2009 Renesas Technology Corp.
  *
@@ -15,43 +15,43 @@
 #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
+/*					(SIOCIWFIRSTPRIV + 0) */
+/* former KS_WLAN_GET_DRIVER_VERSION	(SIOCIWFIRSTPRIV + 1) */
+/*					(SIOCIWFIRSTPRIV + 2) */
+#define KS_WLAN_GET_FIRM_VERSION	(SIOCIWFIRSTPRIV + 3)
 #ifdef WPS
-#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_SET_WPS_ENABLE		(SIOCIWFIRSTPRIV + 4)
+#define KS_WLAN_GET_WPS_ENABLE		(SIOCIWFIRSTPRIV + 5)
+#define KS_WLAN_SET_WPS_PROBE_REQ	(SIOCIWFIRSTPRIV + 6)
 #endif
-#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_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
+#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 */
+#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__
 
@@ -60,7 +60,7 @@
 
 int ks_wlan_read_config_file(struct ks_wlan_private *priv);
 int ks_wlan_setup_parameter(struct ks_wlan_private *priv,
-		             unsigned int commit_flag);
+			    unsigned int commit_flag);
 
 #endif /* __KERNEL__ */
 
diff --git a/drivers/staging/ks7010/ks_wlan_net.c b/drivers/staging/ks7010/ks_wlan_net.c
index 121e1530fdba..5a43f193dcc8 100644
--- a/drivers/staging/ks7010/ks_wlan_net.c
+++ b/drivers/staging/ks7010/ks_wlan_net.c
@@ -89,10 +89,10 @@ int ks_wlan_update_phy_information(struct ks_wlan_private *priv)
 	DPRINTK(4, "in_interrupt = %ld\n", in_interrupt());
 
 	if (priv->dev_state < DEVICE_STATE_READY)
-		return -1;	/* not finished initialize */
+		return -EBUSY;	/* not finished initialize */
 
 	if (atomic_read(&update_phyinfo))
-		return 1;
+		return -EPERM;
 
 	/* The status */
 	wstats->status = priv->reg.operation_mode;	/* Operation mode */
@@ -173,14 +173,11 @@ int ks_wlan_setup_parameter(struct ks_wlan_private *priv,
  * would not work at all... - Jean II
  */
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get protocol name */
 static int ks_wlan_get_name(struct net_device *dev,
 			    struct iw_request_info *info, char *cwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -198,15 +195,12 @@ static int ks_wlan_get_name(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set frequency */
 static int ks_wlan_set_freq(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_freq *fwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
-	int rc = -EINPROGRESS;	/* Call commit handler */
+	struct ks_wlan_private *priv = netdev_priv(dev);
+	int channel;
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -226,58 +220,52 @@ static int ks_wlan_set_freq(struct net_device *dev,
 	}
 	/* Setting by channel number */
 	if ((fwrq->m > 1000) || (fwrq->e > 0))
-		rc = -EOPNOTSUPP;
-	else {
-		int channel = fwrq->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->m);
-			rc = -EINVAL;
-		} else {
-			/* Yes ! We can set it !!! */
-			priv->reg.channel = (u8) (channel);
-			priv->need_commit |= SME_MODE_SET;
-		}
+		return -EOPNOTSUPP;
+
+	channel = fwrq->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->m);
+		return -EINVAL;
 	}
 
-	return rc;
+	/* Yes ! We can set it !!! */
+	priv->reg.channel = (u8)(channel);
+	priv->need_commit |= SME_MODE_SET;
+
+	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get frequency */
 static int ks_wlan_get_freq(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_freq *fwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int f;
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 
 	/* for SLEEP MODE */
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS)
+	if (is_connect_status(priv->connect_status))
 		f = (int)priv->current_ap.channel;
-		else
+	else
 		f = (int)priv->reg.channel;
+
 	fwrq->m = frequency_list[f - 1] * 100000;
 	fwrq->e = 1;
 
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set ESSID */
 static int ks_wlan_set_essid(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	size_t len;
 
 	DPRINTK(2, " %d\n", dwrq->flags);
@@ -287,7 +275,7 @@ static int ks_wlan_set_essid(struct net_device *dev,
 
 	/* for SLEEP MODE */
 	/* Check if we asked for `any' */
-	if (dwrq->flags == 0) {
+	if (!dwrq->flags) {
 		/* Just send an empty SSID list */
 		memset(priv->reg.ssid.body, 0, sizeof(priv->reg.ssid.body));
 		priv->reg.ssid.size = 0;
@@ -329,56 +317,40 @@ static int ks_wlan_set_essid(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get ESSID */
 static int ks_wlan_get_essid(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	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) {
+	 * 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 0
-		extra[priv->reg.ssid.size] = '\0';
-#endif
+
 		/* If none, we may want to get the one that was set */
 
 		/* Push it out ! */
-#if 1
 		dwrq->length = priv->reg.ssid.size;
-#else
-		dwrq->length = priv->reg.ssid.size + 1;
-#endif
 		dwrq->flags = 1;	/* active */
 	} else {
-#if 1
 		dwrq->length = 0;
-#else
-		extra[0] = '\0';
-		dwrq->length = 1;
-#endif
 		dwrq->flags = 0;	/* ANY */
 	}
 
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set AP address */
 static int ks_wlan_set_wap(struct net_device *dev, struct iw_request_info *info,
 			   struct sockaddr *ap_addr, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "\n");
 
@@ -408,20 +380,17 @@ static int ks_wlan_set_wap(struct net_device *dev, struct iw_request_info *info,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get AP address */
 static int ks_wlan_get_wap(struct net_device *dev, struct iw_request_info *info,
 			   struct sockaddr *awrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 
 	/* for SLEEP MODE */
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS)
-		memcpy(awrq->sa_data, &(priv->current_ap.bssid[0]), ETH_ALEN);
+	if (is_connect_status(priv->connect_status))
+		memcpy(awrq->sa_data, priv->current_ap.bssid, ETH_ALEN);
 	else
 		eth_zero_addr(awrq->sa_data);
 
@@ -430,14 +399,11 @@ static int ks_wlan_get_wap(struct net_device *dev, struct iw_request_info *info,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Nickname */
 static int ks_wlan_set_nick(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_point *dwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -453,14 +419,11 @@ static int ks_wlan_set_nick(struct net_device *dev,
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Nickname */
 static int ks_wlan_get_nick(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_point *dwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -473,14 +436,11 @@ static int ks_wlan_get_nick(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Bit-Rate */
 static int ks_wlan_set_rate(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int i = 0;
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -493,12 +453,12 @@ static int ks_wlan_set_rate(struct net_device *dev,
 			case 11000000:
 			case 5500000:
 				priv->reg.rate_set.body[0] =
-				    (uint8_t) (vwrq->value / 500000);
+				    (uint8_t)(vwrq->value / 500000);
 				break;
 			case 2000000:
 			case 1000000:
 				priv->reg.rate_set.body[0] =
-				    ((uint8_t) (vwrq->value / 500000)) |
+				    ((uint8_t)(vwrq->value / 500000)) |
 				    BASIC_RATE;
 				break;
 			default:
@@ -550,7 +510,7 @@ static int ks_wlan_set_rate(struct net_device *dev,
 			case 18000000:
 			case 9000000:
 				priv->reg.rate_set.body[0] =
-				    (uint8_t) (vwrq->value / 500000);
+				    (uint8_t)(vwrq->value / 500000);
 				break;
 			case 24000000:
 			case 12000000:
@@ -560,7 +520,7 @@ static int ks_wlan_set_rate(struct net_device *dev,
 			case 2000000:
 			case 1000000:
 				priv->reg.rate_set.body[0] =
-				    ((uint8_t) (vwrq->value / 500000)) |
+				    ((uint8_t)(vwrq->value / 500000)) |
 				    BASIC_RATE;
 				break;
 			default:
@@ -708,14 +668,11 @@ static int ks_wlan_set_rate(struct net_device *dev,
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Bit-Rate */
 static int ks_wlan_get_rate(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "in_interrupt = %ld update_phyinfo = %d\n",
 		in_interrupt(), atomic_read(&update_phyinfo));
@@ -736,13 +693,10 @@ static int ks_wlan_get_rate(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set RTS threshold */
 static int ks_wlan_set_rts(struct net_device *dev, struct iw_request_info *info,
 			   struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int rthr = vwrq->value;
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -760,13 +714,10 @@ static int ks_wlan_set_rts(struct net_device *dev, struct iw_request_info *info,
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get RTS threshold */
 static int ks_wlan_get_rts(struct net_device *dev, struct iw_request_info *info,
 			   struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -779,14 +730,11 @@ static int ks_wlan_get_rts(struct net_device *dev, struct iw_request_info *info,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Fragmentation threshold */
 static int ks_wlan_set_frag(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int fthr = vwrq->value;
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -805,14 +753,11 @@ static int ks_wlan_set_frag(struct net_device *dev,
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Fragmentation threshold */
 static int ks_wlan_get_frag(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -825,14 +770,11 @@ static int ks_wlan_get_frag(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Mode of Operation */
 static int ks_wlan_set_mode(struct net_device *dev,
 			    struct iw_request_info *info, __u32 *uwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "mode=%d\n", *uwrq);
 
@@ -861,14 +803,11 @@ static int ks_wlan_set_mode(struct net_device *dev,
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Mode of Operation */
 static int ks_wlan_get_mode(struct net_device *dev,
 			    struct iw_request_info *info, __u32 *uwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -889,14 +828,11 @@ static int ks_wlan_get_mode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Encryption Key */
 static int ks_wlan_set_encode(struct net_device *dev,
 			      struct iw_request_info *info,
 			      struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	struct wep_key key;
 	int index = (dwrq->flags & IW_ENCODE_INDEX);
@@ -974,11 +910,12 @@ static int ks_wlan_set_encode(struct net_device *dev,
 			/* Do we want to just set the transmit key index ? */
 			if ((index >= 0) && (index < 4)) {
 				/* set_wep_key(priv, index, 0, 0, 1);   xxx */
-				if (priv->reg.wep_key[index].size) {
+				if (priv->reg.wep_key[index].size != 0) {
 					priv->reg.wep_index = index;
 					priv->need_commit |= SME_WEP_INDEX;
-				} else
+				} else {
 					return -EINVAL;
+				}
 			}
 		}
 	}
@@ -1006,14 +943,11 @@ static int ks_wlan_set_encode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Encryption Key */
 static int ks_wlan_get_encode(struct net_device *dev,
 			      struct iw_request_info *info,
 			      struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	char zeros[16];
 	int index = (dwrq->flags & IW_ENCODE_INDEX) - 1;
 
@@ -1054,15 +988,14 @@ static int ks_wlan_get_encode(struct net_device *dev,
 		if ((index >= 0) && (index < 4))
 			memcpy(extra, priv->reg.wep_key[index].val,
 			       dwrq->length);
-	} else
+	} else {
 		memcpy(extra, zeros, dwrq->length);
+	}
 #endif
 	return 0;
 }
 
 #ifndef KSC_OPNOTSUPP
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Tx-Power */
 static int ks_wlan_set_txpow(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
@@ -1070,8 +1003,6 @@ static int ks_wlan_set_txpow(struct net_device *dev,
 	return -EOPNOTSUPP;	/* Not Support */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Tx-Power */
 static int ks_wlan_get_txpow(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
@@ -1087,8 +1018,6 @@ static int ks_wlan_get_txpow(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Retry limits */
 static int ks_wlan_set_retry(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
@@ -1096,8 +1025,6 @@ static int ks_wlan_set_retry(struct net_device *dev,
 	return -EOPNOTSUPP;	/* Not Support */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Retry limits */
 static int ks_wlan_get_retry(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
@@ -1114,14 +1041,11 @@ static int ks_wlan_get_retry(struct net_device *dev,
 }
 #endif /* KSC_OPNOTSUPP */
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get range info */
 static int ks_wlan_get_range(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_range *range = (struct iw_range *)extra;
 	int i, k;
 
@@ -1137,7 +1061,8 @@ static int ks_wlan_get_range(struct net_device *dev,
 	range->max_nwid = 0x0000;
 	range->num_channels = 14;
 	/* Should be based on cap_rid.country to give only
-	 * what the current card support */
+	 * what the current card support
+	 */
 	k = 0;
 	for (i = 0; i < 13; i++) {	/* channel 1 -- 13 */
 		range->freq[k].i = i + 1;	/* List index */
@@ -1189,7 +1114,8 @@ static int ks_wlan_get_range(struct net_device *dev,
 
 	/* 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 */
+	 * May be use for QoS stuff... Jean II
+	 */
 	if (i > 2)
 		range->throughput = 5000 * 1000;
 	else
@@ -1223,9 +1149,11 @@ static int ks_wlan_get_range(struct net_device *dev,
 	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 */
+	/* 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;
@@ -1245,54 +1173,39 @@ static int ks_wlan_get_range(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Power Management */
 static int ks_wlan_set_power(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
-	short enabled;
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 
-	/* for SLEEP MODE */
-	enabled = vwrq->disabled ? 0 : 1;
-	if (enabled == 0) {	/* 0 */
-		priv->reg.powermgt = POWMGT_ACTIVE_MODE;
-	} else if (enabled) {	/* 1 */
-		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE)
-			priv->reg.powermgt = POWMGT_SAVE1_MODE;
-		else
-			return -EINVAL;
-	} else if (enabled) {	/* 2 */
+	if (vwrq->disabled) {
+		priv->reg.power_mgmt = POWER_MGMT_ACTIVE;
+	} else {
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE)
-			priv->reg.powermgt = POWMGT_SAVE2_MODE;
+			priv->reg.power_mgmt = POWER_MGMT_SAVE1;
 		else
 			return -EINVAL;
-	} else
-		return -EINVAL;
+	}
 
 	hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
 
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Power Management */
 static int ks_wlan_get_power(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 	/* for SLEEP MODE */
-	if (priv->reg.powermgt > 0)
+	if (priv->reg.power_mgmt > 0)
 		vwrq->disabled = 0;
 	else
 		vwrq->disabled = 1;
@@ -1300,14 +1213,11 @@ static int ks_wlan_get_power(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get wirless statistics */
 static int ks_wlan_get_iwstats(struct net_device *dev,
 			       struct iw_request_info *info,
 			       struct iw_quality *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -1321,8 +1231,7 @@ static int ks_wlan_get_iwstats(struct net_device *dev,
 }
 
 #ifndef KSC_OPNOTSUPP
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set Sensitivity */
+
 static int ks_wlan_set_sens(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
@@ -1330,8 +1239,6 @@ static int ks_wlan_set_sens(struct net_device *dev,
 	return -EOPNOTSUPP;	/* Not Support */
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get Sensitivity */
 static int ks_wlan_get_sens(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_param *vwrq,
 			    char *extra)
@@ -1344,15 +1251,12 @@ static int ks_wlan_get_sens(struct net_device *dev,
 }
 #endif /* KSC_OPNOTSUPP */
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get AP List */
 /* Note : this is deprecated in favor of IWSCAN */
 static int ks_wlan_get_aplist(struct net_device *dev,
 			      struct iw_request_info *info,
 			      struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct sockaddr *address = (struct sockaddr *)extra;
 	struct iw_quality qual[LOCAL_APLIST_MAX];
 
@@ -1380,14 +1284,11 @@ static int ks_wlan_get_aplist(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : Initiate Scan */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_scan_req *req = NULL;
 
 	DPRINTK(2, "\n");
@@ -1397,8 +1298,8 @@ static int ks_wlan_set_scan(struct net_device *dev,
 
 	/* for SLEEP MODE */
 	/* specified SSID SCAN */
-	if (wrqu->data.length == sizeof(struct iw_scan_req)
-	    && wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+	if (wrqu->data.length == sizeof(struct iw_scan_req) &&
+	    wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		req = (struct iw_scan_req *)extra;
 		priv->scan_ssid_len = req->essid_len;
 		memcpy(priv->scan_ssid, req->essid, priv->scan_ssid_len);
@@ -1414,7 +1315,6 @@ static int ks_wlan_set_scan(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
 /*
  * Translate scan data returned from the card to a card independent
  * format that the Wireless Tools will understand - Jean II
@@ -1452,7 +1352,7 @@ static inline char *ks_wlan_translate_scan(struct net_device *dev,
 	iwe.u.data.flags = 1;
 	current_ev =
 	    iwe_stream_add_point(info, current_ev, end_buf, &iwe,
-				 &(ap->ssid.body[0]));
+				 ap->ssid.body);
 
 	/* Add mode */
 	iwe.cmd = SIOCGIWMODE;
@@ -1494,15 +1394,18 @@ static inline char *ks_wlan_translate_scan(struct net_device *dev,
 	iwe.u.data.length = 0;
 	current_ev =
 	    iwe_stream_add_point(info, current_ev, end_buf, &iwe,
-				 &(ap->ssid.body[0]));
+				 ap->ssid.body);
 
 	/* Rate : stuffing multiple values in a single event require a bit
-	 * more of magic - Jean II */
+	 * more of magic - Jean II
+	 */
 	current_val = current_ev + IW_EV_LCP_LEN;
 
 	iwe.cmd = SIOCGIWRATE;
-	/* Those two flags are ignored... */
-	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+
+	/* These two flags are ignored... */
+	iwe.u.bitrate.fixed = 0;
+	iwe.u.bitrate.disabled = 0;
 
 	/* Max 16 values */
 	for (i = 0; i < 16; i++) {
@@ -1569,18 +1472,16 @@ static inline char *ks_wlan_translate_scan(struct net_device *dev,
 	}
 
 	/* The other data in the scan result are not really
-	 * interesting, so for now drop it - Jean II */
+	 * interesting, so for now drop it - Jean II
+	 */
 	return current_ev;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : Read Scan Results */
 static int ks_wlan_get_scan(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_point *dwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int i;
 	char *current_ev = extra;
 
@@ -1596,23 +1497,12 @@ static int ks_wlan_get_scan(struct net_device *dev,
 
 	if (priv->aplist.size == 0) {
 		/* Client error, no scan results...
-		 * The caller need to restart the scan. */
+		 * The caller need to restart the scan.
+		 */
 		DPRINTK(2, "aplist 0\n");
 		return -ENODATA;
 	}
-#if 0
-	/* current connect ap */
-	if ((priv->connect_status & CONNECT_STATUS_MASK) == CONNECT_STATUS) {
-		if ((extra + dwrq->length) - current_ev <= IW_EV_ADDR_LEN) {
-			dwrq->length = 0;
-			return -E2BIG;
-		}
-		current_ev = ks_wlan_translate_scan(dev, current_ev,
-//                                                  extra + IW_SCAN_MAX_DATA,
-						    extra + dwrq->length,
-						    &(priv->current_ap));
-	}
-#endif
+
 	/* Read and parse all entries */
 	for (i = 0; i < priv->aplist.size; i++) {
 		if ((extra + dwrq->length) - current_ev <= IW_EV_ADDR_LEN) {
@@ -1621,9 +1511,8 @@ static int ks_wlan_get_scan(struct net_device *dev,
 		}
 		/* Translate to WE format this entry */
 		current_ev = ks_wlan_translate_scan(dev, info, current_ev,
-//                                                  extra + IW_SCAN_MAX_DATA,
 						    extra + dwrq->length,
-						    &(priv->aplist.ap[i]));
+						    &priv->aplist.ap[i]);
 	}
 	/* Length of data */
 	dwrq->length = (current_ev - extra);
@@ -1632,14 +1521,12 @@ static int ks_wlan_get_scan(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Commit handler : called after a bunch of SET operations */
+/* called after a bunch of SET operations */
 static int ks_wlan_config_commit(struct net_device *dev,
 				 struct iw_request_info *info, void *zwrq,
 				 char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (!priv->need_commit)
 		return 0;
@@ -1649,14 +1536,12 @@ static int ks_wlan_config_commit(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless handler : set association ie params */
+/* set association ie params */
 static int ks_wlan_set_genie(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "\n");
 
@@ -1667,14 +1552,11 @@ static int ks_wlan_set_genie(struct net_device *dev,
 //      return -EOPNOTSUPP;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless handler : set authentication mode params */
 static int ks_wlan_set_auth_mode(struct net_device *dev,
 				 struct iw_request_info *info,
 				 struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int index = (vwrq->flags & IW_AUTH_INDEX);
 	int value = vwrq->value;
 
@@ -1804,14 +1686,11 @@ static int ks_wlan_set_auth_mode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless handler : get authentication mode params */
 static int ks_wlan_get_auth_mode(struct net_device *dev,
 				 struct iw_request_info *info,
 				 struct iw_param *vwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	int index = (vwrq->flags & IW_AUTH_INDEX);
 
 	DPRINTK(2, "index=%d\n", index);
@@ -1850,19 +1729,20 @@ static int ks_wlan_get_auth_mode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set encoding token & mode (WPA)*/
+/* set encoding token & mode (WPA)*/
 static int ks_wlan_set_encode_ext(struct net_device *dev,
 				  struct iw_request_info *info,
 				  struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_encode_ext *enc;
 	int index = dwrq->flags & IW_ENCODE_INDEX;
 	unsigned int commit = 0;
+	struct wpa_key_t *key;
 
 	enc = (struct iw_encode_ext *)extra;
+	if (!enc)
+		return -EINVAL;
 
 	DPRINTK(2, "flags=%04X:: ext_flags=%08X\n", dwrq->flags,
 		enc->ext_flags);
@@ -1873,77 +1753,65 @@ static int ks_wlan_set_encode_ext(struct net_device *dev,
 	/* for SLEEP MODE */
 	if (index < 1 || index > 4)
 		return -EINVAL;
-	else
-		index--;
+	index--;
+	key = &priv->wpa.key[index];
 
 	if (dwrq->flags & IW_ENCODE_DISABLED)
-		priv->wpa.key[index].key_len = 0;
-
-	if (enc) {
-		priv->wpa.key[index].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(&priv->wpa.key[index].rx_seq[0],
-			       enc->rx_seq, IW_ENCODE_SEQ_MAX_SIZE);
-		}
+		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);
+	}
 
-		memcpy(&priv->wpa.key[index].addr.sa_data[0],
-		       &enc->addr.sa_data[0], ETH_ALEN);
+	memcpy(&key->addr.sa_data[0], &enc->addr.sa_data[0], ETH_ALEN);
 
-		switch (enc->alg) {
-		case IW_ENCODE_ALG_NONE:
-			if (priv->reg.privacy_invoked) {
-				priv->reg.privacy_invoked = 0x00;
-				commit |= SME_WEP_FLAG;
-			}
-			priv->wpa.key[index].key_len = 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) {
-				memcpy(&priv->wpa.key[index].key_val[0],
-				       &enc->key[0], enc->key_len);
-				priv->wpa.key[index].key_len = enc->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(&priv->wpa.key[index].key_val[0],
-				       &enc->key[0], enc->key_len - 16);
-				priv->wpa.key[index].key_len =
-				    enc->key_len - 16;
-				if (priv->wpa.key_mgmt_suite == 4) {	/* WPA_NONE */
-					memcpy(&priv->wpa.key[index].
-					       tx_mic_key[0], &enc->key[16], 8);
-					memcpy(&priv->wpa.key[index].
-					       rx_mic_key[0], &enc->key[16], 8);
-				} else {
-					memcpy(&priv->wpa.key[index].
-					       tx_mic_key[0], &enc->key[16], 8);
-					memcpy(&priv->wpa.key[index].
-					       rx_mic_key[0], &enc->key[24], 8);
-				}
-				commit |= (SME_WEP_VAL1 << index);
+		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) {
+			memcpy(&key->key_val[0], &enc->key[0], enc->key_len);
+			key->key_len = enc->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);
 			}
-			break;
-		default:
-			return -EINVAL;
+			commit |= (SME_WEP_VAL1 << index);
 		}
-		priv->wpa.key[index].alg = enc->alg;
-	} else
+		break;
+	default:
 		return -EINVAL;
+	}
+	key->alg = enc->alg;
 
 	if (commit) {
 		if (commit & SME_WEP_INDEX)
@@ -1957,36 +1825,32 @@ static int ks_wlan_set_encode_ext(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : get encoding token & mode (WPA)*/
+/* get encoding token & mode (WPA)*/
 static int ks_wlan_get_encode_ext(struct net_device *dev,
 				  struct iw_request_info *info,
 				  struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	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) */
+	/* 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;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : PMKSA cache operation (WPA2) */
 static int ks_wlan_set_pmksa(struct net_device *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *dwrq, char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_pmksa *pmksa;
 	int i;
 	struct pmk_t *pmk;
@@ -2009,76 +1873,68 @@ static int ks_wlan_set_pmksa(struct net_device *dev,
 		if (list_empty(&priv->pmklist.head)) {	/* new list */
 			for (i = 0; i < PMK_LIST_MAX; i++) {
 				pmk = &priv->pmklist.pmk[i];
-				if (!memcmp
-				    ("\x00\x00\x00\x00\x00\x00", pmk->bssid,
-				     ETH_ALEN))
-					break;
+				if (memcmp("\x00\x00\x00\x00\x00\x00",
+					   pmk->bssid, ETH_ALEN) == 0)
+					break; /* loop */
 			}
 			memcpy(pmk->bssid, pmksa->bssid.sa_data, ETH_ALEN);
 			memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
 			list_add(&pmk->list, &priv->pmklist.head);
 			priv->pmklist.size++;
-		} else {	/* search cache data */
-			list_for_each(ptr, &priv->pmklist.head) {
-				pmk = list_entry(ptr, struct pmk_t, list);
-				if (!memcmp(pmksa->bssid.sa_data, pmk->bssid, ETH_ALEN)) {	/* match address! list move to head. */
-					memcpy(pmk->pmkid, pmksa->pmkid,
-					       IW_PMKID_LEN);
-					list_move(&pmk->list,
-						  &priv->pmklist.head);
-					break;
-				}
+			break;	/* case */
+		}
+		/* search cache data */
+		list_for_each(ptr, &priv->pmklist.head) {
+			pmk = list_entry(ptr, struct pmk_t, list);
+			if (memcmp(pmksa->bssid.sa_data, pmk->bssid, ETH_ALEN) == 0) {
+				memcpy(pmk->pmkid, pmksa->pmkid, IW_PMKID_LEN);
+				list_move(&pmk->list, &priv->pmklist.head);
+				break; /* list_for_each */
 			}
-			if (ptr == &priv->pmklist.head) {	/* not find address. */
-				if (PMK_LIST_MAX > priv->pmklist.size) {	/* new cache data */
-					for (i = 0; i < PMK_LIST_MAX; i++) {
-						pmk = &priv->pmklist.pmk[i];
-						if (!memcmp
-						    ("\x00\x00\x00\x00\x00\x00",
-						     pmk->bssid, ETH_ALEN))
-							break;
-					}
-					memcpy(pmk->bssid, pmksa->bssid.sa_data,
-					       ETH_ALEN);
-					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_t, list);
-					memcpy(pmk->bssid, pmksa->bssid.sa_data,
-					       ETH_ALEN);
-					memcpy(pmk->pmkid, pmksa->pmkid,
-					       IW_PMKID_LEN);
-					list_move(&pmk->list,
-						  &priv->pmklist.head);
-				}
+		}
+		if (ptr != &priv->pmklist.head)	/* not find address. */
+			break;	/* case */
+
+		if (priv->pmklist.size < PMK_LIST_MAX) {	/* new cache data */
+			for (i = 0; i < PMK_LIST_MAX; i++) {
+				pmk = &priv->pmklist.pmk[i];
+				if (memcmp("\x00\x00\x00\x00\x00\x00",
+					   pmk->bssid, ETH_ALEN) == 0)
+					break; /* loop */
 			}
+			memcpy(pmk->bssid, pmksa->bssid.sa_data, ETH_ALEN);
+			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_t,
+					 list);
+			memcpy(pmk->bssid, pmksa->bssid.sa_data, ETH_ALEN);
+			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)) {	/* list empty */
 			return -EINVAL;
-		} else {	/* search cache data */
-			list_for_each(ptr, &priv->pmklist.head) {
-				pmk = list_entry(ptr, struct pmk_t, list);
-				if (!memcmp(pmksa->bssid.sa_data, pmk->bssid, ETH_ALEN)) {	/* match address! list del. */
-					eth_zero_addr(pmk->bssid);
-					memset(pmk->pmkid, 0, IW_PMKID_LEN);
-					list_del_init(&pmk->list);
-					break;
-				}
-			}
-			if (ptr == &priv->pmklist.head) {	/* not find address. */
-				return 0;
+		}
+		/* search cache data */
+		list_for_each(ptr, &priv->pmklist.head) {
+			pmk = list_entry(ptr, struct pmk_t, list);
+			if (memcmp(pmksa->bssid.sa_data, pmk->bssid, ETH_ALEN) == 0) {
+				eth_zero_addr(pmk->bssid);
+				memset(pmk->pmkid, 0, IW_PMKID_LEN);
+				list_del_init(&pmk->list);
+				break;
 			}
 		}
+		if (ptr == &priv->pmklist.head) {	/* not find address. */
+			return 0;
+		}
+
 		break;
 	case IW_PMKSA_FLUSH:
-		memset(&(priv->pmklist), 0, sizeof(priv->pmklist));
+		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);
@@ -2093,8 +1949,7 @@ static int ks_wlan_set_pmksa(struct net_device *dev,
 
 static struct iw_statistics *ks_get_wireless_stats(struct net_device *dev)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_statistics *wstats = &priv->wstats;
 
 	if (!atomic_read(&update_phyinfo)) {
@@ -2105,7 +1960,8 @@ static struct iw_statistics *ks_get_wireless_stats(struct net_device *dev)
 	}
 
 	/* Packets discarded in the wireless adapter due to wireless
-	 * specific problems */
+	 * specific problems
+	 */
 	wstats->discard.nwid = 0;	/* Rx invalid nwid      */
 	wstats->discard.code = 0;	/* Rx invalid crypt     */
 	wstats->discard.fragment = 0;	/* Rx invalid frag      */
@@ -2116,14 +1972,12 @@ static struct iw_statistics *ks_get_wireless_stats(struct net_device *dev)
 	return wstats;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set stop request */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
 	DPRINTK(2, "\n");
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -2137,15 +1991,12 @@ static int ks_wlan_set_stop_request(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Wireless Handler : set MLME */
 #include <linux/ieee80211.h>
 static int ks_wlan_set_mlme(struct net_device *dev,
 			    struct iw_request_info *info, struct iw_point *dwrq,
 			    char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 	struct iw_mlme *mlme = (struct iw_mlme *)extra;
 	__u32 mode;
 
@@ -2167,86 +2018,22 @@ static int ks_wlan_set_mlme(struct net_device *dev,
 	}
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get firemware version */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
-	strcpy(extra, &(priv->firmware_version[0]));
-	dwrq->length = priv->version_size + 1;
-	return 0;
-}
-
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : set force disconnect status */
-static int ks_wlan_set_detach(struct net_device *dev,
-			      struct iw_request_info *info, __u32 *uwrq,
-			      char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-
-	/* for SLEEP MODE */
-	if (*uwrq == CONNECT_STATUS) {	/* 0 */
-		priv->connect_status &= ~FORCE_DISCONNECT;
-		if ((priv->connect_status & CONNECT_STATUS_MASK) ==
-		    CONNECT_STATUS)
-			netif_carrier_on(dev);
-	} else if (*uwrq == DISCONNECT_STATUS) {	/* 1 */
-		priv->connect_status |= FORCE_DISCONNECT;
-		netif_carrier_off(dev);
-	} else
-		return -EINVAL;
-	return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Private handler : get force disconnect status */
-static int ks_wlan_get_detach(struct net_device *dev,
-			      struct iw_request_info *info, __u32 *uwrq,
-			      char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-
-	/* for SLEEP MODE */
-	*uwrq = ((priv->connect_status & FORCE_DISCONNECT) ? 1 : 0);
-	return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Private handler : get connect status */
-static int ks_wlan_get_connect(struct net_device *dev,
-			       struct iw_request_info *info, __u32 *uwrq,
-			       char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
-	/* for SLEEP MODE */
-	*uwrq = (priv->connect_status & CONNECT_STATUS_MASK);
+	strcpy(extra, priv->firmware_version);
+	dwrq->length = priv->version_size + 1;
 	return 0;
 }
-#endif
 
-/*------------------------------------------------------------------*/
-/* Private handler : set preamble */
 static int ks_wlan_set_preamble(struct net_device *dev,
 				struct iw_request_info *info, __u32 *uwrq,
 				char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2256,21 +2043,19 @@ static int ks_wlan_set_preamble(struct net_device *dev,
 		priv->reg.preamble = LONG_PREAMBLE;
 	} else if (*uwrq == SHORT_PREAMBLE) {	/* 1 */
 		priv->reg.preamble = SHORT_PREAMBLE;
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	priv->need_commit |= SME_MODE_SET;
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get preamble */
 static int ks_wlan_get_preamble(struct net_device *dev,
 				struct iw_request_info *info, __u32 *uwrq,
 				char *extra)
 {
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2280,64 +2065,56 @@ static int ks_wlan_get_preamble(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set power save mode */
-static int ks_wlan_set_powermgt(struct net_device *dev,
-				struct iw_request_info *info, __u32 *uwrq,
-				char *extra)
+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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 
 	/* for SLEEP MODE */
-	if (*uwrq == POWMGT_ACTIVE_MODE) {	/* 0 */
-		priv->reg.powermgt = POWMGT_ACTIVE_MODE;
-	} else if (*uwrq == POWMGT_SAVE1_MODE) {	/* 1 */
+	if (*uwrq == POWER_MGMT_ACTIVE) {	/* 0 */
+		priv->reg.power_mgmt = POWER_MGMT_ACTIVE;
+	} else if (*uwrq == POWER_MGMT_SAVE1) {	/* 1 */
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE)
-			priv->reg.powermgt = POWMGT_SAVE1_MODE;
+			priv->reg.power_mgmt = POWER_MGMT_SAVE1;
 		else
 			return -EINVAL;
-	} else if (*uwrq == POWMGT_SAVE2_MODE) {	/* 2 */
+	} else if (*uwrq == POWER_MGMT_SAVE2) {	/* 2 */
 		if (priv->reg.operation_mode == MODE_INFRASTRUCTURE)
-			priv->reg.powermgt = POWMGT_SAVE2_MODE;
+			priv->reg.power_mgmt = POWER_MGMT_SAVE2;
 		else
 			return -EINVAL;
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	hostif_sme_enqueue(priv, SME_POW_MNGMT_REQUEST);
 
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get power save made */
-static int ks_wlan_get_powermgt(struct net_device *dev,
-				struct iw_request_info *info, __u32 *uwrq,
-				char *extra)
+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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
 
 	/* for SLEEP MODE */
-	*uwrq = priv->reg.powermgt;
+	*uwrq = priv->reg.power_mgmt;
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set scan type */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2346,20 +2123,18 @@ static int ks_wlan_set_scan_type(struct net_device *dev,
 		priv->reg.scan_type = ACTIVE_SCAN;
 	} else if (*uwrq == PASSIVE_SCAN) {	/* 1 */
 		priv->reg.scan_type = PASSIVE_SCAN;
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get scan type */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2368,145 +2143,11 @@ static int ks_wlan_get_scan_type(struct net_device *dev,
 	return 0;
 }
 
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : write raw data to device */
-static int ks_wlan_data_write(struct net_device *dev,
-			      struct iw_request_info *info,
-			      struct iw_point *dwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-	unsigned char *wbuff = NULL;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	wbuff = (unsigned char *)kmalloc(dwrq->length, GFP_ATOMIC);
-	if (!wbuff)
-		return -EFAULT;
-	memcpy(wbuff, extra, dwrq->length);
-
-	/* write to device */
-	ks_wlan_hw_tx(priv, wbuff, dwrq->length, NULL, NULL, NULL);
-
-	return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Private handler : read raw data form device */
-static int ks_wlan_data_read(struct net_device *dev,
-			     struct iw_request_info *info,
-			     struct iw_point *dwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-	unsigned short read_length;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	if (!atomic_read(&priv->event_count)) {
-		if (priv->dev_state < DEVICE_STATE_BOOT) {	/* Remove device */
-			read_length = 4;
-			memset(extra, 0xff, read_length);
-			dwrq->length = read_length;
-			return 0;
-		}
-		read_length = 0;
-		memset(extra, 0, 1);
-		dwrq->length = 0;
-		return 0;
-	}
-
-	if (atomic_read(&priv->event_count) > 0)
-		atomic_dec(&priv->event_count);
-
-	spin_lock(&priv->dev_read_lock);	/* request spin lock */
-
-	/* Copy length max size 0x07ff */
-	if (priv->dev_size[priv->dev_count] > 2047)
-		read_length = 2047;
-	else
-		read_length = priv->dev_size[priv->dev_count];
-
-	/* Copy data */
-	memcpy(extra, &(priv->dev_data[priv->dev_count][0]), read_length);
-
-	spin_unlock(&priv->dev_read_lock);	/* release spin lock */
-
-	/* Initialize */
-	priv->dev_data[priv->dev_count] = 0;
-	priv->dev_size[priv->dev_count] = 0;
-
-	priv->dev_count++;
-	if (priv->dev_count == DEVICE_STOCK_COUNT)
-		priv->dev_count = 0;
-
-	/* Set read size */
-	dwrq->length = read_length;
-
-	return 0;
-}
-#endif
-
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : get wep string */
-#define WEP_ASCII_BUFF_SIZE (17 + 64 * 4 + 1)
-static int ks_wlan_get_wep_ascii(struct net_device *dev,
-				 struct iw_request_info *info,
-				 struct iw_point *dwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-	int i, j, len = 0;
-	char tmp[WEP_ASCII_BUFF_SIZE];
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	strcpy(tmp, " WEP keys ASCII \n");
-	len += strlen(" WEP keys ASCII \n");
-
-	for (i = 0; i < 4; i++) {
-		strcpy(tmp + len, "\t[");
-		len += strlen("\t[");
-		tmp[len] = '1' + i;
-		len++;
-		strcpy(tmp + len, "] ");
-		len += strlen("] ");
-		if (priv->reg.wep_key[i].size) {
-			strcpy(tmp + len,
-			       (priv->reg.wep_key[i].size <
-				6 ? "(40bits) [" : "(104bits) ["));
-			len +=
-			    strlen((priv->reg.wep_key[i].size <
-				    6 ? "(40bits) [" : "(104bits) ["));
-			for (j = 0; j < priv->reg.wep_key[i].size; j++, len++)
-				tmp[len] =
-				    (isprint(priv->reg.wep_key[i].val[j]) ?
-				     priv->reg.wep_key[i].val[j] : ' ');
-
-			strcpy(tmp + len, "]\n");
-			len += strlen("]\n");
-		} else {
-			strcpy(tmp + len, "off\n");
-			len += strlen("off\n");
-		}
-	}
-
-	memcpy(extra, tmp, len);
-	dwrq->length = len + 1;
-	return 0;
-}
-#endif
-
-/*------------------------------------------------------------------*/
-/* Private handler : set beacon lost count */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2519,18 +2160,16 @@ static int ks_wlan_set_beacon_lost(struct net_device *dev,
 	if (priv->reg.operation_mode == MODE_INFRASTRUCTURE) {
 		priv->need_commit |= SME_MODE_SET;
 		return -EINPROGRESS;	/* Call commit handler */
-	} else
+	} else {
 		return 0;
+	}
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get beacon lost count */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2539,14 +2178,11 @@ static int ks_wlan_get_beacon_lost(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set phy type */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2557,21 +2193,19 @@ static int ks_wlan_set_phy_type(struct net_device *dev,
 		priv->reg.phy_type = D_11G_ONLY_MODE;
 	} else if (*uwrq == D_11BG_COMPATIBLE_MODE) {	/* 2 */
 		priv->reg.phy_type = D_11BG_COMPATIBLE_MODE;
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	priv->need_commit |= SME_MODE_SET;
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get phy type */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2580,14 +2214,11 @@ static int ks_wlan_get_phy_type(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set cts mode */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2596,25 +2227,24 @@ static int ks_wlan_set_cts_mode(struct net_device *dev,
 		priv->reg.cts_mode = CTS_MODE_FALSE;
 	} else if (*uwrq == CTS_MODE_TRUE) {	/* 1 */
 		if (priv->reg.phy_type == D_11G_ONLY_MODE ||
-		    priv->reg.phy_type == D_11BG_COMPATIBLE_MODE)
+		    priv->reg.phy_type == D_11BG_COMPATIBLE_MODE) {
 			priv->reg.cts_mode = CTS_MODE_TRUE;
-		else
+		} else {
 			priv->reg.cts_mode = CTS_MODE_FALSE;
-	} else
+		}
+	} else {
 		return -EINVAL;
+	}
 
 	priv->need_commit |= SME_MODE_SET;
 	return -EINPROGRESS;	/* Call commit handler */
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get cts mode */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2623,14 +2253,11 @@ static int ks_wlan_get_cts_mode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set sleep mode */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "\n");
 
@@ -2653,14 +2280,11 @@ static int ks_wlan_set_sleep_mode(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get sleep mode */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "GET_SLEEP_MODE %d\n", priv->sleep_mode);
 	*uwrq = priv->sleep_mode;
@@ -2668,53 +2292,14 @@ static int ks_wlan_get_sleep_mode(struct net_device *dev,
 	return 0;
 }
 
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : set phy information timer */
-static int ks_wlan_set_phy_information_timer(struct net_device *dev,
-					     struct iw_request_info *info,
-					     __u32 *uwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	if (*uwrq >= 0 && *uwrq <= 0xFFFF)	/* 0-65535 */
-		priv->reg.phy_info_timer = (uint16_t)*uwrq;
-	else
-		return -EINVAL;
-
-	hostif_sme_enqueue(priv, SME_PHY_INFO_REQUEST);
-
-	return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Private handler : get phy information timer */
-static int ks_wlan_get_phy_information_timer(struct net_device *dev,
-					     struct iw_request_info *info,
-					     __u32 *uwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	*uwrq = priv->reg.phy_info_timer;
-	return 0;
-}
-#endif
-
 #ifdef WPS
-/*------------------------------------------------------------------*/
-/* Private handler : set WPS enable */
+
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
 	DPRINTK(2, "\n");
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -2730,14 +2315,12 @@ static int ks_wlan_set_wps_enable(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get WPS enable */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
+
 	DPRINTK(2, "\n");
 
 	if (priv->sleep_mode == SLP_SLEEP)
@@ -2749,16 +2332,13 @@ static int ks_wlan_get_wps_enable(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set WPS probe req */
 static int ks_wlan_set_wps_probe_req(struct net_device *dev,
 				     struct iw_request_info *info,
 				     struct iw_point *dwrq, char *extra)
 {
-	uint8_t *p = extra;
+	u8 *p = extra;
 	unsigned char len;
-	struct ks_wlan_private *priv =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	DPRINTK(2, "\n");
 
@@ -2786,34 +2366,13 @@ static int ks_wlan_set_wps_probe_req(struct net_device *dev,
 
 	return 0;
 }
-
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : get WPS probe req */
-static int ks_wlan_get_wps_probe_req(struct net_device *dev,
-				     struct iw_request_info *info,
-				     __u32 *uwrq, char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	DPRINTK(2, "\n");
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	return 0;
-}
-#endif
 #endif /* WPS */
 
-/*------------------------------------------------------------------*/
-/* Private handler : set tx gain control value */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2832,14 +2391,11 @@ static int ks_wlan_set_tx_gain(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get tx gain control value */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2849,14 +2405,11 @@ static int ks_wlan_get_tx_gain(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : set rx gain control value */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2875,14 +2428,11 @@ static int ks_wlan_set_rx_gain(struct net_device *dev,
 	return 0;
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get rx gain control value */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	if (priv->sleep_mode == SLP_SLEEP)
 		return -EPERM;
@@ -2892,36 +2442,11 @@ static int ks_wlan_get_rx_gain(struct net_device *dev,
 	return 0;
 }
 
-#if 0
-/*------------------------------------------------------------------*/
-/* Private handler : set region value */
-static int ks_wlan_set_region(struct net_device *dev,
-			      struct iw_request_info *info, __u32 *uwrq,
-			      char *extra)
-{
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)dev->priv;
-
-	if (priv->sleep_mode == SLP_SLEEP)
-		return -EPERM;
-	/* for SLEEP MODE */
-	if (*uwrq >= 0x9 && *uwrq <= 0xF)	/* 0x9-0xf */
-		priv->region = (uint8_t)*uwrq;
-	else
-		return -EINVAL;
-
-	hostif_sme_enqueue(priv, SME_SET_REGION);
-	return 0;
-}
-#endif
-
-/*------------------------------------------------------------------*/
-/* Private handler : get eeprom checksum result */
 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	*uwrq = priv->eeprom_checksum;
 	return 0;
@@ -2948,11 +2473,11 @@ static void print_hif_event(struct net_device *dev, int event)
 	case HIF_MIB_SET_CONF:
 		netdev_info(dev, "HIF_MIB_SET_CONF\n");
 		break;
-	case HIF_POWERMGT_REQ:
-		netdev_info(dev, "HIF_POWERMGT_REQ\n");
+	case HIF_POWER_MGMT_REQ:
+		netdev_info(dev, "HIF_POWER_MGMT_REQ\n");
 		break;
-	case HIF_POWERMGT_CONF:
-		netdev_info(dev, "HIF_POWERMGT_CONF\n");
+	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");
@@ -3040,14 +2565,12 @@ static void print_hif_event(struct net_device *dev, int event)
 	}
 }
 
-/*------------------------------------------------------------------*/
-/* Private handler : get host command history */
+/* 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 =
-	    (struct ks_wlan_private *)netdev_priv(dev);
+	struct ks_wlan_private *priv = netdev_priv(dev);
 
 	for (i = 63; i >= 0; i--) {
 		event =
@@ -3115,119 +2638,119 @@ static const struct iw_priv_args ks_wlan_private_args[] = {
 };
 
 static const iw_handler ks_wlan_handler[] = {
-	(iw_handler) ks_wlan_config_commit,	/* SIOCSIWCOMMIT */
-	(iw_handler) ks_wlan_get_name,	/* SIOCGIWNAME */
-	(iw_handler) NULL,	/* SIOCSIWNWID */
-	(iw_handler) NULL,	/* SIOCGIWNWID */
-	(iw_handler) ks_wlan_set_freq,	/* SIOCSIWFREQ */
-	(iw_handler) ks_wlan_get_freq,	/* SIOCGIWFREQ */
-	(iw_handler) ks_wlan_set_mode,	/* SIOCSIWMODE */
-	(iw_handler) ks_wlan_get_mode,	/* SIOCGIWMODE */
+	(iw_handler)ks_wlan_config_commit,	/* SIOCSIWCOMMIT */
+	(iw_handler)ks_wlan_get_name,	/* SIOCGIWNAME */
+	(iw_handler)NULL,	/* SIOCSIWNWID */
+	(iw_handler)NULL,	/* SIOCGIWNWID */
+	(iw_handler)ks_wlan_set_freq,	/* SIOCSIWFREQ */
+	(iw_handler)ks_wlan_get_freq,	/* SIOCGIWFREQ */
+	(iw_handler)ks_wlan_set_mode,	/* SIOCSIWMODE */
+	(iw_handler)ks_wlan_get_mode,	/* SIOCGIWMODE */
 #ifndef KSC_OPNOTSUPP
-	(iw_handler) ks_wlan_set_sens,	/* SIOCSIWSENS */
-	(iw_handler) ks_wlan_get_sens,	/* SIOCGIWSENS */
+	(iw_handler)ks_wlan_set_sens,	/* SIOCSIWSENS */
+	(iw_handler)ks_wlan_get_sens,	/* SIOCGIWSENS */
 #else /* KSC_OPNOTSUPP */
-	(iw_handler) NULL,	/* SIOCSIWSENS */
-	(iw_handler) NULL,	/* SIOCGIWSENS */
+	(iw_handler)NULL,	/* SIOCSIWSENS */
+	(iw_handler)NULL,	/* SIOCGIWSENS */
 #endif /* KSC_OPNOTSUPP */
-	(iw_handler) NULL,	/* SIOCSIWRANGE */
-	(iw_handler) ks_wlan_get_range,	/* SIOCGIWRANGE */
-	(iw_handler) NULL,	/* SIOCSIWPRIV */
-	(iw_handler) NULL,	/* SIOCGIWPRIV */
-	(iw_handler) NULL,	/* SIOCSIWSTATS */
-	(iw_handler) ks_wlan_get_iwstats,	/* SIOCGIWSTATS */
-	(iw_handler) NULL,	/* SIOCSIWSPY */
-	(iw_handler) NULL,	/* SIOCGIWSPY */
-	(iw_handler) NULL,	/* SIOCSIWTHRSPY */
-	(iw_handler) NULL,	/* SIOCGIWTHRSPY */
-	(iw_handler) ks_wlan_set_wap,	/* SIOCSIWAP */
-	(iw_handler) ks_wlan_get_wap,	/* SIOCGIWAP */
-//      (iw_handler) NULL,                      /* SIOCSIWMLME */
-	(iw_handler) ks_wlan_set_mlme,	/* SIOCSIWMLME */
-	(iw_handler) ks_wlan_get_aplist,	/* SIOCGIWAPLIST */
-	(iw_handler) ks_wlan_set_scan,	/* SIOCSIWSCAN */
-	(iw_handler) ks_wlan_get_scan,	/* SIOCGIWSCAN */
-	(iw_handler) ks_wlan_set_essid,	/* SIOCSIWESSID */
-	(iw_handler) ks_wlan_get_essid,	/* SIOCGIWESSID */
-	(iw_handler) ks_wlan_set_nick,	/* SIOCSIWNICKN */
-	(iw_handler) ks_wlan_get_nick,	/* SIOCGIWNICKN */
-	(iw_handler) NULL,	/* -- hole -- */
-	(iw_handler) NULL,	/* -- hole -- */
-	(iw_handler) ks_wlan_set_rate,	/* SIOCSIWRATE */
-	(iw_handler) ks_wlan_get_rate,	/* SIOCGIWRATE */
-	(iw_handler) ks_wlan_set_rts,	/* SIOCSIWRTS */
-	(iw_handler) ks_wlan_get_rts,	/* SIOCGIWRTS */
-	(iw_handler) ks_wlan_set_frag,	/* SIOCSIWFRAG */
-	(iw_handler) ks_wlan_get_frag,	/* SIOCGIWFRAG */
+	(iw_handler)NULL,	/* SIOCSIWRANGE */
+	(iw_handler)ks_wlan_get_range,	/* SIOCGIWRANGE */
+	(iw_handler)NULL,	/* SIOCSIWPRIV */
+	(iw_handler)NULL,	/* SIOCGIWPRIV */
+	(iw_handler)NULL,	/* SIOCSIWSTATS */
+	(iw_handler)ks_wlan_get_iwstats,	/* SIOCGIWSTATS */
+	(iw_handler)NULL,	/* SIOCSIWSPY */
+	(iw_handler)NULL,	/* SIOCGIWSPY */
+	(iw_handler)NULL,	/* SIOCSIWTHRSPY */
+	(iw_handler)NULL,	/* SIOCGIWTHRSPY */
+	(iw_handler)ks_wlan_set_wap,	/* SIOCSIWAP */
+	(iw_handler)ks_wlan_get_wap,	/* SIOCGIWAP */
+//      (iw_handler)NULL,                      /* SIOCSIWMLME */
+	(iw_handler)ks_wlan_set_mlme,	/* SIOCSIWMLME */
+	(iw_handler)ks_wlan_get_aplist,	/* SIOCGIWAPLIST */
+	(iw_handler)ks_wlan_set_scan,	/* SIOCSIWSCAN */
+	(iw_handler)ks_wlan_get_scan,	/* SIOCGIWSCAN */
+	(iw_handler)ks_wlan_set_essid,	/* SIOCSIWESSID */
+	(iw_handler)ks_wlan_get_essid,	/* SIOCGIWESSID */
+	(iw_handler)ks_wlan_set_nick,	/* SIOCSIWNICKN */
+	(iw_handler)ks_wlan_get_nick,	/* SIOCGIWNICKN */
+	(iw_handler)NULL,	/* -- hole -- */
+	(iw_handler)NULL,	/* -- hole -- */
+	(iw_handler)ks_wlan_set_rate,	/* SIOCSIWRATE */
+	(iw_handler)ks_wlan_get_rate,	/* SIOCGIWRATE */
+	(iw_handler)ks_wlan_set_rts,	/* SIOCSIWRTS */
+	(iw_handler)ks_wlan_get_rts,	/* SIOCGIWRTS */
+	(iw_handler)ks_wlan_set_frag,	/* SIOCSIWFRAG */
+	(iw_handler)ks_wlan_get_frag,	/* SIOCGIWFRAG */
 #ifndef KSC_OPNOTSUPP
-	(iw_handler) ks_wlan_set_txpow,	/* SIOCSIWTXPOW */
-	(iw_handler) ks_wlan_get_txpow,	/* SIOCGIWTXPOW */
-	(iw_handler) ks_wlan_set_retry,	/* SIOCSIWRETRY */
-	(iw_handler) ks_wlan_get_retry,	/* SIOCGIWRETRY */
+	(iw_handler)ks_wlan_set_txpow,	/* SIOCSIWTXPOW */
+	(iw_handler)ks_wlan_get_txpow,	/* SIOCGIWTXPOW */
+	(iw_handler)ks_wlan_set_retry,	/* SIOCSIWRETRY */
+	(iw_handler)ks_wlan_get_retry,	/* SIOCGIWRETRY */
 #else /* KSC_OPNOTSUPP */
-	(iw_handler) NULL,	/* SIOCSIWTXPOW */
-	(iw_handler) NULL,	/* SIOCGIWTXPOW */
-	(iw_handler) NULL,	/* SIOCSIWRETRY */
-	(iw_handler) NULL,	/* SIOCGIWRETRY */
+	(iw_handler)NULL,	/* SIOCSIWTXPOW */
+	(iw_handler)NULL,	/* SIOCGIWTXPOW */
+	(iw_handler)NULL,	/* SIOCSIWRETRY */
+	(iw_handler)NULL,	/* SIOCGIWRETRY */
 #endif /* KSC_OPNOTSUPP */
-	(iw_handler) ks_wlan_set_encode,	/* SIOCSIWENCODE */
-	(iw_handler) ks_wlan_get_encode,	/* SIOCGIWENCODE */
-	(iw_handler) ks_wlan_set_power,	/* SIOCSIWPOWER */
-	(iw_handler) ks_wlan_get_power,	/* SIOCGIWPOWER */
-	(iw_handler) NULL,	/* -- hole -- */
-	(iw_handler) NULL,	/* -- hole -- */
-//      (iw_handler) NULL,                      /* SIOCSIWGENIE */
-	(iw_handler) ks_wlan_set_genie,	/* SIOCSIWGENIE */
-	(iw_handler) NULL,	/* SIOCGIWGENIE */
-	(iw_handler) ks_wlan_set_auth_mode,	/* SIOCSIWAUTH */
-	(iw_handler) ks_wlan_get_auth_mode,	/* SIOCGIWAUTH */
-	(iw_handler) ks_wlan_set_encode_ext,	/* SIOCSIWENCODEEXT */
-	(iw_handler) ks_wlan_get_encode_ext,	/* SIOCGIWENCODEEXT */
-	(iw_handler) ks_wlan_set_pmksa,	/* SIOCSIWPMKSA */
-	(iw_handler) NULL,	/* -- hole -- */
+	(iw_handler)ks_wlan_set_encode,	/* SIOCSIWENCODE */
+	(iw_handler)ks_wlan_get_encode,	/* SIOCGIWENCODE */
+	(iw_handler)ks_wlan_set_power,	/* SIOCSIWPOWER */
+	(iw_handler)ks_wlan_get_power,	/* SIOCGIWPOWER */
+	(iw_handler)NULL,	/* -- hole -- */
+	(iw_handler)NULL,	/* -- hole -- */
+//      (iw_handler)NULL,                      /* SIOCSIWGENIE */
+	(iw_handler)ks_wlan_set_genie,	/* SIOCSIWGENIE */
+	(iw_handler)NULL,	/* SIOCGIWGENIE */
+	(iw_handler)ks_wlan_set_auth_mode,	/* SIOCSIWAUTH */
+	(iw_handler)ks_wlan_get_auth_mode,	/* SIOCGIWAUTH */
+	(iw_handler)ks_wlan_set_encode_ext,	/* SIOCSIWENCODEEXT */
+	(iw_handler)ks_wlan_get_encode_ext,	/* SIOCGIWENCODEEXT */
+	(iw_handler)ks_wlan_set_pmksa,	/* SIOCSIWPMKSA */
+	(iw_handler)NULL,	/* -- hole -- */
 };
 
 /* private_handler */
 static const iw_handler ks_wlan_private_handler[] = {
-	(iw_handler) NULL,	/*  0 */
-	(iw_handler) NULL,	/*  1, used to be: KS_WLAN_GET_DRIVER_VERSION */
-	(iw_handler) NULL,	/*  2 */
-	(iw_handler) ks_wlan_get_firmware_version,	/*  3 KS_WLAN_GET_FIRM_VERSION */
+	(iw_handler)NULL,	/*  0 */
+	(iw_handler)NULL,	/*  1, used to be: KS_WLAN_GET_DRIVER_VERSION */
+	(iw_handler)NULL,	/*  2 */
+	(iw_handler)ks_wlan_get_firmware_version,	/*  3 KS_WLAN_GET_FIRM_VERSION */
 #ifdef WPS
-	(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_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 */
 #else
-	(iw_handler) NULL,	/*  4 */
-	(iw_handler) NULL,	/*  5 */
-	(iw_handler) NULL,	/*  6 */
+	(iw_handler)NULL,	/*  4 */
+	(iw_handler)NULL,	/*  5 */
+	(iw_handler)NULL,	/*  6 */
 #endif /* WPS */
 
-	(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_powermgt,	/* 10 KS_WLAN_SET_POWER_SAVE */
-	(iw_handler) ks_wlan_get_powermgt,	/* 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 */
+	(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 = {
@@ -3235,8 +2758,8 @@ static const struct iw_handler_def ks_wlan_handler_def = {
 	.num_private = sizeof(ks_wlan_private_handler) / sizeof(iw_handler),
 	.num_private_args =
 	    sizeof(ks_wlan_private_args) / sizeof(struct iw_priv_args),
-	.standard = (iw_handler *) ks_wlan_handler,
-	.private = (iw_handler *) ks_wlan_private_handler,
+	.standard = (iw_handler *)ks_wlan_handler,
+	.private = (iw_handler *)ks_wlan_private_handler,
 	.private_args = (struct iw_priv_args *)ks_wlan_private_args,
 	.get_wireless_stats = ks_get_wireless_stats,
 };
@@ -3244,20 +2767,20 @@ static const struct iw_handler_def ks_wlan_handler_def = {
 static int ks_wlan_netdev_ioctl(struct net_device *dev, struct ifreq *rq,
 				int cmd)
 {
-	int rc = 0;
+	int ret;
 	struct iwreq *wrq = (struct iwreq *)rq;
 
 	switch (cmd) {
 	case SIOCIWFIRSTPRIV + 20:	/* KS_WLAN_SET_STOP_REQ */
-		rc = ks_wlan_set_stop_request(dev, NULL, &(wrq->u.mode), NULL);
+		ret = ks_wlan_set_stop_request(dev, NULL, &wrq->u.mode, NULL);
 		break;
 		// All other calls are currently unsupported
 	default:
-		rc = -EOPNOTSUPP;
+		ret = -EOPNOTSUPP;
 	}
 
-	DPRINTK(5, "return=%d\n", rc);
-	return rc;
+	DPRINTK(5, "return=%d\n", ret);
+	return ret;
 }
 
 static
@@ -3305,7 +2828,7 @@ static
 int ks_wlan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ks_wlan_private *priv = netdev_priv(dev);
-	int rc = 0;
+	int ret;
 
 	DPRINTK(3, "in_interrupt()=%ld\n", in_interrupt());
 
@@ -3321,21 +2844,17 @@ int ks_wlan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (netif_running(dev))
 		netif_stop_queue(dev);
 
-	rc = hostif_data_request(priv, skb);
+	ret = hostif_data_request(priv, skb);
 	netif_trans_update(dev);
 
-	DPRINTK(4, "rc=%d\n", rc);
-	if (rc)
-		rc = 0;
+	if (ret)
+		DPRINTK(4, "hostif_data_request error: =%d\n", ret);
 
-	return rc;
+	return 0;
 }
 
-void send_packet_complete(void *arg1, void *arg2)
+void send_packet_complete(struct ks_wlan_private *priv, struct sk_buff *skb)
 {
-	struct ks_wlan_private *priv = (struct ks_wlan_private *)arg1;
-	struct sk_buff *packet = (struct sk_buff *)arg2;
-
 	DPRINTK(3, "\n");
 
 	priv->nstats.tx_packets++;
@@ -3343,15 +2862,16 @@ void send_packet_complete(void *arg1, void *arg2)
 	if (netif_queue_stopped(priv->net_dev))
 		netif_wake_queue(priv->net_dev);
 
-	if (packet) {
-		priv->nstats.tx_bytes += packet->len;
-		dev_kfree_skb(packet);
-		packet = NULL;
+	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. */
+/*
+ * 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_multicast_list(struct net_device *dev)
 {
@@ -3457,3 +2977,23 @@ int ks_wlan_net_stop(struct net_device *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/ks7010/michael_mic.c b/drivers/staging/ks7010/michael_mic.c
index f6e70fa2a12f..80497ef5b1aa 100644
--- a/drivers/staging/ks7010/michael_mic.c
+++ b/drivers/staging/ks7010/michael_mic.c
@@ -38,7 +38,7 @@ do {					\
 } while (0)
 
 static
-void MichaelInitializeFunction(struct michel_mic_t *Mic, uint8_t *key)
+void MichaelInitializeFunction(struct michael_mic_t *Mic, uint8_t *key)
 {
 	// Set the key
 	Mic->K0 = getUInt32(key, 0);
@@ -61,7 +61,7 @@ do {								\
 } while (0)
 
 static
-void MichaelAppend(struct michel_mic_t *Mic, uint8_t *src, int nBytes)
+void MichaelAppend(struct michael_mic_t *Mic, uint8_t *src, int nBytes)
 {
 	int addlen;
 
@@ -96,7 +96,7 @@ void MichaelAppend(struct michel_mic_t *Mic, uint8_t *src, int nBytes)
 }
 
 static
-void MichaelGetMIC(struct michel_mic_t *Mic, uint8_t *dst)
+void MichaelGetMIC(struct michael_mic_t *Mic, uint8_t *dst)
 {
 	u8 *data = Mic->M;
 
@@ -125,7 +125,7 @@ void MichaelGetMIC(struct michel_mic_t *Mic, uint8_t *dst)
 	MichaelClear(Mic);
 }
 
-void MichaelMICFunction(struct michel_mic_t *Mic, u8 *Key,
+void MichaelMICFunction(struct michael_mic_t *Mic, u8 *Key,
 			u8 *Data, int Len, u8 priority,
 			u8 *Result)
 {
@@ -141,8 +141,8 @@ void MichaelMICFunction(struct michel_mic_t *Mic, u8 *Key,
 	 * +--+--+--------+--+----+--+--+--+--+--+--+--+--+
 	 */
 	MichaelInitializeFunction(Mic, Key);
-	MichaelAppend(Mic, (uint8_t *) Data, 12);	/* |DA|SA| */
+	MichaelAppend(Mic, (uint8_t *)Data, 12);	/* |DA|SA| */
 	MichaelAppend(Mic, pad_data, 4);	/* |Priority|0|0|0| */
-	MichaelAppend(Mic, (uint8_t *) (Data + 12), Len - 12);	/* |Data| */
+	MichaelAppend(Mic, (uint8_t *)(Data + 12), Len - 12);	/* |Data| */
 	MichaelGetMIC(Mic, Result);
 }
diff --git a/drivers/staging/ks7010/michael_mic.h b/drivers/staging/ks7010/michael_mic.h
index 248f849fc4a5..758e429446f1 100644
--- a/drivers/staging/ks7010/michael_mic.h
+++ b/drivers/staging/ks7010/michael_mic.h
@@ -9,8 +9,8 @@
  *   published by the Free Software Foundation.
  */
 
-/* MichelMIC routine define */
-struct michel_mic_t {
+/* MichaelMIC routine define */
+struct michael_mic_t {
 	u32 K0;	// Key
 	u32 K1;	// Key
 	u32 L;	// Current state
@@ -20,6 +20,6 @@ struct michel_mic_t {
 	u8 Result[8];
 };
 
-void MichaelMICFunction(struct michel_mic_t *Mic, u8 *Key,
+void MichaelMICFunction(struct michael_mic_t *Mic, u8 *Key,
 			u8 *Data, int Len, u8 priority,
 			u8 *Result);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index 2dae85798ec1..e774c75ecadd 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -87,12 +87,9 @@ do {								    \
 #define LIBCFS_VMALLOC_SIZE	(2 << PAGE_SHIFT) /* 2 pages */
 #endif
 
-#define LIBCFS_ALLOC_PRE(size, mask)					    \
-do {									    \
-	LASSERT(!in_interrupt() ||					    \
-		((size) <= LIBCFS_VMALLOC_SIZE &&			    \
-		 !gfpflags_allow_blocking(mask)));			    \
-} while (0)
+#define LIBCFS_ALLOC_PRE(size, mask)					\
+	LASSERT(!in_interrupt() || ((size) <= LIBCFS_VMALLOC_SIZE &&	\
+				    !gfpflags_allow_blocking(mask)))
 
 #define LIBCFS_ALLOC_POST(ptr, size)					    \
 do {									    \
@@ -187,46 +184,28 @@ void  cfs_array_free(void *vars);
 #if LASSERT_ATOMIC_ENABLED
 
 /** assert value of @a is equal to @v */
-#define LASSERT_ATOMIC_EQ(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) == v,		       \
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_EQ(a, v)			\
+	LASSERTF(atomic_read(a) == v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is unequal to @v */
-#define LASSERT_ATOMIC_NE(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) != v,		       \
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_NE(a, v)		\
+	LASSERTF(atomic_read(a) != v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is little than @v */
-#define LASSERT_ATOMIC_LT(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) < v,			\
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_LT(a, v)		\
+	LASSERTF(atomic_read(a) < v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is little/equal to @v */
-#define LASSERT_ATOMIC_LE(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) <= v,		       \
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_LE(a, v)		\
+	LASSERTF(atomic_read(a) <= v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is great than @v */
-#define LASSERT_ATOMIC_GT(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) > v,			\
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_GT(a, v)		\
+	LASSERTF(atomic_read(a) > v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is great/equal to @v */
-#define LASSERT_ATOMIC_GE(a, v)				 \
-do {							    \
-	LASSERTF(atomic_read(a) >= v,		       \
-		 "value: %d\n", atomic_read((a)));	  \
-} while (0)
+#define LASSERT_ATOMIC_GE(a, v)		\
+	LASSERTF(atomic_read(a) >= v, "value: %d\n", atomic_read((a)))
 
 /** assert value of @a is great than @v1 and little than @v2 */
 #define LASSERT_ATOMIC_GT_LT(a, v1, v2)			 \
diff --git a/drivers/staging/lustre/include/linux/lnet/api.h b/drivers/staging/lustre/include/linux/lnet/api.h
index cb0d6b481455..f4b6de2ec0ff 100644
--- a/drivers/staging/lustre/include/linux/lnet/api.h
+++ b/drivers/staging/lustre/include/linux/lnet/api.h
@@ -74,9 +74,8 @@ int LNetNIFini(void);
  * \see LNetMEAttach
  * @{
  */
-int LNetGetId(unsigned int index, lnet_process_id_t *id);
+int LNetGetId(unsigned int index, struct lnet_process_id *id);
 int LNetDist(lnet_nid_t nid, lnet_nid_t *srcnid, __u32 *order);
-void LNetSnprintHandle(char *str, int str_len, lnet_handle_any_t handle);
 
 /** @} lnet_addr */
 
@@ -94,22 +93,22 @@ void LNetSnprintHandle(char *str, int str_len, lnet_handle_any_t handle);
  * @{
  */
 int LNetMEAttach(unsigned int      portal,
-		 lnet_process_id_t match_id_in,
+		 struct lnet_process_id match_id_in,
 		 __u64		   match_bits_in,
 		 __u64		   ignore_bits_in,
-		 lnet_unlink_t     unlink_in,
-		 lnet_ins_pos_t    pos_in,
-		 lnet_handle_me_t *handle_out);
+		 enum lnet_unlink unlink_in,
+		 enum lnet_ins_pos pos_in,
+		 struct lnet_handle_me *handle_out);
 
-int LNetMEInsert(lnet_handle_me_t  current_in,
-		 lnet_process_id_t match_id_in,
+int LNetMEInsert(struct lnet_handle_me current_in,
+		 struct lnet_process_id match_id_in,
 		 __u64		   match_bits_in,
 		 __u64		   ignore_bits_in,
-		 lnet_unlink_t     unlink_in,
-		 lnet_ins_pos_t    position_in,
-		 lnet_handle_me_t *handle_out);
+		 enum lnet_unlink unlink_in,
+		 enum lnet_ins_pos position_in,
+		 struct lnet_handle_me *handle_out);
 
-int LNetMEUnlink(lnet_handle_me_t current_in);
+int LNetMEUnlink(struct lnet_handle_me current_in);
 /** @} lnet_me */
 
 /** \defgroup lnet_md Memory descriptors
@@ -125,16 +124,16 @@ int LNetMEUnlink(lnet_handle_me_t current_in);
  * associated with a MD: LNetMDUnlink().
  * @{
  */
-int LNetMDAttach(lnet_handle_me_t  current_in,
-		 lnet_md_t	   md_in,
-		 lnet_unlink_t     unlink_in,
-		 lnet_handle_md_t *handle_out);
+int LNetMDAttach(struct lnet_handle_me current_in,
+		 struct lnet_md md_in,
+		 enum lnet_unlink unlink_in,
+		 struct lnet_handle_md *md_handle_out);
 
-int LNetMDBind(lnet_md_t	   md_in,
-	       lnet_unlink_t       unlink_in,
-	       lnet_handle_md_t   *handle_out);
+int LNetMDBind(struct lnet_md md_in,
+	       enum lnet_unlink unlink_in,
+	       struct lnet_handle_md *md_handle_out);
 
-int LNetMDUnlink(lnet_handle_md_t md_in);
+int LNetMDUnlink(struct lnet_handle_md md_in);
 /** @} lnet_md */
 
 /** \defgroup lnet_eq Events and event queues
@@ -147,9 +146,9 @@ int LNetMDUnlink(lnet_handle_md_t md_in);
  * associated with it. If an event handler exists, it will be run for each
  * event that is deposited into the EQ.
  *
- * In addition to the lnet_handle_eq_t, the LNet API defines two types
- * associated with events: The ::lnet_event_kind_t defines the kinds of events
- * that can be stored in an EQ. The lnet_event_t defines a structure that
+ * In addition to the lnet_handle_eq, the LNet API defines two types
+ * associated with events: The ::lnet_event_kind defines the kinds of events
+ * that can be stored in an EQ. The lnet_event defines a structure that
  * holds the information about with an event.
  *
  * There are five functions for dealing with EQs: LNetEQAlloc() is used to
@@ -162,14 +161,14 @@ int LNetMDUnlink(lnet_handle_md_t md_in);
  */
 int LNetEQAlloc(unsigned int       count_in,
 		lnet_eq_handler_t  handler,
-		lnet_handle_eq_t  *handle_out);
+		struct lnet_handle_eq *handle_out);
 
-int LNetEQFree(lnet_handle_eq_t eventq_in);
+int LNetEQFree(struct lnet_handle_eq eventq_in);
 
-int LNetEQPoll(lnet_handle_eq_t *eventqs_in,
+int LNetEQPoll(struct lnet_handle_eq *eventqs_in,
 	       int		 neq_in,
 	       int		 timeout_ms,
-	       lnet_event_t     *event_out,
+	       struct lnet_event *event_out,
 	       int		*which_eq_out);
 /** @} lnet_eq */
 
@@ -180,17 +179,17 @@ int LNetEQPoll(lnet_handle_eq_t *eventqs_in,
  * @{
  */
 int LNetPut(lnet_nid_t	      self,
-	    lnet_handle_md_t  md_in,
-	    lnet_ack_req_t    ack_req_in,
-	    lnet_process_id_t target_in,
+	    struct lnet_handle_md md_in,
+	    enum lnet_ack_req ack_req_in,
+	    struct lnet_process_id target_in,
 	    unsigned int      portal_in,
 	    __u64	      match_bits_in,
 	    unsigned int      offset_in,
 	    __u64	      hdr_data_in);
 
 int LNetGet(lnet_nid_t	      self,
-	    lnet_handle_md_t  md_in,
-	    lnet_process_id_t target_in,
+	    struct lnet_handle_md md_in,
+	    struct lnet_process_id target_in,
 	    unsigned int      portal_in,
 	    __u64	      match_bits_in,
 	    unsigned int      offset_in);
@@ -203,7 +202,7 @@ int LNetGet(lnet_nid_t	      self,
 int LNetSetLazyPortal(int portal);
 int LNetClearLazyPortal(int portal);
 int LNetCtl(unsigned int cmd, void *arg);
-void LNetDebugPeer(lnet_process_id_t id);
+void LNetDebugPeer(struct lnet_process_id id);
 
 /** @} lnet_misc */
 
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index 3d19402ba728..8ae7423b4543 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -41,7 +41,7 @@
 #include "lib-types.h"
 #include "lib-dlc.h"
 
-extern lnet_t	the_lnet;	/* THE network */
+extern struct lnet the_lnet;	/* THE network */
 
 #if (BITS_PER_LONG == 32)
 /* 2 CPTs, allowing more CPTs might make us under memory pressure */
@@ -65,7 +65,7 @@ extern lnet_t	the_lnet;	/* THE network */
 /** exclusive lock */
 #define LNET_LOCK_EX		CFS_PERCPT_LOCK_EX
 
-static inline int lnet_is_route_alive(lnet_route_t *route)
+static inline int lnet_is_route_alive(struct lnet_route *route)
 {
 	/* gateway is down */
 	if (!route->lr_gateway->lp_alive)
@@ -84,14 +84,14 @@ static inline int lnet_is_wire_handle_none(struct lnet_handle_wire *wh)
 		wh->wh_object_cookie == LNET_WIRE_HANDLE_COOKIE_NONE);
 }
 
-static inline int lnet_md_exhausted(lnet_libmd_t *md)
+static inline int lnet_md_exhausted(struct lnet_libmd *md)
 {
 	return (!md->md_threshold ||
 		((md->md_options & LNET_MD_MAX_SIZE) &&
 		 md->md_offset + md->md_max_size > md->md_length));
 }
 
-static inline int lnet_md_unlinkable(lnet_libmd_t *md)
+static inline int lnet_md_unlinkable(struct lnet_libmd *md)
 {
 	/*
 	 * Should unlink md when its refcount is 0 and either:
@@ -178,34 +178,34 @@ lnet_net_lock_current(void)
 
 #define MAX_PORTALS		64
 
-static inline lnet_eq_t *
+static inline struct lnet_eq *
 lnet_eq_alloc(void)
 {
-	lnet_eq_t *eq;
+	struct lnet_eq *eq;
 
 	LIBCFS_ALLOC(eq, sizeof(*eq));
 	return eq;
 }
 
 static inline void
-lnet_eq_free(lnet_eq_t *eq)
+lnet_eq_free(struct lnet_eq *eq)
 {
 	LIBCFS_FREE(eq, sizeof(*eq));
 }
 
-static inline lnet_libmd_t *
-lnet_md_alloc(lnet_md_t *umd)
+static inline struct lnet_libmd *
+lnet_md_alloc(struct lnet_md *umd)
 {
-	lnet_libmd_t *md;
+	struct lnet_libmd *md;
 	unsigned int size;
 	unsigned int niov;
 
 	if (umd->options & LNET_MD_KIOV) {
 		niov = umd->length;
-		size = offsetof(lnet_libmd_t, md_iov.kiov[niov]);
+		size = offsetof(struct lnet_libmd, md_iov.kiov[niov]);
 	} else {
 		niov = umd->options & LNET_MD_IOVEC ? umd->length : 1;
-		size = offsetof(lnet_libmd_t, md_iov.iov[niov]);
+		size = offsetof(struct lnet_libmd, md_iov.iov[niov]);
 	}
 
 	LIBCFS_ALLOC(md, size);
@@ -221,37 +221,37 @@ lnet_md_alloc(lnet_md_t *umd)
 }
 
 static inline void
-lnet_md_free(lnet_libmd_t *md)
+lnet_md_free(struct lnet_libmd *md)
 {
 	unsigned int size;
 
 	if (md->md_options & LNET_MD_KIOV)
-		size = offsetof(lnet_libmd_t, md_iov.kiov[md->md_niov]);
+		size = offsetof(struct lnet_libmd, md_iov.kiov[md->md_niov]);
 	else
-		size = offsetof(lnet_libmd_t, md_iov.iov[md->md_niov]);
+		size = offsetof(struct lnet_libmd, md_iov.iov[md->md_niov]);
 
 	LIBCFS_FREE(md, size);
 }
 
-static inline lnet_me_t *
+static inline struct lnet_me *
 lnet_me_alloc(void)
 {
-	lnet_me_t *me;
+	struct lnet_me *me;
 
 	LIBCFS_ALLOC(me, sizeof(*me));
 	return me;
 }
 
 static inline void
-lnet_me_free(lnet_me_t *me)
+lnet_me_free(struct lnet_me *me)
 {
 	LIBCFS_FREE(me, sizeof(*me));
 }
 
-static inline lnet_msg_t *
+static inline struct lnet_msg *
 lnet_msg_alloc(void)
 {
-	lnet_msg_t *msg;
+	struct lnet_msg *msg;
 
 	LIBCFS_ALLOC(msg, sizeof(*msg));
 
@@ -260,57 +260,57 @@ lnet_msg_alloc(void)
 }
 
 static inline void
-lnet_msg_free(lnet_msg_t *msg)
+lnet_msg_free(struct lnet_msg *msg)
 {
 	LASSERT(!msg->msg_onactivelist);
 	LIBCFS_FREE(msg, sizeof(*msg));
 }
 
-lnet_libhandle_t *lnet_res_lh_lookup(struct lnet_res_container *rec,
-				     __u64 cookie);
+struct lnet_libhandle *lnet_res_lh_lookup(struct lnet_res_container *rec,
+					  __u64 cookie);
 void lnet_res_lh_initialize(struct lnet_res_container *rec,
-			    lnet_libhandle_t *lh);
+			    struct lnet_libhandle *lh);
 static inline void
-lnet_res_lh_invalidate(lnet_libhandle_t *lh)
+lnet_res_lh_invalidate(struct lnet_libhandle *lh)
 {
 	/* NB: cookie is still useful, don't reset it */
 	list_del(&lh->lh_hash_chain);
 }
 
 static inline void
-lnet_eq2handle(lnet_handle_eq_t *handle, lnet_eq_t *eq)
+lnet_eq2handle(struct lnet_handle_eq *handle, struct lnet_eq *eq)
 {
 	if (!eq) {
-		LNetInvalidateHandle(handle);
+		LNetInvalidateEQHandle(handle);
 		return;
 	}
 
 	handle->cookie = eq->eq_lh.lh_cookie;
 }
 
-static inline lnet_eq_t *
-lnet_handle2eq(lnet_handle_eq_t *handle)
+static inline struct lnet_eq *
+lnet_handle2eq(struct lnet_handle_eq *handle)
 {
-	lnet_libhandle_t *lh;
+	struct lnet_libhandle *lh;
 
 	lh = lnet_res_lh_lookup(&the_lnet.ln_eq_container, handle->cookie);
 	if (!lh)
 		return NULL;
 
-	return lh_entry(lh, lnet_eq_t, eq_lh);
+	return lh_entry(lh, struct lnet_eq, eq_lh);
 }
 
 static inline void
-lnet_md2handle(lnet_handle_md_t *handle, lnet_libmd_t *md)
+lnet_md2handle(struct lnet_handle_md *handle, struct lnet_libmd *md)
 {
 	handle->cookie = md->md_lh.lh_cookie;
 }
 
-static inline lnet_libmd_t *
-lnet_handle2md(lnet_handle_md_t *handle)
+static inline struct lnet_libmd *
+lnet_handle2md(struct lnet_handle_md *handle)
 {
 	/* ALWAYS called with resource lock held */
-	lnet_libhandle_t *lh;
+	struct lnet_libhandle *lh;
 	int cpt;
 
 	cpt = lnet_cpt_of_cookie(handle->cookie);
@@ -319,14 +319,14 @@ lnet_handle2md(lnet_handle_md_t *handle)
 	if (!lh)
 		return NULL;
 
-	return lh_entry(lh, lnet_libmd_t, md_lh);
+	return lh_entry(lh, struct lnet_libmd, md_lh);
 }
 
-static inline lnet_libmd_t *
+static inline struct lnet_libmd *
 lnet_wire_handle2md(struct lnet_handle_wire *wh)
 {
 	/* ALWAYS called with resource lock held */
-	lnet_libhandle_t *lh;
+	struct lnet_libhandle *lh;
 	int cpt;
 
 	if (wh->wh_interface_cookie != the_lnet.ln_interface_cookie)
@@ -338,20 +338,20 @@ lnet_wire_handle2md(struct lnet_handle_wire *wh)
 	if (!lh)
 		return NULL;
 
-	return lh_entry(lh, lnet_libmd_t, md_lh);
+	return lh_entry(lh, struct lnet_libmd, md_lh);
 }
 
 static inline void
-lnet_me2handle(lnet_handle_me_t *handle, lnet_me_t *me)
+lnet_me2handle(struct lnet_handle_me *handle, struct lnet_me *me)
 {
 	handle->cookie = me->me_lh.lh_cookie;
 }
 
-static inline lnet_me_t *
-lnet_handle2me(lnet_handle_me_t *handle)
+static inline struct lnet_me *
+lnet_handle2me(struct lnet_handle_me *handle)
 {
 	/* ALWAYS called with resource lock held */
-	lnet_libhandle_t *lh;
+	struct lnet_libhandle *lh;
 	int cpt;
 
 	cpt = lnet_cpt_of_cookie(handle->cookie);
@@ -360,20 +360,20 @@ lnet_handle2me(lnet_handle_me_t *handle)
 	if (!lh)
 		return NULL;
 
-	return lh_entry(lh, lnet_me_t, me_lh);
+	return lh_entry(lh, struct lnet_me, me_lh);
 }
 
 static inline void
-lnet_peer_addref_locked(lnet_peer_t *lp)
+lnet_peer_addref_locked(struct lnet_peer *lp)
 {
 	LASSERT(lp->lp_refcount > 0);
 	lp->lp_refcount++;
 }
 
-void lnet_destroy_peer_locked(lnet_peer_t *lp);
+void lnet_destroy_peer_locked(struct lnet_peer *lp);
 
 static inline void
-lnet_peer_decref_locked(lnet_peer_t *lp)
+lnet_peer_decref_locked(struct lnet_peer *lp)
 {
 	LASSERT(lp->lp_refcount > 0);
 	lp->lp_refcount--;
@@ -382,13 +382,13 @@ lnet_peer_decref_locked(lnet_peer_t *lp)
 }
 
 static inline int
-lnet_isrouter(lnet_peer_t *lp)
+lnet_isrouter(struct lnet_peer *lp)
 {
 	return lp->lp_rtr_refcount ? 1 : 0;
 }
 
 static inline void
-lnet_ni_addref_locked(lnet_ni_t *ni, int cpt)
+lnet_ni_addref_locked(struct lnet_ni *ni, int cpt)
 {
 	LASSERT(cpt >= 0 && cpt < LNET_CPT_NUMBER);
 	LASSERT(*ni->ni_refs[cpt] >= 0);
@@ -397,7 +397,7 @@ lnet_ni_addref_locked(lnet_ni_t *ni, int cpt)
 }
 
 static inline void
-lnet_ni_addref(lnet_ni_t *ni)
+lnet_ni_addref(struct lnet_ni *ni)
 {
 	lnet_net_lock(0);
 	lnet_ni_addref_locked(ni, 0);
@@ -405,7 +405,7 @@ lnet_ni_addref(lnet_ni_t *ni)
 }
 
 static inline void
-lnet_ni_decref_locked(lnet_ni_t *ni, int cpt)
+lnet_ni_decref_locked(struct lnet_ni *ni, int cpt)
 {
 	LASSERT(cpt >= 0 && cpt < LNET_CPT_NUMBER);
 	LASSERT(*ni->ni_refs[cpt] > 0);
@@ -414,15 +414,15 @@ lnet_ni_decref_locked(lnet_ni_t *ni, int cpt)
 }
 
 static inline void
-lnet_ni_decref(lnet_ni_t *ni)
+lnet_ni_decref(struct lnet_ni *ni)
 {
 	lnet_net_lock(0);
 	lnet_ni_decref_locked(ni, 0);
 	lnet_net_unlock(0);
 }
 
-void lnet_ni_free(lnet_ni_t *ni);
-lnet_ni_t *
+void lnet_ni_free(struct lnet_ni *ni);
+struct lnet_ni *
 lnet_ni_alloc(__u32 net, struct cfs_expr_list *el, struct list_head *nilist);
 
 static inline int
@@ -439,22 +439,22 @@ lnet_net2rnethash(__u32 net)
 		((1U << the_lnet.ln_remote_nets_hbits) - 1)];
 }
 
-extern lnd_t the_lolnd;
+extern struct lnet_lnd the_lolnd;
 extern int avoid_asym_router_failure;
 
 int lnet_cpt_of_nid_locked(lnet_nid_t nid);
 int lnet_cpt_of_nid(lnet_nid_t nid);
-lnet_ni_t *lnet_nid2ni_locked(lnet_nid_t nid, int cpt);
-lnet_ni_t *lnet_net2ni_locked(__u32 net, int cpt);
-lnet_ni_t *lnet_net2ni(__u32 net);
+struct lnet_ni *lnet_nid2ni_locked(lnet_nid_t nid, int cpt);
+struct lnet_ni *lnet_net2ni_locked(__u32 net, int cpt);
+struct lnet_ni *lnet_net2ni(__u32 net);
 
 extern int portal_rotor;
 
 int lnet_lib_init(void);
 void lnet_lib_exit(void);
 
-int lnet_notify(lnet_ni_t *ni, lnet_nid_t peer, int alive, unsigned long when);
-void lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
+int lnet_notify(struct lnet_ni *ni, lnet_nid_t peer, int alive, unsigned long when);
+void lnet_notify_locked(struct lnet_peer *lp, int notifylnd, int alive,
 			unsigned long when);
 int lnet_add_route(__u32 net, __u32 hops, lnet_nid_t gateway_nid,
 		   unsigned int priority);
@@ -468,12 +468,12 @@ int lnet_get_rtr_pool_cfg(int idx, struct lnet_ioctl_pool_cfg *pool_cfg);
 void lnet_router_debugfs_init(void);
 void lnet_router_debugfs_fini(void);
 int  lnet_rtrpools_alloc(int im_a_router);
-void lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages);
+void lnet_destroy_rtrbuf(struct lnet_rtrbuf *rb, int npages);
 int lnet_rtrpools_adjust(int tiny, int small, int large);
 int lnet_rtrpools_enable(void);
 void lnet_rtrpools_disable(void);
 void lnet_rtrpools_free(int keep_pools);
-lnet_remotenet_t *lnet_find_net_locked(__u32 net);
+struct lnet_remotenet *lnet_find_net_locked(__u32 net);
 int lnet_dyn_add_ni(lnet_pid_t requested_pid,
 		    struct lnet_ioctl_config_data *conf);
 int lnet_dyn_del_ni(__u32 net);
@@ -482,69 +482,70 @@ int lnet_clear_lazy_portal(struct lnet_ni *ni, int portal, char *reason);
 int lnet_islocalnid(lnet_nid_t nid);
 int lnet_islocalnet(__u32 net);
 
-void lnet_msg_attach_md(lnet_msg_t *msg, lnet_libmd_t *md,
+void lnet_msg_attach_md(struct lnet_msg *msg, struct lnet_libmd *md,
 			unsigned int offset, unsigned int mlen);
-void lnet_msg_detach_md(lnet_msg_t *msg, int status);
-void lnet_build_unlink_event(lnet_libmd_t *md, lnet_event_t *ev);
-void lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type);
-void lnet_msg_commit(lnet_msg_t *msg, int cpt);
-void lnet_msg_decommit(lnet_msg_t *msg, int cpt, int status);
-
-void lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev);
-void lnet_prep_send(lnet_msg_t *msg, int type, lnet_process_id_t target,
-		    unsigned int offset, unsigned int len);
-int lnet_send(lnet_nid_t nid, lnet_msg_t *msg, lnet_nid_t rtr_nid);
-void lnet_return_tx_credits_locked(lnet_msg_t *msg);
-void lnet_return_rx_credits_locked(lnet_msg_t *msg);
-void lnet_schedule_blocked_locked(lnet_rtrbufpool_t *rbp);
+void lnet_msg_detach_md(struct lnet_msg *msg, int status);
+void lnet_build_unlink_event(struct lnet_libmd *md, struct lnet_event *ev);
+void lnet_build_msg_event(struct lnet_msg *msg, enum lnet_event_kind ev_type);
+void lnet_msg_commit(struct lnet_msg *msg, int cpt);
+void lnet_msg_decommit(struct lnet_msg *msg, int cpt, int status);
+
+void lnet_eq_enqueue_event(struct lnet_eq *eq, struct lnet_event *ev);
+void lnet_prep_send(struct lnet_msg *msg, int type,
+		    struct lnet_process_id target, unsigned int offset,
+		    unsigned int len);
+int lnet_send(lnet_nid_t nid, struct lnet_msg *msg, lnet_nid_t rtr_nid);
+void lnet_return_tx_credits_locked(struct lnet_msg *msg);
+void lnet_return_rx_credits_locked(struct lnet_msg *msg);
+void lnet_schedule_blocked_locked(struct lnet_rtrbufpool *rbp);
 void lnet_drop_routed_msgs_locked(struct list_head *list, int cpt);
 
 /* portals functions */
 /* portals attributes */
 static inline int
-lnet_ptl_is_lazy(lnet_portal_t *ptl)
+lnet_ptl_is_lazy(struct lnet_portal *ptl)
 {
 	return !!(ptl->ptl_options & LNET_PTL_LAZY);
 }
 
 static inline int
-lnet_ptl_is_unique(lnet_portal_t *ptl)
+lnet_ptl_is_unique(struct lnet_portal *ptl)
 {
 	return !!(ptl->ptl_options & LNET_PTL_MATCH_UNIQUE);
 }
 
 static inline int
-lnet_ptl_is_wildcard(lnet_portal_t *ptl)
+lnet_ptl_is_wildcard(struct lnet_portal *ptl)
 {
 	return !!(ptl->ptl_options & LNET_PTL_MATCH_WILDCARD);
 }
 
 static inline void
-lnet_ptl_setopt(lnet_portal_t *ptl, int opt)
+lnet_ptl_setopt(struct lnet_portal *ptl, int opt)
 {
 	ptl->ptl_options |= opt;
 }
 
 static inline void
-lnet_ptl_unsetopt(lnet_portal_t *ptl, int opt)
+lnet_ptl_unsetopt(struct lnet_portal *ptl, int opt)
 {
 	ptl->ptl_options &= ~opt;
 }
 
 /* match-table functions */
 struct list_head *lnet_mt_match_head(struct lnet_match_table *mtable,
-				     lnet_process_id_t id, __u64 mbits);
+				     struct lnet_process_id id, __u64 mbits);
 struct lnet_match_table *lnet_mt_of_attach(unsigned int index,
-					   lnet_process_id_t id, __u64 mbits,
-					   __u64 ignore_bits,
-					   lnet_ins_pos_t pos);
+					   struct lnet_process_id id,
+					   __u64 mbits, __u64 ignore_bits,
+					   enum lnet_ins_pos pos);
 int lnet_mt_match_md(struct lnet_match_table *mtable,
 		     struct lnet_match_info *info, struct lnet_msg *msg);
 
 /* portals match/attach functions */
-void lnet_ptl_attach_md(lnet_me_t *me, lnet_libmd_t *md,
+void lnet_ptl_attach_md(struct lnet_me *me, struct lnet_libmd *md,
 			struct list_head *matches, struct list_head *drops);
-void lnet_ptl_detach_md(lnet_me_t *me, lnet_libmd_t *md);
+void lnet_ptl_detach_md(struct lnet_me *me, struct lnet_libmd *md);
 int lnet_ptl_match_md(struct lnet_match_info *info, struct lnet_msg *msg);
 
 /* initialized and finalize portals */
@@ -552,23 +553,26 @@ int lnet_portals_create(void);
 void lnet_portals_destroy(void);
 
 /* message functions */
-int lnet_parse(lnet_ni_t *ni, struct lnet_hdr *hdr,
+int lnet_parse(struct lnet_ni *ni, struct lnet_hdr *hdr,
 	       lnet_nid_t fromnid, void *private, int rdma_req);
-int lnet_parse_local(lnet_ni_t *ni, lnet_msg_t *msg);
-int lnet_parse_forward_locked(lnet_ni_t *ni, lnet_msg_t *msg);
+int lnet_parse_local(struct lnet_ni *ni, struct lnet_msg *msg);
+int lnet_parse_forward_locked(struct lnet_ni *ni, struct lnet_msg *msg);
 
-void lnet_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
-	       unsigned int offset, unsigned int mlen, unsigned int rlen);
-void lnet_ni_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg,
+void lnet_recv(struct lnet_ni *ni, void *private, struct lnet_msg *msg,
+	       int delayed, unsigned int offset, unsigned int mlen,
+	       unsigned int rlen);
+void lnet_ni_recv(struct lnet_ni *ni, void *private, struct lnet_msg *msg,
 		  int delayed, unsigned int offset,
 		  unsigned int mlen, unsigned int rlen);
 
-lnet_msg_t *lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *get_msg);
-void lnet_set_reply_msg_len(lnet_ni_t *ni, lnet_msg_t *msg, unsigned int len);
+struct lnet_msg *lnet_create_reply_msg(struct lnet_ni *ni,
+				       struct lnet_msg *get_msg);
+void lnet_set_reply_msg_len(struct lnet_ni *ni, struct lnet_msg *msg,
+			    unsigned int len);
 
-void lnet_finalize(lnet_ni_t *ni, lnet_msg_t *msg, int rc);
+void lnet_finalize(struct lnet_ni *ni, struct lnet_msg *msg, int rc);
 
-void lnet_drop_message(lnet_ni_t *ni, int cpt, void *private,
+void lnet_drop_message(struct lnet_ni *ni, int cpt, void *private,
 		       unsigned int nob);
 void lnet_drop_delayed_msg_list(struct list_head *head, char *reason);
 void lnet_recv_delayed_msg_list(struct list_head *head);
@@ -600,7 +604,7 @@ bool lnet_delay_rule_match_locked(struct lnet_hdr *hdr, struct lnet_msg *msg);
 
 /** @} lnet_fault_simulation */
 
-void lnet_counters_get(lnet_counters_t *counters);
+void lnet_counters_get(struct lnet_counters *counters);
 void lnet_counters_reset(void);
 
 unsigned int lnet_iov_nob(unsigned int niov, struct kvec *iov);
@@ -608,25 +612,25 @@ int lnet_extract_iov(int dst_niov, struct kvec *dst,
 		     int src_niov, const struct kvec *src,
 		      unsigned int offset, unsigned int len);
 
-unsigned int lnet_kiov_nob(unsigned int niov, lnet_kiov_t *iov);
-int lnet_extract_kiov(int dst_niov, lnet_kiov_t *dst,
-		      int src_niov, const lnet_kiov_t *src,
+unsigned int lnet_kiov_nob(unsigned int niov, struct bio_vec *iov);
+int lnet_extract_kiov(int dst_niov, struct bio_vec *dst,
+		      int src_niov, const struct bio_vec *src,
 		      unsigned int offset, unsigned int len);
 
 void lnet_copy_iov2iter(struct iov_iter *to,
 			unsigned int nsiov, const struct kvec *siov,
 			unsigned int soffset, unsigned int nob);
 void lnet_copy_kiov2iter(struct iov_iter *to,
-			 unsigned int nkiov, const lnet_kiov_t *kiov,
+			 unsigned int nkiov, const struct bio_vec *kiov,
 			 unsigned int kiovoffset, unsigned int nob);
 
-void lnet_me_unlink(lnet_me_t *me);
+void lnet_me_unlink(struct lnet_me *me);
 
-void lnet_md_unlink(lnet_libmd_t *md);
-void lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd);
+void lnet_md_unlink(struct lnet_libmd *md);
+void lnet_md_deconstruct(struct lnet_libmd *lmd, struct lnet_md *umd);
 
-void lnet_register_lnd(lnd_t *lnd);
-void lnet_unregister_lnd(lnd_t *lnd);
+void lnet_register_lnd(struct lnet_lnd *lnd);
+void lnet_unregister_lnd(struct lnet_lnd *lnd);
 
 int lnet_connect(struct socket **sockp, lnet_nid_t peer_nid,
 		 __u32 local_ip, __u32 peer_ip, int peer_port);
@@ -659,11 +663,11 @@ int lnet_sock_connect(struct socket **sockp, int *fatal,
 void libcfs_sock_release(struct socket *sock);
 
 int lnet_peers_start_down(void);
-int lnet_peer_buffer_credits(lnet_ni_t *ni);
+int lnet_peer_buffer_credits(struct lnet_ni *ni);
 
 int lnet_router_checker_start(void);
 void lnet_router_checker_stop(void);
-void lnet_router_ni_update_locked(lnet_peer_t *gw, __u32 net);
+void lnet_router_ni_update_locked(struct lnet_peer *gw, __u32 net);
 void lnet_swap_pinginfo(struct lnet_ping_info *info);
 
 int lnet_parse_ip2nets(char **networksp, char *ip2nets);
@@ -671,10 +675,10 @@ int lnet_parse_routes(char *route_str, int *im_a_router);
 int lnet_parse_networks(struct list_head *nilist, char *networks);
 int lnet_net_unique(__u32 net, struct list_head *nilist);
 
-int lnet_nid2peer_locked(lnet_peer_t **lpp, lnet_nid_t nid, int cpt);
-lnet_peer_t *lnet_find_peer_locked(struct lnet_peer_table *ptable,
-				   lnet_nid_t nid);
-void lnet_peer_tables_cleanup(lnet_ni_t *ni);
+int lnet_nid2peer_locked(struct lnet_peer **lpp, lnet_nid_t nid, int cpt);
+struct lnet_peer *lnet_find_peer_locked(struct lnet_peer_table *ptable,
+					lnet_nid_t nid);
+void lnet_peer_tables_cleanup(struct lnet_ni *ni);
 void lnet_peer_tables_destroy(void);
 int lnet_peer_tables_create(void);
 void lnet_debug_peer(lnet_nid_t nid);
@@ -686,7 +690,7 @@ int lnet_get_peer_info(__u32 peer_index, __u64 *nid,
 		       __u32 *peer_tx_qnob);
 
 static inline void
-lnet_peer_set_alive(lnet_peer_t *lp)
+lnet_peer_set_alive(struct lnet_peer *lp)
 {
 	lp->lp_last_query = jiffies;
 	lp->lp_last_alive = jiffies;
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-types.h b/drivers/staging/lustre/include/linux/lnet/lib-types.h
index 9850398bf29a..321752dfe58b 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-types.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-types.h
@@ -54,11 +54,11 @@
 /* forward refs */
 struct lnet_libmd;
 
-typedef struct lnet_msg {
+struct lnet_msg {
 	struct list_head	msg_activelist;
 	struct list_head	msg_list;	   /* Q for credits/MD */
 
-	lnet_process_id_t	msg_target;
+	struct lnet_process_id	msg_target;
 	/* where is it from, it's only for building event */
 	lnet_nid_t		msg_from;
 	__u32			msg_type;
@@ -102,47 +102,47 @@ typedef struct lnet_msg {
 	unsigned int		 msg_offset;
 	unsigned int		 msg_niov;
 	struct kvec		*msg_iov;
-	lnet_kiov_t		*msg_kiov;
+	struct bio_vec		*msg_kiov;
 
-	lnet_event_t		 msg_ev;
+	struct lnet_event	 msg_ev;
 	struct lnet_hdr		 msg_hdr;
-} lnet_msg_t;
+};
 
-typedef struct lnet_libhandle {
+struct lnet_libhandle {
 	struct list_head	lh_hash_chain;
 	__u64			lh_cookie;
-} lnet_libhandle_t;
+};
 
 #define lh_entry(ptr, type, member) \
 	((type *)((char *)(ptr) - (char *)(&((type *)0)->member)))
 
-typedef struct lnet_eq {
+struct lnet_eq {
 	struct list_head	  eq_list;
-	lnet_libhandle_t	  eq_lh;
-	lnet_seq_t		  eq_enq_seq;
-	lnet_seq_t		  eq_deq_seq;
+	struct lnet_libhandle	  eq_lh;
+	unsigned long		  eq_enq_seq;
+	unsigned long		  eq_deq_seq;
 	unsigned int		  eq_size;
 	lnet_eq_handler_t	  eq_callback;
-	lnet_event_t		 *eq_events;
+	struct lnet_event	 *eq_events;
 	int			**eq_refs;	/* percpt refcount for EQ */
-} lnet_eq_t;
+};
 
-typedef struct lnet_me {
+struct lnet_me {
 	struct list_head	 me_list;
-	lnet_libhandle_t	 me_lh;
-	lnet_process_id_t	 me_match_id;
+	struct lnet_libhandle	 me_lh;
+	struct lnet_process_id	 me_match_id;
 	unsigned int		 me_portal;
 	unsigned int		 me_pos;	/* hash offset in mt_hash */
 	__u64			 me_match_bits;
 	__u64			 me_ignore_bits;
-	lnet_unlink_t		 me_unlink;
+	enum lnet_unlink	 me_unlink;
 	struct lnet_libmd	*me_md;
-} lnet_me_t;
+};
 
-typedef struct lnet_libmd {
+struct lnet_libmd {
 	struct list_head	 md_list;
-	lnet_libhandle_t	 md_lh;
-	lnet_me_t		*md_me;
+	struct lnet_libhandle	 md_lh;
+	struct lnet_me		*md_me;
 	char			*md_start;
 	unsigned int		 md_offset;
 	unsigned int		 md_length;
@@ -152,24 +152,24 @@ typedef struct lnet_libmd {
 	unsigned int		 md_options;
 	unsigned int		 md_flags;
 	void			*md_user_ptr;
-	lnet_eq_t		*md_eq;
+	struct lnet_eq		*md_eq;
 	unsigned int		 md_niov;	/* # frags */
 	union {
 		struct kvec	iov[LNET_MAX_IOV];
-		lnet_kiov_t	kiov[LNET_MAX_IOV];
+		struct bio_vec	kiov[LNET_MAX_IOV];
 	} md_iov;
-} lnet_libmd_t;
+};
 
 #define LNET_MD_FLAG_ZOMBIE		(1 << 0)
 #define LNET_MD_FLAG_AUTO_UNLINK	(1 << 1)
 #define LNET_MD_FLAG_ABORTED		(1 << 2)
 
-typedef struct {
+struct lnet_test_peer {
 	/* info about peers we are trying to fail */
 	struct list_head	tp_list;	/* ln_test_peers */
 	lnet_nid_t		tp_nid;		/* matching nid */
 	unsigned int		tp_threshold;	/* # failures to simulate */
-} lnet_test_peer_t;
+};
 
 #define LNET_COOKIE_TYPE_MD	1
 #define LNET_COOKIE_TYPE_ME	2
@@ -179,7 +179,7 @@ typedef struct {
 
 struct lnet_ni;			/* forward ref */
 
-typedef struct lnet_lnd {
+struct lnet_lnd {
 	/* fields managed by portals */
 	struct list_head	lnd_list;	/* stash in the LND table */
 	int			lnd_refcount;	/* # active instances */
@@ -210,7 +210,8 @@ typedef struct lnet_lnd {
 	 * non-zero for immediate failure, otherwise complete later with
 	 * lnet_finalize()
 	 */
-	int (*lnd_send)(struct lnet_ni *ni, void *private, lnet_msg_t *msg);
+	int (*lnd_send)(struct lnet_ni *ni, void *private,
+			struct lnet_msg *msg);
 
 	/*
 	 * Start receiving 'mlen' bytes of payload data, skipping the following
@@ -219,7 +220,7 @@ typedef struct lnet_lnd {
 	 * complete later with lnet_finalize().  This also gives back a receive
 	 * credit if the LND does flow control.
 	 */
-	int (*lnd_recv)(struct lnet_ni *ni, void *private, lnet_msg_t *msg,
+	int (*lnd_recv)(struct lnet_ni *ni, void *private, struct lnet_msg *msg,
 			int delayed, struct iov_iter *to, unsigned int rlen);
 
 	/*
@@ -231,7 +232,7 @@ typedef struct lnet_lnd {
 	 * release resources; lnd_recv() will not be called.
 	 */
 	int (*lnd_eager_recv)(struct lnet_ni *ni, void *private,
-			      lnet_msg_t *msg, void **new_privatep);
+			      struct lnet_msg *msg, void **new_privatep);
 
 	/* notification of peer health */
 	void (*lnd_notify)(struct lnet_ni *ni, lnet_nid_t peer, int alive);
@@ -242,7 +243,7 @@ typedef struct lnet_lnd {
 
 	/* accept a new connection */
 	int (*lnd_accept)(struct lnet_ni *ni, struct socket *sock);
-} lnd_t;
+};
 
 struct lnet_tx_queue {
 	int			tq_credits;	/* # tx credits free */
@@ -251,7 +252,7 @@ struct lnet_tx_queue {
 	struct list_head	tq_delayed;	/* delayed TXs */
 };
 
-typedef struct lnet_ni {
+struct lnet_ni {
 	spinlock_t		  ni_lock;
 	struct list_head	  ni_list;	/* chain on ln_nis */
 	struct list_head	  ni_cptlist;	/* chain on ln_nis_cpt */
@@ -266,7 +267,7 @@ typedef struct lnet_ni {
 	__u32			 *ni_cpts;	/* bond NI on some CPTs */
 	lnet_nid_t		  ni_nid;	/* interface's NID */
 	void			 *ni_data;	/* instance-specific data */
-	lnd_t			 *ni_lnd;	/* procedural interface */
+	struct lnet_lnd		 *ni_lnd;	/* procedural interface */
 	struct lnet_tx_queue	**ni_tx_queues;	/* percpt TX queues */
 	int			**ni_refs;	/* percpt reference count */
 	time64_t		  ni_last_alive;/* when I was last alive */
@@ -277,7 +278,7 @@ typedef struct lnet_ni {
 	char			 *ni_interfaces[LNET_MAX_INTERFACES];
 	/* original net namespace */
 	struct net		 *ni_net_ns;
-} lnet_ni_t;
+};
 
 #define LNET_PROTO_PING_MATCHBITS	0x8000000000000000LL
 
@@ -296,15 +297,15 @@ typedef struct lnet_ni {
 /* router checker data, per router */
 #define LNET_MAX_RTR_NIS   16
 #define LNET_PINGINFO_SIZE offsetof(struct lnet_ping_info, pi_ni[LNET_MAX_RTR_NIS])
-typedef struct {
+struct lnet_rc_data {
 	/* chain on the_lnet.ln_zombie_rcd or ln_deathrow_rcd */
 	struct list_head	 rcd_list;
-	lnet_handle_md_t	 rcd_mdh;	/* ping buffer MD */
+	struct lnet_handle_md	 rcd_mdh;	/* ping buffer MD */
 	struct lnet_peer	*rcd_gateway;	/* reference to gateway */
 	struct lnet_ping_info	*rcd_pinginfo;	/* ping buffer */
-} lnet_rc_data_t;
+};
 
-typedef struct lnet_peer {
+struct lnet_peer {
 	struct list_head	 lp_hashlist;	/* chain on peer hash */
 	struct list_head	 lp_txq;	/* messages blocking for
 						   tx credits */
@@ -335,17 +336,17 @@ typedef struct lnet_peer {
 	unsigned long		 lp_last_alive;	/* when I was last alive */
 	unsigned long		 lp_last_query;	/* when lp_ni was queried
 						   last time */
-	lnet_ni_t		*lp_ni;		/* interface peer is on */
+	struct lnet_ni		*lp_ni;		/* interface peer is on */
 	lnet_nid_t		 lp_nid;	/* peer's NID */
 	int			 lp_refcount;	/* # refs */
 	int			 lp_cpt;	/* CPT this peer attached on */
-	/* # refs from lnet_route_t::lr_gateway */
+	/* # refs from lnet_route::lr_gateway */
 	int			 lp_rtr_refcount;
 	/* returned RC ping features */
 	unsigned int		 lp_ping_feats;
 	struct list_head	 lp_routes;	/* routers on this peer */
-	lnet_rc_data_t		*lp_rcd;	/* router checker state */
-} lnet_peer_t;
+	struct lnet_rc_data	*lp_rcd;	/* router checker state */
+};
 
 /* peer hash size */
 #define LNET_PEER_HASH_BITS	9
@@ -363,39 +364,39 @@ struct lnet_peer_table {
 
 /*
  * peer aliveness is enabled only on routers for peers in a network where the
- * lnet_ni_t::ni_peertimeout has been set to a positive value
+ * lnet_ni::ni_peertimeout has been set to a positive value
  */
 #define lnet_peer_aliveness_enabled(lp) (the_lnet.ln_routing && \
 					 (lp)->lp_ni->ni_peertimeout > 0)
 
-typedef struct {
+struct lnet_route {
 	struct list_head	 lr_list;	/* chain on net */
 	struct list_head	 lr_gwlist;	/* chain on gateway */
-	lnet_peer_t		*lr_gateway;	/* router node */
+	struct lnet_peer	*lr_gateway;	/* router node */
 	__u32			 lr_net;	/* remote network number */
 	int			 lr_seq;	/* sequence for round-robin */
 	unsigned int		 lr_downis;	/* number of down NIs */
 	__u32			 lr_hops;	/* how far I am */
 	unsigned int             lr_priority;	/* route priority */
-} lnet_route_t;
+};
 
 #define LNET_REMOTE_NETS_HASH_DEFAULT	(1U << 7)
 #define LNET_REMOTE_NETS_HASH_MAX	(1U << 16)
 #define LNET_REMOTE_NETS_HASH_SIZE	(1 << the_lnet.ln_remote_nets_hbits)
 
-typedef struct {
+struct lnet_remotenet {
 	struct list_head	lrn_list;	/* chain on
 						   ln_remote_nets_hash */
 	struct list_head	lrn_routes;	/* routes to me */
 	__u32			lrn_net;	/* my net number */
-} lnet_remotenet_t;
+};
 
 /** lnet message has credit and can be submitted to lnd for send/receive */
 #define LNET_CREDIT_OK		0
 /** lnet message is waiting for credit */
 #define LNET_CREDIT_WAIT	1
 
-typedef struct {
+struct lnet_rtrbufpool {
 	struct list_head	rbp_bufs;	/* my free buffer pool */
 	struct list_head	rbp_msgs;	/* messages blocking
 						   for a buffer */
@@ -407,13 +408,13 @@ typedef struct {
 	int			rbp_credits;	/* # free buffers /
 						     blocked messages */
 	int			rbp_mincredits;	/* low water mark */
-} lnet_rtrbufpool_t;
+};
 
-typedef struct {
+struct lnet_rtrbuf {
 	struct list_head	 rb_list;	/* chain on rbp_bufs */
-	lnet_rtrbufpool_t	*rb_pool;	/* owning pool */
-	lnet_kiov_t		 rb_kiov[0];	/* the buffer space */
-} lnet_rtrbuf_t;
+	struct lnet_rtrbufpool	*rb_pool;	/* owning pool */
+	struct bio_vec		 rb_kiov[0];	/* the buffer space */
+};
 
 #define LNET_PEER_HASHSIZE	503	/* prime! */
 
@@ -424,7 +425,7 @@ typedef struct {
 /* # different router buffer pools */
 #define LNET_NRBPOOLS		(LNET_LARGE_BUF_IDX + 1)
 
-enum {
+enum lnet_match_flags {
 	/* Didn't match anything */
 	LNET_MATCHMD_NONE	= (1 << 0),
 	/* Matched OK */
@@ -437,7 +438,7 @@ enum {
 	LNET_MATCHMD_FINISH	= (LNET_MATCHMD_OK | LNET_MATCHMD_DROP),
 };
 
-/* Options for lnet_portal_t::ptl_options */
+/* Options for lnet_portal::ptl_options */
 #define LNET_PTL_LAZY		(1 << 0)
 #define LNET_PTL_MATCH_UNIQUE	(1 << 1)	/* unique match, for RDMA */
 #define LNET_PTL_MATCH_WILDCARD	(1 << 2)	/* wildcard match,
@@ -446,7 +447,7 @@ enum {
 /* parameter for matching operations (GET, PUT) */
 struct lnet_match_info {
 	__u64			mi_mbits;
-	lnet_process_id_t	mi_id;
+	struct lnet_process_id	mi_id;
 	unsigned int		mi_opc;
 	unsigned int		mi_portal;
 	unsigned int		mi_rlength;
@@ -496,7 +497,7 @@ struct lnet_match_table {
 /* dispatch routed PUT message by hashing source NID for wildcard portals */
 #define	LNET_PTL_ROTOR_HASH_RT	3
 
-typedef struct lnet_portal {
+struct lnet_portal {
 	spinlock_t		  ptl_lock;
 	unsigned int		  ptl_index;	/* portal ID, reserved */
 	/* flags on this portal: lazy, unique... */
@@ -513,7 +514,7 @@ typedef struct lnet_portal {
 	int			  ptl_mt_nmaps;
 	/* array of active entries' cpu-partition-id */
 	int			  ptl_mt_maps[0];
-} lnet_portal_t;
+};
 
 #define LNET_LH_HASH_BITS	12
 #define LNET_LH_HASH_SIZE	(1ULL << LNET_LH_HASH_BITS)
@@ -544,7 +545,7 @@ struct lnet_msg_container {
 #define LNET_RC_STATE_RUNNING		1	/* started up OK */
 #define LNET_RC_STATE_STOPPING		2	/* telling thread to stop */
 
-typedef struct {
+struct lnet {
 	/* CPU partition table of LNet */
 	struct cfs_cpt_table		 *ln_cpt_table;
 	/* number of CPTs in ln_cpt_table */
@@ -556,7 +557,7 @@ typedef struct {
 	/* # portals */
 	int				  ln_nportals;
 	/* the vector of portals */
-	lnet_portal_t			**ln_portals;
+	struct lnet_portal		**ln_portals;
 	/* percpt ME containers */
 	struct lnet_res_container	**ln_me_containers;
 	/* percpt MD container */
@@ -572,7 +573,7 @@ typedef struct {
 	struct cfs_percpt_lock		 *ln_net_lock;
 	/* percpt message containers for active/finalizing/freed message */
 	struct lnet_msg_container	**ln_msg_containers;
-	lnet_counters_t			**ln_counters;
+	struct lnet_counters		**ln_counters;
 	struct lnet_peer_table		**ln_peer_tables;
 	/* failure simulation */
 	struct list_head		  ln_test_peers;
@@ -584,7 +585,7 @@ typedef struct {
 	struct list_head		  ln_nis_cpt;
 	/* dying LND instances */
 	struct list_head		  ln_nis_zombie;
-	lnet_ni_t			 *ln_loni;	/* the loopback NI */
+	struct lnet_ni			 *ln_loni;	/* the loopback NI */
 
 	/* remote networks with routes to them */
 	struct list_head		 *ln_remote_nets_hash;
@@ -595,16 +596,16 @@ typedef struct {
 	/* validity stamp */
 	__u64				  ln_routers_version;
 	/* percpt router buffer pools */
-	lnet_rtrbufpool_t		**ln_rtrpools;
+	struct lnet_rtrbufpool		**ln_rtrpools;
 
-	lnet_handle_md_t		  ln_ping_target_md;
-	lnet_handle_eq_t		  ln_ping_target_eq;
+	struct lnet_handle_md		  ln_ping_target_md;
+	struct lnet_handle_eq		  ln_ping_target_eq;
 	struct lnet_ping_info		 *ln_ping_info;
 
 	/* router checker startup/shutdown state */
 	int				  ln_rc_state;
 	/* router checker's event queue */
-	lnet_handle_eq_t		  ln_rc_eqh;
+	struct lnet_handle_eq		  ln_rc_eqh;
 	/* rcd still pending on net */
 	struct list_head		  ln_rcd_deathrow;
 	/* rcd ready for free */
@@ -647,6 +648,6 @@ typedef struct {
 	 */
 	wait_queue_head_t		  ln_rc_waitq;
 
-} lnet_t;
+};
 
 #endif
diff --git a/drivers/staging/lustre/include/linux/lnet/lnetst.h b/drivers/staging/lustre/include/linux/lnet/lnetst.h
index c81c246ef221..ea736f8d5231 100644
--- a/drivers/staging/lustre/include/linux/lnet/lnetst.h
+++ b/drivers/staging/lustre/include/linux/lnet/lnetst.h
@@ -86,7 +86,7 @@ struct lst_bid {
 #define LST_NODE_UNKNOWN	0x8	/* node not in session */
 
 struct lstcon_node_ent {
-	lnet_process_id_t       nde_id;		/* id of node */
+	struct lnet_process_id	nde_id;		/* id of node */
 	int			nde_state;	/* state of node */
 };				/*** node entry, for list_group command */
 
@@ -126,7 +126,7 @@ struct lstcon_test_batch_ent {
 
 struct lstcon_rpc_ent {
 	struct list_head	rpe_link;	/* link chain */
-	lnet_process_id_t	rpe_peer;	/* peer's id */
+	struct lnet_process_id	rpe_peer;	/* peer's id */
 	struct timeval		rpe_stamp;	/* time stamp of RPC */
 	int			rpe_state;	/* peer's state */
 	int			rpe_rpc_errno;	/* RPC errno */
@@ -287,7 +287,7 @@ struct lstio_debug_args {
 							       group|batch */
 	int			 lstio_dbg_count;	/* IN: # of test nodes
 							       to debug */
-	lnet_process_id_t __user *lstio_dbg_idsp;	/* IN: id of test
+	struct lnet_process_id __user *lstio_dbg_idsp;	/* IN: id of test
 							       nodes */
 	struct list_head __user	*lstio_dbg_resultp;	/* OUT: list head of
 								result buffer */
@@ -317,7 +317,7 @@ struct lstio_group_update_args {
 	int			 lstio_grp_nmlen;	/* IN: name length */
 	char __user		*lstio_grp_namep;	/* IN: group name */
 	int			 lstio_grp_count;	/* IN: # of nodes id */
-	lnet_process_id_t __user *lstio_grp_idsp;	/* IN: array of nodes */
+	struct lnet_process_id __user *lstio_grp_idsp;	/* IN: array of nodes */
 	struct list_head __user	*lstio_grp_resultp;	/* OUT: list head of
 								result buffer */
 };
@@ -329,7 +329,7 @@ struct lstio_group_nodes_args {
 	int			 lstio_grp_count;	/* IN: # of nodes */
 	/** OUT: session features */
 	unsigned int __user	*lstio_grp_featp;
-	lnet_process_id_t __user *lstio_grp_idsp;	/* IN: nodes */
+	struct lnet_process_id __user *lstio_grp_idsp;	/* IN: nodes */
 	struct list_head __user	*lstio_grp_resultp;	/* OUT: list head of
 								result buffer */
 };
@@ -429,7 +429,7 @@ struct lstio_stat_args {
 							       length */
 	char __user		*lstio_sta_namep;	/* IN: group name */
 	int			 lstio_sta_count;	/* IN: # of pid */
-	lnet_process_id_t __user *lstio_sta_idsp;	/* IN: pid */
+	struct lnet_process_id __user *lstio_sta_idsp;	/* IN: pid */
 	struct list_head __user	*lstio_sta_resultp;	/* OUT: list head of
 								result buffer */
 };
diff --git a/drivers/staging/lustre/include/linux/lnet/nidstr.h b/drivers/staging/lustre/include/linux/lnet/nidstr.h
index 937fcc9e4a30..ecdd0db04d0a 100644
--- a/drivers/staging/lustre/include/linux/lnet/nidstr.h
+++ b/drivers/staging/lustre/include/linux/lnet/nidstr.h
@@ -88,7 +88,7 @@ static inline char *libcfs_nid2str(lnet_nid_t nid)
 __u32 libcfs_str2net(const char *str);
 lnet_nid_t libcfs_str2nid(const char *str);
 int libcfs_str2anynid(lnet_nid_t *nid, const char *str);
-char *libcfs_id2str(lnet_process_id_t id);
+char *libcfs_id2str(struct lnet_process_id id);
 void cfs_free_nidlist(struct list_head *list);
 int cfs_parse_nidlist(char *str, int len, struct list_head *list);
 int cfs_print_nidlist(char *buffer, int count, struct list_head *list);
diff --git a/drivers/staging/lustre/include/linux/lnet/socklnd.h b/drivers/staging/lustre/include/linux/lnet/socklnd.h
index acf20ce6f403..dd5bc0e46560 100644
--- a/drivers/staging/lustre/include/linux/lnet/socklnd.h
+++ b/drivers/staging/lustre/include/linux/lnet/socklnd.h
@@ -45,7 +45,7 @@
 
 #define SOCKLND_CONN_ACK	SOCKLND_CONN_BULK_IN
 
-typedef struct {
+struct ksock_hello_msg {
 	__u32		kshm_magic;	/* magic number of socklnd message */
 	__u32		kshm_version;	/* version of socklnd message */
 	lnet_nid_t      kshm_src_nid;	/* sender's nid */
@@ -57,9 +57,9 @@ typedef struct {
 	__u32		kshm_ctype;	/* connection type */
 	__u32		kshm_nips;	/* # IP addrs */
 	__u32		kshm_ips[0];	/* IP addrs */
-} WIRE_ATTR ksock_hello_msg_t;
+} WIRE_ATTR;
 
-typedef struct {
+struct ksock_lnet_msg {
 	struct lnet_hdr	ksnm_hdr;	/* lnet hdr */
 
 	/*
@@ -68,17 +68,17 @@ typedef struct {
 	 * structure definitions. lnet payload will be stored just after
 	 * the body of structure ksock_lnet_msg_t
 	 */
-} WIRE_ATTR ksock_lnet_msg_t;
+} WIRE_ATTR;
 
-typedef struct {
+struct ksock_msg {
 	__u32	ksm_type;		/* type of socklnd message */
 	__u32	ksm_csum;		/* checksum if != 0 */
 	__u64	ksm_zc_cookies[2];	/* Zero-Copy request/ACK cookie */
 	union {
-		ksock_lnet_msg_t lnetmsg;/* lnet message, it's empty if
+		struct ksock_lnet_msg lnetmsg; /* lnet message, it's empty if
 					  * it's NOOP */
 	} WIRE_ATTR ksm_u;
-} WIRE_ATTR ksock_msg_t;
+} WIRE_ATTR;
 
 #define KSOCK_MSG_NOOP	0xC0	/* ksm_u empty */
 #define KSOCK_MSG_LNET	0xC1	/* lnet msg */
diff --git a/drivers/staging/lustre/include/linux/lnet/types.h b/drivers/staging/lustre/include/linux/lnet/types.h
index 1c8de72e6d6b..1be9b7aa7326 100644
--- a/drivers/staging/lustre/include/linux/lnet/types.h
+++ b/drivers/staging/lustre/include/linux/lnet/types.h
@@ -64,7 +64,7 @@
 typedef __u64 lnet_nid_t;
 /**
  * ID of a process in a node. Shortened as PID to distinguish from
- * lnet_process_id_t, the global process ID.
+ * lnet_process_id, the global process ID.
  */
 typedef __u32 lnet_pid_t;
 
@@ -114,7 +114,7 @@ static inline __u32 LNET_MKNET(__u32 type, __u32 num)
 
 #define WIRE_ATTR	__packed
 
-/* Packed version of lnet_process_id_t to transfer via network */
+/* Packed version of lnet_process_id to transfer via network */
 struct lnet_process_id_packed {
 	/* node id / process id */
 	lnet_nid_t	nid;
@@ -132,13 +132,13 @@ struct lnet_handle_wire {
 	__u64	wh_object_cookie;
 } WIRE_ATTR;
 
-typedef enum {
+enum lnet_msg_type {
 	LNET_MSG_ACK = 0,
 	LNET_MSG_PUT,
 	LNET_MSG_GET,
 	LNET_MSG_REPLY,
 	LNET_MSG_HELLO,
-} lnet_msg_type_t;
+};
 
 /*
  * The variant fields of the portals message header are aligned on an 8
@@ -182,7 +182,7 @@ struct lnet_hdr {
 	lnet_nid_t	src_nid;
 	lnet_pid_t	dest_pid;
 	lnet_pid_t	src_pid;
-	__u32		type;		/* lnet_msg_type_t */
+	__u32		type;		/* enum lnet_msg_type */
 	__u32		payload_length;	/* payload data to follow */
 	/*<------__u64 aligned------->*/
 	union {
@@ -250,7 +250,7 @@ struct lnet_ping_info {
 	struct lnet_ni_status	pi_ni[0];
 } WIRE_ATTR;
 
-typedef struct lnet_counters {
+struct lnet_counters {
 	__u32	msgs_alloc;
 	__u32	msgs_max;
 	__u32	errors;
@@ -262,7 +262,7 @@ typedef struct lnet_counters {
 	__u64	recv_length;
 	__u64	route_length;
 	__u64	drop_length;
-} WIRE_ATTR lnet_counters_t;
+} WIRE_ATTR;
 
 #define LNET_NI_STATUS_UP      0x15aac0de
 #define LNET_NI_STATUS_DOWN    0xdeadface
@@ -272,61 +272,70 @@ typedef struct lnet_counters {
 
 /**
  * Objects maintained by the LNet are accessed through handles. Handle types
- * have names of the form lnet_handle_xx_t, where xx is one of the two letter
+ * have names of the form lnet_handle_xx, where xx is one of the two letter
  * object type codes ('eq' for event queue, 'md' for memory descriptor, and
- * 'me' for match entry).
- * Each type of object is given a unique handle type to enhance type checking.
- * The type lnet_handle_any_t can be used when a generic handle is needed.
- * Every handle value can be converted into a value of type lnet_handle_any_t
- * without loss of information.
+ * 'me' for match entry). Each type of object is given a unique handle type
+ * to enhance type checking.
  */
-typedef struct {
-	__u64	 cookie;
-} lnet_handle_any_t;
-
-typedef lnet_handle_any_t lnet_handle_eq_t;
-typedef lnet_handle_any_t lnet_handle_md_t;
-typedef lnet_handle_any_t lnet_handle_me_t;
-
 #define LNET_WIRE_HANDLE_COOKIE_NONE   (-1)
 
+struct lnet_handle_eq {
+	u64	cookie;
+};
+
 /**
- * Invalidate handle \a h.
+ * Invalidate eq handle @h.
  */
-static inline void LNetInvalidateHandle(lnet_handle_any_t *h)
+static inline void LNetInvalidateEQHandle(struct lnet_handle_eq *h)
 {
 	h->cookie = LNET_WIRE_HANDLE_COOKIE_NONE;
 }
 
 /**
- * Compare handles \a h1 and \a h2.
+ * Check whether eq handle @h is invalid.
  *
- * \return 1 if handles are equal, 0 if otherwise.
+ * @return 1 if handle is invalid, 0 if valid.
  */
-static inline int LNetHandleIsEqual(lnet_handle_any_t h1, lnet_handle_any_t h2)
+static inline int LNetEQHandleIsInvalid(struct lnet_handle_eq h)
 {
-	return h1.cookie == h2.cookie;
+	return (LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie);
 }
 
+struct lnet_handle_md {
+	u64	cookie;
+};
+
 /**
- * Check whether handle \a h is invalid.
+ * Invalidate md handle @h.
+ */
+static inline void LNetInvalidateMDHandle(struct lnet_handle_md *h)
+{
+	h->cookie = LNET_WIRE_HANDLE_COOKIE_NONE;
+}
+
+/**
+ * Check whether eq handle @h is invalid.
  *
- * \return 1 if handle is invalid, 0 if valid.
+ * @return 1 if handle is invalid, 0 if valid.
  */
-static inline int LNetHandleIsInvalid(lnet_handle_any_t h)
+static inline int LNetMDHandleIsInvalid(struct lnet_handle_md h)
 {
-	return h.cookie == LNET_WIRE_HANDLE_COOKIE_NONE;
+	return (LNET_WIRE_HANDLE_COOKIE_NONE == h.cookie);
 }
 
+struct lnet_handle_me {
+	u64	cookie;
+};
+
 /**
  * Global process ID.
  */
-typedef struct {
+struct lnet_process_id {
 	/** node id */
 	lnet_nid_t nid;
 	/** process id */
 	lnet_pid_t pid;
-} lnet_process_id_t;
+};
 /** @} lnet_addr */
 
 /** \addtogroup lnet_me
@@ -337,26 +346,26 @@ typedef struct {
  * Specifies whether the match entry or memory descriptor should be unlinked
  * automatically (LNET_UNLINK) or not (LNET_RETAIN).
  */
-typedef enum {
+enum lnet_unlink {
 	LNET_RETAIN = 0,
 	LNET_UNLINK
-} lnet_unlink_t;
+};
 
 /**
- * Values of the type lnet_ins_pos_t are used to control where a new match
+ * Values of the type lnet_ins_pos are used to control where a new match
  * entry is inserted. The value LNET_INS_BEFORE is used to insert the new
  * entry before the current entry or before the head of the list. The value
  * LNET_INS_AFTER is used to insert the new entry after the current entry
  * or after the last item in the list.
  */
-typedef enum {
+enum lnet_ins_pos {
 	/** insert ME before current position or head of the list */
 	LNET_INS_BEFORE,
 	/** insert ME after current position or tail of the list */
 	LNET_INS_AFTER,
 	/** attach ME at tail of local CPU partition ME list */
 	LNET_INS_LOCAL
-} lnet_ins_pos_t;
+};
 
 /** @} lnet_me */
 
@@ -368,14 +377,14 @@ typedef enum {
  * Defines the visible parts of a memory descriptor. Values of this type
  * are used to initialize memory descriptors.
  */
-typedef struct {
+struct lnet_md {
 	/**
 	 * Specify the memory region associated with the memory descriptor.
 	 * If the options field has:
 	 * - LNET_MD_KIOV bit set: The start field points to the starting
-	 * address of an array of lnet_kiov_t and the length field specifies
+	 * address of an array of struct bio_vec and the length field specifies
 	 * the number of entries in the array. The length can't be bigger
-	 * than LNET_MAX_IOV. The lnet_kiov_t is used to describe page-based
+	 * than LNET_MAX_IOV. The struct bio_vec is used to describe page-based
 	 * fragments that are not necessarily mapped in virtual memory.
 	 * - LNET_MD_IOVEC bit set: The start field points to the starting
 	 * address of an array of struct iovec and the length field specifies
@@ -435,7 +444,7 @@ typedef struct {
 	 *   acknowledgment. Acknowledgments are never sent for GET operations.
 	 *   The data sent in the REPLY serves as an implicit acknowledgment.
 	 * - LNET_MD_KIOV: The start and length fields specify an array of
-	 *   lnet_kiov_t.
+	 *   struct bio_vec.
 	 * - LNET_MD_IOVEC: The start and length fields specify an array of
 	 *   struct iovec.
 	 * - LNET_MD_MAX_SIZE: The max_size field is valid.
@@ -461,8 +470,8 @@ typedef struct {
 	 * by LNetInvalidateHandle()), operations performed on this memory
 	 * descriptor are not logged.
 	 */
-	lnet_handle_eq_t eq_handle;
-} lnet_md_t;
+	struct lnet_handle_eq eq_handle;
+};
 
 /*
  * Max Transfer Unit (minimum supported everywhere).
@@ -476,35 +485,31 @@ typedef struct {
 #define LNET_MAX_IOV	256
 
 /**
- * Options for the MD structure. See lnet_md_t::options.
+ * Options for the MD structure. See lnet_md::options.
  */
 #define LNET_MD_OP_PUT		(1 << 0)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_OP_GET		(1 << 1)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_MANAGE_REMOTE	(1 << 2)
 /* unused			(1 << 3) */
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_TRUNCATE	(1 << 4)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_ACK_DISABLE	(1 << 5)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_IOVEC		(1 << 6)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_MAX_SIZE	(1 << 7)
-/** See lnet_md_t::options. */
+/** See lnet_md::options. */
 #define LNET_MD_KIOV		(1 << 8)
 
 /* For compatibility with Cray Portals */
 #define LNET_MD_PHYS		0
 
-/** Infinite threshold on MD operations. See lnet_md_t::threshold */
+/** Infinite threshold on MD operations. See lnet_md::threshold */
 #define LNET_MD_THRESH_INF	(-1)
 
-/* NB lustre portals uses struct iovec internally! */
-typedef struct iovec lnet_md_iovec_t;
-
-typedef struct bio_vec lnet_kiov_t;
 /** @} lnet_md */
 
 /** \addtogroup lnet_eq
@@ -514,7 +519,7 @@ typedef struct bio_vec lnet_kiov_t;
 /**
  * Six types of events can be logged in an event queue.
  */
-typedef enum {
+enum lnet_event_kind {
 	/** An incoming GET operation has completed on the MD. */
 	LNET_EVENT_GET		= 1,
 	/**
@@ -550,20 +555,18 @@ typedef enum {
 	 * \see LNetMDUnlink
 	 */
 	LNET_EVENT_UNLINK,
-} lnet_event_kind_t;
+};
 
-#define LNET_SEQ_BASETYPE	long
-typedef unsigned LNET_SEQ_BASETYPE lnet_seq_t;
-#define LNET_SEQ_GT(a, b)	(((signed LNET_SEQ_BASETYPE)((a) - (b))) > 0)
+#define LNET_SEQ_GT(a, b)      (((signed long)((a) - (b))) > 0)
 
 /**
  * Information about an event on a MD.
  */
-typedef struct {
+struct lnet_event {
 	/** The identifier (nid, pid) of the target. */
-	lnet_process_id_t	target;
+	struct lnet_process_id	target;
 	/** The identifier (nid, pid) of the initiator. */
-	lnet_process_id_t	initiator;
+	struct lnet_process_id	initiator;
 	/**
 	 * The NID of the immediate sender. If the request has been forwarded
 	 * by routers, this is the NID of the last hop; otherwise it's the
@@ -571,7 +574,7 @@ typedef struct {
 	 */
 	lnet_nid_t		sender;
 	/** Indicates the type of the event. */
-	lnet_event_kind_t	type;
+	enum lnet_event_kind	type;
 	/** The portal table index specified in the request */
 	unsigned int		pt_index;
 	/** A copy of the match bits specified in the request. */
@@ -582,7 +585,7 @@ typedef struct {
 	 * The length (in bytes) of the data that was manipulated by the
 	 * operation. For truncated operations, the manipulated length will be
 	 * the number of bytes specified by the MD (possibly with an offset,
-	 * see lnet_md_t). For all other operations, the manipulated length
+	 * see lnet_md). For all other operations, the manipulated length
 	 * will be the length of the requested operation, i.e. rlength.
 	 */
 	unsigned int		mlength;
@@ -590,13 +593,13 @@ typedef struct {
 	 * The handle to the MD associated with the event. The handle may be
 	 * invalid if the MD has been unlinked.
 	 */
-	lnet_handle_md_t	md_handle;
+	struct lnet_handle_md	md_handle;
 	/**
 	 * A snapshot of the state of the MD immediately after the event has
 	 * been processed. In particular, the threshold field in md will
 	 * reflect the value of the threshold after the operation occurred.
 	 */
-	lnet_md_t		md;
+	struct lnet_md		md;
 	/**
 	 * 64 bits of out-of-band user data. Only valid for LNET_EVENT_PUT.
 	 * \see LNetPut
@@ -618,15 +621,15 @@ typedef struct {
 	 * The displacement (in bytes) into the memory region that the
 	 * operation used. The offset can be determined by the operation for
 	 * a remote managed MD or by the local MD.
-	 * \see lnet_md_t::options
+	 * \see lnet_md::options
 	 */
 	unsigned int		offset;
 	/**
 	 * The sequence number for this event. Sequence numbers are unique
 	 * to each event.
 	 */
-	volatile lnet_seq_t	sequence;
-} lnet_event_t;
+	volatile unsigned long	sequence;
+};
 
 /**
  * Event queue handler function type.
@@ -638,7 +641,7 @@ typedef struct {
  * The handler must not block, must be reentrant, and must not call any LNet
  * API functions. It should return as quickly as possible.
  */
-typedef void (*lnet_eq_handler_t)(lnet_event_t *event);
+typedef void (*lnet_eq_handler_t)(struct lnet_event *event);
 #define LNET_EQ_HANDLER_NONE NULL
 /** @} lnet_eq */
 
@@ -651,15 +654,15 @@ typedef void (*lnet_eq_handler_t)(lnet_event_t *event);
  * operation completes (i.e., when the data has been written to a MD of the
  * target process).
  *
- * \see lnet_md_t::options for the discussion on LNET_MD_ACK_DISABLE by which
+ * \see lnet_md::options for the discussion on LNET_MD_ACK_DISABLE by which
  * acknowledgments can be disabled for a MD.
  */
-typedef enum {
+enum lnet_ack_req {
 	/** Request an acknowledgment */
 	LNET_ACK_REQ,
 	/** Request that no acknowledgment should be generated. */
 	LNET_NOACK_REQ
-} lnet_ack_req_t;
+};
 /** @} lnet_data */
 
 /** @} lnet */
diff --git a/drivers/staging/lustre/lnet/Kconfig b/drivers/staging/lustre/lnet/Kconfig
index 13b43278a38d..2b5930150cda 100644
--- a/drivers/staging/lustre/lnet/Kconfig
+++ b/drivers/staging/lustre/lnet/Kconfig
@@ -35,7 +35,6 @@ config LNET_SELFTEST
 config LNET_XPRT_IB
 	tristate "LNET infiniband support"
 	depends on LNET && INFINIBAND && INFINIBAND_ADDR_TRANS
-	depends on BROKEN
 	default LNET && INFINIBAND
 	help
 	  This option allows the LNET users to use infiniband as an
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index b1e8508f9fc7..79321e4aaf30 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -38,7 +38,7 @@
 #include <asm/page.h>
 #include "o2iblnd.h"
 
-static lnd_t the_o2iblnd;
+static struct lnet_lnd the_o2iblnd;
 
 struct kib_data kiblnd_data;
 
@@ -174,7 +174,7 @@ static int kiblnd_unpack_rd(struct kib_msg *msg, int flip)
 	return 0;
 }
 
-void kiblnd_pack_msg(lnet_ni_t *ni, struct kib_msg *msg, int version,
+void kiblnd_pack_msg(struct lnet_ni *ni, struct kib_msg *msg, int version,
 		     int credits, lnet_nid_t dstnid, __u64 dststamp)
 {
 	struct kib_net *net = ni->ni_data;
@@ -313,7 +313,8 @@ int kiblnd_unpack_msg(struct kib_msg *msg, int nob)
 	return 0;
 }
 
-int kiblnd_create_peer(lnet_ni_t *ni, struct kib_peer **peerp, lnet_nid_t nid)
+int kiblnd_create_peer(struct lnet_ni *ni, struct kib_peer **peerp,
+		       lnet_nid_t nid)
 {
 	struct kib_peer *peer;
 	struct kib_net *net = ni->ni_data;
@@ -412,7 +413,7 @@ void kiblnd_unlink_peer_locked(struct kib_peer *peer)
 	kiblnd_peer_decref(peer);
 }
 
-static int kiblnd_get_peer_info(lnet_ni_t *ni, int index,
+static int kiblnd_get_peer_info(struct lnet_ni *ni, int index,
 				lnet_nid_t *nidp, int *count)
 {
 	struct kib_peer *peer;
@@ -468,7 +469,7 @@ static void kiblnd_del_peer_locked(struct kib_peer *peer)
 	 */
 }
 
-static int kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
+static int kiblnd_del_peer(struct lnet_ni *ni, lnet_nid_t nid)
 {
 	LIST_HEAD(zombies);
 	struct list_head *ptmp;
@@ -520,7 +521,7 @@ static int kiblnd_del_peer(lnet_ni_t *ni, lnet_nid_t nid)
 	return rc;
 }
 
-static struct kib_conn *kiblnd_get_conn_by_idx(lnet_ni_t *ni, int index)
+static struct kib_conn *kiblnd_get_conn_by_idx(struct lnet_ni *ni, int index)
 {
 	struct kib_peer *peer;
 	struct list_head *ptmp;
@@ -947,7 +948,7 @@ int kiblnd_close_stale_conns_locked(struct kib_peer *peer,
 	return count;
 }
 
-static int kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
+static int kiblnd_close_matching_conns(struct lnet_ni *ni, lnet_nid_t nid)
 {
 	struct kib_peer *peer;
 	struct list_head *ptmp;
@@ -992,7 +993,7 @@ static int kiblnd_close_matching_conns(lnet_ni_t *ni, lnet_nid_t nid)
 	return !count ? -ENOENT : 0;
 }
 
-static int kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
+static int kiblnd_ctl(struct lnet_ni *ni, unsigned int cmd, void *arg)
 {
 	struct libcfs_ioctl_data *data = arg;
 	int rc = -EINVAL;
@@ -1045,7 +1046,8 @@ static int kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	return rc;
 }
 
-static void kiblnd_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
+static void kiblnd_query(struct lnet_ni *ni, lnet_nid_t nid,
+			 unsigned long *when)
 {
 	unsigned long last_alive = 0;
 	unsigned long now = cfs_time_current();
@@ -1281,27 +1283,6 @@ static void kiblnd_map_tx_pool(struct kib_tx_pool *tpo)
 	}
 }
 
-struct ib_mr *kiblnd_find_rd_dma_mr(struct lnet_ni *ni, struct kib_rdma_desc *rd,
-				    int negotiated_nfrags)
-{
-	struct kib_net *net = ni->ni_data;
-	struct kib_hca_dev *hdev = net->ibn_dev->ibd_hdev;
-	struct lnet_ioctl_config_o2iblnd_tunables *tunables;
-	__u16 nfrags;
-	int mod;
-
-	tunables = &ni->ni_lnd_tunables->lt_tun_u.lt_o2ib;
-	mod = tunables->lnd_map_on_demand;
-	nfrags = (negotiated_nfrags != -1) ? negotiated_nfrags : mod;
-
-	LASSERT(hdev->ibh_mrs);
-
-	if (mod > 0 && nfrags <= rd->rd_nfrags)
-		return NULL;
-
-	return hdev->ibh_mrs;
-}
-
 static void kiblnd_destroy_fmr_pool(struct kib_fmr_pool *fpo)
 {
 	LASSERT(!fpo->fpo_map_count);
@@ -2058,7 +2039,7 @@ static int kiblnd_create_tx_pool(struct kib_poolset *ps, int size,
 	tpo->tpo_tx_descs = NULL;
 	tpo->tpo_tx_pages = NULL;
 
-	npg = (size * IBLND_MSG_SIZE + PAGE_SIZE - 1) / PAGE_SIZE;
+	npg = DIV_ROUND_UP(size * IBLND_MSG_SIZE, PAGE_SIZE);
 	if (kiblnd_alloc_pages(&tpo->tpo_tx_pages, ps->ps_cpt, npg)) {
 		CERROR("Can't allocate tx pages: %d\n", npg);
 		LIBCFS_FREE(tpo, sizeof(*tpo));
@@ -2164,25 +2145,16 @@ static void kiblnd_net_fini_pools(struct kib_net *net)
 	}
 }
 
-static int kiblnd_net_init_pools(struct kib_net *net, lnet_ni_t *ni, __u32 *cpts,
-				 int ncpts)
+static int kiblnd_net_init_pools(struct kib_net *net, struct lnet_ni *ni,
+				 __u32 *cpts, int ncpts)
 {
 	struct lnet_ioctl_config_o2iblnd_tunables *tunables;
-	unsigned long flags;
 	int cpt;
 	int rc;
 	int i;
 
 	tunables = &ni->ni_lnd_tunables->lt_tun_u.lt_o2ib;
 
-	read_lock_irqsave(&kiblnd_data.kib_global_lock, flags);
-	if (!tunables->lnd_map_on_demand) {
-		read_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
-		goto create_tx_pool;
-	}
-
-	read_unlock_irqrestore(&kiblnd_data.kib_global_lock, flags);
-
 	if (tunables->lnd_fmr_pool_size < *kiblnd_tunables.kib_ntx / 4) {
 		CERROR("Can't set fmr pool size (%d) < ntx / 4(%d)\n",
 		       tunables->lnd_fmr_pool_size,
@@ -2227,7 +2199,6 @@ static int kiblnd_net_init_pools(struct kib_net *net, lnet_ni_t *ni, __u32 *cpts
 	if (i > 0)
 		LASSERT(i == ncpts);
 
- create_tx_pool:
 	/*
 	 * cfs_precpt_alloc is creating an array of struct kib_tx_poolset
 	 * The number of struct kib_tx_poolsets create is equal to the
@@ -2283,20 +2254,8 @@ static int kiblnd_hdev_get_attr(struct kib_hca_dev *hdev)
 	return -EINVAL;
 }
 
-static void kiblnd_hdev_cleanup_mrs(struct kib_hca_dev *hdev)
-{
-	if (!hdev->ibh_mrs)
-		return;
-
-	ib_dereg_mr(hdev->ibh_mrs);
-
-	hdev->ibh_mrs = NULL;
-}
-
 void kiblnd_hdev_destroy(struct kib_hca_dev *hdev)
 {
-	kiblnd_hdev_cleanup_mrs(hdev);
-
 	if (hdev->ibh_pd)
 		ib_dealloc_pd(hdev->ibh_pd);
 
@@ -2306,28 +2265,6 @@ void kiblnd_hdev_destroy(struct kib_hca_dev *hdev)
 	LIBCFS_FREE(hdev, sizeof(*hdev));
 }
 
-static int kiblnd_hdev_setup_mrs(struct kib_hca_dev *hdev)
-{
-	struct ib_mr *mr;
-	int rc;
-	int acflags = IB_ACCESS_LOCAL_WRITE | IB_ACCESS_REMOTE_WRITE;
-
-	rc = kiblnd_hdev_get_attr(hdev);
-	if (rc)
-		return rc;
-
-	mr = ib_get_dma_mr(hdev->ibh_pd, acflags);
-	if (IS_ERR(mr)) {
-		CERROR("Failed ib_get_dma_mr : %ld\n", PTR_ERR(mr));
-		kiblnd_hdev_cleanup_mrs(hdev);
-		return PTR_ERR(mr);
-	}
-
-	hdev->ibh_mrs = mr;
-
-	return 0;
-}
-
 /* DUMMY */
 static int kiblnd_dummy_callback(struct rdma_cm_id *cmid,
 				 struct rdma_cm_event *event)
@@ -2482,9 +2419,9 @@ int kiblnd_dev_failover(struct kib_dev *dev)
 		goto out;
 	}
 
-	rc = kiblnd_hdev_setup_mrs(hdev);
+	rc = kiblnd_hdev_get_attr(hdev);
 	if (rc) {
-		CERROR("Can't setup device: %d\n", rc);
+		CERROR("Can't get device attributes: %d\n", rc);
 		goto out;
 	}
 
@@ -2652,7 +2589,7 @@ static void kiblnd_base_shutdown(void)
 	module_put(THIS_MODULE);
 }
 
-static void kiblnd_shutdown(lnet_ni_t *ni)
+static void kiblnd_shutdown(struct lnet_ni *ni)
 {
 	struct kib_net *net = ni->ni_data;
 	rwlock_t *g_lock = &kiblnd_data.kib_global_lock;
@@ -2909,7 +2846,7 @@ static struct kib_dev *kiblnd_dev_search(char *ifname)
 	return alias;
 }
 
-static int kiblnd_startup(lnet_ni_t *ni)
+static int kiblnd_startup(struct lnet_ni *ni)
 {
 	char *ifname;
 	struct kib_dev *ibdev = NULL;
@@ -3003,7 +2940,7 @@ net_failed:
 	return -ENETDOWN;
 }
 
-static lnd_t the_o2iblnd = {
+static struct lnet_lnd the_o2iblnd = {
 	.lnd_type	= O2IBLND,
 	.lnd_startup	= kiblnd_startup,
 	.lnd_shutdown	= kiblnd_shutdown,
@@ -3021,12 +2958,12 @@ static void __exit ko2iblnd_exit(void)
 static int __init ko2iblnd_init(void)
 {
 	BUILD_BUG_ON(sizeof(struct kib_msg) > IBLND_MSG_SIZE);
-	BUILD_BUG_ON(!offsetof(struct kib_msg,
+	BUILD_BUG_ON(offsetof(struct kib_msg,
 			  ibm_u.get.ibgm_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
-			  <= IBLND_MSG_SIZE);
-	BUILD_BUG_ON(!offsetof(struct kib_msg,
+			  > IBLND_MSG_SIZE);
+	BUILD_BUG_ON(offsetof(struct kib_msg,
 			  ibm_u.putack.ibpam_rd.rd_frags[IBLND_MAX_RDMA_FRAGS])
-			  <= IBLND_MSG_SIZE);
+			  > IBLND_MSG_SIZE);
 
 	kiblnd_tunables_init();
 
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
index 2cb429830681..16e437b3ad1e 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
@@ -172,7 +172,6 @@ struct kib_hca_dev {
 	__u64              ibh_page_mask;       /* page mask of current HCA */
 	int                ibh_mr_shift;        /* bits shift of max MR size */
 	__u64              ibh_mr_size;         /* size of MR */
-	struct ib_mr	   *ibh_mrs;		/* global MR */
 	struct ib_pd       *ibh_pd;             /* PD */
 	struct kib_dev	   *ibh_dev;		/* owner */
 	atomic_t           ibh_ref;             /* refcount */
@@ -491,7 +490,7 @@ struct kib_tx {					/* transmit message */
 	int                   tx_status;      /* LNET completion status */
 	unsigned long         tx_deadline;    /* completion deadline */
 	__u64                 tx_cookie;      /* completion cookie */
-	lnet_msg_t *tx_lntmsg[2]; /* lnet msgs to finalize on completion */
+	struct lnet_msg		*tx_lntmsg[2];	/* lnet msgs to finalize on completion */
 	struct kib_msg	      *tx_msg;        /* message buffer (host vaddr) */
 	__u64                 tx_msgaddr;     /* message buffer (I/O addr) */
 	DECLARE_PCI_UNMAP_ADDR(tx_msgunmap);  /* for dma_unmap_single() */
@@ -567,7 +566,7 @@ struct kib_conn {
 struct kib_peer {
 	struct list_head ibp_list;        /* stash on global peer list */
 	lnet_nid_t       ibp_nid;         /* who's on the other end(s) */
-	lnet_ni_t        *ibp_ni;         /* LNet interface */
+	struct lnet_ni	*ibp_ni;         /* LNet interface */
 	struct list_head ibp_conns;       /* all active connections */
 	struct list_head ibp_tx_queue;    /* msgs waiting for a conn */
 	__u64            ibp_incarnation; /* incarnation of peer */
@@ -764,7 +763,7 @@ static inline int
 kiblnd_need_noop(struct kib_conn *conn)
 {
 	struct lnet_ioctl_config_o2iblnd_tunables *tunables;
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 
 	LASSERT(conn->ibc_state >= IBLND_CONN_ESTABLISHED);
 	tunables = &ni->ni_lnd_tunables->lt_tun_u.lt_o2ib;
@@ -978,8 +977,6 @@ static inline unsigned int kiblnd_sg_dma_len(struct ib_device *dev,
 #define KIBLND_CONN_PARAM(e)     ((e)->param.conn.private_data)
 #define KIBLND_CONN_PARAM_LEN(e) ((e)->param.conn.private_data_len)
 
-struct ib_mr *kiblnd_find_rd_dma_mr(struct lnet_ni *ni, struct kib_rdma_desc *rd,
-				    int negotiated_nfrags);
 void kiblnd_map_rx_descs(struct kib_conn *conn);
 void kiblnd_unmap_rx_descs(struct kib_conn *conn);
 void kiblnd_pool_free_node(struct kib_pool *pool, struct list_head *node);
@@ -1005,7 +1002,8 @@ int  kiblnd_cm_callback(struct rdma_cm_id *cmid,
 int  kiblnd_translate_mtu(int value);
 
 int  kiblnd_dev_failover(struct kib_dev *dev);
-int  kiblnd_create_peer(lnet_ni_t *ni, struct kib_peer **peerp, lnet_nid_t nid);
+int kiblnd_create_peer(struct lnet_ni *ni, struct kib_peer **peerp,
+		       lnet_nid_t nid);
 void kiblnd_destroy_peer(struct kib_peer *peer);
 bool kiblnd_reconnect_peer(struct kib_peer *peer);
 void kiblnd_destroy_dev(struct kib_dev *dev);
@@ -1022,19 +1020,19 @@ void kiblnd_destroy_conn(struct kib_conn *conn, bool free_conn);
 void kiblnd_close_conn(struct kib_conn *conn, int error);
 void kiblnd_close_conn_locked(struct kib_conn *conn, int error);
 
-void kiblnd_launch_tx(lnet_ni_t *ni, struct kib_tx *tx, lnet_nid_t nid);
-void kiblnd_txlist_done(lnet_ni_t *ni, struct list_head *txlist,
+void kiblnd_launch_tx(struct lnet_ni *ni, struct kib_tx *tx, lnet_nid_t nid);
+void kiblnd_txlist_done(struct lnet_ni *ni, struct list_head *txlist,
 			int status);
 
 void kiblnd_qp_event(struct ib_event *event, void *arg);
 void kiblnd_cq_event(struct ib_event *event, void *arg);
 void kiblnd_cq_completion(struct ib_cq *cq, void *arg);
 
-void kiblnd_pack_msg(lnet_ni_t *ni, struct kib_msg *msg, int version,
+void kiblnd_pack_msg(struct lnet_ni *ni, struct kib_msg *msg, int version,
 		     int credits, lnet_nid_t dstnid, __u64 dststamp);
 int  kiblnd_unpack_msg(struct kib_msg *msg, int nob);
 int  kiblnd_post_rx(struct kib_rx *rx, int credit);
 
-int  kiblnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg);
-int  kiblnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
-		 struct iov_iter *to, unsigned int rlen);
+int kiblnd_send(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg);
+int kiblnd_recv(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg,
+		int delayed, struct iov_iter *to, unsigned int rlen);
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index e2f3f7294260..0db662d6abdd 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -40,20 +40,20 @@
 
 static void kiblnd_peer_alive(struct kib_peer *peer);
 static void kiblnd_peer_connect_failed(struct kib_peer *peer, int active, int error);
-static void kiblnd_init_tx_msg(lnet_ni_t *ni, struct kib_tx *tx,
+static void kiblnd_init_tx_msg(struct lnet_ni *ni, struct kib_tx *tx,
 			       int type, int body_nob);
 static int kiblnd_init_rdma(struct kib_conn *conn, struct kib_tx *tx, int type,
 			    int resid, struct kib_rdma_desc *dstrd,
 			    __u64 dstcookie);
 static void kiblnd_queue_tx_locked(struct kib_tx *tx, struct kib_conn *conn);
 static void kiblnd_queue_tx(struct kib_tx *tx, struct kib_conn *conn);
-static void kiblnd_unmap_tx(lnet_ni_t *ni, struct kib_tx *tx);
+static void kiblnd_unmap_tx(struct lnet_ni *ni, struct kib_tx *tx);
 static void kiblnd_check_sends_locked(struct kib_conn *conn);
 
 static void
-kiblnd_tx_done(lnet_ni_t *ni, struct kib_tx *tx)
+kiblnd_tx_done(struct lnet_ni *ni, struct kib_tx *tx)
 {
-	lnet_msg_t *lntmsg[2];
+	struct lnet_msg *lntmsg[2];
 	struct kib_net *net = ni->ni_data;
 	int rc;
 	int i;
@@ -94,7 +94,7 @@ kiblnd_tx_done(lnet_ni_t *ni, struct kib_tx *tx)
 }
 
 void
-kiblnd_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int status)
+kiblnd_txlist_done(struct lnet_ni *ni, struct list_head *txlist, int status)
 {
 	struct kib_tx *tx;
 
@@ -110,7 +110,7 @@ kiblnd_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int status)
 }
 
 static struct kib_tx *
-kiblnd_get_idle_tx(lnet_ni_t *ni, lnet_nid_t target)
+kiblnd_get_idle_tx(struct lnet_ni *ni, lnet_nid_t target)
 {
 	struct kib_net *net = (struct kib_net *)ni->ni_data;
 	struct list_head *node;
@@ -157,7 +157,6 @@ kiblnd_post_rx(struct kib_rx *rx, int credit)
 	struct kib_conn *conn = rx->rx_conn;
 	struct kib_net *net = conn->ibc_peer->ibp_ni->ni_data;
 	struct ib_recv_wr *bad_wrq = NULL;
-	struct ib_mr *mr = conn->ibc_hdev->ibh_mrs;
 	int rc;
 
 	LASSERT(net);
@@ -165,9 +164,8 @@ kiblnd_post_rx(struct kib_rx *rx, int credit)
 	LASSERT(credit == IBLND_POSTRX_NO_CREDIT ||
 		credit == IBLND_POSTRX_PEER_CREDIT ||
 		credit == IBLND_POSTRX_RSRVD_CREDIT);
-	LASSERT(mr);
 
-	rx->rx_sge.lkey   = mr->lkey;
+	rx->rx_sge.lkey   = conn->ibc_hdev->ibh_pd->local_dma_lkey;
 	rx->rx_sge.addr   = rx->rx_msgaddr;
 	rx->rx_sge.length = IBLND_MSG_SIZE;
 
@@ -251,7 +249,7 @@ static void
 kiblnd_handle_completion(struct kib_conn *conn, int txtype, int status, __u64 cookie)
 {
 	struct kib_tx *tx;
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 	int idle;
 
 	spin_lock(&conn->ibc_lock);
@@ -288,7 +286,7 @@ kiblnd_handle_completion(struct kib_conn *conn, int txtype, int status, __u64 co
 static void
 kiblnd_send_completion(struct kib_conn *conn, int type, int status, __u64 cookie)
 {
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 	struct kib_tx *tx = kiblnd_get_idle_tx(ni, conn->ibc_peer->ibp_nid);
 
 	if (!tx) {
@@ -309,7 +307,7 @@ kiblnd_handle_rx(struct kib_rx *rx)
 {
 	struct kib_msg *msg = rx->rx_msg;
 	struct kib_conn *conn = rx->rx_conn;
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 	int credits = msg->ibm_credits;
 	struct kib_tx *tx;
 	int rc = 0;
@@ -470,7 +468,7 @@ kiblnd_rx_complete(struct kib_rx *rx, int status, int nob)
 {
 	struct kib_msg *msg = rx->rx_msg;
 	struct kib_conn *conn = rx->rx_conn;
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 	struct kib_net *net = ni->ni_data;
 	int rc;
 	int err = -EIO;
@@ -592,7 +590,7 @@ kiblnd_fmr_map_tx(struct kib_net *net, struct kib_tx *tx, struct kib_rdma_desc *
 	return 0;
 }
 
-static void kiblnd_unmap_tx(lnet_ni_t *ni, struct kib_tx *tx)
+static void kiblnd_unmap_tx(struct lnet_ni *ni, struct kib_tx *tx)
 {
 	struct kib_net *net = ni->ni_data;
 
@@ -608,12 +606,11 @@ static void kiblnd_unmap_tx(lnet_ni_t *ni, struct kib_tx *tx)
 	}
 }
 
-static int kiblnd_map_tx(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc *rd,
-			 int nfrags)
+static int kiblnd_map_tx(struct lnet_ni *ni, struct kib_tx *tx,
+			 struct kib_rdma_desc *rd, int nfrags)
 {
 	struct kib_net *net = ni->ni_data;
 	struct kib_hca_dev *hdev = net->ibn_dev->ibd_hdev;
-	struct ib_mr *mr    = NULL;
 	__u32 nob;
 	int i;
 
@@ -635,14 +632,6 @@ static int kiblnd_map_tx(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc
 		nob += rd->rd_frags[i].rf_nob;
 	}
 
-	mr = kiblnd_find_rd_dma_mr(ni, rd, tx->tx_conn ?
-				   tx->tx_conn->ibc_max_frags : -1);
-	if (mr) {
-		/* found pre-mapping MR */
-		rd->rd_key = (rd != tx->tx_rd) ? mr->rkey : mr->lkey;
-		return 0;
-	}
-
 	if (net->ibn_fmr_ps)
 		return kiblnd_fmr_map_tx(net, tx, rd, nob);
 
@@ -650,8 +639,9 @@ static int kiblnd_map_tx(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc
 }
 
 static int
-kiblnd_setup_rd_iov(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc *rd,
-		    unsigned int niov, const struct kvec *iov, int offset, int nob)
+kiblnd_setup_rd_iov(struct lnet_ni *ni, struct kib_tx *tx,
+		    struct kib_rdma_desc *rd, unsigned int niov,
+		    const struct kvec *iov, int offset, int nob)
 {
 	struct kib_net *net = ni->ni_data;
 	struct page *page;
@@ -707,8 +697,9 @@ kiblnd_setup_rd_iov(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc *rd,
 }
 
 static int
-kiblnd_setup_rd_kiov(lnet_ni_t *ni, struct kib_tx *tx, struct kib_rdma_desc *rd,
-		     int nkiov, const lnet_kiov_t *kiov, int offset, int nob)
+kiblnd_setup_rd_kiov(struct lnet_ni *ni, struct kib_tx *tx,
+		     struct kib_rdma_desc *rd, int nkiov,
+		     const struct bio_vec *kiov, int offset, int nob)
 {
 	struct kib_net *net = ni->ni_data;
 	struct scatterlist *sg;
@@ -910,7 +901,7 @@ static void
 kiblnd_check_sends_locked(struct kib_conn *conn)
 {
 	int ver = conn->ibc_version;
-	lnet_ni_t *ni = conn->ibc_peer->ibp_ni;
+	struct lnet_ni *ni = conn->ibc_peer->ibp_ni;
 	struct kib_tx *tx;
 
 	/* Don't send anything until after the connection is established */
@@ -1022,22 +1013,21 @@ kiblnd_tx_complete(struct kib_tx *tx, int status)
 }
 
 static void
-kiblnd_init_tx_msg(lnet_ni_t *ni, struct kib_tx *tx, int type, int body_nob)
+kiblnd_init_tx_msg(struct lnet_ni *ni, struct kib_tx *tx, int type,
+		   int body_nob)
 {
 	struct kib_hca_dev *hdev = tx->tx_pool->tpo_hdev;
 	struct ib_sge *sge = &tx->tx_sge[tx->tx_nwrq];
 	struct ib_rdma_wr *wrq = &tx->tx_wrq[tx->tx_nwrq];
 	int nob = offsetof(struct kib_msg, ibm_u) + body_nob;
-	struct ib_mr *mr = hdev->ibh_mrs;
 
 	LASSERT(tx->tx_nwrq >= 0);
 	LASSERT(tx->tx_nwrq < IBLND_MAX_RDMA_FRAGS + 1);
 	LASSERT(nob <= IBLND_MSG_SIZE);
-	LASSERT(mr);
 
 	kiblnd_init_msg(tx->tx_msg, type, body_nob);
 
-	sge->lkey   = mr->lkey;
+	sge->lkey   = hdev->ibh_pd->local_dma_lkey;
 	sge->addr   = tx->tx_msgaddr;
 	sge->length = nob;
 
@@ -1103,9 +1093,9 @@ kiblnd_init_rdma(struct kib_conn *conn, struct kib_tx *tx, int type,
 			break;
 		}
 
-		wrknob = min(min(kiblnd_rd_frag_size(srcrd, srcidx),
-				 kiblnd_rd_frag_size(dstrd, dstidx)),
-			     (__u32)resid);
+		wrknob = min3(kiblnd_rd_frag_size(srcrd, srcidx),
+			      kiblnd_rd_frag_size(dstrd, dstidx),
+			      (__u32)resid);
 
 		sge = &tx->tx_sge[tx->tx_nwrq];
 		sge->addr   = kiblnd_rd_frag_addr(srcrd, srcidx);
@@ -1366,7 +1356,7 @@ no_reconnect:
 }
 
 void
-kiblnd_launch_tx(lnet_ni_t *ni, struct kib_tx *tx, lnet_nid_t nid)
+kiblnd_launch_tx(struct lnet_ni *ni, struct kib_tx *tx, lnet_nid_t nid)
 {
 	struct kib_peer *peer;
 	struct kib_peer *peer2;
@@ -1488,16 +1478,16 @@ kiblnd_launch_tx(lnet_ni_t *ni, struct kib_tx *tx, lnet_nid_t nid)
 }
 
 int
-kiblnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
+kiblnd_send(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg)
 {
 	struct lnet_hdr *hdr = &lntmsg->msg_hdr;
 	int type = lntmsg->msg_type;
-	lnet_process_id_t target = lntmsg->msg_target;
+	struct lnet_process_id target = lntmsg->msg_target;
 	int target_is_router = lntmsg->msg_target_is_router;
 	int routing = lntmsg->msg_routing;
 	unsigned int payload_niov = lntmsg->msg_niov;
 	struct kvec *payload_iov = lntmsg->msg_iov;
-	lnet_kiov_t *payload_kiov = lntmsg->msg_kiov;
+	struct bio_vec *payload_kiov = lntmsg->msg_kiov;
 	unsigned int payload_offset = lntmsg->msg_offset;
 	unsigned int payload_nob = lntmsg->msg_len;
 	struct iov_iter from;
@@ -1661,12 +1651,12 @@ kiblnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 }
 
 static void
-kiblnd_reply(lnet_ni_t *ni, struct kib_rx *rx, lnet_msg_t *lntmsg)
+kiblnd_reply(struct lnet_ni *ni, struct kib_rx *rx, struct lnet_msg *lntmsg)
 {
-	lnet_process_id_t target = lntmsg->msg_target;
+	struct lnet_process_id target = lntmsg->msg_target;
 	unsigned int niov = lntmsg->msg_niov;
 	struct kvec *iov = lntmsg->msg_iov;
-	lnet_kiov_t *kiov = lntmsg->msg_kiov;
+	struct bio_vec *kiov = lntmsg->msg_kiov;
 	unsigned int offset = lntmsg->msg_offset;
 	unsigned int nob = lntmsg->msg_len;
 	struct kib_tx *tx;
@@ -1722,8 +1712,8 @@ kiblnd_reply(lnet_ni_t *ni, struct kib_rx *rx, lnet_msg_t *lntmsg)
 }
 
 int
-kiblnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg, int delayed,
-	    struct iov_iter *to, unsigned int rlen)
+kiblnd_recv(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg,
+	    int delayed, struct iov_iter *to, unsigned int rlen)
 {
 	struct kib_rx *rx = private;
 	struct kib_msg *rxmsg = rx->rx_msg;
@@ -2170,7 +2160,7 @@ kiblnd_connreq_done(struct kib_conn *conn, int status)
 
 	if (!kiblnd_peer_active(peer) ||	/* peer has been deleted */
 	    conn->ibc_comms_error) {       /* error has happened already */
-		lnet_ni_t *ni = peer->ibp_ni;
+		struct lnet_ni *ni = peer->ibp_ni;
 
 		/* start to shut down connection */
 		kiblnd_close_conn_locked(conn, -ECONNABORTED);
@@ -2227,7 +2217,7 @@ kiblnd_passive_connect(struct rdma_cm_id *cmid, void *priv, int priv_nob)
 	struct kib_peer *peer;
 	struct kib_peer *peer2;
 	struct kib_conn *conn;
-	lnet_ni_t *ni  = NULL;
+	struct lnet_ni *ni  = NULL;
 	struct kib_net *net = NULL;
 	lnet_nid_t nid;
 	struct rdma_conn_param cp;
@@ -2789,7 +2779,7 @@ static void
 kiblnd_check_connreply(struct kib_conn *conn, void *priv, int priv_nob)
 {
 	struct kib_peer *peer = conn->ibc_peer;
-	lnet_ni_t *ni = peer->ibp_ni;
+	struct lnet_ni *ni = peer->ibp_ni;
 	struct kib_net *net = ni->ni_data;
 	struct kib_msg *msg = priv;
 	int ver = conn->ibc_version;
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
index 44e960f60833..3fe4d4858eba 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_modparams.c
@@ -106,7 +106,8 @@ static int concurrent_sends;
 module_param(concurrent_sends, int, 0444);
 MODULE_PARM_DESC(concurrent_sends, "send work-queue sizing");
 
-static int map_on_demand;
+#define IBLND_DEFAULT_MAP_ON_DEMAND IBLND_MAX_RDMA_FRAGS
+static int map_on_demand = IBLND_DEFAULT_MAP_ON_DEMAND;
 module_param(map_on_demand, int, 0444);
 MODULE_PARM_DESC(map_on_demand, "map on demand");
 
@@ -160,7 +161,7 @@ struct kib_tunables kiblnd_tunables = {
 static struct lnet_ioctl_config_o2iblnd_tunables default_tunables;
 
 /* # messages/RDMAs in-flight */
-int kiblnd_msg_queue_size(int version, lnet_ni_t *ni)
+int kiblnd_msg_queue_size(int version, struct lnet_ni *ni)
 {
 	if (version == IBLND_MSG_VERSION_1)
 		return IBLND_MSG_QUEUE_SIZE_V1;
@@ -228,10 +229,13 @@ int kiblnd_tunables_setup(struct lnet_ni *ni)
 	if (tunables->lnd_peercredits_hiw >= ni->ni_peertxcredits)
 		tunables->lnd_peercredits_hiw = ni->ni_peertxcredits - 1;
 
-	if (tunables->lnd_map_on_demand < 0 ||
+	if (tunables->lnd_map_on_demand <= 0 ||
 	    tunables->lnd_map_on_demand > IBLND_MAX_RDMA_FRAGS) {
-		/* disable map-on-demand */
-		tunables->lnd_map_on_demand = 0;
+		/* Use the default */
+		CWARN("Invalid map_on_demand (%d), expects 1 - %d. Using default of %d\n",
+		      tunables->lnd_map_on_demand,
+		      IBLND_MAX_RDMA_FRAGS, IBLND_DEFAULT_MAP_ON_DEMAND);
+		tunables->lnd_map_on_demand = IBLND_DEFAULT_MAP_ON_DEMAND;
 	}
 
 	if (tunables->lnd_map_on_demand == 1) {
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index f25de3d7f6e8..fbbd8a5489e9 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -39,11 +39,11 @@
 
 #include "socklnd.h"
 
-static lnd_t the_ksocklnd;
+static struct lnet_lnd the_ksocklnd;
 struct ksock_nal_data ksocknal_data;
 
 static struct ksock_interface *
-ksocknal_ip2iface(lnet_ni_t *ni, __u32 ip)
+ksocknal_ip2iface(struct lnet_ni *ni, __u32 ip)
 {
 	struct ksock_net *net = ni->ni_data;
 	int i;
@@ -96,8 +96,8 @@ ksocknal_destroy_route(struct ksock_route *route)
 }
 
 static int
-ksocknal_create_peer(struct ksock_peer **peerp, lnet_ni_t *ni,
-		     lnet_process_id_t id)
+ksocknal_create_peer(struct ksock_peer **peerp, struct lnet_ni *ni,
+		     struct lnet_process_id id)
 {
 	int cpt = lnet_cpt_of_nid(id.nid);
 	struct ksock_net *net = ni->ni_data;
@@ -173,7 +173,7 @@ ksocknal_destroy_peer(struct ksock_peer *peer)
 }
 
 struct ksock_peer *
-ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_find_peer_locked(struct lnet_ni *ni, struct lnet_process_id id)
 {
 	struct list_head *peer_list = ksocknal_nid2peerlist(id.nid);
 	struct list_head *tmp;
@@ -200,7 +200,7 @@ ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id)
 }
 
 struct ksock_peer *
-ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id)
+ksocknal_find_peer(struct lnet_ni *ni, struct lnet_process_id id)
 {
 	struct ksock_peer *peer;
 
@@ -246,8 +246,8 @@ ksocknal_unlink_peer_locked(struct ksock_peer *peer)
 }
 
 static int
-ksocknal_get_peer_info(lnet_ni_t *ni, int index,
-		       lnet_process_id_t *id, __u32 *myip, __u32 *peer_ip,
+ksocknal_get_peer_info(struct lnet_ni *ni, int index,
+		       struct lnet_process_id *id, __u32 *myip, __u32 *peer_ip,
 		       int *port, int *conn_count, int *share_count)
 {
 	struct ksock_peer *peer;
@@ -450,7 +450,8 @@ ksocknal_del_route_locked(struct ksock_route *route)
 }
 
 int
-ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ipaddr, int port)
+ksocknal_add_peer(struct lnet_ni *ni, struct lnet_process_id id, __u32 ipaddr,
+		  int port)
 {
 	struct list_head *tmp;
 	struct ksock_peer *peer;
@@ -568,7 +569,7 @@ ksocknal_del_peer_locked(struct ksock_peer *peer, __u32 ip)
 }
 
 static int
-ksocknal_del_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
+ksocknal_del_peer(struct lnet_ni *ni, struct lnet_process_id id, __u32 ip)
 {
 	LIST_HEAD(zombies);
 	struct list_head *ptmp;
@@ -627,7 +628,7 @@ ksocknal_del_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip)
 }
 
 static struct ksock_conn *
-ksocknal_get_conn_by_idx(lnet_ni_t *ni, int index)
+ksocknal_get_conn_by_idx(struct lnet_ni *ni, int index)
 {
 	struct ksock_peer *peer;
 	struct list_head *ptmp;
@@ -687,7 +688,7 @@ ksocknal_choose_scheduler_locked(unsigned int cpt)
 }
 
 static int
-ksocknal_local_ipvec(lnet_ni_t *ni, __u32 *ipaddrs)
+ksocknal_local_ipvec(struct lnet_ni *ni, __u32 *ipaddrs)
 {
 	struct ksock_net *net = ni->ni_data;
 	int i;
@@ -866,7 +867,7 @@ ksocknal_create_routes(struct ksock_peer *peer, int port,
 {
 	struct ksock_route *newroute = NULL;
 	rwlock_t *global_lock = &ksocknal_data.ksnd_global_lock;
-	lnet_ni_t *ni = peer->ksnp_ni;
+	struct lnet_ni *ni = peer->ksnp_ni;
 	struct ksock_net *net = ni->ni_data;
 	struct list_head *rtmp;
 	struct ksock_route *route;
@@ -982,7 +983,7 @@ ksocknal_create_routes(struct ksock_peer *peer, int port,
 }
 
 int
-ksocknal_accept(lnet_ni_t *ni, struct socket *sock)
+ksocknal_accept(struct lnet_ni *ni, struct socket *sock)
 {
 	struct ksock_connreq *cr;
 	int rc;
@@ -1025,12 +1026,12 @@ ksocknal_connecting(struct ksock_peer *peer, __u32 ipaddr)
 }
 
 int
-ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
+ksocknal_create_conn(struct lnet_ni *ni, struct ksock_route *route,
 		     struct socket *sock, int type)
 {
 	rwlock_t *global_lock = &ksocknal_data.ksnd_global_lock;
 	LIST_HEAD(zombies);
-	lnet_process_id_t peerid;
+	struct lnet_process_id peerid;
 	struct list_head *tmp;
 	__u64 incarnation;
 	struct ksock_conn *conn;
@@ -1038,7 +1039,7 @@ ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
 	struct ksock_peer *peer = NULL;
 	struct ksock_peer *peer2;
 	struct ksock_sched *sched;
-	ksock_hello_msg_t *hello;
+	struct ksock_hello_msg *hello;
 	int cpt;
 	struct ksock_tx *tx;
 	struct ksock_tx *txtmp;
@@ -1077,7 +1078,7 @@ ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
 	conn->ksnc_tx_carrier = NULL;
 	atomic_set(&conn->ksnc_tx_nob, 0);
 
-	LIBCFS_ALLOC(hello, offsetof(ksock_hello_msg_t,
+	LIBCFS_ALLOC(hello, offsetof(struct ksock_hello_msg,
 				     kshm_ips[LNET_MAX_INTERFACES]));
 	if (!hello) {
 		rc = -ENOMEM;
@@ -1341,7 +1342,7 @@ ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
 		rc = ksocknal_send_hello(ni, conn, peerid.nid, hello);
 	}
 
-	LIBCFS_FREE(hello, offsetof(ksock_hello_msg_t,
+	LIBCFS_FREE(hello, offsetof(struct ksock_hello_msg,
 				    kshm_ips[LNET_MAX_INTERFACES]));
 
 	/*
@@ -1423,7 +1424,7 @@ ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
 
 failed_1:
 	if (hello)
-		LIBCFS_FREE(hello, offsetof(ksock_hello_msg_t,
+		LIBCFS_FREE(hello, offsetof(struct ksock_hello_msg,
 					    kshm_ips[LNET_MAX_INTERFACES]));
 
 	LIBCFS_FREE(conn, sizeof(*conn));
@@ -1763,7 +1764,7 @@ ksocknal_close_conn_and_siblings(struct ksock_conn *conn, int why)
 }
 
 int
-ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr)
+ksocknal_close_matching_conns(struct lnet_process_id id, __u32 ipaddr)
 {
 	struct ksock_peer *peer;
 	struct list_head *ptmp;
@@ -1810,13 +1811,13 @@ ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr)
 }
 
 void
-ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
+ksocknal_notify(struct lnet_ni *ni, lnet_nid_t gw_nid, int alive)
 {
 	/*
 	 * The router is telling me she's been notified of a change in
 	 * gateway state....
 	 */
-	lnet_process_id_t id = {0};
+	struct lnet_process_id id = {0};
 
 	id.nid = gw_nid;
 	id.pid = LNET_PID_ANY;
@@ -1837,14 +1838,14 @@ ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive)
 }
 
 void
-ksocknal_query(lnet_ni_t *ni, lnet_nid_t nid, unsigned long *when)
+ksocknal_query(struct lnet_ni *ni, lnet_nid_t nid, unsigned long *when)
 {
 	int connect = 1;
 	unsigned long last_alive = 0;
 	unsigned long now = cfs_time_current();
 	struct ksock_peer *peer = NULL;
 	rwlock_t *glock = &ksocknal_data.ksnd_global_lock;
-	lnet_process_id_t id = {
+	struct lnet_process_id id = {
 		.nid = nid,
 		.pid = LNET_PID_LUSTRE,
 	};
@@ -1932,7 +1933,7 @@ ksocknal_push_peer(struct ksock_peer *peer)
 	}
 }
 
-static int ksocknal_push(lnet_ni_t *ni, lnet_process_id_t id)
+static int ksocknal_push(struct lnet_ni *ni, struct lnet_process_id id)
 {
 	struct list_head *start;
 	struct list_head *end;
@@ -1982,7 +1983,7 @@ static int ksocknal_push(lnet_ni_t *ni, lnet_process_id_t id)
 }
 
 static int
-ksocknal_add_interface(lnet_ni_t *ni, __u32 ipaddress, __u32 netmask)
+ksocknal_add_interface(struct lnet_ni *ni, __u32 ipaddress, __u32 netmask)
 {
 	struct ksock_net *net = ni->ni_data;
 	struct ksock_interface *iface;
@@ -2086,7 +2087,7 @@ ksocknal_peer_del_interface_locked(struct ksock_peer *peer, __u32 ipaddr)
 }
 
 static int
-ksocknal_del_interface(lnet_ni_t *ni, __u32 ipaddress)
+ksocknal_del_interface(struct lnet_ni *ni, __u32 ipaddress)
 {
 	struct ksock_net *net = ni->ni_data;
 	int rc = -ENOENT;
@@ -2132,9 +2133,9 @@ ksocknal_del_interface(lnet_ni_t *ni, __u32 ipaddress)
 }
 
 int
-ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
+ksocknal_ctl(struct lnet_ni *ni, unsigned int cmd, void *arg)
 {
-	lnet_process_id_t id = {0};
+	struct lnet_process_id id = {0};
 	struct libcfs_ioctl_data *data = arg;
 	int rc;
 
@@ -2534,7 +2535,7 @@ ksocknal_base_startup(void)
 }
 
 static void
-ksocknal_debug_peerhash(lnet_ni_t *ni)
+ksocknal_debug_peerhash(struct lnet_ni *ni)
 {
 	struct ksock_peer *peer = NULL;
 	struct list_head *tmp;
@@ -2587,11 +2588,11 @@ ksocknal_debug_peerhash(lnet_ni_t *ni)
 }
 
 void
-ksocknal_shutdown(lnet_ni_t *ni)
+ksocknal_shutdown(struct lnet_ni *ni)
 {
 	struct ksock_net *net = ni->ni_data;
 	int i;
-	lnet_process_id_t anyid = {0};
+	struct lnet_process_id anyid = {0};
 
 	anyid.nid = LNET_NID_ANY;
 	anyid.pid = LNET_PID_ANY;
@@ -2810,7 +2811,7 @@ ksocknal_net_start_threads(struct ksock_net *net, __u32 *cpts, int ncpts)
 }
 
 int
-ksocknal_startup(lnet_ni_t *ni)
+ksocknal_startup(struct lnet_ni *ni)
 {
 	struct ksock_net *net;
 	int rc;
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index 9e86563b8c70..5540de65f9a2 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -256,11 +256,11 @@ struct ksock_nal_data {
 /*
  * A packet just assembled for transmission is represented by 1 or more
  * struct iovec fragments (the first frag contains the portals header),
- * followed by 0 or more lnet_kiov_t fragments.
+ * followed by 0 or more struct bio_vec fragments.
  *
  * On the receive side, initially 1 struct iovec fragment is posted for
  * receive (the header).  Once the header has been received, the payload is
- * received into either struct iovec or lnet_kiov_t fragments, depending on
+ * received into either struct iovec or struct bio_vec fragments, depending on
  * what the header matched or whether the message needs forwarding.
  */
 struct ksock_conn;  /* forward ref */
@@ -282,17 +282,17 @@ struct ksock_tx {			   /* transmit packet */
 	unsigned short    tx_zc_capable:1; /* payload is large enough for ZC */
 	unsigned short    tx_zc_checked:1; /* Have I checked if I should ZC? */
 	unsigned short    tx_nonblk:1;     /* it's a non-blocking ACK */
-	lnet_kiov_t       *tx_kiov;        /* packet page frags */
+	struct bio_vec	  *tx_kiov;	   /* packet page frags */
 	struct ksock_conn *tx_conn;        /* owning conn */
-	lnet_msg_t        *tx_lnetmsg;     /* lnet message for lnet_finalize()
+	struct lnet_msg        *tx_lnetmsg;     /* lnet message for lnet_finalize()
 					    */
 	unsigned long     tx_deadline;     /* when (in jiffies) tx times out */
-	ksock_msg_t       tx_msg;          /* socklnd message buffer */
+	struct ksock_msg       tx_msg;          /* socklnd message buffer */
 	int               tx_desc_size;    /* size of this descriptor */
 	union {
 		struct {
 			struct kvec iov;     /* virt hdr */
-			lnet_kiov_t kiov[0]; /* paged payload */
+			struct bio_vec kiov[0]; /* paged payload */
 		} paged;
 		struct {
 			struct kvec iov[1];  /* virt hdr + payload */
@@ -310,7 +310,7 @@ struct ksock_tx {			   /* transmit packet */
  */
 union ksock_rxiovspace {
 	struct kvec      iov[LNET_MAX_IOV];
-	lnet_kiov_t      kiov[LNET_MAX_IOV];
+	struct bio_vec	kiov[LNET_MAX_IOV];
 };
 
 #define SOCKNAL_RX_KSM_HEADER   1 /* reading ksock message header */
@@ -362,14 +362,14 @@ struct ksock_conn {
 	int                ksnc_rx_niov;      /* # iovec frags */
 	struct kvec        *ksnc_rx_iov;      /* the iovec frags */
 	int                ksnc_rx_nkiov;     /* # page frags */
-	lnet_kiov_t        *ksnc_rx_kiov;     /* the page frags */
+	struct bio_vec		*ksnc_rx_kiov;	/* the page frags */
 	union ksock_rxiovspace ksnc_rx_iov_space; /* space for frag descriptors */
 	__u32              ksnc_rx_csum;      /* partial checksum for incoming
 					       * data
 					       */
 	void               *ksnc_cookie;      /* rx lnet_finalize passthru arg
 					       */
-	ksock_msg_t        ksnc_msg;          /* incoming message buffer:
+	struct ksock_msg        ksnc_msg;          /* incoming message buffer:
 					       * V2.x message takes the
 					       * whole struct
 					       * V1.x message is a bare
@@ -428,7 +428,7 @@ struct ksock_peer {
 	unsigned long      ksnp_last_alive;     /* when (in jiffies) I was last
 						 * alive
 						 */
-	lnet_process_id_t  ksnp_id;             /* who's on the other end(s) */
+	struct lnet_process_id  ksnp_id;	/* who's on the other end(s) */
 	atomic_t           ksnp_refcount;       /* # users */
 	int                ksnp_sharecount;     /* lconf usage counter */
 	int                ksnp_closing;        /* being closed */
@@ -447,7 +447,7 @@ struct ksock_peer {
 						 * ACK
 						 */
 	unsigned long      ksnp_send_keepalive; /* time to send keepalive */
-	lnet_ni_t          *ksnp_ni;            /* which network */
+	struct lnet_ni	   *ksnp_ni;		/* which network */
 	int                ksnp_n_passive_ips;  /* # of... */
 
 	/* preferred local interfaces */
@@ -456,7 +456,7 @@ struct ksock_peer {
 
 struct ksock_connreq {
 	struct list_head ksncr_list;  /* stash on ksnd_connd_connreqs */
-	lnet_ni_t        *ksncr_ni;   /* chosen NI */
+	struct lnet_ni	 *ksncr_ni;	/* chosen NI */
 	struct socket    *ksncr_sock; /* accepted socket */
 };
 
@@ -474,16 +474,16 @@ struct ksock_proto {
 	int        pro_version;
 
 	/* handshake function */
-	int        (*pro_send_hello)(struct ksock_conn *, ksock_hello_msg_t *);
+	int        (*pro_send_hello)(struct ksock_conn *, struct ksock_hello_msg *);
 
 	/* handshake function */
-	int        (*pro_recv_hello)(struct ksock_conn *, ksock_hello_msg_t *, int);
+	int        (*pro_recv_hello)(struct ksock_conn *, struct ksock_hello_msg *, int);
 
 	/* message pack */
 	void       (*pro_pack)(struct ksock_tx *);
 
 	/* message unpack */
-	void       (*pro_unpack)(ksock_msg_t *);
+	void       (*pro_unpack)(struct ksock_msg *);
 
 	/* queue tx on the connection */
 	struct ksock_tx *(*pro_queue_tx_msg)(struct ksock_conn *, struct ksock_tx *);
@@ -603,7 +603,7 @@ ksocknal_tx_addref(struct ksock_tx *tx)
 }
 
 void ksocknal_tx_prep(struct ksock_conn *, struct ksock_tx *tx);
-void ksocknal_tx_done(lnet_ni_t *ni, struct ksock_tx *tx);
+void ksocknal_tx_done(struct lnet_ni *ni, struct ksock_tx *tx);
 
 static inline void
 ksocknal_tx_decref(struct ksock_tx *tx)
@@ -647,19 +647,22 @@ ksocknal_peer_decref(struct ksock_peer *peer)
 		ksocknal_destroy_peer(peer);
 }
 
-int ksocknal_startup(lnet_ni_t *ni);
-void ksocknal_shutdown(lnet_ni_t *ni);
-int ksocknal_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg);
-int ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg);
-int ksocknal_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
+int ksocknal_startup(struct lnet_ni *ni);
+void ksocknal_shutdown(struct lnet_ni *ni);
+int ksocknal_ctl(struct lnet_ni *ni, unsigned int cmd, void *arg);
+int ksocknal_send(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg);
+int ksocknal_recv(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg,
 		  int delayed, struct iov_iter *to, unsigned int rlen);
-int ksocknal_accept(lnet_ni_t *ni, struct socket *sock);
-
-int ksocknal_add_peer(lnet_ni_t *ni, lnet_process_id_t id, __u32 ip, int port);
-struct ksock_peer *ksocknal_find_peer_locked(lnet_ni_t *ni, lnet_process_id_t id);
-struct ksock_peer *ksocknal_find_peer(lnet_ni_t *ni, lnet_process_id_t id);
+int ksocknal_accept(struct lnet_ni *ni, struct socket *sock);
+
+int ksocknal_add_peer(struct lnet_ni *ni, struct lnet_process_id id, __u32 ip,
+		      int port);
+struct ksock_peer *ksocknal_find_peer_locked(struct lnet_ni *ni,
+					     struct lnet_process_id id);
+struct ksock_peer *ksocknal_find_peer(struct lnet_ni *ni,
+				      struct lnet_process_id id);
 void ksocknal_peer_failed(struct ksock_peer *peer);
-int ksocknal_create_conn(lnet_ni_t *ni, struct ksock_route *route,
+int ksocknal_create_conn(struct lnet_ni *ni, struct ksock_route *route,
 			 struct socket *sock, int type);
 void ksocknal_close_conn_locked(struct ksock_conn *conn, int why);
 void ksocknal_terminate_conn(struct ksock_conn *conn);
@@ -667,19 +670,19 @@ void ksocknal_destroy_conn(struct ksock_conn *conn);
 int  ksocknal_close_peer_conns_locked(struct ksock_peer *peer,
 				      __u32 ipaddr, int why);
 int ksocknal_close_conn_and_siblings(struct ksock_conn *conn, int why);
-int ksocknal_close_matching_conns(lnet_process_id_t id, __u32 ipaddr);
+int ksocknal_close_matching_conns(struct lnet_process_id id, __u32 ipaddr);
 struct ksock_conn *ksocknal_find_conn_locked(struct ksock_peer *peer,
 					     struct ksock_tx *tx, int nonblk);
 
-int  ksocknal_launch_packet(lnet_ni_t *ni, struct ksock_tx *tx,
-			    lnet_process_id_t id);
+int  ksocknal_launch_packet(struct lnet_ni *ni, struct ksock_tx *tx,
+			    struct lnet_process_id id);
 struct ksock_tx *ksocknal_alloc_tx(int type, int size);
 void ksocknal_free_tx(struct ksock_tx *tx);
 struct ksock_tx *ksocknal_alloc_tx_noop(__u64 cookie, int nonblk);
 void ksocknal_next_tx_carrier(struct ksock_conn *conn);
 void ksocknal_queue_tx_locked(struct ksock_tx *tx, struct ksock_conn *conn);
-void ksocknal_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int error);
-void ksocknal_notify(lnet_ni_t *ni, lnet_nid_t gw_nid, int alive);
+void ksocknal_txlist_done(struct lnet_ni *ni, struct list_head *txlist, int error);
+void ksocknal_notify(struct lnet_ni *ni, lnet_nid_t gw_nid, int alive);
 void ksocknal_query(struct lnet_ni *ni, lnet_nid_t nid, unsigned long *when);
 int ksocknal_thread_start(int (*fn)(void *arg), void *arg, char *name);
 void ksocknal_thread_fini(void);
@@ -690,10 +693,11 @@ int ksocknal_new_packet(struct ksock_conn *conn, int skip);
 int ksocknal_scheduler(void *arg);
 int ksocknal_connd(void *arg);
 int ksocknal_reaper(void *arg);
-int ksocknal_send_hello(lnet_ni_t *ni, struct ksock_conn *conn,
-			lnet_nid_t peer_nid, ksock_hello_msg_t *hello);
-int ksocknal_recv_hello(lnet_ni_t *ni, struct ksock_conn *conn,
-			ksock_hello_msg_t *hello, lnet_process_id_t *id,
+int ksocknal_send_hello(struct lnet_ni *ni, struct ksock_conn *conn,
+			lnet_nid_t peer_nid, struct ksock_hello_msg *hello);
+int ksocknal_recv_hello(struct lnet_ni *ni, struct ksock_conn *conn,
+			struct ksock_hello_msg *hello,
+			struct lnet_process_id *id,
 			__u64 *incarnation);
 void ksocknal_read_callback(struct ksock_conn *conn);
 void ksocknal_write_callback(struct ksock_conn *conn);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index 4c9f92725a44..3ed3b08c122c 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -145,7 +145,7 @@ ksocknal_send_iov(struct ksock_conn *conn, struct ksock_tx *tx)
 static int
 ksocknal_send_kiov(struct ksock_conn *conn, struct ksock_tx *tx)
 {
-	lnet_kiov_t *kiov = tx->tx_kiov;
+	struct bio_vec *kiov = tx->tx_kiov;
 	int nob;
 	int rc;
 
@@ -298,7 +298,7 @@ ksocknal_recv_iov(struct ksock_conn *conn)
 static int
 ksocknal_recv_kiov(struct ksock_conn *conn)
 {
-	lnet_kiov_t *kiov = conn->ksnc_rx_kiov;
+	struct bio_vec *kiov = conn->ksnc_rx_kiov;
 	int nob;
 	int rc;
 
@@ -393,9 +393,9 @@ ksocknal_receive(struct ksock_conn *conn)
 }
 
 void
-ksocknal_tx_done(lnet_ni_t *ni, struct ksock_tx *tx)
+ksocknal_tx_done(struct lnet_ni *ni, struct ksock_tx *tx)
 {
-	lnet_msg_t *lnetmsg = tx->tx_lnetmsg;
+	struct lnet_msg *lnetmsg = tx->tx_lnetmsg;
 	int rc = (!tx->tx_resid && !tx->tx_zc_aborted) ? 0 : -EIO;
 
 	LASSERT(ni || tx->tx_conn);
@@ -412,7 +412,7 @@ ksocknal_tx_done(lnet_ni_t *ni, struct ksock_tx *tx)
 }
 
 void
-ksocknal_txlist_done(lnet_ni_t *ni, struct list_head *txlist, int error)
+ksocknal_txlist_done(struct lnet_ni *ni, struct list_head *txlist, int error)
 {
 	struct ksock_tx *tx;
 
@@ -695,7 +695,7 @@ void
 ksocknal_queue_tx_locked(struct ksock_tx *tx, struct ksock_conn *conn)
 {
 	struct ksock_sched *sched = conn->ksnc_scheduler;
-	ksock_msg_t *msg = &tx->tx_msg;
+	struct ksock_msg *msg = &tx->tx_msg;
 	struct ksock_tx *ztx = NULL;
 	int bufnob = 0;
 
@@ -845,7 +845,8 @@ ksocknal_find_connecting_route_locked(struct ksock_peer *peer)
 }
 
 int
-ksocknal_launch_packet(lnet_ni_t *ni, struct ksock_tx *tx, lnet_process_id_t id)
+ksocknal_launch_packet(struct lnet_ni *ni, struct ksock_tx *tx,
+		       struct lnet_process_id id)
 {
 	struct ksock_peer *peer;
 	struct ksock_conn *conn;
@@ -938,14 +939,14 @@ ksocknal_launch_packet(lnet_ni_t *ni, struct ksock_tx *tx, lnet_process_id_t id)
 }
 
 int
-ksocknal_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
+ksocknal_send(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg)
 {
 	int mpflag = 1;
 	int type = lntmsg->msg_type;
-	lnet_process_id_t target = lntmsg->msg_target;
+	struct lnet_process_id target = lntmsg->msg_target;
 	unsigned int payload_niov = lntmsg->msg_niov;
 	struct kvec *payload_iov = lntmsg->msg_iov;
-	lnet_kiov_t *payload_kiov = lntmsg->msg_kiov;
+	struct bio_vec *payload_kiov = lntmsg->msg_kiov;
 	unsigned int payload_offset = lntmsg->msg_offset;
 	unsigned int payload_nob = lntmsg->msg_len;
 	struct ksock_tx *tx;
@@ -1072,9 +1073,9 @@ ksocknal_new_packet(struct ksock_conn *conn, int nob_to_skip)
 			conn->ksnc_rx_iov = (struct kvec *)&conn->ksnc_rx_iov_space;
 			conn->ksnc_rx_iov[0].iov_base = &conn->ksnc_msg;
 
-			conn->ksnc_rx_nob_wanted = offsetof(ksock_msg_t, ksm_u);
-			conn->ksnc_rx_nob_left = offsetof(ksock_msg_t, ksm_u);
-			conn->ksnc_rx_iov[0].iov_len  = offsetof(ksock_msg_t, ksm_u);
+			conn->ksnc_rx_nob_wanted = offsetof(struct ksock_msg, ksm_u);
+			conn->ksnc_rx_nob_left = offsetof(struct ksock_msg, ksm_u);
+			conn->ksnc_rx_iov[0].iov_len = offsetof(struct ksock_msg, ksm_u);
 			break;
 
 		case KSOCK_PROTO_V1:
@@ -1132,7 +1133,7 @@ static int
 ksocknal_process_receive(struct ksock_conn *conn)
 {
 	struct lnet_hdr *lhdr;
-	lnet_process_id_t *id;
+	struct lnet_process_id *id;
 	int rc;
 
 	LASSERT(atomic_read(&conn->ksnc_conn_refcount) > 0);
@@ -1232,12 +1233,12 @@ ksocknal_process_receive(struct ksock_conn *conn)
 		}
 
 		conn->ksnc_rx_state = SOCKNAL_RX_LNET_HEADER;
-		conn->ksnc_rx_nob_wanted = sizeof(ksock_lnet_msg_t);
-		conn->ksnc_rx_nob_left = sizeof(ksock_lnet_msg_t);
+		conn->ksnc_rx_nob_wanted = sizeof(struct ksock_lnet_msg);
+		conn->ksnc_rx_nob_left = sizeof(struct ksock_lnet_msg);
 
 		conn->ksnc_rx_iov = (struct kvec *)&conn->ksnc_rx_iov_space;
 		conn->ksnc_rx_iov[0].iov_base = &conn->ksnc_msg.ksm_u.lnetmsg;
-		conn->ksnc_rx_iov[0].iov_len  = sizeof(ksock_lnet_msg_t);
+		conn->ksnc_rx_iov[0].iov_len = sizeof(struct ksock_lnet_msg);
 
 		conn->ksnc_rx_niov = 1;
 		conn->ksnc_rx_kiov = NULL;
@@ -1333,8 +1334,8 @@ ksocknal_process_receive(struct ksock_conn *conn)
 }
 
 int
-ksocknal_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
-	      struct iov_iter *to, unsigned int rlen)
+ksocknal_recv(struct lnet_ni *ni, void *private, struct lnet_msg *msg,
+	      int delayed, struct iov_iter *to, unsigned int rlen)
 {
 	struct ksock_conn *conn = private;
 	struct ksock_sched *sched = conn->ksnc_scheduler;
@@ -1633,7 +1634,7 @@ void ksocknal_write_callback(struct ksock_conn *conn)
 }
 
 static struct ksock_proto *
-ksocknal_parse_proto_version(ksock_hello_msg_t *hello)
+ksocknal_parse_proto_version(struct ksock_hello_msg *hello)
 {
 	__u32 version = 0;
 
@@ -1664,7 +1665,7 @@ ksocknal_parse_proto_version(ksock_hello_msg_t *hello)
 		struct lnet_magicversion *hmv = (struct lnet_magicversion *)hello;
 
 		BUILD_BUG_ON(sizeof(struct lnet_magicversion) !=
-			 offsetof(ksock_hello_msg_t, kshm_src_nid));
+			     offsetof(struct ksock_hello_msg, kshm_src_nid));
 
 		if (hmv->version_major == cpu_to_le16(KSOCK_PROTO_V1_MAJOR) &&
 		    hmv->version_minor == cpu_to_le16(KSOCK_PROTO_V1_MINOR))
@@ -1675,8 +1676,8 @@ ksocknal_parse_proto_version(ksock_hello_msg_t *hello)
 }
 
 int
-ksocknal_send_hello(lnet_ni_t *ni, struct ksock_conn *conn,
-		    lnet_nid_t peer_nid, ksock_hello_msg_t *hello)
+ksocknal_send_hello(struct lnet_ni *ni, struct ksock_conn *conn,
+		    lnet_nid_t peer_nid, struct ksock_hello_msg *hello)
 {
 	/* CAVEAT EMPTOR: this byte flips 'ipaddrs' */
 	struct ksock_net *net = (struct ksock_net *)ni->ni_data;
@@ -1713,8 +1714,9 @@ ksocknal_invert_type(int type)
 }
 
 int
-ksocknal_recv_hello(lnet_ni_t *ni, struct ksock_conn *conn,
-		    ksock_hello_msg_t *hello, lnet_process_id_t *peerid,
+ksocknal_recv_hello(struct lnet_ni *ni, struct ksock_conn *conn,
+		    struct ksock_hello_msg *hello,
+		    struct lnet_process_id *peerid,
 		    __u64 *incarnation)
 {
 	/* Return < 0	fatal error
@@ -1728,7 +1730,7 @@ ksocknal_recv_hello(lnet_ni_t *ni, struct ksock_conn *conn,
 	int proto_match;
 	int rc;
 	struct ksock_proto *proto;
-	lnet_process_id_t recv_id;
+	struct lnet_process_id recv_id;
 
 	/* socket type set on active connections - not set on passive */
 	LASSERT(!active == !(conn->ksnc_type != SOCKLND_CONN_NONE));
@@ -1904,14 +1906,14 @@ ksocknal_connect(struct ksock_route *route)
 		if (retry_later) /* needs reschedule */
 			break;
 
-		if (wanted & (1 << SOCKLND_CONN_ANY)) {
+		if (wanted & BIT(SOCKLND_CONN_ANY)) {
 			type = SOCKLND_CONN_ANY;
-		} else if (wanted & (1 << SOCKLND_CONN_CONTROL)) {
+		} else if (wanted & BIT(SOCKLND_CONN_CONTROL)) {
 			type = SOCKLND_CONN_CONTROL;
-		} else if (wanted & (1 << SOCKLND_CONN_BULK_IN)) {
+		} else if (wanted & BIT(SOCKLND_CONN_BULK_IN)) {
 			type = SOCKLND_CONN_BULK_IN;
 		} else {
-			LASSERT(wanted & (1 << SOCKLND_CONN_BULK_OUT));
+			LASSERT(wanted & BIT(SOCKLND_CONN_BULK_OUT));
 			type = SOCKLND_CONN_BULK_OUT;
 		}
 
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
index 4bcab4bcc2de..8a036f4eb8d8 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
@@ -99,7 +99,7 @@ int
 ksocknal_lib_send_kiov(struct ksock_conn *conn, struct ksock_tx *tx)
 {
 	struct socket *sock = conn->ksnc_sock;
-	lnet_kiov_t *kiov = tx->tx_kiov;
+	struct bio_vec *kiov = tx->tx_kiov;
 	int rc;
 	int nob;
 
@@ -215,7 +215,7 @@ int
 ksocknal_lib_recv_kiov(struct ksock_conn *conn)
 {
 	unsigned int niov = conn->ksnc_rx_nkiov;
-	lnet_kiov_t   *kiov = conn->ksnc_rx_kiov;
+	struct bio_vec *kiov = conn->ksnc_rx_kiov;
 	struct msghdr msg = {
 		.msg_flags = 0
 	};
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
index d367e74d46c2..84be9a518190 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
@@ -287,11 +287,11 @@ ksocknal_match_tx(struct ksock_conn *conn, struct ksock_tx *tx, int nonblk)
 
 	if (!tx || !tx->tx_lnetmsg) {
 		/* noop packet */
-		nob = offsetof(ksock_msg_t, ksm_u);
+		nob = offsetof(struct ksock_msg, ksm_u);
 	} else {
 		nob = tx->tx_lnetmsg->msg_len +
 		      ((conn->ksnc_proto == &ksocknal_protocol_v1x) ?
-		       sizeof(struct lnet_hdr) : sizeof(ksock_msg_t));
+		       sizeof(struct lnet_hdr) : sizeof(struct ksock_msg));
 	}
 
 	/* default checking for typed connection */
@@ -325,9 +325,9 @@ ksocknal_match_tx_v3(struct ksock_conn *conn, struct ksock_tx *tx, int nonblk)
 	int nob;
 
 	if (!tx || !tx->tx_lnetmsg)
-		nob = offsetof(ksock_msg_t, ksm_u);
+		nob = offsetof(struct ksock_msg, ksm_u);
 	else
-		nob = tx->tx_lnetmsg->msg_len + sizeof(ksock_msg_t);
+		nob = tx->tx_lnetmsg->msg_len + sizeof(struct ksock_msg);
 
 	switch (conn->ksnc_type) {
 	default:
@@ -456,7 +456,7 @@ ksocknal_handle_zcack(struct ksock_conn *conn, __u64 cookie1, __u64 cookie2)
 }
 
 static int
-ksocknal_send_hello_v1(struct ksock_conn *conn, ksock_hello_msg_t *hello)
+ksocknal_send_hello_v1(struct ksock_conn *conn, struct ksock_hello_msg *hello)
 {
 	struct socket *sock = conn->ksnc_sock;
 	struct lnet_hdr *hdr;
@@ -531,7 +531,7 @@ out:
 }
 
 static int
-ksocknal_send_hello_v2(struct ksock_conn *conn, ksock_hello_msg_t *hello)
+ksocknal_send_hello_v2(struct ksock_conn *conn, struct ksock_hello_msg *hello)
 {
 	struct socket *sock = conn->ksnc_sock;
 	int rc;
@@ -549,7 +549,7 @@ ksocknal_send_hello_v2(struct ksock_conn *conn, ksock_hello_msg_t *hello)
 		LNET_UNLOCK();
 	}
 
-	rc = lnet_sock_write(sock, hello, offsetof(ksock_hello_msg_t, kshm_ips),
+	rc = lnet_sock_write(sock, hello, offsetof(struct ksock_hello_msg, kshm_ips),
 			     lnet_acceptor_timeout());
 	if (rc) {
 		CNETERR("Error %d sending HELLO hdr to %pI4h/%d\n",
@@ -573,7 +573,7 @@ ksocknal_send_hello_v2(struct ksock_conn *conn, ksock_hello_msg_t *hello)
 }
 
 static int
-ksocknal_recv_hello_v1(struct ksock_conn *conn, ksock_hello_msg_t *hello,
+ksocknal_recv_hello_v1(struct ksock_conn *conn, struct ksock_hello_msg *hello,
 		       int timeout)
 {
 	struct socket *sock = conn->ksnc_sock;
@@ -649,7 +649,7 @@ out:
 }
 
 static int
-ksocknal_recv_hello_v2(struct ksock_conn *conn, ksock_hello_msg_t *hello,
+ksocknal_recv_hello_v2(struct ksock_conn *conn, struct ksock_hello_msg *hello,
 		       int timeout)
 {
 	struct socket *sock = conn->ksnc_sock;
@@ -662,8 +662,8 @@ ksocknal_recv_hello_v2(struct ksock_conn *conn, ksock_hello_msg_t *hello,
 		conn->ksnc_flip = 1;
 
 	rc = lnet_sock_read(sock, &hello->kshm_src_nid,
-			    offsetof(ksock_hello_msg_t, kshm_ips) -
-				     offsetof(ksock_hello_msg_t, kshm_src_nid),
+			    offsetof(struct ksock_hello_msg, kshm_ips) -
+				     offsetof(struct ksock_hello_msg, kshm_src_nid),
 			    timeout);
 	if (rc) {
 		CERROR("Error %d reading HELLO from %pI4h\n",
@@ -738,15 +738,15 @@ ksocknal_pack_msg_v2(struct ksock_tx *tx)
 		LASSERT(tx->tx_msg.ksm_type != KSOCK_MSG_NOOP);
 
 		tx->tx_msg.ksm_u.lnetmsg.ksnm_hdr = tx->tx_lnetmsg->msg_hdr;
-		tx->tx_iov[0].iov_len = sizeof(ksock_msg_t);
-		tx->tx_nob = sizeof(ksock_msg_t) + tx->tx_lnetmsg->msg_len;
-		tx->tx_resid = sizeof(ksock_msg_t) + tx->tx_lnetmsg->msg_len;
+		tx->tx_iov[0].iov_len = sizeof(struct ksock_msg);
+		tx->tx_nob = sizeof(struct ksock_msg) + tx->tx_lnetmsg->msg_len;
+		tx->tx_resid = sizeof(struct ksock_msg) + tx->tx_lnetmsg->msg_len;
 	} else {
 		LASSERT(tx->tx_msg.ksm_type == KSOCK_MSG_NOOP);
 
-		tx->tx_iov[0].iov_len = offsetof(ksock_msg_t, ksm_u.lnetmsg.ksnm_hdr);
-		tx->tx_nob = offsetof(ksock_msg_t,  ksm_u.lnetmsg.ksnm_hdr);
-		tx->tx_resid = offsetof(ksock_msg_t,  ksm_u.lnetmsg.ksnm_hdr);
+		tx->tx_iov[0].iov_len = offsetof(struct ksock_msg, ksm_u.lnetmsg.ksnm_hdr);
+		tx->tx_nob = offsetof(struct ksock_msg,  ksm_u.lnetmsg.ksnm_hdr);
+		tx->tx_resid = offsetof(struct ksock_msg,  ksm_u.lnetmsg.ksnm_hdr);
 	}
 	/*
 	 * Don't checksum before start sending, because packet can be
@@ -755,7 +755,7 @@ ksocknal_pack_msg_v2(struct ksock_tx *tx)
 }
 
 static void
-ksocknal_unpack_msg_v1(ksock_msg_t *msg)
+ksocknal_unpack_msg_v1(struct ksock_msg *msg)
 {
 	msg->ksm_csum = 0;
 	msg->ksm_type = KSOCK_MSG_LNET;
@@ -764,7 +764,7 @@ ksocknal_unpack_msg_v1(ksock_msg_t *msg)
 }
 
 static void
-ksocknal_unpack_msg_v2(ksock_msg_t *msg)
+ksocknal_unpack_msg_v2(struct ksock_msg *msg)
 {
 	return;  /* Do nothing */
 }
diff --git a/drivers/staging/lustre/lnet/libcfs/debug.c b/drivers/staging/lustre/lnet/libcfs/debug.c
index 3408041355e3..c56e9922cd5b 100644
--- a/drivers/staging/lustre/lnet/libcfs/debug.c
+++ b/drivers/staging/lustre/lnet/libcfs/debug.c
@@ -80,7 +80,7 @@ static int libcfs_param_debug_mb_set(const char *val,
  * it needs quite a bunch of extra processing, so we define special
  * debugmb parameter type with corresponding methods to handle this case
  */
-static struct kernel_param_ops param_ops_debugmb = {
+static const struct kernel_param_ops param_ops_debugmb = {
 	.set = libcfs_param_debug_mb_set,
 	.get = param_get_uint,
 };
@@ -138,7 +138,7 @@ static int param_set_console_max_delay(const char *val,
 				      libcfs_console_min_delay, INT_MAX);
 }
 
-static struct kernel_param_ops param_ops_console_max_delay = {
+static const struct kernel_param_ops param_ops_console_max_delay = {
 	.set = param_set_console_max_delay,
 	.get = param_get_delay,
 };
@@ -156,7 +156,7 @@ static int param_set_console_min_delay(const char *val,
 				      1, libcfs_console_max_delay);
 }
 
-static struct kernel_param_ops param_ops_console_min_delay = {
+static const struct kernel_param_ops param_ops_console_min_delay = {
 	.set = param_set_console_min_delay,
 	.get = param_get_delay,
 };
@@ -188,7 +188,7 @@ static int param_set_uintpos(const char *val, const struct kernel_param *kp)
 	return param_set_uint_minmax(val, kp, 1, -1);
 }
 
-static struct kernel_param_ops param_ops_uintpos = {
+static const struct kernel_param_ops param_ops_uintpos = {
 	.set = param_set_uintpos,
 	.get = param_get_uint,
 };
diff --git a/drivers/staging/lustre/lnet/libcfs/tracefile.c b/drivers/staging/lustre/lnet/libcfs/tracefile.c
index d7b29f8997c0..9599b7441feb 100644
--- a/drivers/staging/lustre/lnet/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lnet/libcfs/tracefile.c
@@ -37,6 +37,7 @@
 
 #define DEBUG_SUBSYSTEM S_LNET
 #define LUSTRE_TRACEFILE_PRIVATE
+#define pr_fmt(fmt) "Lustre: " fmt
 #include "tracefile.h"
 
 #include "../../include/linux/libcfs/libcfs.h"
@@ -192,9 +193,8 @@ cfs_trace_get_tage_try(struct cfs_trace_cpu_data *tcd, unsigned long len)
 			if (unlikely(!tage)) {
 				if ((!memory_pressure_get() ||
 				     in_interrupt()) && printk_ratelimit())
-					printk(KERN_WARNING
-					       "cannot allocate a tage (%ld)\n",
-					       tcd->tcd_cur_pages);
+					pr_warn("cannot allocate a tage (%ld)\n",
+						tcd->tcd_cur_pages);
 				return NULL;
 			}
 		}
@@ -230,8 +230,8 @@ static void cfs_tcd_shrink(struct cfs_trace_cpu_data *tcd)
 	 */
 
 	if (printk_ratelimit())
-		printk(KERN_WARNING "debug daemon buffer overflowed; discarding 10%% of pages (%d of %ld)\n",
-		       pgcount + 1, tcd->tcd_cur_pages);
+		pr_warn("debug daemon buffer overflowed; discarding 10%% of pages (%d of %ld)\n",
+			pgcount + 1, tcd->tcd_cur_pages);
 
 	INIT_LIST_HEAD(&pc.pc_pages);
 
@@ -358,8 +358,7 @@ int libcfs_debug_vmsg2(struct libcfs_debug_msg_data *msgdata,
 
 		max_nob = PAGE_SIZE - tage->used - known_size;
 		if (max_nob <= 0) {
-			printk(KERN_EMERG "negative max_nob: %d\n",
-			       max_nob);
+			pr_emerg("negative max_nob: %d\n", max_nob);
 			mask |= D_ERROR;
 			cfs_trace_put_tcd(tcd);
 			tcd = NULL;
@@ -389,8 +388,8 @@ int libcfs_debug_vmsg2(struct libcfs_debug_msg_data *msgdata,
 	}
 
 	if (*(string_buf + needed - 1) != '\n')
-		printk(KERN_INFO "format at %s:%d:%s doesn't end in newline\n",
-		       file, msgdata->msg_line, msgdata->msg_fn);
+		pr_info("format at %s:%d:%s doesn't end in newline\n", file,
+			msgdata->msg_line, msgdata->msg_fn);
 
 	header.ph_len = known_size + needed;
 	debug_buf = (char *)page_address(tage->page) + tage->used;
@@ -739,8 +738,8 @@ int cfs_tracefile_dump_all_pages(char *filename)
 		kunmap(tage->page);
 
 		if (rc != (int)tage->used) {
-			printk(KERN_WARNING "wanted to write %u but wrote %d\n",
-			       tage->used, rc);
+			pr_warn("wanted to write %u but wrote %d\n", tage->used,
+				rc);
 			put_pages_back(&pc);
 			__LASSERT(list_empty(&pc.pc_pages));
 			break;
@@ -894,10 +893,9 @@ int cfs_trace_daemon_command(char *str)
 	} else {
 		strcpy(cfs_tracefile, str);
 
-		printk(KERN_INFO
-		       "Lustre: debug daemon will attempt to start writing to %s (%lukB max)\n",
-		       cfs_tracefile,
-		       (long)(cfs_tracefile_size >> 10));
+		pr_info("debug daemon will attempt to start writing to %s (%lukB max)\n",
+			cfs_tracefile,
+			(long)(cfs_tracefile_size >> 10));
 
 		cfs_trace_start_thread();
 	}
@@ -933,16 +931,14 @@ int cfs_trace_set_debug_mb(int mb)
 	struct cfs_trace_cpu_data *tcd;
 
 	if (mb < num_possible_cpus()) {
-		printk(KERN_WARNING
-		       "Lustre: %d MB is too small for debug buffer size, setting it to %d MB.\n",
-		       mb, num_possible_cpus());
+		pr_warn("%d MB is too small for debug buffer size, setting it to %d MB.\n",
+			mb, num_possible_cpus());
 		mb = num_possible_cpus();
 	}
 
 	if (mb > limit) {
-		printk(KERN_WARNING
-		       "Lustre: %d MB is too large for debug buffer size, setting it to %d MB.\n",
-		       mb, limit);
+		pr_warn("%d MB is too large for debug buffer size, setting it to %d MB.\n",
+			mb, limit);
 		mb = limit;
 	}
 
@@ -1010,8 +1006,8 @@ static int tracefiled(void *arg)
 			if (IS_ERR(filp)) {
 				rc = PTR_ERR(filp);
 				filp = NULL;
-				printk(KERN_WARNING "couldn't open %s: %d\n",
-				       cfs_tracefile, rc);
+				pr_warn("couldn't open %s: %d\n", cfs_tracefile,
+					rc);
 			}
 		}
 		cfs_tracefile_read_unlock();
@@ -1039,8 +1035,8 @@ static int tracefiled(void *arg)
 			kunmap(tage->page);
 
 			if (rc != (int)tage->used) {
-				printk(KERN_WARNING "wanted to write %u but wrote %d\n",
-				       tage->used, rc);
+				pr_warn("wanted to write %u but wrote %d\n",
+					tage->used, rc);
 				put_pages_back(&pc);
 				__LASSERT(list_empty(&pc.pc_pages));
 				break;
@@ -1053,7 +1049,7 @@ static int tracefiled(void *arg)
 		if (!list_empty(&pc.pc_pages)) {
 			int i;
 
-			printk(KERN_ALERT "Lustre: trace pages aren't empty\n");
+			pr_alert("trace pages aren't empty\n");
 			pr_err("total cpus(%d): ", num_possible_cpus());
 			for (i = 0; i < num_possible_cpus(); i++)
 				if (cpu_online(i))
@@ -1123,8 +1119,7 @@ void cfs_trace_stop_thread(void)
 
 	mutex_lock(&cfs_trace_thread_mutex);
 	if (thread_running) {
-		printk(KERN_INFO
-		       "Lustre: shutting down debug daemon thread...\n");
+		pr_info("shutting down debug daemon thread...\n");
 		atomic_set(&tctl->tctl_shutdown, 1);
 		wait_for_completion(&tctl->tctl_stop);
 		thread_running = 0;
diff --git a/drivers/staging/lustre/lnet/lnet/acceptor.c b/drivers/staging/lustre/lnet/lnet/acceptor.c
index 69bbd594b9bd..a6f60c3e1184 100644
--- a/drivers/staging/lustre/lnet/lnet/acceptor.c
+++ b/drivers/staging/lustre/lnet/lnet/acceptor.c
@@ -211,7 +211,7 @@ lnet_accept(struct socket *sock, __u32 magic)
 	int peer_port;
 	int rc;
 	int flip;
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 	char *str;
 
 	LASSERT(sizeof(cr) <= 16);	     /* not too big for the stack */
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 08b38ef67784..0b91d1809cb1 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -39,7 +39,7 @@
 
 #define D_LNI D_CONSOLE
 
-lnet_t the_lnet;			   /* THE state of the network */
+struct lnet the_lnet;		/* THE state of the network */
 EXPORT_SYMBOL(the_lnet);
 
 static char *ip2nets = "";
@@ -58,8 +58,8 @@ static int rnet_htable_size = LNET_REMOTE_NETS_HASH_DEFAULT;
 module_param(rnet_htable_size, int, 0444);
 MODULE_PARM_DESC(rnet_htable_size, "size of remote network hash table");
 
-static int lnet_ping(lnet_process_id_t id, int timeout_ms,
-		     lnet_process_id_t __user *ids, int n_ids);
+static int lnet_ping(struct lnet_process_id id, int timeout_ms,
+		     struct lnet_process_id __user *ids, int n_ids);
 
 static char *
 lnet_get_routes(void)
@@ -265,15 +265,15 @@ static void lnet_assert_wire_constants(void)
 	BUILD_BUG_ON((int)sizeof(((struct lnet_hdr *)0)->msg.hello.type) != 4);
 }
 
-static lnd_t *
+static struct lnet_lnd *
 lnet_find_lnd_by_type(__u32 type)
 {
-	lnd_t *lnd;
+	struct lnet_lnd *lnd;
 	struct list_head *tmp;
 
 	/* holding lnd mutex */
 	list_for_each(tmp, &the_lnet.ln_lnds) {
-		lnd = list_entry(tmp, lnd_t, lnd_list);
+		lnd = list_entry(tmp, struct lnet_lnd, lnd_list);
 
 		if (lnd->lnd_type == type)
 			return lnd;
@@ -283,7 +283,7 @@ lnet_find_lnd_by_type(__u32 type)
 }
 
 void
-lnet_register_lnd(lnd_t *lnd)
+lnet_register_lnd(struct lnet_lnd *lnd)
 {
 	mutex_lock(&the_lnet.ln_lnd_mutex);
 
@@ -300,7 +300,7 @@ lnet_register_lnd(lnd_t *lnd)
 EXPORT_SYMBOL(lnet_register_lnd);
 
 void
-lnet_unregister_lnd(lnd_t *lnd)
+lnet_unregister_lnd(struct lnet_lnd *lnd)
 {
 	mutex_lock(&the_lnet.ln_lnd_mutex);
 
@@ -315,9 +315,9 @@ lnet_unregister_lnd(lnd_t *lnd)
 EXPORT_SYMBOL(lnet_unregister_lnd);
 
 void
-lnet_counters_get(lnet_counters_t *counters)
+lnet_counters_get(struct lnet_counters *counters)
 {
-	lnet_counters_t *ctr;
+	struct lnet_counters *ctr;
 	int i;
 
 	memset(counters, 0, sizeof(*counters));
@@ -344,13 +344,13 @@ EXPORT_SYMBOL(lnet_counters_get);
 void
 lnet_counters_reset(void)
 {
-	lnet_counters_t *counters;
+	struct lnet_counters *counters;
 	int i;
 
 	lnet_net_lock(LNET_LOCK_EX);
 
 	cfs_percpt_for_each(counters, i, the_lnet.ln_counters)
-		memset(counters, 0, sizeof(lnet_counters_t));
+		memset(counters, 0, sizeof(struct lnet_counters));
 
 	lnet_net_unlock(LNET_LOCK_EX);
 }
@@ -383,10 +383,10 @@ lnet_res_container_cleanup(struct lnet_res_container *rec)
 
 		list_del_init(e);
 		if (rec->rec_type == LNET_COOKIE_TYPE_EQ) {
-			lnet_eq_free(list_entry(e, lnet_eq_t, eq_list));
+			lnet_eq_free(list_entry(e, struct lnet_eq, eq_list));
 
 		} else if (rec->rec_type == LNET_COOKIE_TYPE_MD) {
-			lnet_md_free(list_entry(e, lnet_libmd_t, md_list));
+			lnet_md_free(list_entry(e, struct lnet_libmd, md_list));
 
 		} else { /* NB: Active MEs should be attached on portals */
 			LBUG();
@@ -483,12 +483,12 @@ lnet_res_containers_create(int type)
 	return recs;
 }
 
-lnet_libhandle_t *
+struct lnet_libhandle *
 lnet_res_lh_lookup(struct lnet_res_container *rec, __u64 cookie)
 {
 	/* ALWAYS called with lnet_res_lock held */
 	struct list_head *head;
-	lnet_libhandle_t *lh;
+	struct lnet_libhandle *lh;
 	unsigned int hash;
 
 	if ((cookie & LNET_COOKIE_MASK) != rec->rec_type)
@@ -506,7 +506,8 @@ lnet_res_lh_lookup(struct lnet_res_container *rec, __u64 cookie)
 }
 
 void
-lnet_res_lh_initialize(struct lnet_res_container *rec, lnet_libhandle_t *lh)
+lnet_res_lh_initialize(struct lnet_res_container *rec,
+		       struct lnet_libhandle *lh)
 {
 	/* ALWAYS called with lnet_res_lock held */
 	unsigned int ibits = LNET_COOKIE_TYPE_BITS + LNET_CPT_BITS;
@@ -559,7 +560,7 @@ lnet_prepare(lnet_pid_t requested_pid)
 	the_lnet.ln_interface_cookie = ktime_get_ns();
 
 	the_lnet.ln_counters = cfs_percpt_alloc(lnet_cpt_table(),
-						sizeof(lnet_counters_t));
+						sizeof(struct lnet_counters));
 	if (!the_lnet.ln_counters) {
 		CERROR("Failed to allocate counters for LNet\n");
 		rc = -ENOMEM;
@@ -652,16 +653,16 @@ lnet_unprepare(void)
 	return 0;
 }
 
-lnet_ni_t  *
+struct lnet_ni  *
 lnet_net2ni_locked(__u32 net, int cpt)
 {
 	struct list_head *tmp;
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 
 	LASSERT(cpt != LNET_LOCK_EX);
 
 	list_for_each(tmp, &the_lnet.ln_nis) {
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 
 		if (LNET_NIDNET(ni->ni_nid) == net) {
 			lnet_ni_addref_locked(ni, cpt);
@@ -672,10 +673,10 @@ lnet_net2ni_locked(__u32 net, int cpt)
 	return NULL;
 }
 
-lnet_ni_t *
+struct lnet_ni *
 lnet_net2ni(__u32 net)
 {
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 
 	lnet_net_lock(0);
 	ni = lnet_net2ni_locked(net, 0);
@@ -765,7 +766,7 @@ lnet_islocalnet(__u32 net)
 	return !!ni;
 }
 
-lnet_ni_t  *
+struct lnet_ni  *
 lnet_nid2ni_locked(lnet_nid_t nid, int cpt)
 {
 	struct lnet_ni *ni;
@@ -774,7 +775,7 @@ lnet_nid2ni_locked(lnet_nid_t nid, int cpt)
 	LASSERT(cpt != LNET_LOCK_EX);
 
 	list_for_each(tmp, &the_lnet.ln_nis) {
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 
 		if (ni->ni_nid == nid) {
 			lnet_ni_addref_locked(ni, cpt);
@@ -811,7 +812,7 @@ lnet_count_acceptor_nis(void)
 
 	cpt = lnet_net_lock_current();
 	list_for_each(tmp, &the_lnet.ln_nis) {
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 
 		if (ni->ni_lnd->lnd_accept)
 			count++;
@@ -887,7 +888,7 @@ lnet_ping_info_destroy(void)
 }
 
 static void
-lnet_ping_event_handler(lnet_event_t *event)
+lnet_ping_event_handler(struct lnet_event *event)
 {
 	struct lnet_ping_info *pinfo = event->md.user_ptr;
 
@@ -896,12 +897,13 @@ lnet_ping_event_handler(lnet_event_t *event)
 }
 
 static int
-lnet_ping_info_setup(struct lnet_ping_info **ppinfo, lnet_handle_md_t *md_handle,
+lnet_ping_info_setup(struct lnet_ping_info **ppinfo,
+		     struct lnet_handle_md *md_handle,
 		     int ni_count, bool set_eq)
 {
-	lnet_process_id_t id = {LNET_NID_ANY, LNET_PID_ANY};
-	lnet_handle_me_t me_handle;
-	lnet_md_t md = { NULL };
+	struct lnet_process_id id = {LNET_NID_ANY, LNET_PID_ANY};
+	struct lnet_handle_me me_handle;
+	struct lnet_md md = { NULL };
 	int rc, rc2;
 
 	if (set_eq) {
@@ -961,12 +963,13 @@ failed_0:
 }
 
 static void
-lnet_ping_md_unlink(struct lnet_ping_info *pinfo, lnet_handle_md_t *md_handle)
+lnet_ping_md_unlink(struct lnet_ping_info *pinfo,
+		    struct lnet_handle_md *md_handle)
 {
 	sigset_t blocked = cfs_block_allsigs();
 
 	LNetMDUnlink(*md_handle);
-	LNetInvalidateHandle(md_handle);
+	LNetInvalidateMDHandle(md_handle);
 
 	/* NB md could be busy; this just starts the unlink */
 	while (pinfo->pi_features != LNET_PING_FEAT_INVAL) {
@@ -982,7 +985,7 @@ static void
 lnet_ping_info_install_locked(struct lnet_ping_info *ping_info)
 {
 	struct lnet_ni_status *ns;
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 	int i = 0;
 
 	list_for_each_entry(ni, &the_lnet.ln_nis, ni_list) {
@@ -1003,10 +1006,11 @@ lnet_ping_info_install_locked(struct lnet_ping_info *ping_info)
 }
 
 static void
-lnet_ping_target_update(struct lnet_ping_info *pinfo, lnet_handle_md_t md_handle)
+lnet_ping_target_update(struct lnet_ping_info *pinfo,
+			struct lnet_handle_md md_handle)
 {
 	struct lnet_ping_info *old_pinfo = NULL;
-	lnet_handle_md_t old_md;
+	struct lnet_handle_md old_md;
 
 	/* switch the NIs to point to the new ping info created */
 	lnet_net_lock(LNET_LOCK_EX);
@@ -1046,7 +1050,7 @@ lnet_ping_target_fini(void)
 }
 
 static int
-lnet_ni_tq_credits(lnet_ni_t *ni)
+lnet_ni_tq_credits(struct lnet_ni *ni)
 {
 	int credits;
 
@@ -1063,7 +1067,7 @@ lnet_ni_tq_credits(lnet_ni_t *ni)
 }
 
 static void
-lnet_ni_unlink_locked(lnet_ni_t *ni)
+lnet_ni_unlink_locked(struct lnet_ni *ni)
 {
 	if (!list_empty(&ni->ni_cptlist)) {
 		list_del_init(&ni->ni_cptlist);
@@ -1081,8 +1085,8 @@ lnet_clear_zombies_nis_locked(void)
 {
 	int i;
 	int islo;
-	lnet_ni_t *ni;
-	lnet_ni_t *temp;
+	struct lnet_ni *ni;
+	struct lnet_ni *temp;
 
 	/*
 	 * Now wait for the NI's I just nuked to show up on ln_zombie_nis
@@ -1141,8 +1145,8 @@ lnet_clear_zombies_nis_locked(void)
 static void
 lnet_shutdown_lndnis(void)
 {
-	lnet_ni_t *ni;
-	lnet_ni_t *temp;
+	struct lnet_ni *ni;
+	struct lnet_ni *temp;
 	int i;
 
 	/* NB called holding the global mutex */
@@ -1170,7 +1174,7 @@ lnet_shutdown_lndnis(void)
 
 	/*
 	 * Clear lazy portals and drop delayed messages which hold refs
-	 * on their lnet_msg_t::msg_rxpeer
+	 * on their lnet_msg::msg_rxpeer
 	 */
 	for (i = 0; i < the_lnet.ln_nportals; i++)
 		LNetClearLazyPortal(i);
@@ -1216,7 +1220,7 @@ lnet_startup_lndni(struct lnet_ni *ni, struct lnet_ioctl_config_data *conf)
 	struct lnet_ioctl_config_lnd_tunables *lnd_tunables = NULL;
 	int rc = -EINVAL;
 	int lnd_type;
-	lnd_t *lnd;
+	struct lnet_lnd *lnd;
 	struct lnet_tx_queue *tq;
 	int i;
 
@@ -1376,7 +1380,7 @@ lnet_startup_lndnis(struct list_head *nilist)
 	int ni_count = 0;
 
 	while (!list_empty(nilist)) {
-		ni = list_entry(nilist->next, lnet_ni_t, ni_list);
+		ni = list_entry(nilist->next, struct lnet_ni, ni_list);
 		list_del(&ni->ni_list);
 		rc = lnet_startup_lndni(ni, NULL);
 
@@ -1433,7 +1437,7 @@ int lnet_lib_init(void)
 	}
 
 	the_lnet.ln_refcount = 0;
-	LNetInvalidateHandle(&the_lnet.ln_rc_eqh);
+	LNetInvalidateEQHandle(&the_lnet.ln_rc_eqh);
 	INIT_LIST_HEAD(&the_lnet.ln_lnds);
 	INIT_LIST_HEAD(&the_lnet.ln_rcd_zombie);
 	INIT_LIST_HEAD(&the_lnet.ln_rcd_deathrow);
@@ -1471,7 +1475,7 @@ void lnet_lib_exit(void)
 
 	while (!list_empty(&the_lnet.ln_lnds))
 		lnet_unregister_lnd(list_entry(the_lnet.ln_lnds.next,
-					       lnd_t, lnd_list));
+					       struct lnet_lnd, lnd_list));
 	lnet_destroy_locks();
 }
 
@@ -1497,7 +1501,7 @@ LNetNIInit(lnet_pid_t requested_pid)
 	int rc;
 	int ni_count;
 	struct lnet_ping_info *pinfo;
-	lnet_handle_md_t md_handle;
+	struct lnet_handle_md md_handle;
 	struct list_head net_head;
 
 	INIT_LIST_HEAD(&net_head);
@@ -1738,7 +1742,7 @@ lnet_get_net_config(struct lnet_ioctl_config_data *config)
 		if (i++ != idx)
 			continue;
 
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 		lnet_ni_lock(ni);
 		lnet_fill_ni_info(ni, config);
 		lnet_ni_unlock(ni);
@@ -1755,10 +1759,10 @@ lnet_dyn_add_ni(lnet_pid_t requested_pid, struct lnet_ioctl_config_data *conf)
 {
 	char *nets = conf->cfg_config_u.cfg_net.net_intf;
 	struct lnet_ping_info *pinfo;
-	lnet_handle_md_t md_handle;
+	struct lnet_handle_md md_handle;
 	struct lnet_ni *ni;
 	struct list_head net_head;
-	lnet_remotenet_t *rnet;
+	struct lnet_remotenet *rnet;
 	int rc;
 
 	INIT_LIST_HEAD(&net_head);
@@ -1833,9 +1837,9 @@ failed0:
 int
 lnet_dyn_del_ni(__u32 net)
 {
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 	struct lnet_ping_info *pinfo;
-	lnet_handle_md_t md_handle;
+	struct lnet_handle_md md_handle;
 	int rc;
 
 	/* don't allow userspace to shutdown the LOLND */
@@ -1883,8 +1887,8 @@ LNetCtl(unsigned int cmd, void *arg)
 {
 	struct libcfs_ioctl_data *data = arg;
 	struct lnet_ioctl_config_data *config;
-	lnet_process_id_t id = {0};
-	lnet_ni_t *ni;
+	struct lnet_process_id id = {0};
+	struct lnet_ni *ni;
 	int rc;
 	unsigned long secs_passed;
 
@@ -2055,7 +2059,7 @@ LNetCtl(unsigned int cmd, void *arg)
 		id.pid = data->ioc_u32[0];
 		rc = lnet_ping(id, data->ioc_u32[1], /* timeout */
 			       data->ioc_pbuf1,
-			       data->ioc_plen1 / sizeof(lnet_process_id_t));
+			       data->ioc_plen1 / sizeof(struct lnet_process_id));
 		if (rc < 0)
 			return rc;
 		data->ioc_count = rc;
@@ -2078,25 +2082,25 @@ LNetCtl(unsigned int cmd, void *arg)
 }
 EXPORT_SYMBOL(LNetCtl);
 
-void LNetDebugPeer(lnet_process_id_t id)
+void LNetDebugPeer(struct lnet_process_id id)
 {
 	lnet_debug_peer(id.nid);
 }
 EXPORT_SYMBOL(LNetDebugPeer);
 
 /**
- * Retrieve the lnet_process_id_t ID of LNet interface at \a index. Note that
+ * Retrieve the lnet_process_id ID of LNet interface at \a index. Note that
  * all interfaces share a same PID, as requested by LNetNIInit().
  *
  * \param index Index of the interface to look up.
  * \param id On successful return, this location will hold the
- * lnet_process_id_t ID of the interface.
+ * lnet_process_id ID of the interface.
  *
  * \retval 0 If an interface exists at \a index.
  * \retval -ENOENT If no interface has been found.
  */
 int
-LNetGetId(unsigned int index, lnet_process_id_t *id)
+LNetGetId(unsigned int index, struct lnet_process_id *id)
 {
 	struct lnet_ni *ni;
 	struct list_head *tmp;
@@ -2111,7 +2115,7 @@ LNetGetId(unsigned int index, lnet_process_id_t *id)
 		if (index--)
 			continue;
 
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 
 		id->nid = ni->ni_nid;
 		id->pid = the_lnet.ln_pid;
@@ -2124,31 +2128,20 @@ LNetGetId(unsigned int index, lnet_process_id_t *id)
 }
 EXPORT_SYMBOL(LNetGetId);
 
-/**
- * Print a string representation of handle \a h into buffer \a str of
- * \a len bytes.
- */
-void
-LNetSnprintHandle(char *str, int len, lnet_handle_any_t h)
-{
-	snprintf(str, len, "%#llx", h.cookie);
-}
-EXPORT_SYMBOL(LNetSnprintHandle);
-
-static int lnet_ping(lnet_process_id_t id, int timeout_ms,
-		     lnet_process_id_t __user *ids, int n_ids)
+static int lnet_ping(struct lnet_process_id id, int timeout_ms,
+		     struct lnet_process_id __user *ids, int n_ids)
 {
-	lnet_handle_eq_t eqh;
-	lnet_handle_md_t mdh;
-	lnet_event_t event;
-	lnet_md_t md = { NULL };
+	struct lnet_handle_eq eqh;
+	struct lnet_handle_md mdh;
+	struct lnet_event event;
+	struct lnet_md md = { NULL };
 	int which;
 	int unlinked = 0;
 	int replied = 0;
 	const int a_long_time = 60000; /* mS */
 	int infosz;
 	struct lnet_ping_info *info;
-	lnet_process_id_t tmpid;
+	struct lnet_process_id tmpid;
 	int i;
 	int nob;
 	int rc;
diff --git a/drivers/staging/lustre/lnet/lnet/config.c b/drivers/staging/lustre/lnet/lnet/config.c
index 9e2183ff847e..933988da7fe2 100644
--- a/drivers/staging/lustre/lnet/lnet/config.c
+++ b/drivers/staging/lustre/lnet/lnet/config.c
@@ -79,10 +79,10 @@ int
 lnet_net_unique(__u32 net, struct list_head *nilist)
 {
 	struct list_head *tmp;
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 
 	list_for_each(tmp, nilist) {
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 
 		if (LNET_NIDNET(ni->ni_nid) == net)
 			return 0;
@@ -120,7 +120,7 @@ lnet_ni_free(struct lnet_ni *ni)
 	LIBCFS_FREE(ni, sizeof(*ni));
 }
 
-lnet_ni_t *
+struct lnet_ni *
 lnet_ni_alloc(__u32 net, struct cfs_expr_list *el, struct list_head *nilist)
 {
 	struct lnet_tx_queue *tq;
@@ -390,7 +390,7 @@ lnet_parse_networks(struct list_head *nilist, char *networks)
 	lnet_syntax("networks", networks, (int)(tmp - tokens), strlen(tmp));
  failed:
 	while (!list_empty(nilist)) {
-		ni = list_entry(nilist->next, lnet_ni_t, ni_list);
+		ni = list_entry(nilist->next, struct lnet_ni, ni_list);
 
 		list_del(&ni->ni_list);
 		lnet_ni_free(ni);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index d05c6cc797f6..ce4b83584e17 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -64,9 +64,9 @@
  */
 int
 LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
-	    lnet_handle_eq_t *handle)
+	    struct lnet_handle_eq *handle)
 {
-	lnet_eq_t *eq;
+	struct lnet_eq *eq;
 
 	LASSERT(the_lnet.ln_refcount > 0);
 
@@ -93,7 +93,7 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 		return -ENOMEM;
 
 	if (count) {
-		LIBCFS_ALLOC(eq->eq_events, count * sizeof(lnet_event_t));
+		LIBCFS_ALLOC(eq->eq_events, count * sizeof(struct lnet_event));
 		if (!eq->eq_events)
 			goto failed;
 		/*
@@ -131,7 +131,7 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 
 failed:
 	if (eq->eq_events)
-		LIBCFS_FREE(eq->eq_events, count * sizeof(lnet_event_t));
+		LIBCFS_FREE(eq->eq_events, count * sizeof(struct lnet_event));
 
 	if (eq->eq_refs)
 		cfs_percpt_free(eq->eq_refs);
@@ -152,10 +152,10 @@ EXPORT_SYMBOL(LNetEQAlloc);
  * \retval -EBUSY  If the EQ is still in use by some MDs.
  */
 int
-LNetEQFree(lnet_handle_eq_t eqh)
+LNetEQFree(struct lnet_handle_eq eqh)
 {
 	struct lnet_eq *eq;
-	lnet_event_t *events = NULL;
+	struct lnet_event *events = NULL;
 	int **refs = NULL;
 	int *ref;
 	int rc = 0;
@@ -201,7 +201,7 @@ LNetEQFree(lnet_handle_eq_t eqh)
 	lnet_res_unlock(LNET_LOCK_EX);
 
 	if (events)
-		LIBCFS_FREE(events, size * sizeof(lnet_event_t));
+		LIBCFS_FREE(events, size * sizeof(struct lnet_event));
 	if (refs)
 		cfs_percpt_free(refs);
 
@@ -210,7 +210,7 @@ LNetEQFree(lnet_handle_eq_t eqh)
 EXPORT_SYMBOL(LNetEQFree);
 
 void
-lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev)
+lnet_eq_enqueue_event(struct lnet_eq *eq, struct lnet_event *ev)
 {
 	/* MUST called with resource lock hold but w/o lnet_eq_wait_lock */
 	int index;
@@ -239,10 +239,10 @@ lnet_eq_enqueue_event(lnet_eq_t *eq, lnet_event_t *ev)
 }
 
 static int
-lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
+lnet_eq_dequeue_event(struct lnet_eq *eq, struct lnet_event *ev)
 {
 	int new_index = eq->eq_deq_seq & (eq->eq_size - 1);
-	lnet_event_t *new_event = &eq->eq_events[new_index];
+	struct lnet_event *new_event = &eq->eq_events[new_index];
 	int rc;
 
 	/* must called with lnet_eq_wait_lock hold */
@@ -370,8 +370,8 @@ __must_hold(&the_lnet.ln_eq_wait_lock)
  * \retval -ENOENT    If there's an invalid handle in \a eventqs.
  */
 int
-LNetEQPoll(lnet_handle_eq_t *eventqs, int neq, int timeout_ms,
-	   lnet_event_t *event, int *which)
+LNetEQPoll(struct lnet_handle_eq *eventqs, int neq, int timeout_ms,
+	   struct lnet_event *event, int *which)
 {
 	int wait = 1;
 	int rc;
@@ -386,7 +386,7 @@ LNetEQPoll(lnet_handle_eq_t *eventqs, int neq, int timeout_ms,
 
 	for (;;) {
 		for (i = 0; i < neq; i++) {
-			lnet_eq_t *eq = lnet_handle2eq(&eventqs[i]);
+			struct lnet_eq *eq = lnet_handle2eq(&eventqs[i]);
 
 			if (!eq) {
 				lnet_eq_wait_unlock();
diff --git a/drivers/staging/lustre/lnet/lnet/lib-md.c b/drivers/staging/lustre/lnet/lnet/lib-md.c
index eab53cd57296..f08e944f412b 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-md.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-md.c
@@ -40,11 +40,11 @@
 
 /* must be called with lnet_res_lock held */
 void
-lnet_md_unlink(lnet_libmd_t *md)
+lnet_md_unlink(struct lnet_libmd *md)
 {
 	if (!(md->md_flags & LNET_MD_FLAG_ZOMBIE)) {
 		/* first unlink attempt... */
-		lnet_me_t *me = md->md_me;
+		struct lnet_me *me = md->md_me;
 
 		md->md_flags |= LNET_MD_FLAG_ZOMBIE;
 
@@ -84,7 +84,7 @@ lnet_md_unlink(lnet_libmd_t *md)
 }
 
 static int
-lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
+lnet_md_build(struct lnet_libmd *lmd, struct lnet_md *umd, int unlink)
 {
 	int i;
 	unsigned int niov;
@@ -165,7 +165,7 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 /* must be called with resource lock held */
 static int
-lnet_md_link(lnet_libmd_t *md, lnet_handle_eq_t eq_handle, int cpt)
+lnet_md_link(struct lnet_libmd *md, struct lnet_handle_eq eq_handle, int cpt)
 {
 	struct lnet_res_container *container = the_lnet.ln_md_containers[cpt];
 
@@ -185,7 +185,7 @@ lnet_md_link(lnet_libmd_t *md, lnet_handle_eq_t eq_handle, int cpt)
 	 * maybe there we shouldn't even allow LNET_EQ_NONE!)
 	 * LASSERT(!eq);
 	 */
-	if (!LNetHandleIsInvalid(eq_handle)) {
+	if (!LNetEQHandleIsInvalid(eq_handle)) {
 		md->md_eq = lnet_handle2eq(&eq_handle);
 
 		if (!md->md_eq)
@@ -204,7 +204,7 @@ lnet_md_link(lnet_libmd_t *md, lnet_handle_eq_t eq_handle, int cpt)
 
 /* must be called with lnet_res_lock held */
 void
-lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd)
+lnet_md_deconstruct(struct lnet_libmd *lmd, struct lnet_md *umd)
 {
 	/* NB this doesn't copy out all the iov entries so when a
 	 * discontiguous MD is copied out, the target gets to know the
@@ -223,7 +223,7 @@ lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd)
 }
 
 static int
-lnet_md_validate(lnet_md_t *umd)
+lnet_md_validate(struct lnet_md *umd)
 {
 	if (!umd->start && umd->length) {
 		CERROR("MD start pointer can not be NULL with length %u\n",
@@ -267,8 +267,8 @@ lnet_md_validate(lnet_md_t *umd)
  * a MD.
  */
 int
-LNetMDAttach(lnet_handle_me_t meh, lnet_md_t umd,
-	     lnet_unlink_t unlink, lnet_handle_md_t *handle)
+LNetMDAttach(struct lnet_handle_me meh, struct lnet_md umd,
+	     enum lnet_unlink unlink, struct lnet_handle_md *handle)
 {
 	LIST_HEAD(matches);
 	LIST_HEAD(drops);
@@ -350,9 +350,10 @@ EXPORT_SYMBOL(LNetMDAttach);
  * LNetInvalidateHandle() on it.
  */
 int
-LNetMDBind(lnet_md_t umd, lnet_unlink_t unlink, lnet_handle_md_t *handle)
+LNetMDBind(struct lnet_md umd, enum lnet_unlink unlink,
+	   struct lnet_handle_md *handle)
 {
-	lnet_libmd_t *md;
+	struct lnet_libmd *md;
 	int cpt;
 	int rc;
 
@@ -425,10 +426,10 @@ EXPORT_SYMBOL(LNetMDBind);
  * \retval -ENOENT If \a mdh does not point to a valid MD object.
  */
 int
-LNetMDUnlink(lnet_handle_md_t mdh)
+LNetMDUnlink(struct lnet_handle_md mdh)
 {
-	lnet_event_t ev;
-	lnet_libmd_t *md;
+	struct lnet_event ev;
+	struct lnet_libmd *md;
 	int cpt;
 
 	LASSERT(the_lnet.ln_refcount > 0);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-me.c b/drivers/staging/lustre/lnet/lnet/lib-me.c
index eb796a86e6ab..e9b3eedca4db 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-me.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-me.c
@@ -46,7 +46,7 @@
  * \param portal The portal table index where the ME should be attached.
  * \param match_id Specifies the match criteria for the process ID of
  * the requester. The constants LNET_PID_ANY and LNET_NID_ANY can be
- * used to wildcard either of the identifiers in the lnet_process_id_t
+ * used to wildcard either of the identifiers in the lnet_process_id
  * structure.
  * \param match_bits,ignore_bits Specify the match criteria to apply
  * to the match bits in the incoming request. The ignore bits are used
@@ -70,10 +70,10 @@
  */
 int
 LNetMEAttach(unsigned int portal,
-	     lnet_process_id_t match_id,
+	     struct lnet_process_id match_id,
 	     __u64 match_bits, __u64 ignore_bits,
-	     lnet_unlink_t unlink, lnet_ins_pos_t pos,
-	     lnet_handle_me_t *handle)
+	     enum lnet_unlink unlink, enum lnet_ins_pos pos,
+	     struct lnet_handle_me *handle)
 {
 	struct lnet_match_table *mtable;
 	struct lnet_me *me;
@@ -140,11 +140,11 @@ EXPORT_SYMBOL(LNetMEAttach);
  * \retval -ENOENT If \a current_meh does not point to a valid match entry.
  */
 int
-LNetMEInsert(lnet_handle_me_t current_meh,
-	     lnet_process_id_t match_id,
+LNetMEInsert(struct lnet_handle_me current_meh,
+	     struct lnet_process_id match_id,
 	     __u64 match_bits, __u64 ignore_bits,
-	     lnet_unlink_t unlink, lnet_ins_pos_t pos,
-	     lnet_handle_me_t *handle)
+	     enum lnet_unlink unlink, enum lnet_ins_pos pos,
+	     struct lnet_handle_me *handle)
 {
 	struct lnet_me *current_me;
 	struct lnet_me *new_me;
@@ -220,11 +220,11 @@ EXPORT_SYMBOL(LNetMEInsert);
  * \see LNetMDUnlink() for the discussion on delivering unlink event.
  */
 int
-LNetMEUnlink(lnet_handle_me_t meh)
+LNetMEUnlink(struct lnet_handle_me meh)
 {
-	lnet_me_t *me;
-	lnet_libmd_t *md;
-	lnet_event_t ev;
+	struct lnet_me *me;
+	struct lnet_libmd *md;
+	struct lnet_event ev;
 	int cpt;
 
 	LASSERT(the_lnet.ln_refcount > 0);
@@ -256,12 +256,12 @@ EXPORT_SYMBOL(LNetMEUnlink);
 
 /* call with lnet_res_lock please */
 void
-lnet_me_unlink(lnet_me_t *me)
+lnet_me_unlink(struct lnet_me *me)
 {
 	list_del(&me->me_list);
 
 	if (me->me_md) {
-		lnet_libmd_t *md = me->me_md;
+		struct lnet_libmd *md = me->me_md;
 
 		/* detach MD from portal of this ME */
 		lnet_ptl_detach_md(me, md);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index 6b0be6c23fff..a99c5c0aa672 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -47,8 +47,8 @@ MODULE_PARM_DESC(local_nid_dist_zero, "Reserved");
 int
 lnet_fail_nid(lnet_nid_t nid, unsigned int threshold)
 {
-	lnet_test_peer_t *tp;
-	lnet_test_peer_t *temp;
+	struct lnet_test_peer *tp;
+	struct lnet_test_peer *temp;
 	struct list_head *el;
 	struct list_head *next;
 	struct list_head cull;
@@ -75,7 +75,7 @@ lnet_fail_nid(lnet_nid_t nid, unsigned int threshold)
 	lnet_net_lock(0);
 
 	list_for_each_safe(el, next, &the_lnet.ln_test_peers) {
-		tp = list_entry(el, lnet_test_peer_t, tp_list);
+		tp = list_entry(el, struct lnet_test_peer, tp_list);
 
 		if (!tp->tp_threshold ||    /* needs culling anyway */
 		    nid == LNET_NID_ANY ||       /* removing all entries */
@@ -97,8 +97,8 @@ lnet_fail_nid(lnet_nid_t nid, unsigned int threshold)
 static int
 fail_peer(lnet_nid_t nid, int outgoing)
 {
-	lnet_test_peer_t *tp;
-	lnet_test_peer_t *temp;
+	struct lnet_test_peer *tp;
+	struct lnet_test_peer *temp;
 	struct list_head *el;
 	struct list_head *next;
 	struct list_head cull;
@@ -110,7 +110,7 @@ fail_peer(lnet_nid_t nid, int outgoing)
 	lnet_net_lock(0);
 
 	list_for_each_safe(el, next, &the_lnet.ln_test_peers) {
-		tp = list_entry(el, lnet_test_peer_t, tp_list);
+		tp = list_entry(el, struct lnet_test_peer, tp_list);
 
 		if (!tp->tp_threshold) {
 			/* zombie entry */
@@ -212,7 +212,7 @@ EXPORT_SYMBOL(lnet_copy_iov2iter);
 
 void
 lnet_copy_kiov2iter(struct iov_iter *to,
-		    unsigned int nsiov, const lnet_kiov_t *siov,
+		    unsigned int nsiov, const struct bio_vec *siov,
 		    unsigned int soffset, unsigned int nob)
 {
 	if (!nob)
@@ -298,7 +298,7 @@ lnet_extract_iov(int dst_niov, struct kvec *dst,
 EXPORT_SYMBOL(lnet_extract_iov);
 
 unsigned int
-lnet_kiov_nob(unsigned int niov, lnet_kiov_t *kiov)
+lnet_kiov_nob(unsigned int niov, struct bio_vec *kiov)
 {
 	unsigned int nob = 0;
 
@@ -311,8 +311,8 @@ lnet_kiov_nob(unsigned int niov, lnet_kiov_t *kiov)
 EXPORT_SYMBOL(lnet_kiov_nob);
 
 int
-lnet_extract_kiov(int dst_niov, lnet_kiov_t *dst,
-		  int src_niov, const lnet_kiov_t *src,
+lnet_extract_kiov(int dst_niov, struct bio_vec *dst,
+		  int src_niov, const struct bio_vec *src,
 		  unsigned int offset, unsigned int len)
 {
 	/*
@@ -364,12 +364,13 @@ lnet_extract_kiov(int dst_niov, lnet_kiov_t *dst,
 EXPORT_SYMBOL(lnet_extract_kiov);
 
 void
-lnet_ni_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
-	     unsigned int offset, unsigned int mlen, unsigned int rlen)
+lnet_ni_recv(struct lnet_ni *ni, void *private, struct lnet_msg *msg,
+	     int delayed, unsigned int offset, unsigned int mlen,
+	     unsigned int rlen)
 {
 	unsigned int niov = 0;
 	struct kvec *iov = NULL;
-	lnet_kiov_t *kiov = NULL;
+	struct bio_vec *kiov = NULL;
 	struct iov_iter to;
 	int rc;
 
@@ -409,9 +410,9 @@ lnet_ni_recv(lnet_ni_t *ni, void *private, lnet_msg_t *msg, int delayed,
 }
 
 static void
-lnet_setpayloadbuffer(lnet_msg_t *msg)
+lnet_setpayloadbuffer(struct lnet_msg *msg)
 {
-	lnet_libmd_t *md = msg->msg_md;
+	struct lnet_libmd *md = msg->msg_md;
 
 	LASSERT(msg->msg_len > 0);
 	LASSERT(!msg->msg_routing);
@@ -428,7 +429,7 @@ lnet_setpayloadbuffer(lnet_msg_t *msg)
 }
 
 void
-lnet_prep_send(lnet_msg_t *msg, int type, lnet_process_id_t target,
+lnet_prep_send(struct lnet_msg *msg, int type, struct lnet_process_id target,
 	       unsigned int offset, unsigned int len)
 {
 	msg->msg_type = type;
@@ -449,7 +450,7 @@ lnet_prep_send(lnet_msg_t *msg, int type, lnet_process_id_t target,
 }
 
 static void
-lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_ni_send(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	void *priv = msg->msg_private;
 	int rc;
@@ -464,7 +465,7 @@ lnet_ni_send(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 static int
-lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_ni_eager_recv(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	int rc;
 
@@ -488,7 +489,7 @@ lnet_ni_eager_recv(lnet_ni_t *ni, lnet_msg_t *msg)
 
 /* NB: caller shall hold a ref on 'lp' as I'd drop lnet_net_lock */
 static void
-lnet_ni_query_locked(lnet_ni_t *ni, lnet_peer_t *lp)
+lnet_ni_query_locked(struct lnet_ni *ni, struct lnet_peer *lp)
 {
 	unsigned long last_alive = 0;
 
@@ -507,7 +508,7 @@ lnet_ni_query_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 
 /* NB: always called with lnet_net_lock held */
 static inline int
-lnet_peer_is_alive(lnet_peer_t *lp, unsigned long now)
+lnet_peer_is_alive(struct lnet_peer *lp, unsigned long now)
 {
 	int alive;
 	unsigned long deadline;
@@ -541,7 +542,7 @@ lnet_peer_is_alive(lnet_peer_t *lp, unsigned long now)
  *     may drop the lnet_net_lock
  */
 static int
-lnet_peer_alive_locked(lnet_peer_t *lp)
+lnet_peer_alive_locked(struct lnet_peer *lp)
 {
 	unsigned long now = cfs_time_current();
 
@@ -595,10 +596,10 @@ lnet_peer_alive_locked(lnet_peer_t *lp)
  * \retval -ECANCELED If the MD of the message has been unlinked.
  */
 static int
-lnet_post_send_locked(lnet_msg_t *msg, int do_send)
+lnet_post_send_locked(struct lnet_msg *msg, int do_send)
 {
-	lnet_peer_t *lp = msg->msg_txpeer;
-	lnet_ni_t *ni = lp->lp_ni;
+	struct lnet_peer *lp = msg->msg_txpeer;
+	struct lnet_ni *ni = lp->lp_ni;
 	int cpt = msg->msg_tx_cpt;
 	struct lnet_tx_queue *tq = ni->ni_tx_queues[cpt];
 
@@ -679,10 +680,10 @@ lnet_post_send_locked(lnet_msg_t *msg, int do_send)
 	return LNET_CREDIT_OK;
 }
 
-static lnet_rtrbufpool_t *
-lnet_msg2bufpool(lnet_msg_t *msg)
+static struct lnet_rtrbufpool *
+lnet_msg2bufpool(struct lnet_msg *msg)
 {
-	lnet_rtrbufpool_t *rbp;
+	struct lnet_rtrbufpool *rbp;
 	int cpt;
 
 	LASSERT(msg->msg_rx_committed);
@@ -700,7 +701,7 @@ lnet_msg2bufpool(lnet_msg_t *msg)
 }
 
 static int
-lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
+lnet_post_routed_recv_locked(struct lnet_msg *msg, int do_recv)
 {
 	/*
 	 * lnet_parse is going to lnet_net_unlock immediately after this, so it
@@ -708,9 +709,9 @@ lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
 	 * I return LNET_CREDIT_WAIT if msg blocked and LNET_CREDIT_OK if
 	 * received or OK to receive
 	 */
-	lnet_peer_t *lp = msg->msg_rxpeer;
-	lnet_rtrbufpool_t *rbp;
-	lnet_rtrbuf_t *rb;
+	struct lnet_peer *lp = msg->msg_rxpeer;
+	struct lnet_rtrbufpool *rbp;
+	struct lnet_rtrbuf *rb;
 
 	LASSERT(!msg->msg_iov);
 	LASSERT(!msg->msg_kiov);
@@ -758,7 +759,7 @@ lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
 	}
 
 	LASSERT(!list_empty(&rbp->rbp_bufs));
-	rb = list_entry(rbp->rbp_bufs.next, lnet_rtrbuf_t, rb_list);
+	rb = list_entry(rbp->rbp_bufs.next, struct lnet_rtrbuf, rb_list);
 	list_del(&rb->rb_list);
 
 	msg->msg_niov = rbp->rbp_npages;
@@ -776,10 +777,10 @@ lnet_post_routed_recv_locked(lnet_msg_t *msg, int do_recv)
 }
 
 void
-lnet_return_tx_credits_locked(lnet_msg_t *msg)
+lnet_return_tx_credits_locked(struct lnet_msg *msg)
 {
-	lnet_peer_t *txpeer = msg->msg_txpeer;
-	lnet_msg_t *msg2;
+	struct lnet_peer *txpeer = msg->msg_txpeer;
+	struct lnet_msg *msg2;
 
 	if (msg->msg_txcredit) {
 		struct lnet_ni *ni = txpeer->lp_ni;
@@ -794,7 +795,7 @@ lnet_return_tx_credits_locked(lnet_msg_t *msg)
 		tq->tq_credits++;
 		if (tq->tq_credits <= 0) {
 			msg2 = list_entry(tq->tq_delayed.next,
-					  lnet_msg_t, msg_list);
+					  struct lnet_msg, msg_list);
 			list_del(&msg2->msg_list);
 
 			LASSERT(msg2->msg_txpeer->lp_ni == ni);
@@ -817,7 +818,7 @@ lnet_return_tx_credits_locked(lnet_msg_t *msg)
 		txpeer->lp_txcredits++;
 		if (txpeer->lp_txcredits <= 0) {
 			msg2 = list_entry(txpeer->lp_txq.next,
-					  lnet_msg_t, msg_list);
+					  struct lnet_msg, msg_list);
 			list_del(&msg2->msg_list);
 
 			LASSERT(msg2->msg_txpeer == txpeer);
@@ -834,14 +835,14 @@ lnet_return_tx_credits_locked(lnet_msg_t *msg)
 }
 
 void
-lnet_schedule_blocked_locked(lnet_rtrbufpool_t *rbp)
+lnet_schedule_blocked_locked(struct lnet_rtrbufpool *rbp)
 {
-	lnet_msg_t *msg;
+	struct lnet_msg *msg;
 
 	if (list_empty(&rbp->rbp_msgs))
 		return;
 	msg = list_entry(rbp->rbp_msgs.next,
-			 lnet_msg_t, msg_list);
+			 struct lnet_msg, msg_list);
 	list_del(&msg->msg_list);
 
 	(void)lnet_post_routed_recv_locked(msg, 1);
@@ -851,8 +852,8 @@ void
 lnet_drop_routed_msgs_locked(struct list_head *list, int cpt)
 {
 	struct list_head drop;
-	lnet_msg_t *msg;
-	lnet_msg_t *tmp;
+	struct lnet_msg *msg;
+	struct lnet_msg *tmp;
 
 	INIT_LIST_HEAD(&drop);
 
@@ -871,15 +872,15 @@ lnet_drop_routed_msgs_locked(struct list_head *list, int cpt)
 }
 
 void
-lnet_return_rx_credits_locked(lnet_msg_t *msg)
+lnet_return_rx_credits_locked(struct lnet_msg *msg)
 {
-	lnet_peer_t *rxpeer = msg->msg_rxpeer;
-	lnet_msg_t *msg2;
+	struct lnet_peer *rxpeer = msg->msg_rxpeer;
+	struct lnet_msg *msg2;
 
 	if (msg->msg_rtrcredit) {
 		/* give back global router credits */
-		lnet_rtrbuf_t *rb;
-		lnet_rtrbufpool_t *rbp;
+		struct lnet_rtrbuf *rb;
+		struct lnet_rtrbufpool *rbp;
 
 		/*
 		 * NB If a msg ever blocks for a buffer in rbp_msgs, it stays
@@ -888,7 +889,7 @@ lnet_return_rx_credits_locked(lnet_msg_t *msg)
 		 */
 		LASSERT(msg->msg_kiov);
 
-		rb = list_entry(msg->msg_kiov, lnet_rtrbuf_t, rb_kiov[0]);
+		rb = list_entry(msg->msg_kiov, struct lnet_rtrbuf, rb_kiov[0]);
 		rbp = rb->rb_pool;
 
 		msg->msg_kiov = NULL;
@@ -943,7 +944,7 @@ routing_off:
 						     msg->msg_rx_cpt);
 		} else if (rxpeer->lp_rtrcredits <= 0) {
 			msg2 = list_entry(rxpeer->lp_rtrq.next,
-					  lnet_msg_t, msg_list);
+					  struct lnet_msg, msg_list);
 			list_del(&msg2->msg_list);
 
 			(void)lnet_post_routed_recv_locked(msg2, 1);
@@ -956,10 +957,10 @@ routing_off:
 }
 
 static int
-lnet_compare_routes(lnet_route_t *r1, lnet_route_t *r2)
+lnet_compare_routes(struct lnet_route *r1, struct lnet_route *r2)
 {
-	lnet_peer_t *p1 = r1->lr_gateway;
-	lnet_peer_t *p2 = r2->lr_gateway;
+	struct lnet_peer *p1 = r1->lr_gateway;
+	struct lnet_peer *p2 = r2->lr_gateway;
 	int r1_hops = (r1->lr_hops == LNET_UNDEFINED_HOPS) ? 1 : r1->lr_hops;
 	int r2_hops = (r2->lr_hops == LNET_UNDEFINED_HOPS) ? 1 : r2->lr_hops;
 
@@ -993,13 +994,14 @@ lnet_compare_routes(lnet_route_t *r1, lnet_route_t *r2)
 	return -ERANGE;
 }
 
-static lnet_peer_t *
-lnet_find_route_locked(lnet_ni_t *ni, lnet_nid_t target, lnet_nid_t rtr_nid)
+static struct lnet_peer *
+lnet_find_route_locked(struct lnet_ni *ni, lnet_nid_t target,
+		       lnet_nid_t rtr_nid)
 {
-	lnet_remotenet_t *rnet;
-	lnet_route_t *route;
-	lnet_route_t *best_route;
-	lnet_route_t *last_route;
+	struct lnet_remotenet *rnet;
+	struct lnet_route *route;
+	struct lnet_route *best_route;
+	struct lnet_route *last_route;
 	struct lnet_peer *lp_best;
 	struct lnet_peer *lp;
 	int rc;
@@ -1057,7 +1059,7 @@ lnet_find_route_locked(lnet_ni_t *ni, lnet_nid_t target, lnet_nid_t rtr_nid)
 }
 
 int
-lnet_send(lnet_nid_t src_nid, lnet_msg_t *msg, lnet_nid_t rtr_nid)
+lnet_send(lnet_nid_t src_nid, struct lnet_msg *msg, lnet_nid_t rtr_nid)
 {
 	lnet_nid_t dst_nid = msg->msg_target.nid;
 	struct lnet_ni *src_ni;
@@ -1232,7 +1234,7 @@ lnet_send(lnet_nid_t src_nid, lnet_msg_t *msg, lnet_nid_t rtr_nid)
 }
 
 void
-lnet_drop_message(lnet_ni_t *ni, int cpt, void *private, unsigned int nob)
+lnet_drop_message(struct lnet_ni *ni, int cpt, void *private, unsigned int nob)
 {
 	lnet_net_lock(cpt);
 	the_lnet.ln_counters[cpt]->drop_count++;
@@ -1243,7 +1245,7 @@ lnet_drop_message(lnet_ni_t *ni, int cpt, void *private, unsigned int nob)
 }
 
 static void
-lnet_recv_put(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_recv_put(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	struct lnet_hdr *hdr = &msg->msg_hdr;
 
@@ -1264,7 +1266,7 @@ lnet_recv_put(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 static int
-lnet_parse_put(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_parse_put(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	struct lnet_hdr *hdr = &msg->msg_hdr;
 	struct lnet_match_info info;
@@ -1322,7 +1324,7 @@ lnet_parse_put(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 static int
-lnet_parse_get(lnet_ni_t *ni, lnet_msg_t *msg, int rdma_get)
+lnet_parse_get(struct lnet_ni *ni, struct lnet_msg *msg, int rdma_get)
 {
 	struct lnet_match_info info;
 	struct lnet_hdr *hdr = &msg->msg_hdr;
@@ -1386,12 +1388,12 @@ lnet_parse_get(lnet_ni_t *ni, lnet_msg_t *msg, int rdma_get)
 }
 
 static int
-lnet_parse_reply(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_parse_reply(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	void *private = msg->msg_private;
 	struct lnet_hdr *hdr = &msg->msg_hdr;
-	lnet_process_id_t src = {0};
-	lnet_libmd_t *md;
+	struct lnet_process_id src = {0};
+	struct lnet_libmd *md;
 	int rlength;
 	int mlength;
 	int cpt;
@@ -1451,11 +1453,11 @@ lnet_parse_reply(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 static int
-lnet_parse_ack(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_parse_ack(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	struct lnet_hdr *hdr = &msg->msg_hdr;
-	lnet_process_id_t src = {0};
-	lnet_libmd_t *md;
+	struct lnet_process_id src = {0};
+	struct lnet_libmd *md;
 	int cpt;
 
 	src.nid = hdr->src_nid;
@@ -1506,7 +1508,7 @@ lnet_parse_ack(lnet_ni_t *ni, lnet_msg_t *msg)
  * \retval -ve			error code
  */
 int
-lnet_parse_forward_locked(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_parse_forward_locked(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	int rc = 0;
 
@@ -1530,7 +1532,7 @@ lnet_parse_forward_locked(lnet_ni_t *ni, lnet_msg_t *msg)
 }
 
 int
-lnet_parse_local(lnet_ni_t *ni, lnet_msg_t *msg)
+lnet_parse_local(struct lnet_ni *ni, struct lnet_msg *msg)
 {
 	int rc;
 
@@ -1578,8 +1580,8 @@ lnet_msgtyp2str(int type)
 void
 lnet_print_hdr(struct lnet_hdr *hdr)
 {
-	lnet_process_id_t src = {0};
-	lnet_process_id_t dst = {0};
+	struct lnet_process_id src = {0};
+	struct lnet_process_id dst = {0};
 	char *type_str = lnet_msgtyp2str(hdr->type);
 
 	src.nid = hdr->src_nid;
@@ -1634,7 +1636,7 @@ lnet_print_hdr(struct lnet_hdr *hdr)
 }
 
 int
-lnet_parse(lnet_ni_t *ni, struct lnet_hdr *hdr, lnet_nid_t from_nid,
+lnet_parse(struct lnet_ni *ni, struct lnet_hdr *hdr, lnet_nid_t from_nid,
 	   void *private, int rdma_req)
 {
 	int rc = 0;
@@ -1873,10 +1875,10 @@ void
 lnet_drop_delayed_msg_list(struct list_head *head, char *reason)
 {
 	while (!list_empty(head)) {
-		lnet_process_id_t id = {0};
-		lnet_msg_t *msg;
+		struct lnet_process_id id = {0};
+		struct lnet_msg *msg;
 
-		msg = list_entry(head->next, lnet_msg_t, msg_list);
+		msg = list_entry(head->next, struct lnet_msg, msg_list);
 		list_del(&msg->msg_list);
 
 		id.nid = msg->msg_hdr.src_nid;
@@ -1915,10 +1917,10 @@ void
 lnet_recv_delayed_msg_list(struct list_head *head)
 {
 	while (!list_empty(head)) {
-		lnet_msg_t *msg;
-		lnet_process_id_t id;
+		struct lnet_msg *msg;
+		struct lnet_process_id id;
 
-		msg = list_entry(head->next, lnet_msg_t, msg_list);
+		msg = list_entry(head->next, struct lnet_msg, msg_list);
 		list_del(&msg->msg_list);
 
 		/*
@@ -1985,11 +1987,11 @@ lnet_recv_delayed_msg_list(struct list_head *head)
  * \retval -ENOMEM Memory allocation failure.
  * \retval -ENOENT Invalid MD object.
  *
- * \see lnet_event_t::hdr_data and lnet_event_kind_t.
+ * \see lnet_event::hdr_data and lnet_event_kind.
  */
 int
-LNetPut(lnet_nid_t self, lnet_handle_md_t mdh, lnet_ack_req_t ack,
-	lnet_process_id_t target, unsigned int portal,
+LNetPut(lnet_nid_t self, struct lnet_handle_md mdh, enum lnet_ack_req ack,
+	struct lnet_process_id target, unsigned int portal,
 	__u64 match_bits, unsigned int offset,
 	__u64 hdr_data)
 {
@@ -2009,7 +2011,7 @@ LNetPut(lnet_nid_t self, lnet_handle_md_t mdh, lnet_ack_req_t ack,
 
 	msg = lnet_msg_alloc();
 	if (!msg) {
-		CERROR("Dropping PUT to %s: ENOMEM on lnet_msg_t\n",
+		CERROR("Dropping PUT to %s: ENOMEM on struct lnet_msg\n",
 		       libcfs_id2str(target));
 		return -ENOMEM;
 	}
@@ -2072,8 +2074,8 @@ LNetPut(lnet_nid_t self, lnet_handle_md_t mdh, lnet_ack_req_t ack,
 }
 EXPORT_SYMBOL(LNetPut);
 
-lnet_msg_t *
-lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *getmsg)
+struct lnet_msg *
+lnet_create_reply_msg(struct lnet_ni *ni, struct lnet_msg *getmsg)
 {
 	/*
 	 * The LND can DMA direct to the GET md (i.e. no REPLY msg).  This
@@ -2085,7 +2087,7 @@ lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *getmsg)
 	 */
 	struct lnet_msg *msg = lnet_msg_alloc();
 	struct lnet_libmd *getmd = getmsg->msg_md;
-	lnet_process_id_t peer_id = getmsg->msg_target;
+	struct lnet_process_id peer_id = getmsg->msg_target;
 	int cpt;
 
 	LASSERT(!getmsg->msg_target_is_router);
@@ -2151,7 +2153,8 @@ lnet_create_reply_msg(lnet_ni_t *ni, lnet_msg_t *getmsg)
 EXPORT_SYMBOL(lnet_create_reply_msg);
 
 void
-lnet_set_reply_msg_len(lnet_ni_t *ni, lnet_msg_t *reply, unsigned int len)
+lnet_set_reply_msg_len(struct lnet_ni *ni, struct lnet_msg *reply,
+		       unsigned int len)
 {
 	/*
 	 * Set the REPLY length, now the RDMA that elides the REPLY message has
@@ -2193,8 +2196,8 @@ EXPORT_SYMBOL(lnet_set_reply_msg_len);
  * \retval -ENOENT Invalid MD object.
  */
 int
-LNetGet(lnet_nid_t self, lnet_handle_md_t mdh,
-	lnet_process_id_t target, unsigned int portal,
+LNetGet(lnet_nid_t self, struct lnet_handle_md mdh,
+	struct lnet_process_id target, unsigned int portal,
 	__u64 match_bits, unsigned int offset)
 {
 	struct lnet_msg *msg;
@@ -2213,7 +2216,7 @@ LNetGet(lnet_nid_t self, lnet_handle_md_t mdh,
 
 	msg = lnet_msg_alloc();
 	if (!msg) {
-		CERROR("Dropping GET to %s: ENOMEM on lnet_msg_t\n",
+		CERROR("Dropping GET to %s: ENOMEM on struct lnet_msg\n",
 		       libcfs_id2str(target));
 		return -ENOMEM;
 	}
@@ -2288,7 +2291,7 @@ LNetDist(lnet_nid_t dstnid, lnet_nid_t *srcnidp, __u32 *orderp)
 {
 	struct list_head *e;
 	struct lnet_ni *ni;
-	lnet_remotenet_t *rnet;
+	struct lnet_remotenet *rnet;
 	__u32 dstnet = LNET_NIDNET(dstnid);
 	int hops;
 	int cpt;
@@ -2306,7 +2309,7 @@ LNetDist(lnet_nid_t dstnid, lnet_nid_t *srcnidp, __u32 *orderp)
 	cpt = lnet_net_lock_current();
 
 	list_for_each(e, &the_lnet.ln_nis) {
-		ni = list_entry(e, lnet_ni_t, ni_list);
+		ni = list_entry(e, struct lnet_ni, ni_list);
 
 		if (ni->ni_nid == dstnid) {
 			if (srcnidp)
@@ -2345,11 +2348,11 @@ LNetDist(lnet_nid_t dstnid, lnet_nid_t *srcnidp, __u32 *orderp)
 
 	rn_list = lnet_net2rnethash(dstnet);
 	list_for_each(e, rn_list) {
-		rnet = list_entry(e, lnet_remotenet_t, lrn_list);
+		rnet = list_entry(e, struct lnet_remotenet, lrn_list);
 
 		if (rnet->lrn_net == dstnet) {
-			lnet_route_t *route;
-			lnet_route_t *shortest = NULL;
+			struct lnet_route *route;
+			struct lnet_route *shortest = NULL;
 			__u32 shortest_hops = LNET_UNDEFINED_HOPS;
 			__u32 route_hops;
 
diff --git a/drivers/staging/lustre/lnet/lnet/lib-msg.c b/drivers/staging/lustre/lnet/lnet/lib-msg.c
index 7ee164e67fbe..008ac503f27d 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-msg.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-msg.c
@@ -39,7 +39,7 @@
 #include "../../include/linux/lnet/lib-lnet.h"
 
 void
-lnet_build_unlink_event(lnet_libmd_t *md, lnet_event_t *ev)
+lnet_build_unlink_event(struct lnet_libmd *md, struct lnet_event *ev)
 {
 	memset(ev, 0, sizeof(*ev));
 
@@ -54,10 +54,10 @@ lnet_build_unlink_event(lnet_libmd_t *md, lnet_event_t *ev)
  * Don't need any lock, must be called after lnet_commit_md
  */
 void
-lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type)
+lnet_build_msg_event(struct lnet_msg *msg, enum lnet_event_kind ev_type)
 {
 	struct lnet_hdr *hdr = &msg->msg_hdr;
-	lnet_event_t *ev  = &msg->msg_ev;
+	struct lnet_event *ev  = &msg->msg_ev;
 
 	LASSERT(!msg->msg_routing);
 
@@ -129,10 +129,10 @@ lnet_build_msg_event(lnet_msg_t *msg, lnet_event_kind_t ev_type)
 }
 
 void
-lnet_msg_commit(lnet_msg_t *msg, int cpt)
+lnet_msg_commit(struct lnet_msg *msg, int cpt)
 {
 	struct lnet_msg_container *container = the_lnet.ln_msg_containers[cpt];
-	lnet_counters_t *counters  = the_lnet.ln_counters[cpt];
+	struct lnet_counters *counters  = the_lnet.ln_counters[cpt];
 
 	/* routed message can be committed for both receiving and sending */
 	LASSERT(!msg->msg_tx_committed);
@@ -162,10 +162,10 @@ lnet_msg_commit(lnet_msg_t *msg, int cpt)
 }
 
 static void
-lnet_msg_decommit_tx(lnet_msg_t *msg, int status)
+lnet_msg_decommit_tx(struct lnet_msg *msg, int status)
 {
-	lnet_counters_t	*counters;
-	lnet_event_t *ev = &msg->msg_ev;
+	struct lnet_counters	*counters;
+	struct lnet_event *ev = &msg->msg_ev;
 
 	LASSERT(msg->msg_tx_committed);
 	if (status)
@@ -214,10 +214,10 @@ lnet_msg_decommit_tx(lnet_msg_t *msg, int status)
 }
 
 static void
-lnet_msg_decommit_rx(lnet_msg_t *msg, int status)
+lnet_msg_decommit_rx(struct lnet_msg *msg, int status)
 {
-	lnet_counters_t *counters;
-	lnet_event_t *ev = &msg->msg_ev;
+	struct lnet_counters *counters;
+	struct lnet_event *ev = &msg->msg_ev;
 
 	LASSERT(!msg->msg_tx_committed); /* decommitted or never committed */
 	LASSERT(msg->msg_rx_committed);
@@ -272,7 +272,7 @@ lnet_msg_decommit_rx(lnet_msg_t *msg, int status)
 }
 
 void
-lnet_msg_decommit(lnet_msg_t *msg, int cpt, int status)
+lnet_msg_decommit(struct lnet_msg *msg, int cpt, int status)
 {
 	int cpt2 = cpt;
 
@@ -306,7 +306,7 @@ lnet_msg_decommit(lnet_msg_t *msg, int cpt, int status)
 }
 
 void
-lnet_msg_attach_md(lnet_msg_t *msg, lnet_libmd_t *md,
+lnet_msg_attach_md(struct lnet_msg *msg, struct lnet_libmd *md,
 		   unsigned int offset, unsigned int mlen)
 {
 	/* NB: @offset and @len are only useful for receiving */
@@ -336,9 +336,9 @@ lnet_msg_attach_md(lnet_msg_t *msg, lnet_libmd_t *md,
 }
 
 void
-lnet_msg_detach_md(lnet_msg_t *msg, int status)
+lnet_msg_detach_md(struct lnet_msg *msg, int status)
 {
-	lnet_libmd_t *md = msg->msg_md;
+	struct lnet_libmd *md = msg->msg_md;
 	int unlink;
 
 	/* Now it's safe to drop my caller's ref */
@@ -359,7 +359,7 @@ lnet_msg_detach_md(lnet_msg_t *msg, int status)
 }
 
 static int
-lnet_complete_msg_locked(lnet_msg_t *msg, int cpt)
+lnet_complete_msg_locked(struct lnet_msg *msg, int cpt)
 {
 	struct lnet_handle_wire ack_wmd;
 	int rc;
@@ -437,7 +437,7 @@ lnet_complete_msg_locked(lnet_msg_t *msg, int cpt)
 }
 
 void
-lnet_finalize(lnet_ni_t *ni, lnet_msg_t *msg, int status)
+lnet_finalize(struct lnet_ni *ni, struct lnet_msg *msg, int status)
 {
 	struct lnet_msg_container *container;
 	int my_slot;
@@ -502,7 +502,7 @@ lnet_finalize(lnet_ni_t *ni, lnet_msg_t *msg, int status)
 
 	while (!list_empty(&container->msc_finalizing)) {
 		msg = list_entry(container->msc_finalizing.next,
-				 lnet_msg_t, msg_list);
+				 struct lnet_msg, msg_list);
 
 		list_del(&msg->msg_list);
 
@@ -538,9 +538,10 @@ lnet_msg_container_cleanup(struct lnet_msg_container *container)
 		return;
 
 	while (!list_empty(&container->msc_active)) {
-		lnet_msg_t *msg = list_entry(container->msc_active.next,
-					     lnet_msg_t, msg_activelist);
+		struct lnet_msg *msg;
 
+		msg = list_entry(container->msc_active.next,
+				 struct lnet_msg, msg_activelist);
 		LASSERT(msg->msg_onactivelist);
 		msg->msg_onactivelist = 0;
 		list_del(&msg->msg_activelist);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-ptl.c b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
index fa515af2ac1d..33332724ab94 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-ptl.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
@@ -39,7 +39,7 @@ module_param(portal_rotor, int, 0644);
 MODULE_PARM_DESC(portal_rotor, "redirect PUTs to different cpu-partitions");
 
 static int
-lnet_ptl_match_type(unsigned int index, lnet_process_id_t match_id,
+lnet_ptl_match_type(unsigned int index, struct lnet_process_id match_id,
 		    __u64 mbits, __u64 ignore_bits)
 {
 	struct lnet_portal *ptl = the_lnet.ln_portals[index];
@@ -131,7 +131,7 @@ lnet_ptl_disable_mt(struct lnet_portal *ptl, int cpt)
 }
 
 static int
-lnet_try_match_md(lnet_libmd_t *md,
+lnet_try_match_md(struct lnet_libmd *md,
 		  struct lnet_match_info *info, struct lnet_msg *msg)
 {
 	/*
@@ -140,7 +140,7 @@ lnet_try_match_md(lnet_libmd_t *md,
 	 */
 	unsigned int offset;
 	unsigned int mlength;
-	lnet_me_t *me = md->md_me;
+	struct lnet_me *me = md->md_me;
 
 	/* MD exhausted */
 	if (lnet_md_exhausted(md))
@@ -212,7 +212,7 @@ lnet_try_match_md(lnet_libmd_t *md,
 }
 
 static struct lnet_match_table *
-lnet_match2mt(struct lnet_portal *ptl, lnet_process_id_t id, __u64 mbits)
+lnet_match2mt(struct lnet_portal *ptl, struct lnet_process_id id, __u64 mbits)
 {
 	if (LNET_CPT_NUMBER == 1)
 		return ptl->ptl_mtables[0]; /* the only one */
@@ -223,8 +223,8 @@ lnet_match2mt(struct lnet_portal *ptl, lnet_process_id_t id, __u64 mbits)
 }
 
 struct lnet_match_table *
-lnet_mt_of_attach(unsigned int index, lnet_process_id_t id,
-		  __u64 mbits, __u64 ignore_bits, lnet_ins_pos_t pos)
+lnet_mt_of_attach(unsigned int index, struct lnet_process_id id,
+		  __u64 mbits, __u64 ignore_bits, enum lnet_ins_pos pos)
 {
 	struct lnet_portal *ptl;
 	struct lnet_match_table	*mtable;
@@ -334,7 +334,7 @@ lnet_mt_test_exhausted(struct lnet_match_table *mtable, int pos)
 	bmap = &mtable->mt_exhausted[pos >> LNET_MT_BITS_U64];
 	pos &= (1 << LNET_MT_BITS_U64) - 1;
 
-	return (*bmap & (1ULL << pos));
+	return (*bmap & BIT(pos));
 }
 
 static void
@@ -357,7 +357,7 @@ lnet_mt_set_exhausted(struct lnet_match_table *mtable, int pos, int exhausted)
 
 struct list_head *
 lnet_mt_match_head(struct lnet_match_table *mtable,
-		   lnet_process_id_t id, __u64 mbits)
+		   struct lnet_process_id id, __u64 mbits)
 {
 	struct lnet_portal *ptl = the_lnet.ln_portals[mtable->mt_portal];
 	unsigned long hash = mbits;
@@ -376,8 +376,8 @@ lnet_mt_match_md(struct lnet_match_table *mtable,
 		 struct lnet_match_info *info, struct lnet_msg *msg)
 {
 	struct list_head *head;
-	lnet_me_t *me;
-	lnet_me_t *tmp;
+	struct lnet_me *me;
+	struct lnet_me *tmp;
 	int exhausted = 0;
 	int rc;
 
@@ -641,7 +641,7 @@ lnet_ptl_match_md(struct lnet_match_info *info, struct lnet_msg *msg)
 }
 
 void
-lnet_ptl_detach_md(lnet_me_t *me, lnet_libmd_t *md)
+lnet_ptl_detach_md(struct lnet_me *me, struct lnet_libmd *md)
 {
 	LASSERT(me->me_md == md && md->md_me == me);
 
@@ -651,14 +651,14 @@ lnet_ptl_detach_md(lnet_me_t *me, lnet_libmd_t *md)
 
 /* called with lnet_res_lock held */
 void
-lnet_ptl_attach_md(lnet_me_t *me, lnet_libmd_t *md,
+lnet_ptl_attach_md(struct lnet_me *me, struct lnet_libmd *md,
 		   struct list_head *matches, struct list_head *drops)
 {
 	struct lnet_portal *ptl = the_lnet.ln_portals[me->me_portal];
 	struct lnet_match_table	*mtable;
 	struct list_head *head;
-	lnet_msg_t *tmp;
-	lnet_msg_t *msg;
+	struct lnet_msg *tmp;
+	struct lnet_msg *msg;
 	int exhausted = 0;
 	int cpt;
 
@@ -756,7 +756,7 @@ lnet_ptl_cleanup(struct lnet_portal *ptl)
 	LASSERT(list_empty(&ptl->ptl_msg_stealing));
 	cfs_percpt_for_each(mtable, i, ptl->ptl_mtables) {
 		struct list_head *mhash;
-		lnet_me_t *me;
+		struct lnet_me *me;
 		int j;
 
 		if (!mtable->mt_mhash) /* uninitialized match-table */
@@ -767,7 +767,7 @@ lnet_ptl_cleanup(struct lnet_portal *ptl)
 		for (j = 0; j < LNET_MT_HASH_SIZE + 1; j++) {
 			while (!list_empty(&mhash[j])) {
 				me = list_entry(mhash[j].next,
-						lnet_me_t, me_list);
+						struct lnet_me, me_list);
 				CERROR("Active ME %p on exit\n", me);
 				list_del(&me->me_list);
 				lnet_me_free(me);
diff --git a/drivers/staging/lustre/lnet/lnet/lo.c b/drivers/staging/lustre/lnet/lnet/lo.c
index cb213b8f51cf..a7504b8edf6d 100644
--- a/drivers/staging/lustre/lnet/lnet/lo.c
+++ b/drivers/staging/lustre/lnet/lnet/lo.c
@@ -32,7 +32,7 @@
 #include "../../include/linux/lnet/lib-lnet.h"
 
 static int
-lolnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
+lolnd_send(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg)
 {
 	LASSERT(!lntmsg->msg_routing);
 	LASSERT(!lntmsg->msg_target_is_router);
@@ -41,10 +41,10 @@ lolnd_send(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 }
 
 static int
-lolnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
+lolnd_recv(struct lnet_ni *ni, void *private, struct lnet_msg *lntmsg,
 	   int delayed, struct iov_iter *to, unsigned int rlen)
 {
-	lnet_msg_t *sendmsg = private;
+	struct lnet_msg *sendmsg = private;
 
 	if (lntmsg) {		   /* not discarding */
 		if (sendmsg->msg_iov)
@@ -70,7 +70,7 @@ lolnd_recv(lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg,
 static int lolnd_instanced;
 
 static void
-lolnd_shutdown(lnet_ni_t *ni)
+lolnd_shutdown(struct lnet_ni *ni)
 {
 	CDEBUG(D_NET, "shutdown\n");
 	LASSERT(lolnd_instanced);
@@ -79,7 +79,7 @@ lolnd_shutdown(lnet_ni_t *ni)
 }
 
 static int
-lolnd_startup(lnet_ni_t *ni)
+lolnd_startup(struct lnet_ni *ni)
 {
 	LASSERT(ni->ni_lnd == &the_lolnd);
 	LASSERT(!lolnd_instanced);
@@ -88,7 +88,7 @@ lolnd_startup(lnet_ni_t *ni)
 	return 0;
 }
 
-lnd_t the_lolnd = {
+struct lnet_lnd the_lolnd = {
 	/* .lnd_list       = */ {&the_lolnd.lnd_list, &the_lolnd.lnd_list},
 	/* .lnd_refcount   = */ 0,
 	/* .lnd_type       = */ LOLND,
diff --git a/drivers/staging/lustre/lnet/lnet/nidstrings.c b/drivers/staging/lustre/lnet/lnet/nidstrings.c
index a9fe3e69daae..298533d7b04c 100644
--- a/drivers/staging/lustre/lnet/lnet/nidstrings.c
+++ b/drivers/staging/lustre/lnet/lnet/nidstrings.c
@@ -1226,7 +1226,7 @@ libcfs_str2nid(const char *str)
 EXPORT_SYMBOL(libcfs_str2nid);
 
 char *
-libcfs_id2str(lnet_process_id_t id)
+libcfs_id2str(struct lnet_process_id id)
 {
 	char *str = libcfs_next_nidstring();
 
diff --git a/drivers/staging/lustre/lnet/lnet/peer.c b/drivers/staging/lustre/lnet/lnet/peer.c
index e8061916c241..e62b21f3ab4d 100644
--- a/drivers/staging/lustre/lnet/lnet/peer.c
+++ b/drivers/staging/lustre/lnet/lnet/peer.c
@@ -101,11 +101,12 @@ lnet_peer_tables_destroy(void)
 }
 
 static void
-lnet_peer_table_cleanup_locked(lnet_ni_t *ni, struct lnet_peer_table *ptable)
+lnet_peer_table_cleanup_locked(struct lnet_ni *ni,
+			       struct lnet_peer_table *ptable)
 {
 	int i;
-	lnet_peer_t *lp;
-	lnet_peer_t *tmp;
+	struct lnet_peer *lp;
+	struct lnet_peer *tmp;
 
 	for (i = 0; i < LNET_PEER_HASH_SIZE; i++) {
 		list_for_each_entry_safe(lp, tmp, &ptable->pt_hash[i],
@@ -141,11 +142,12 @@ lnet_peer_table_deathrow_wait_locked(struct lnet_peer_table *ptable,
 }
 
 static void
-lnet_peer_table_del_rtrs_locked(lnet_ni_t *ni, struct lnet_peer_table *ptable,
+lnet_peer_table_del_rtrs_locked(struct lnet_ni *ni,
+				struct lnet_peer_table *ptable,
 				int cpt_locked)
 {
-	lnet_peer_t *lp;
-	lnet_peer_t *tmp;
+	struct lnet_peer *lp;
+	struct lnet_peer *tmp;
 	lnet_nid_t lp_nid;
 	int i;
 
@@ -168,12 +170,12 @@ lnet_peer_table_del_rtrs_locked(lnet_ni_t *ni, struct lnet_peer_table *ptable,
 }
 
 void
-lnet_peer_tables_cleanup(lnet_ni_t *ni)
+lnet_peer_tables_cleanup(struct lnet_ni *ni)
 {
 	struct lnet_peer_table *ptable;
 	struct list_head deathrow;
-	lnet_peer_t *lp;
-	lnet_peer_t *temp;
+	struct lnet_peer *lp;
+	struct lnet_peer *temp;
 	int i;
 
 	INIT_LIST_HEAD(&deathrow);
@@ -214,7 +216,7 @@ lnet_peer_tables_cleanup(lnet_ni_t *ni)
 }
 
 void
-lnet_destroy_peer_locked(lnet_peer_t *lp)
+lnet_destroy_peer_locked(struct lnet_peer *lp)
 {
 	struct lnet_peer_table *ptable;
 
@@ -236,11 +238,11 @@ lnet_destroy_peer_locked(lnet_peer_t *lp)
 	ptable->pt_zombies--;
 }
 
-lnet_peer_t *
+struct lnet_peer *
 lnet_find_peer_locked(struct lnet_peer_table *ptable, lnet_nid_t nid)
 {
 	struct list_head *peers;
-	lnet_peer_t *lp;
+	struct lnet_peer *lp;
 
 	LASSERT(!the_lnet.ln_shutdown);
 
@@ -256,11 +258,11 @@ lnet_find_peer_locked(struct lnet_peer_table *ptable, lnet_nid_t nid)
 }
 
 int
-lnet_nid2peer_locked(lnet_peer_t **lpp, lnet_nid_t nid, int cpt)
+lnet_nid2peer_locked(struct lnet_peer **lpp, lnet_nid_t nid, int cpt)
 {
 	struct lnet_peer_table *ptable;
-	lnet_peer_t *lp = NULL;
-	lnet_peer_t *lp2;
+	struct lnet_peer *lp = NULL;
+	struct lnet_peer *lp2;
 	int cpt2;
 	int rc = 0;
 
@@ -280,7 +282,7 @@ lnet_nid2peer_locked(lnet_peer_t **lpp, lnet_nid_t nid, int cpt)
 
 	if (!list_empty(&ptable->pt_deathrow)) {
 		lp = list_entry(ptable->pt_deathrow.next,
-				lnet_peer_t, lp_hashlist);
+				struct lnet_peer, lp_hashlist);
 		list_del(&lp->lp_hashlist);
 	}
 
@@ -362,7 +364,7 @@ void
 lnet_debug_peer(lnet_nid_t nid)
 {
 	char *aliveness = "NA";
-	lnet_peer_t *lp;
+	struct lnet_peer *lp;
 	int rc;
 	int cpt;
 
@@ -399,7 +401,7 @@ lnet_get_peer_info(__u32 peer_index, __u64 *nid,
 		   __u32 *peer_tx_qnob)
 {
 	struct lnet_peer_table *peer_table;
-	lnet_peer_t *lp;
+	struct lnet_peer *lp;
 	bool found = false;
 	int lncpt, j;
 
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index cf22525d7129..12dd1043f9fa 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -53,7 +53,7 @@ module_param(auto_down, int, 0444);
 MODULE_PARM_DESC(auto_down, "Automatically mark peers down on comms error");
 
 int
-lnet_peer_buffer_credits(lnet_ni_t *ni)
+lnet_peer_buffer_credits(struct lnet_ni *ni)
 {
 	/* NI option overrides LNet default */
 	if (ni->ni_peerrtrcredits > 0)
@@ -98,7 +98,7 @@ lnet_peers_start_down(void)
 }
 
 void
-lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
+lnet_notify_locked(struct lnet_peer *lp, int notifylnd, int alive,
 		   unsigned long when)
 {
 	if (time_before(when, lp->lp_timestamp)) { /* out of date information */
@@ -128,7 +128,7 @@ lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
 }
 
 static void
-lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
+lnet_ni_notify_locked(struct lnet_ni *ni, struct lnet_peer *lp)
 {
 	int alive;
 	int notifylnd;
@@ -167,7 +167,7 @@ lnet_ni_notify_locked(lnet_ni_t *ni, lnet_peer_t *lp)
 }
 
 static void
-lnet_rtr_addref_locked(lnet_peer_t *lp)
+lnet_rtr_addref_locked(struct lnet_peer *lp)
 {
 	LASSERT(lp->lp_refcount > 0);
 	LASSERT(lp->lp_rtr_refcount >= 0);
@@ -179,9 +179,9 @@ lnet_rtr_addref_locked(lnet_peer_t *lp)
 
 		/* a simple insertion sort */
 		list_for_each_prev(pos, &the_lnet.ln_routers) {
-			lnet_peer_t *rtr = list_entry(pos, lnet_peer_t,
-						      lp_rtr_list);
+			struct lnet_peer *rtr;
 
+			rtr = list_entry(pos, struct lnet_peer, lp_rtr_list);
 			if (rtr->lp_nid < lp->lp_nid)
 				break;
 		}
@@ -194,7 +194,7 @@ lnet_rtr_addref_locked(lnet_peer_t *lp)
 }
 
 static void
-lnet_rtr_decref_locked(lnet_peer_t *lp)
+lnet_rtr_decref_locked(struct lnet_peer *lp)
 {
 	LASSERT(lp->lp_refcount > 0);
 	LASSERT(lp->lp_rtr_refcount > 0);
@@ -217,10 +217,10 @@ lnet_rtr_decref_locked(lnet_peer_t *lp)
 	}
 }
 
-lnet_remotenet_t *
+struct lnet_remotenet *
 lnet_find_net_locked(__u32 net)
 {
-	lnet_remotenet_t *rnet;
+	struct lnet_remotenet *rnet;
 	struct list_head *tmp;
 	struct list_head *rn_list;
 
@@ -228,7 +228,7 @@ lnet_find_net_locked(__u32 net)
 
 	rn_list = lnet_net2rnethash(net);
 	list_for_each(tmp, rn_list) {
-		rnet = list_entry(tmp, lnet_remotenet_t, lrn_list);
+		rnet = list_entry(tmp, struct lnet_remotenet, lrn_list);
 
 		if (rnet->lrn_net == net)
 			return rnet;
@@ -241,7 +241,7 @@ static void lnet_shuffle_seed(void)
 	static int seeded;
 	__u32 lnd_type, seed[2];
 	struct timespec64 ts;
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 	struct list_head *tmp;
 
 	if (seeded)
@@ -254,7 +254,7 @@ static void lnet_shuffle_seed(void)
 	 * the NID for this node gives the most entropy in the low bits
 	 */
 	list_for_each(tmp, &the_lnet.ln_nis) {
-		ni = list_entry(tmp, lnet_ni_t, ni_list);
+		ni = list_entry(tmp, struct lnet_ni, ni_list);
 		lnd_type = LNET_NETTYP(LNET_NIDNET(ni->ni_nid));
 
 		if (lnd_type != LOLND)
@@ -268,7 +268,7 @@ static void lnet_shuffle_seed(void)
 
 /* NB expects LNET_LOCK held */
 static void
-lnet_add_route_to_rnet(lnet_remotenet_t *rnet, lnet_route_t *route)
+lnet_add_route_to_rnet(struct lnet_remotenet *rnet, struct lnet_route *route)
 {
 	unsigned int len = 0;
 	unsigned int offset = 0;
@@ -299,10 +299,10 @@ lnet_add_route(__u32 net, __u32 hops, lnet_nid_t gateway,
 	       unsigned int priority)
 {
 	struct list_head *e;
-	lnet_remotenet_t *rnet;
-	lnet_remotenet_t *rnet2;
-	lnet_route_t *route;
-	lnet_ni_t *ni;
+	struct lnet_remotenet *rnet;
+	struct lnet_remotenet *rnet2;
+	struct lnet_route *route;
+	struct lnet_ni *ni;
 	int add_route;
 	int rc;
 
@@ -368,8 +368,9 @@ lnet_add_route(__u32 net, __u32 hops, lnet_nid_t gateway,
 	/* Search for a duplicate route (it's a NOOP if it is) */
 	add_route = 1;
 	list_for_each(e, &rnet2->lrn_routes) {
-		lnet_route_t *route2 = list_entry(e, lnet_route_t, lr_list);
+		struct lnet_route *route2;
 
+		route2 = list_entry(e, struct lnet_route, lr_list);
 		if (route2->lr_gateway == route->lr_gateway) {
 			add_route = 0;
 			break;
@@ -415,9 +416,9 @@ lnet_add_route(__u32 net, __u32 hops, lnet_nid_t gateway,
 int
 lnet_check_routes(void)
 {
-	lnet_remotenet_t *rnet;
-	lnet_route_t *route;
-	lnet_route_t *route2;
+	struct lnet_remotenet *rnet;
+	struct lnet_route *route;
+	struct lnet_route *route2;
 	struct list_head *e1;
 	struct list_head *e2;
 	int cpt;
@@ -429,7 +430,7 @@ lnet_check_routes(void)
 	for (i = 0; i < LNET_REMOTE_NETS_HASH_SIZE; i++) {
 		rn_list = &the_lnet.ln_remote_nets_hash[i];
 		list_for_each(e1, rn_list) {
-			rnet = list_entry(e1, lnet_remotenet_t, lrn_list);
+			rnet = list_entry(e1, struct lnet_remotenet, lrn_list);
 
 			route2 = NULL;
 			list_for_each(e2, &rnet->lrn_routes) {
@@ -437,7 +438,7 @@ lnet_check_routes(void)
 				lnet_nid_t nid2;
 				int net;
 
-				route = list_entry(e2, lnet_route_t, lr_list);
+				route = list_entry(e2, struct lnet_route, lr_list);
 
 				if (!route2) {
 					route2 = route;
@@ -471,8 +472,8 @@ int
 lnet_del_route(__u32 net, lnet_nid_t gw_nid)
 {
 	struct lnet_peer *gateway;
-	lnet_remotenet_t *rnet;
-	lnet_route_t *route;
+	struct lnet_remotenet *rnet;
+	struct lnet_route *route;
 	struct list_head *e1;
 	struct list_head *e2;
 	int rc = -ENOENT;
@@ -494,14 +495,14 @@ lnet_del_route(__u32 net, lnet_nid_t gw_nid)
 
  again:
 	list_for_each(e1, rn_list) {
-		rnet = list_entry(e1, lnet_remotenet_t, lrn_list);
+		rnet = list_entry(e1, struct lnet_remotenet, lrn_list);
 
 		if (!(net == LNET_NIDNET(LNET_NID_ANY) ||
 		      net == rnet->lrn_net))
 			continue;
 
 		list_for_each(e2, &rnet->lrn_routes) {
-			route = list_entry(e2, lnet_route_t, lr_list);
+			route = list_entry(e2, struct lnet_route, lr_list);
 
 			gateway = route->lr_gateway;
 			if (!(gw_nid == LNET_NID_ANY ||
@@ -557,7 +558,7 @@ int lnet_get_rtr_pool_cfg(int idx, struct lnet_ioctl_pool_cfg *pool_cfg)
 		return rc;
 
 	for (i = 0; i < LNET_NRBPOOLS; i++) {
-		lnet_rtrbufpool_t *rbp;
+		struct lnet_rtrbufpool *rbp;
 
 		lnet_net_lock(LNET_LOCK_EX);
 		cfs_percpt_for_each(rbp, j, the_lnet.ln_rtrpools) {
@@ -587,8 +588,8 @@ lnet_get_route(int idx, __u32 *net, __u32 *hops,
 {
 	struct list_head *e1;
 	struct list_head *e2;
-	lnet_remotenet_t *rnet;
-	lnet_route_t *route;
+	struct lnet_remotenet *rnet;
+	struct lnet_route *route;
 	int cpt;
 	int i;
 	struct list_head *rn_list;
@@ -598,10 +599,11 @@ lnet_get_route(int idx, __u32 *net, __u32 *hops,
 	for (i = 0; i < LNET_REMOTE_NETS_HASH_SIZE; i++) {
 		rn_list = &the_lnet.ln_remote_nets_hash[i];
 		list_for_each(e1, rn_list) {
-			rnet = list_entry(e1, lnet_remotenet_t, lrn_list);
+			rnet = list_entry(e1, struct lnet_remotenet, lrn_list);
 
 			list_for_each(e2, &rnet->lrn_routes) {
-				route = list_entry(e2, lnet_route_t, lr_list);
+				route = list_entry(e2, struct lnet_route,
+						   lr_list);
 
 				if (!idx--) {
 					*net      = rnet->lrn_net;
@@ -642,11 +644,11 @@ lnet_swap_pinginfo(struct lnet_ping_info *info)
  * networks on that router.
  */
 static void
-lnet_parse_rc_info(lnet_rc_data_t *rcd)
+lnet_parse_rc_info(struct lnet_rc_data *rcd)
 {
 	struct lnet_ping_info *info = rcd->rcd_pinginfo;
 	struct lnet_peer *gw = rcd->rcd_gateway;
-	lnet_route_t *rte;
+	struct lnet_route *rte;
 
 	if (!gw->lp_alive)
 		return;
@@ -731,15 +733,15 @@ lnet_parse_rc_info(lnet_rc_data_t *rcd)
 }
 
 static void
-lnet_router_checker_event(lnet_event_t *event)
+lnet_router_checker_event(struct lnet_event *event)
 {
-	lnet_rc_data_t *rcd = event->md.user_ptr;
+	struct lnet_rc_data *rcd = event->md.user_ptr;
 	struct lnet_peer *lp;
 
 	LASSERT(rcd);
 
 	if (event->unlinked) {
-		LNetInvalidateHandle(&rcd->rcd_mdh);
+		LNetInvalidateMDHandle(&rcd->rcd_mdh);
 		return;
 	}
 
@@ -791,7 +793,7 @@ lnet_router_checker_event(lnet_event_t *event)
 static void
 lnet_wait_known_routerstate(void)
 {
-	lnet_peer_t *rtr;
+	struct lnet_peer *rtr;
 	struct list_head *entry;
 	int all_known;
 
@@ -802,7 +804,7 @@ lnet_wait_known_routerstate(void)
 
 		all_known = 1;
 		list_for_each(entry, &the_lnet.ln_routers) {
-			rtr = list_entry(entry, lnet_peer_t, lp_rtr_list);
+			rtr = list_entry(entry, struct lnet_peer, lp_rtr_list);
 
 			if (!rtr->lp_alive_count) {
 				all_known = 0;
@@ -821,9 +823,9 @@ lnet_wait_known_routerstate(void)
 }
 
 void
-lnet_router_ni_update_locked(lnet_peer_t *gw, __u32 net)
+lnet_router_ni_update_locked(struct lnet_peer *gw, __u32 net)
 {
-	lnet_route_t *rte;
+	struct lnet_route *rte;
 
 	if ((gw->lp_ping_feats & LNET_PING_FEAT_NI_STATUS)) {
 		list_for_each_entry(rte, &gw->lp_routes, lr_gwlist) {
@@ -838,7 +840,7 @@ lnet_router_ni_update_locked(lnet_peer_t *gw, __u32 net)
 static void
 lnet_update_ni_status_locked(void)
 {
-	lnet_ni_t *ni;
+	struct lnet_ni *ni;
 	time64_t now;
 	int timeout;
 
@@ -878,11 +880,11 @@ lnet_update_ni_status_locked(void)
 }
 
 static void
-lnet_destroy_rc_data(lnet_rc_data_t *rcd)
+lnet_destroy_rc_data(struct lnet_rc_data *rcd)
 {
 	LASSERT(list_empty(&rcd->rcd_list));
 	/* detached from network */
-	LASSERT(LNetHandleIsInvalid(rcd->rcd_mdh));
+	LASSERT(LNetMDHandleIsInvalid(rcd->rcd_mdh));
 
 	if (rcd->rcd_gateway) {
 		int cpt = rcd->rcd_gateway->lp_cpt;
@@ -898,12 +900,12 @@ lnet_destroy_rc_data(lnet_rc_data_t *rcd)
 	LIBCFS_FREE(rcd, sizeof(*rcd));
 }
 
-static lnet_rc_data_t *
-lnet_create_rc_data_locked(lnet_peer_t *gateway)
+static struct lnet_rc_data *
+lnet_create_rc_data_locked(struct lnet_peer *gateway)
 {
-	lnet_rc_data_t *rcd = NULL;
+	struct lnet_rc_data *rcd = NULL;
 	struct lnet_ping_info *pi;
-	lnet_md_t md;
+	struct lnet_md md;
 	int rc;
 	int i;
 
@@ -913,7 +915,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 	if (!rcd)
 		goto out;
 
-	LNetInvalidateHandle(&rcd->rcd_mdh);
+	LNetInvalidateMDHandle(&rcd->rcd_mdh);
 	INIT_LIST_HEAD(&rcd->rcd_list);
 
 	LIBCFS_ALLOC(pi, LNET_PINGINFO_SIZE);
@@ -933,7 +935,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 	md.options = LNET_MD_TRUNCATE;
 	md.eq_handle = the_lnet.ln_rc_eqh;
 
-	LASSERT(!LNetHandleIsInvalid(the_lnet.ln_rc_eqh));
+	LASSERT(!LNetEQHandleIsInvalid(the_lnet.ln_rc_eqh));
 	rc = LNetMDBind(md, LNET_UNLINK, &rcd->rcd_mdh);
 	if (rc < 0) {
 		CERROR("Can't bind MD: %d\n", rc);
@@ -957,7 +959,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 
  out:
 	if (rcd) {
-		if (!LNetHandleIsInvalid(rcd->rcd_mdh)) {
+		if (!LNetMDHandleIsInvalid(rcd->rcd_mdh)) {
 			rc = LNetMDUnlink(rcd->rcd_mdh);
 			LASSERT(!rc);
 		}
@@ -969,7 +971,7 @@ lnet_create_rc_data_locked(lnet_peer_t *gateway)
 }
 
 static int
-lnet_router_check_interval(lnet_peer_t *rtr)
+lnet_router_check_interval(struct lnet_peer *rtr)
 {
 	int secs;
 
@@ -982,9 +984,9 @@ lnet_router_check_interval(lnet_peer_t *rtr)
 }
 
 static void
-lnet_ping_router_locked(lnet_peer_t *rtr)
+lnet_ping_router_locked(struct lnet_peer *rtr)
 {
-	lnet_rc_data_t *rcd = NULL;
+	struct lnet_rc_data *rcd = NULL;
 	unsigned long now = cfs_time_current();
 	int secs;
 
@@ -1022,8 +1024,8 @@ lnet_ping_router_locked(lnet_peer_t *rtr)
 	    cfs_time_after(now, cfs_time_add(rtr->lp_ping_timestamp,
 					     cfs_time_seconds(secs)))) {
 		int rc;
-		lnet_process_id_t id;
-		lnet_handle_md_t mdh;
+		struct lnet_process_id id;
+		struct lnet_handle_md mdh;
 
 		id.nid = rtr->lp_nid;
 		id.pid = LNET_PID_LUSTRE;
@@ -1124,9 +1126,9 @@ lnet_router_checker_stop(void)
 static void
 lnet_prune_rc_data(int wait_unlink)
 {
-	lnet_rc_data_t *rcd;
-	lnet_rc_data_t *tmp;
-	lnet_peer_t *lp;
+	struct lnet_rc_data *rcd;
+	struct lnet_rc_data *tmp;
+	struct lnet_peer *lp;
 	struct list_head head;
 	int i = 2;
 
@@ -1171,7 +1173,7 @@ lnet_prune_rc_data(int wait_unlink)
 	while (!list_empty(&the_lnet.ln_rcd_zombie)) {
 		list_for_each_entry_safe(rcd, tmp, &the_lnet.ln_rcd_zombie,
 					 rcd_list) {
-			if (LNetHandleIsInvalid(rcd->rcd_mdh))
+			if (LNetMDHandleIsInvalid(rcd->rcd_mdh))
 				list_move(&rcd->rcd_list, &head);
 		}
 
@@ -1182,7 +1184,7 @@ lnet_prune_rc_data(int wait_unlink)
 
 		while (!list_empty(&head)) {
 			rcd = list_entry(head.next,
-					 lnet_rc_data_t, rcd_list);
+					 struct lnet_rc_data, rcd_list);
 			list_del_init(&rcd->rcd_list);
 			lnet_destroy_rc_data(rcd);
 		}
@@ -1232,7 +1234,7 @@ lnet_router_checker_active(void)
 static int
 lnet_router_checker(void *arg)
 {
-	lnet_peer_t *rtr;
+	struct lnet_peer *rtr;
 	struct list_head *entry;
 
 	cfs_block_allsigs();
@@ -1247,7 +1249,7 @@ rescan:
 		version = the_lnet.ln_routers_version;
 
 		list_for_each(entry, &the_lnet.ln_routers) {
-			rtr = list_entry(entry, lnet_peer_t, lp_rtr_list);
+			rtr = list_entry(entry, struct lnet_peer, lp_rtr_list);
 
 			cpt2 = lnet_cpt_of_nid_locked(rtr->lp_nid);
 			if (cpt != cpt2) {
@@ -1303,9 +1305,9 @@ rescan:
 }
 
 void
-lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages)
+lnet_destroy_rtrbuf(struct lnet_rtrbuf *rb, int npages)
 {
-	int sz = offsetof(lnet_rtrbuf_t, rb_kiov[npages]);
+	int sz = offsetof(struct lnet_rtrbuf, rb_kiov[npages]);
 
 	while (--npages >= 0)
 		__free_page(rb->rb_kiov[npages].bv_page);
@@ -1313,13 +1315,13 @@ lnet_destroy_rtrbuf(lnet_rtrbuf_t *rb, int npages)
 	LIBCFS_FREE(rb, sz);
 }
 
-static lnet_rtrbuf_t *
-lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
+static struct lnet_rtrbuf *
+lnet_new_rtrbuf(struct lnet_rtrbufpool *rbp, int cpt)
 {
 	int npages = rbp->rbp_npages;
-	int sz = offsetof(lnet_rtrbuf_t, rb_kiov[npages]);
+	int sz = offsetof(struct lnet_rtrbuf, rb_kiov[npages]);
 	struct page *page;
-	lnet_rtrbuf_t *rb;
+	struct lnet_rtrbuf *rb;
 	int i;
 
 	LIBCFS_CPT_ALLOC(rb, lnet_cpt_table(), cpt, sz);
@@ -1349,12 +1351,12 @@ lnet_new_rtrbuf(lnet_rtrbufpool_t *rbp, int cpt)
 }
 
 static void
-lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp, int cpt)
+lnet_rtrpool_free_bufs(struct lnet_rtrbufpool *rbp, int cpt)
 {
 	int npages = rbp->rbp_npages;
 	struct list_head tmp;
-	lnet_rtrbuf_t *rb;
-	lnet_rtrbuf_t *temp;
+	struct lnet_rtrbuf *rb;
+	struct lnet_rtrbuf *temp;
 
 	if (!rbp->rbp_nbuffers) /* not initialized or already freed */
 		return;
@@ -1378,10 +1380,10 @@ lnet_rtrpool_free_bufs(lnet_rtrbufpool_t *rbp, int cpt)
 }
 
 static int
-lnet_rtrpool_adjust_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
+lnet_rtrpool_adjust_bufs(struct lnet_rtrbufpool *rbp, int nbufs, int cpt)
 {
 	struct list_head rb_list;
-	lnet_rtrbuf_t *rb;
+	struct lnet_rtrbuf *rb;
 	int num_rb;
 	int num_buffers = 0;
 	int old_req_nbufs;
@@ -1456,7 +1458,7 @@ lnet_rtrpool_adjust_bufs(lnet_rtrbufpool_t *rbp, int nbufs, int cpt)
 
 failed:
 	while (!list_empty(&rb_list)) {
-		rb = list_entry(rb_list.next, lnet_rtrbuf_t, rb_list);
+		rb = list_entry(rb_list.next, struct lnet_rtrbuf, rb_list);
 		list_del(&rb->rb_list);
 		lnet_destroy_rtrbuf(rb, npages);
 	}
@@ -1465,7 +1467,7 @@ failed:
 }
 
 static void
-lnet_rtrpool_init(lnet_rtrbufpool_t *rbp, int npages)
+lnet_rtrpool_init(struct lnet_rtrbufpool *rbp, int npages)
 {
 	INIT_LIST_HEAD(&rbp->rbp_msgs);
 	INIT_LIST_HEAD(&rbp->rbp_bufs);
@@ -1478,7 +1480,7 @@ lnet_rtrpool_init(lnet_rtrbufpool_t *rbp, int npages)
 void
 lnet_rtrpools_free(int keep_pools)
 {
-	lnet_rtrbufpool_t *rtrp;
+	struct lnet_rtrbufpool *rtrp;
 	int i;
 
 	if (!the_lnet.ln_rtrpools) /* uninitialized or freed */
@@ -1556,7 +1558,7 @@ lnet_nrb_large_calculate(void)
 int
 lnet_rtrpools_alloc(int im_a_router)
 {
-	lnet_rtrbufpool_t *rtrp;
+	struct lnet_rtrbufpool *rtrp;
 	int nrb_tiny;
 	int nrb_small;
 	int nrb_large;
@@ -1591,7 +1593,7 @@ lnet_rtrpools_alloc(int im_a_router)
 
 	the_lnet.ln_rtrpools = cfs_percpt_alloc(lnet_cpt_table(),
 						LNET_NRBPOOLS *
-						sizeof(lnet_rtrbufpool_t));
+						sizeof(struct lnet_rtrbufpool));
 	if (!the_lnet.ln_rtrpools) {
 		LCONSOLE_ERROR_MSG(0x10c,
 				   "Failed to initialize router buffe pool\n");
@@ -1637,7 +1639,7 @@ lnet_rtrpools_adjust_helper(int tiny, int small, int large)
 	int nrb = 0;
 	int rc = 0;
 	int i;
-	lnet_rtrbufpool_t *rtrp;
+	struct lnet_rtrbufpool *rtrp;
 
 	/*
 	 * If the provided values for each buffer pool are different than the
@@ -1740,7 +1742,7 @@ lnet_rtrpools_disable(void)
 }
 
 int
-lnet_notify(lnet_ni_t *ni, lnet_nid_t nid, int alive, unsigned long when)
+lnet_notify(struct lnet_ni *ni, lnet_nid_t nid, int alive, unsigned long when)
 {
 	struct lnet_peer *lp = NULL;
 	unsigned long now = cfs_time_current();
diff --git a/drivers/staging/lustre/lnet/lnet/router_proc.c b/drivers/staging/lustre/lnet/lnet/router_proc.c
index a19e1405e3ea..72b80c594108 100644
--- a/drivers/staging/lustre/lnet/lnet/router_proc.c
+++ b/drivers/staging/lustre/lnet/lnet/router_proc.c
@@ -77,7 +77,7 @@ static int __proc_lnet_stats(void *data, int write,
 			     loff_t pos, void __user *buffer, int nob)
 {
 	int rc;
-	lnet_counters_t *ctrs;
+	struct lnet_counters *ctrs;
 	int len;
 	char *tmpstr;
 	const int tmpsiz = 256; /* 7 %u and 4 %llu */
@@ -171,8 +171,8 @@ static int proc_lnet_routes(struct ctl_table *table, int write,
 	} else {
 		struct list_head *n;
 		struct list_head *r;
-		lnet_route_t *route = NULL;
-		lnet_remotenet_t *rnet  = NULL;
+		struct lnet_route *route = NULL;
+		struct lnet_remotenet *rnet  = NULL;
 		int skip  = off - 1;
 		struct list_head *rn_list;
 		int i;
@@ -191,15 +191,16 @@ static int proc_lnet_routes(struct ctl_table *table, int write,
 			n = rn_list->next;
 
 			while (n != rn_list && !route) {
-				rnet = list_entry(n, lnet_remotenet_t,
+				rnet = list_entry(n, struct lnet_remotenet,
 						  lrn_list);
 
 				r = rnet->lrn_routes.next;
 
 				while (r != &rnet->lrn_routes) {
-					lnet_route_t *re =
-						list_entry(r, lnet_route_t,
-							   lr_list);
+					struct lnet_route *re;
+
+					re = list_entry(r, struct lnet_route,
+							lr_list);
 					if (!skip) {
 						route = re;
 						break;
@@ -307,9 +308,9 @@ static int proc_lnet_routers(struct ctl_table *table, int write,
 		r = the_lnet.ln_routers.next;
 
 		while (r != &the_lnet.ln_routers) {
-			lnet_peer_t *lp = list_entry(r, lnet_peer_t,
-						     lp_rtr_list);
+			struct lnet_peer *lp;
 
+			lp = list_entry(r, struct lnet_peer, lp_rtr_list);
 			if (!skip) {
 				peer = lp;
 				break;
@@ -331,7 +332,7 @@ static int proc_lnet_routers(struct ctl_table *table, int write,
 			int last_ping = cfs_duration_sec(cfs_time_sub(now,
 						     peer->lp_ping_timestamp));
 			int down_ni = 0;
-			lnet_route_t *rtr;
+			struct lnet_route *rtr;
 
 			if ((peer->lp_ping_feats &
 			     LNET_PING_FEAT_NI_STATUS)) {
@@ -454,8 +455,10 @@ static int proc_lnet_peers(struct ctl_table *table, int write,
 				p = ptable->pt_hash[hash].next;
 
 			while (p != &ptable->pt_hash[hash]) {
-				lnet_peer_t *lp = list_entry(p, lnet_peer_t,
-							     lp_hashlist);
+				struct lnet_peer *lp;
+
+				lp = list_entry(p, struct lnet_peer,
+						lp_hashlist);
 				if (!skip) {
 					peer = lp;
 
@@ -589,7 +592,7 @@ static int __proc_lnet_buffers(void *data, int write,
 		goto out; /* I'm not a router */
 
 	for (idx = 0; idx < LNET_NRBPOOLS; idx++) {
-		lnet_rtrbufpool_t *rbp;
+		struct lnet_rtrbufpool *rbp;
 
 		lnet_net_lock(LNET_LOCK_EX);
 		cfs_percpt_for_each(rbp, i, the_lnet.ln_rtrpools) {
@@ -652,7 +655,7 @@ static int proc_lnet_nis(struct ctl_table *table, int write,
 		LASSERT(tmpstr + tmpsiz - s > 0);
 	} else {
 		struct list_head *n;
-		lnet_ni_t *ni   = NULL;
+		struct lnet_ni *ni = NULL;
 		int skip = *ppos - 1;
 
 		lnet_net_lock(0);
@@ -660,8 +663,9 @@ static int proc_lnet_nis(struct ctl_table *table, int write,
 		n = the_lnet.ln_nis.next;
 
 		while (n != &the_lnet.ln_nis) {
-			lnet_ni_t *a_ni = list_entry(n, lnet_ni_t, ni_list);
+			struct lnet_ni *a_ni;
 
+			a_ni = list_entry(n, struct lnet_ni, ni_list);
 			if (!skip) {
 				ni = a_ni;
 				break;
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index b9ac34ecbd53..f8b9175f08d4 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -267,8 +267,8 @@ brw_check_bulk(struct srpc_bulk *bk, int pattern, __u64 magic)
 }
 
 static int
-brw_client_prep_rpc(struct sfw_test_unit *tsu,
-		    lnet_process_id_t dest, struct srpc_client_rpc **rpcpp)
+brw_client_prep_rpc(struct sfw_test_unit *tsu, struct lnet_process_id dest,
+		    struct srpc_client_rpc **rpcpp)
 {
 	struct srpc_bulk *bulk = tsu->tsu_private;
 	struct sfw_test_instance *tsi = tsu->tsu_instance;
diff --git a/drivers/staging/lustre/lnet/selftest/conrpc.c b/drivers/staging/lustre/lnet/selftest/conrpc.c
index c6a683bda75e..da36c55b86d3 100644
--- a/drivers/staging/lustre/lnet/selftest/conrpc.c
+++ b/drivers/staging/lustre/lnet/selftest/conrpc.c
@@ -505,7 +505,7 @@ lstcon_rpc_trans_interpreter(struct lstcon_rpc_trans *trans,
 		jiffies_to_timeval(dur, &tv);
 
 		if (copy_to_user(&ent->rpe_peer, &nd->nd_id,
-				 sizeof(lnet_process_id_t)) ||
+				 sizeof(struct lnet_process_id)) ||
 		    copy_to_user(&ent->rpe_stamp, &tv, sizeof(tv)) ||
 		    copy_to_user(&ent->rpe_state, &nd->nd_state,
 				 sizeof(nd->nd_state)) ||
@@ -699,7 +699,7 @@ lstcon_statrpc_prep(struct lstcon_node *nd, unsigned int feats,
 }
 
 static struct lnet_process_id_packed *
-lstcon_next_id(int idx, int nkiov, lnet_kiov_t *kiov)
+lstcon_next_id(int idx, int nkiov, struct bio_vec *kiov)
 {
 	struct lnet_process_id_packed *pid;
 	int i;
@@ -715,7 +715,7 @@ lstcon_next_id(int idx, int nkiov, lnet_kiov_t *kiov)
 
 static int
 lstcon_dstnodes_prep(struct lstcon_group *grp, int idx,
-		     int dist, int span, int nkiov, lnet_kiov_t *kiov)
+		     int dist, int span, int nkiov, struct bio_vec *kiov)
 {
 	struct lnet_process_id_packed *pid;
 	struct lstcon_ndlink *ndl;
@@ -785,8 +785,7 @@ lstcon_bulkrpc_v0_prep(struct lst_test_bulk_param *param,
 	struct test_bulk_req *brq = &req->tsr_u.bulk_v0;
 
 	brq->blk_opc = param->blk_opc;
-	brq->blk_npg = (param->blk_size + PAGE_SIZE - 1) /
-			PAGE_SIZE;
+	brq->blk_npg = DIV_ROUND_UP(param->blk_size, PAGE_SIZE);
 	brq->blk_flags = param->blk_flags;
 
 	return 0;
@@ -833,11 +832,9 @@ lstcon_testrpc_prep(struct lstcon_node *nd, int transop, unsigned int feats,
 	trq = &(*crpc)->crp_rpc->crpc_reqstmsg.msg_body.tes_reqst;
 
 	if (transop == LST_TRANS_TSBSRVADD) {
-		int ndist = (sgrp->grp_nnode + test->tes_dist - 1) /
-			    test->tes_dist;
-		int nspan = (dgrp->grp_nnode + test->tes_span - 1) /
-			    test->tes_span;
-		int nmax = (ndist + nspan - 1) / nspan;
+		int ndist = DIV_ROUND_UP(sgrp->grp_nnode, test->tes_dist);
+		int nspan = DIV_ROUND_UP(dgrp->grp_nnode, test->tes_span);
+		int nmax = DIV_ROUND_UP(ndist, nspan);
 
 		trq->tsr_ndest = 0;
 		trq->tsr_loop = nmax * test->tes_dist * test->tes_concur;
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index 4e7e5c862c64..d62c448ecf8a 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -65,7 +65,8 @@ lstcon_node_get(struct lstcon_node *nd)
 }
 
 static int
-lstcon_node_find(lnet_process_id_t id, struct lstcon_node **ndpp, int create)
+lstcon_node_find(struct lnet_process_id id, struct lstcon_node **ndpp,
+		 int create)
 {
 	struct lstcon_ndlink	*ndl;
 	unsigned int idx = LNET_NIDADDR(id.nid) % LST_GLOBAL_HASHSIZE;
@@ -135,7 +136,7 @@ lstcon_node_put(struct lstcon_node *nd)
 }
 
 static int
-lstcon_ndlink_find(struct list_head *hash, lnet_process_id_t id,
+lstcon_ndlink_find(struct list_head *hash, struct lnet_process_id id,
 		   struct lstcon_ndlink **ndlpp, int create)
 {
 	unsigned int idx = LNET_NIDADDR(id.nid) % LST_NODE_HASHSIZE;
@@ -283,7 +284,7 @@ lstcon_group_find(const char *name, struct lstcon_group **grpp)
 }
 
 static int
-lstcon_group_ndlink_find(struct lstcon_group *grp, lnet_process_id_t id,
+lstcon_group_ndlink_find(struct lstcon_group *grp, struct lnet_process_id id,
 			 struct lstcon_ndlink **ndlpp, int create)
 {
 	int rc;
@@ -397,14 +398,14 @@ lstcon_sesrpc_readent(int transop, struct srpc_msg *msg,
 
 static int
 lstcon_group_nodes_add(struct lstcon_group *grp,
-		       int count, lnet_process_id_t __user *ids_up,
+		       int count, struct lnet_process_id __user *ids_up,
 		       unsigned int *featp,
 		       struct list_head __user *result_up)
 {
 	struct lstcon_rpc_trans *trans;
 	struct lstcon_ndlink	*ndl;
 	struct lstcon_group *tmp;
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 	int i;
 	int rc;
 
@@ -465,13 +466,13 @@ lstcon_group_nodes_add(struct lstcon_group *grp,
 
 static int
 lstcon_group_nodes_remove(struct lstcon_group *grp,
-			  int count, lnet_process_id_t __user *ids_up,
+			  int count, struct lnet_process_id __user *ids_up,
 			  struct list_head __user *result_up)
 {
 	struct lstcon_rpc_trans *trans;
 	struct lstcon_ndlink *ndl;
 	struct lstcon_group *tmp;
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 	int rc;
 	int i;
 
@@ -543,9 +544,8 @@ lstcon_group_add(char *name)
 }
 
 int
-lstcon_nodes_add(char *name, int count, lnet_process_id_t __user *ids_up,
-		 unsigned int *featp,
-		 struct list_head __user *result_up)
+lstcon_nodes_add(char *name, int count, struct lnet_process_id __user *ids_up,
+		 unsigned int *featp, struct list_head __user *result_up)
 {
 	struct lstcon_group *grp;
 	int rc;
@@ -650,7 +650,8 @@ lstcon_group_clean(char *name, int args)
 }
 
 int
-lstcon_nodes_remove(char *name, int count, lnet_process_id_t __user *ids_up,
+lstcon_nodes_remove(char *name, int count,
+		    struct lnet_process_id __user *ids_up,
 		    struct list_head __user *result_up)
 {
 	struct lstcon_group *grp = NULL;
@@ -1469,14 +1470,14 @@ lstcon_statrpc_readent(int transop, struct srpc_msg *msg,
 	struct srpc_stat_reply *rep = &msg->msg_body.stat_reply;
 	struct sfw_counters __user *sfwk_stat;
 	struct srpc_counters __user *srpc_stat;
-	lnet_counters_t __user *lnet_stat;
+	struct lnet_counters __user *lnet_stat;
 
 	if (rep->str_status)
 		return 0;
 
 	sfwk_stat = (struct sfw_counters __user *)&ent_up->rpe_payload[0];
 	srpc_stat = (struct srpc_counters __user *)(sfwk_stat + 1);
-	lnet_stat = (lnet_counters_t __user *)(srpc_stat + 1);
+	lnet_stat = (struct lnet_counters __user *)(srpc_stat + 1);
 
 	if (copy_to_user(sfwk_stat, &rep->str_fw, sizeof(*sfwk_stat)) ||
 	    copy_to_user(srpc_stat, &rep->str_rpc, sizeof(*srpc_stat)) ||
@@ -1533,12 +1534,12 @@ lstcon_group_stat(char *grp_name, int timeout,
 }
 
 int
-lstcon_nodes_stat(int count, lnet_process_id_t __user *ids_up,
+lstcon_nodes_stat(int count, struct lnet_process_id __user *ids_up,
 		  int timeout, struct list_head __user *result_up)
 {
 	struct lstcon_ndlink	*ndl;
 	struct lstcon_group *tmp;
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 	int i;
 	int rc;
 
@@ -1644,11 +1645,11 @@ lstcon_group_debug(int timeout, char *name,
 }
 
 int
-lstcon_nodes_debug(int timeout,
-		   int count, lnet_process_id_t __user *ids_up,
+lstcon_nodes_debug(int timeout, int count,
+		   struct lnet_process_id __user *ids_up,
 		   struct list_head __user *result_up)
 {
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 	struct lstcon_ndlink *ndl;
 	struct lstcon_group *grp;
 	int i;
@@ -1697,7 +1698,7 @@ lstcon_session_match(struct lst_sid sid)
 static void
 lstcon_new_session_id(struct lst_sid *sid)
 {
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 
 	LASSERT(console_session.ses_state == LST_SESSION_NONE);
 
diff --git a/drivers/staging/lustre/lnet/selftest/console.h b/drivers/staging/lustre/lnet/selftest/console.h
index 05b4b7013d2e..e3e11aa52526 100644
--- a/drivers/staging/lustre/lnet/selftest/console.h
+++ b/drivers/staging/lustre/lnet/selftest/console.h
@@ -48,7 +48,7 @@
 
 /* node descriptor */
 struct lstcon_node {
-	lnet_process_id_t nd_id;      /* id of the node */
+	struct lnet_process_id	nd_id;	/* id of the node */
 	int		  nd_ref;     /* reference count */
 	int		  nd_state;   /* state of the node */
 	int		  nd_timeout; /* session timeout */
@@ -180,7 +180,7 @@ lstcon_trans_stat(void)
 }
 
 static inline struct list_head *
-lstcon_id2hash(lnet_process_id_t id, struct list_head *hash)
+lstcon_id2hash(struct lnet_process_id id, struct list_head *hash)
 {
 	unsigned int idx = LNET_NIDADDR(id.nid) % LST_NODE_HASHSIZE;
 
@@ -203,15 +203,17 @@ int lstcon_batch_debug(int timeout, char *name,
 		       int client, struct list_head __user *result_up);
 int lstcon_group_debug(int timeout, char *name,
 		       struct list_head __user *result_up);
-int lstcon_nodes_debug(int timeout, int nnd, lnet_process_id_t __user *nds_up,
+int lstcon_nodes_debug(int timeout, int nnd,
+		       struct lnet_process_id __user *nds_up,
 		       struct list_head __user *result_up);
 int lstcon_group_add(char *name);
 int lstcon_group_del(char *name);
 int lstcon_group_clean(char *name, int args);
 int lstcon_group_refresh(char *name, struct list_head __user *result_up);
-int lstcon_nodes_add(char *name, int nnd, lnet_process_id_t __user *nds_up,
+int lstcon_nodes_add(char *name, int nnd, struct lnet_process_id __user *nds_up,
 		     unsigned int *featp, struct list_head __user *result_up);
-int lstcon_nodes_remove(char *name, int nnd, lnet_process_id_t __user *nds_up,
+int lstcon_nodes_remove(char *name, int nnd,
+			struct lnet_process_id __user *nds_up,
 			struct list_head __user *result_up);
 int lstcon_group_info(char *name, struct lstcon_ndlist_ent __user *gent_up,
 		      int *index_p, int *ndent_p,
@@ -232,7 +234,7 @@ int lstcon_batch_info(char *name, struct lstcon_test_batch_ent __user *ent_up,
 		      int *ndent_p, struct lstcon_node_ent __user *dents_up);
 int lstcon_group_stat(char *grp_name, int timeout,
 		      struct list_head __user *result_up);
-int lstcon_nodes_stat(int count, lnet_process_id_t __user *ids_up,
+int lstcon_nodes_stat(int count, struct lnet_process_id __user *ids_up,
 		      int timeout, struct list_head __user *result_up);
 int lstcon_test_add(char *batch_name, int type, int loop,
 		    int concur, int dist, int span,
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index 9dd4e1a70329..ef27bfffc230 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -899,7 +899,7 @@ sfw_test_rpc_done(struct srpc_client_rpc *rpc)
 }
 
 int
-sfw_create_test_rpc(struct sfw_test_unit *tsu, lnet_process_id_t peer,
+sfw_create_test_rpc(struct sfw_test_unit *tsu, struct lnet_process_id peer,
 		    unsigned int features, int nblk, int blklen,
 		    struct srpc_client_rpc **rpcpp)
 {
@@ -1379,7 +1379,7 @@ sfw_bulk_ready(struct srpc_server_rpc *rpc, int status)
 }
 
 struct srpc_client_rpc *
-sfw_create_rpc(lnet_process_id_t peer, int service,
+sfw_create_rpc(struct lnet_process_id peer, int service,
 	       unsigned int features, int nbulkiov, int bulklen,
 	       void (*done)(struct srpc_client_rpc *), void *priv)
 {
diff --git a/drivers/staging/lustre/lnet/selftest/ping_test.c b/drivers/staging/lustre/lnet/selftest/ping_test.c
index b9601b00a273..9653ac6fd619 100644
--- a/drivers/staging/lustre/lnet/selftest/ping_test.c
+++ b/drivers/staging/lustre/lnet/selftest/ping_test.c
@@ -82,7 +82,7 @@ ping_client_fini(struct sfw_test_instance *tsi)
 }
 
 static int
-ping_client_prep_rpc(struct sfw_test_unit *tsu, lnet_process_id_t dest,
+ping_client_prep_rpc(struct sfw_test_unit *tsu, struct lnet_process_id dest,
 		     struct srpc_client_rpc **rpc)
 {
 	struct srpc_ping_reqst *req;
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.c b/drivers/staging/lustre/lnet/selftest/rpc.c
index 87fe366f8f70..77c222cca230 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.c
+++ b/drivers/staging/lustre/lnet/selftest/rpc.c
@@ -53,7 +53,7 @@ enum srpc_state {
 static struct smoketest_rpc {
 	spinlock_t	 rpc_glock;	/* global lock */
 	struct srpc_service	*rpc_services[SRPC_SERVICE_MAX_ID + 1];
-	lnet_handle_eq_t rpc_lnet_eq;	/* _the_ LNet event queue */
+	struct lnet_handle_eq	 rpc_lnet_eq;	/* _the_ LNet event queue */
 	enum srpc_state	 rpc_state;
 	struct srpc_counters	 rpc_counters;
 	__u64		 rpc_matchbits;	/* matchbits counter */
@@ -185,7 +185,7 @@ srpc_init_server_rpc(struct srpc_server_rpc *rpc,
 	rpc->srpc_reqstbuf = buffer;
 	rpc->srpc_peer = buffer->buf_peer;
 	rpc->srpc_self = buffer->buf_self;
-	LNetInvalidateHandle(&rpc->srpc_replymdh);
+	LNetInvalidateMDHandle(&rpc->srpc_replymdh);
 }
 
 static void
@@ -355,12 +355,12 @@ srpc_remove_service(struct srpc_service *sv)
 
 static int
 srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
-		       int len, int options, lnet_process_id_t peer,
-		       lnet_handle_md_t *mdh, struct srpc_event *ev)
+		       int len, int options, struct lnet_process_id peer,
+		       struct lnet_handle_md *mdh, struct srpc_event *ev)
 {
 	int rc;
-	lnet_md_t md;
-	lnet_handle_me_t meh;
+	struct lnet_md md;
+	struct lnet_handle_me meh;
 
 	rc = LNetMEAttach(portal, peer, matchbits, 0, LNET_UNLINK,
 			  local ? LNET_INS_LOCAL : LNET_INS_AFTER, &meh);
@@ -394,11 +394,12 @@ srpc_post_passive_rdma(int portal, int local, __u64 matchbits, void *buf,
 
 static int
 srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
-		      int options, lnet_process_id_t peer, lnet_nid_t self,
-		      lnet_handle_md_t *mdh, struct srpc_event *ev)
+		      int options, struct lnet_process_id peer,
+		      lnet_nid_t self, struct lnet_handle_md *mdh,
+		      struct srpc_event *ev)
 {
 	int rc;
-	lnet_md_t md;
+	struct lnet_md md;
 
 	md.user_ptr = ev;
 	md.start = buf;
@@ -448,9 +449,9 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 
 static int
 srpc_post_passive_rqtbuf(int service, int local, void *buf, int len,
-			 lnet_handle_md_t *mdh, struct srpc_event *ev)
+			 struct lnet_handle_md *mdh, struct srpc_event *ev)
 {
-	lnet_process_id_t any = { 0 };
+	struct lnet_process_id any = { 0 };
 
 	any.nid = LNET_NID_ANY;
 	any.pid = LNET_PID_ANY;
@@ -468,7 +469,7 @@ __must_hold(&scd->scd_lock)
 	struct srpc_msg	*msg = &buf->buf_msg;
 	int rc;
 
-	LNetInvalidateHandle(&buf->buf_mdh);
+	LNetInvalidateMDHandle(&buf->buf_mdh);
 	list_add(&buf->buf_list, &scd->scd_buf_posted);
 	scd->scd_buf_nposted++;
 	spin_unlock(&scd->scd_lock);
@@ -1310,7 +1311,7 @@ abort:
 }
 
 struct srpc_client_rpc *
-srpc_create_client_rpc(lnet_process_id_t peer, int service,
+srpc_create_client_rpc(struct lnet_process_id peer, int service,
 		       int nbulkiov, int bulklen,
 		       void (*rpc_done)(struct srpc_client_rpc *),
 		       void (*rpc_fini)(struct srpc_client_rpc *), void *priv)
@@ -1408,7 +1409,7 @@ srpc_send_reply(struct srpc_server_rpc *rpc)
 
 /* when in kernel always called with LNET_LOCK() held, and in thread context */
 static void
-srpc_lnet_ev_handler(lnet_event_t *ev)
+srpc_lnet_ev_handler(struct lnet_event *ev)
 {
 	struct srpc_service_cd *scd;
 	struct srpc_event *rpcev = ev->md.user_ptr;
@@ -1622,7 +1623,7 @@ srpc_startup(void)
 
 	srpc_data.rpc_state = SRPC_STATE_NI_INIT;
 
-	LNetInvalidateHandle(&srpc_data.rpc_lnet_eq);
+	LNetInvalidateEQHandle(&srpc_data.rpc_lnet_eq);
 	rc = LNetEQAlloc(0, srpc_lnet_ev_handler, &srpc_data.rpc_lnet_eq);
 	if (rc) {
 		CERROR("LNetEQAlloc() has failed: %d\n", rc);
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.h b/drivers/staging/lustre/lnet/selftest/rpc.h
index 418c9c96abe6..a765537a79c4 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.h
+++ b/drivers/staging/lustre/lnet/selftest/rpc.h
@@ -163,7 +163,7 @@ struct srpc_stat_reply {
 	struct lst_sid	   str_sid;
 	struct sfw_counters	str_fw;
 	struct srpc_counters	str_rpc;
-	lnet_counters_t    str_lnet;
+	struct lnet_counters    str_lnet;
 } WIRE_ATTR;
 
 struct test_bulk_req {
diff --git a/drivers/staging/lustre/lnet/selftest/selftest.h b/drivers/staging/lustre/lnet/selftest/selftest.h
index f25948087ee0..b614e6f23a70 100644
--- a/drivers/staging/lustre/lnet/selftest/selftest.h
+++ b/drivers/staging/lustre/lnet/selftest/selftest.h
@@ -144,7 +144,7 @@ enum srpc_event_type {
 /* RPC event */
 struct srpc_event {
 	enum srpc_event_type	ev_type;	/* what's up */
-	lnet_event_kind_t ev_lnet;   /* LNet event type */
+	enum lnet_event_kind	ev_lnet;	/* LNet event type */
 	int		  ev_fired;  /* LNet event fired? */
 	int		  ev_status; /* LNet event status */
 	void		  *ev_data;  /* owning server/client RPC */
@@ -153,19 +153,19 @@ struct srpc_event {
 /* bulk descriptor */
 struct srpc_bulk {
 	int		 bk_len;     /* len of bulk data */
-	lnet_handle_md_t bk_mdh;
+	struct lnet_handle_md	bk_mdh;
 	int		 bk_sink;    /* sink/source */
 	int		 bk_niov;    /* # iov in bk_iovs */
-	lnet_kiov_t	 bk_iovs[0];
+	struct bio_vec		bk_iovs[0];
 };
 
 /* message buffer descriptor */
 struct srpc_buffer {
 	struct list_head  buf_list; /* chain on srpc_service::*_msgq */
 	struct srpc_msg	  buf_msg;
-	lnet_handle_md_t  buf_mdh;
+	struct lnet_handle_md	buf_mdh;
 	lnet_nid_t	  buf_self;
-	lnet_process_id_t buf_peer;
+	struct lnet_process_id	buf_peer;
 };
 
 struct swi_workitem;
@@ -186,9 +186,9 @@ struct srpc_server_rpc {
 	struct swi_workitem	srpc_wi;
 	struct srpc_event	srpc_ev;	/* bulk/reply event */
 	lnet_nid_t	       srpc_self;
-	lnet_process_id_t      srpc_peer;
+	struct lnet_process_id	srpc_peer;
 	struct srpc_msg		srpc_replymsg;
-	lnet_handle_md_t       srpc_replymdh;
+	struct lnet_handle_md	srpc_replymdh;
 	struct srpc_buffer	*srpc_reqstbuf;
 	struct srpc_bulk	*srpc_bulk;
 
@@ -206,7 +206,7 @@ struct srpc_client_rpc {
 	int		  crpc_timeout;   /* # seconds to wait for reply */
 	struct stt_timer       crpc_timer;
 	struct swi_workitem	crpc_wi;
-	lnet_process_id_t crpc_dest;
+	struct lnet_process_id	crpc_dest;
 
 	void		  (*crpc_done)(struct srpc_client_rpc *);
 	void		  (*crpc_fini)(struct srpc_client_rpc *);
@@ -225,8 +225,8 @@ struct srpc_client_rpc {
 	/* bulk, request(reqst), and reply exchanged on wire */
 	struct srpc_msg		crpc_reqstmsg;
 	struct srpc_msg		crpc_replymsg;
-	lnet_handle_md_t  crpc_reqstmdh;
-	lnet_handle_md_t  crpc_replymdh;
+	struct lnet_handle_md	crpc_reqstmdh;
+	struct lnet_handle_md	crpc_replymdh;
 	struct srpc_bulk	crpc_bulk;
 };
 
@@ -355,7 +355,7 @@ struct sfw_test_client_ops {
 							  * client
 							  */
 	int  (*tso_prep_rpc)(struct sfw_test_unit *tsu,
-			     lnet_process_id_t dest,
+			     struct lnet_process_id dest,
 			     struct srpc_client_rpc **rpc);	/* prep a tests rpc */
 	void (*tso_done_rpc)(struct sfw_test_unit *tsu,
 			     struct srpc_client_rpc *rpc);	/* done a test rpc */
@@ -392,8 +392,8 @@ struct sfw_test_instance {
 };
 
 /*
- * XXX: trailing (PAGE_SIZE % sizeof(lnet_process_id_t)) bytes at the end of
- * pages are not used
+ * XXX: trailing (PAGE_SIZE % sizeof(struct lnet_process_id)) bytes at the end
+ * of pages are not used
  */
 #define SFW_MAX_CONCUR	   LST_MAX_CONCUR
 #define SFW_ID_PER_PAGE    (PAGE_SIZE / sizeof(struct lnet_process_id_packed))
@@ -402,7 +402,7 @@ struct sfw_test_instance {
 
 struct sfw_test_unit {
 	struct list_head    tsu_list;	   /* chain on lst_test_instance */
-	lnet_process_id_t   tsu_dest;	   /* id of dest node */
+	struct lnet_process_id		tsu_dest;	/* id of dest node */
 	int		    tsu_loop;	   /* loop count of the test */
 	struct sfw_test_instance	*tsu_instance; /* pointer to test instance */
 	void		    *tsu_private;  /* private data */
@@ -416,11 +416,11 @@ struct sfw_test_case {
 };
 
 struct srpc_client_rpc *
-sfw_create_rpc(lnet_process_id_t peer, int service,
+sfw_create_rpc(struct lnet_process_id peer, int service,
 	       unsigned int features, int nbulkiov, int bulklen,
 	       void (*done)(struct srpc_client_rpc *), void *priv);
 int sfw_create_test_rpc(struct sfw_test_unit *tsu,
-			lnet_process_id_t peer, unsigned int features,
+			struct lnet_process_id peer, unsigned int features,
 			int nblk, int blklen, struct srpc_client_rpc **rpc);
 void sfw_abort_rpc(struct srpc_client_rpc *rpc);
 void sfw_post_rpc(struct srpc_client_rpc *rpc);
@@ -434,7 +434,7 @@ int sfw_make_session(struct srpc_mksn_reqst *request,
 		     struct srpc_mksn_reply *reply);
 
 struct srpc_client_rpc *
-srpc_create_client_rpc(lnet_process_id_t peer, int service,
+srpc_create_client_rpc(struct lnet_process_id peer, int service,
 		       int nbulkiov, int bulklen,
 		       void (*rpc_done)(struct srpc_client_rpc *),
 		       void (*rpc_fini)(struct srpc_client_rpc *), void *priv);
@@ -522,7 +522,7 @@ srpc_destroy_client_rpc(struct srpc_client_rpc *rpc)
 }
 
 static inline void
-srpc_init_client_rpc(struct srpc_client_rpc *rpc, lnet_process_id_t peer,
+srpc_init_client_rpc(struct srpc_client_rpc *rpc, struct lnet_process_id peer,
 		     int service, int nbulkiov, int bulklen,
 		     void (*rpc_done)(struct srpc_client_rpc *),
 		     void (*rpc_fini)(struct srpc_client_rpc *), void *priv)
@@ -545,9 +545,9 @@ srpc_init_client_rpc(struct srpc_client_rpc *rpc, lnet_process_id_t peer,
 	rpc->crpc_bulk.bk_niov = nbulkiov;
 	rpc->crpc_done = rpc_done;
 	rpc->crpc_fini = rpc_fini;
-	LNetInvalidateHandle(&rpc->crpc_reqstmdh);
-	LNetInvalidateHandle(&rpc->crpc_replymdh);
-	LNetInvalidateHandle(&rpc->crpc_bulk.bk_mdh);
+	LNetInvalidateMDHandle(&rpc->crpc_reqstmdh);
+	LNetInvalidateMDHandle(&rpc->crpc_replymdh);
+	LNetInvalidateMDHandle(&rpc->crpc_bulk.bk_mdh);
 
 	/* no event is expected at this point */
 	rpc->crpc_bulkev.ev_fired = 1;
diff --git a/drivers/staging/lustre/lustre/include/cl_object.h b/drivers/staging/lustre/lustre/include/cl_object.h
index e4c0c440f01b..2bc3ee51b069 100644
--- a/drivers/staging/lustre/lustre/include/cl_object.h
+++ b/drivers/staging/lustre/lustre/include/cl_object.h
@@ -1640,9 +1640,14 @@ enum cl_enq_flags {
 	 */
 	CEF_PEEK	= 0x00000040,
 	/**
+	 * Lock match only. Used by group lock in I/O as group lock
+	 * is known to exist.
+	 */
+	CEF_LOCK_MATCH	= BIT(7),
+	/**
 	 * mask of enq_flags.
 	 */
-	CEF_MASK         = 0x0000007f,
+	CEF_MASK	= 0x000000ff,
 };
 
 /**
@@ -2432,9 +2437,9 @@ void cl_sync_io_end(const struct lu_env *env, struct cl_sync_io *anchor);
  * @{
  */
 
-struct lu_env *cl_env_get(int *refcheck);
-struct lu_env *cl_env_alloc(int *refcheck, __u32 tags);
-void cl_env_put(struct lu_env *env, int *refcheck);
+struct lu_env *cl_env_get(u16 *refcheck);
+struct lu_env *cl_env_alloc(u16 *refcheck, __u32 tags);
+void cl_env_put(struct lu_env *env, u16 *refcheck);
 unsigned int cl_env_cache_purge(unsigned int nr);
 struct lu_env *cl_env_percpu_get(void);
 void cl_env_percpu_put(struct lu_env *env);
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 62753dae0bfa..242abb881766 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -374,94 +374,15 @@ int lprocfs_write_frac_helper(const char __user *buffer,
 			      unsigned long count, int *val, int mult);
 int lprocfs_read_frac_helper(char *buffer, unsigned long count,
 			     long val, int mult);
-int lprocfs_stats_alloc_one(struct lprocfs_stats *stats, unsigned int cpuid);
 
-/**
- * Lock statistics structure for access, possibly only on this CPU.
- *
- * The statistics struct may be allocated with per-CPU structures for
- * efficient concurrent update (usually only on server-wide stats), or
- * as a single global struct (e.g. for per-client or per-job statistics),
- * so the required locking depends on the type of structure allocated.
- *
- * For per-CPU statistics, pin the thread to the current cpuid so that
- * will only access the statistics for that CPU.  If the stats structure
- * for the current CPU has not been allocated (or previously freed),
- * allocate it now.  The per-CPU statistics do not need locking since
- * the thread is pinned to the CPU during update.
- *
- * For global statistics, lock the stats structure to prevent concurrent update.
- *
- * \param[in] stats	statistics structure to lock
- * \param[in] opc	type of operation:
- *			LPROCFS_GET_SMP_ID: "lock" and return current CPU index
- *				for incrementing statistics for that CPU
- *			LPROCFS_GET_NUM_CPU: "lock" and return number of used
- *				CPU indices to iterate over all indices
- * \param[out] flags	CPU interrupt saved state for IRQ-safe locking
- *
- * \retval cpuid of current thread or number of allocated structs
- * \retval negative on error (only for opc LPROCFS_GET_SMP_ID + per-CPU stats)
- */
-static inline int lprocfs_stats_lock(struct lprocfs_stats *stats,
-				     enum lprocfs_stats_lock_ops opc,
-				     unsigned long *flags)
-{
-	if (stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) {
-		if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
-			spin_lock_irqsave(&stats->ls_lock, *flags);
-		else
-			spin_lock(&stats->ls_lock);
-		return opc == LPROCFS_GET_NUM_CPU ? 1 : 0;
-	}
-
-	switch (opc) {
-	case LPROCFS_GET_SMP_ID: {
-		unsigned int cpuid = get_cpu();
-
-		if (unlikely(!stats->ls_percpu[cpuid])) {
-			int rc = lprocfs_stats_alloc_one(stats, cpuid);
-
-			if (rc < 0) {
-				put_cpu();
-				return rc;
-			}
-		}
-		return cpuid;
-	}
-	case LPROCFS_GET_NUM_CPU:
-		return stats->ls_biggest_alloc_num;
-	default:
-		LBUG();
-	}
-}
-
-/**
- * Unlock statistics structure after access.
- *
- * Unlock the lock acquired via lprocfs_stats_lock() for global statistics,
- * or unpin this thread from the current cpuid for per-CPU statistics.
- *
- * This function must be called using the same arguments as used when calling
- * lprocfs_stats_lock() so that the correct operation can be performed.
- *
- * \param[in] stats	statistics structure to unlock
- * \param[in] opc	type of operation (current cpuid or number of structs)
- * \param[in] flags	CPU interrupt saved state for IRQ-safe locking
- */
-static inline void lprocfs_stats_unlock(struct lprocfs_stats *stats,
-					enum lprocfs_stats_lock_ops opc,
-					unsigned long *flags)
-{
-	if (stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) {
-		if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
-			spin_unlock_irqrestore(&stats->ls_lock, *flags);
-		else
-			spin_unlock(&stats->ls_lock);
-	} else if (opc == LPROCFS_GET_SMP_ID) {
-		put_cpu();
-	}
-}
+int lprocfs_stats_alloc_one(struct lprocfs_stats *stats,
+			    unsigned int cpuid);
+int lprocfs_stats_lock(struct lprocfs_stats *stats,
+		       enum lprocfs_stats_lock_ops opc,
+		       unsigned long *flags);
+void lprocfs_stats_unlock(struct lprocfs_stats *stats,
+			  enum lprocfs_stats_lock_ops opc,
+			  unsigned long *flags);
 
 static inline unsigned int
 lprocfs_stats_counter_size(struct lprocfs_stats *stats)
@@ -513,29 +434,8 @@ __s64 lprocfs_read_helper(struct lprocfs_counter *lc,
 			  struct lprocfs_counter_header *header,
 			  enum lprocfs_stats_flags flags,
 			  enum lprocfs_fields_flags field);
-static inline __u64 lprocfs_stats_collector(struct lprocfs_stats *stats,
-					    int idx,
-					    enum lprocfs_fields_flags field)
-{
-	unsigned int i;
-	unsigned int  num_cpu;
-	unsigned long flags	= 0;
-	__u64	      ret	= 0;
-
-	LASSERT(stats);
-
-	num_cpu = lprocfs_stats_lock(stats, LPROCFS_GET_NUM_CPU, &flags);
-	for (i = 0; i < num_cpu; i++) {
-		if (!stats->ls_percpu[i])
-			continue;
-		ret += lprocfs_read_helper(
-				lprocfs_stats_counter_get(stats, i, idx),
-				&stats->ls_cnt_header[idx], stats->ls_flags,
-				field);
-	}
-	lprocfs_stats_unlock(stats, LPROCFS_GET_NUM_CPU, &flags);
-	return ret;
-}
+__u64 lprocfs_stats_collector(struct lprocfs_stats *stats, int idx,
+			      enum lprocfs_fields_flags field);
 
 extern struct lprocfs_stats *
 lprocfs_alloc_stats(unsigned int num, enum lprocfs_stats_flags flags);
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index 7a4f412a85a3..73ecc232967b 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -147,9 +147,9 @@ struct lu_device_operations {
 				     struct lu_device *);
 
 	/**
-	 * initialize local objects for device. this method called after layer has
-	 * been initialized (after LCFG_SETUP stage) and before it starts serving
-	 * user requests.
+	 * initialize local objects for device. this method called after layer
+	 * has been initialized (after LCFG_SETUP stage) and before it starts
+	 * serving user requests.
 	 */
 
 	int (*ldo_prepare)(const struct lu_env *,
@@ -791,7 +791,7 @@ int lu_cdebug_printer(const struct lu_env *env,
 #define LU_OBJECT_DEBUG(mask, env, object, format, ...)		   \
 do {								      \
 	if (cfs_cdebug_show(mask, DEBUG_SUBSYSTEM)) {		     \
-		LIBCFS_DEBUG_MSG_DATA_DECL(msgdata, mask, NULL);		\
+		LIBCFS_DEBUG_MSG_DATA_DECL(msgdata, mask, NULL);	\
 		lu_object_print(env, &msgdata, lu_cdebug_printer, object);\
 		CDEBUG(mask, format "\n", ## __VA_ARGS__);		    \
 	}								 \
@@ -803,7 +803,7 @@ do {								      \
 #define LU_OBJECT_HEADER(mask, env, object, format, ...)		\
 do {								    \
 	if (cfs_cdebug_show(mask, DEBUG_SUBSYSTEM)) {		   \
-		LIBCFS_DEBUG_MSG_DATA_DECL(msgdata, mask, NULL);		\
+		LIBCFS_DEBUG_MSG_DATA_DECL(msgdata, mask, NULL);	\
 		lu_object_header_print(env, &msgdata, lu_cdebug_printer,\
 				       (object)->lo_header);	    \
 		lu_cdebug_printer(env, &msgdata, "\n");		 \
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 60b827eeefe2..df48b8d6fe52 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -846,10 +846,10 @@ struct luda_type {
 #endif
 
 struct lu_dirpage {
-	__u64	    ldp_hash_start;
-	__u64	    ldp_hash_end;
-	__u32	    ldp_flags;
-	__u32	    ldp_pad0;
+	__le64	    ldp_hash_start;
+	__le64	    ldp_hash_end;
+	__le32	    ldp_flags;
+	__le32	    ldp_pad0;
 	struct lu_dirent ldp_entries[0];
 };
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index b7e61d082e55..1e86fb53388a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -812,13 +812,6 @@ struct ldlm_lock {
 	/** referenced export object */
 	struct obd_export	*l_exp_refs_target;
 #endif
-	/**
-	 * export blocking dlm lock list, protected by
-	 * l_export->exp_bl_list_lock.
-	 * Lock order of waiting_lists_spinlock, exp_bl_list_lock and res lock
-	 * is: res lock -> exp_bl_list_lock -> wanting_lists_spinlock.
-	 */
-	struct list_head		l_exp_list;
 };
 
 /**
@@ -1192,6 +1185,10 @@ ldlm_namespace_new(struct obd_device *obd, char *name,
 		   enum ldlm_side client, enum ldlm_appetite apt,
 		   enum ldlm_ns_type ns_type);
 int ldlm_namespace_cleanup(struct ldlm_namespace *ns, __u64 flags);
+void ldlm_namespace_free_prior(struct ldlm_namespace *ns,
+			       struct obd_import *imp,
+			       int force);
+void ldlm_namespace_free_post(struct ldlm_namespace *ns);
 void ldlm_namespace_get(struct ldlm_namespace *ns);
 void ldlm_namespace_put(struct ldlm_namespace *ns);
 int ldlm_debugfs_setup(void);
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h b/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
index a0f064d237c9..11331ae81d58 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm_flags.h
@@ -121,6 +121,9 @@
 #define ldlm_set_test_lock(_l)          LDLM_SET_FLAG((_l), 1ULL << 19)
 #define ldlm_clear_test_lock(_l)        LDLM_CLEAR_FLAG((_l), 1ULL << 19)
 
+/** match lock only */
+#define LDLM_FL_MATCH_LOCK		0x0000000000100000ULL /* bit  20 */
+
 /**
  * Immediately cancel such locks when they block some other locks. Send
  * cancel notification to original lock holder, but expect no reply. This
diff --git a/drivers/staging/lustre/lustre/include/lustre_eacl.h b/drivers/staging/lustre/lustre/include/lustre_eacl.h
deleted file mode 100644
index 1e71a8638186..000000000000
--- a/drivers/staging/lustre/lustre/include/lustre_eacl.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * 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 version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.gnu.org/licenses/gpl-2.0.html
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/lustre/include/lustre_idmap.h
- *
- * MDS data structures.
- * See also lustre_idl.h for wire formats of requests.
- */
-
-#ifndef _LUSTRE_EACL_H
-#define _LUSTRE_EACL_H
-
-/** \defgroup eacl eacl
- *
- * @{
- */
-
-#ifdef CONFIG_FS_POSIX_ACL
-
-#include <linux/fs.h>
-#include <linux/posix_acl_xattr.h>
-
-typedef struct {
-	__u16		   e_tag;
-	__u16		   e_perm;
-	__u32		   e_id;
-	__u32		   e_stat;
-} ext_acl_xattr_entry;
-
-typedef struct {
-	__u32		   a_count;
-	ext_acl_xattr_entry     a_entries[0];
-} ext_acl_xattr_header;
-
-#define CFS_ACL_XATTR_SIZE(count, prefix) \
-	(sizeof(prefix ## _header) + (count) * sizeof(prefix ## _entry))
-
-#define CFS_ACL_XATTR_COUNT(size, prefix) \
-	(((size) - sizeof(prefix ## _header)) / sizeof(prefix ## _entry))
-
-#endif /* CONFIG_FS_POSIX_ACL */
-
-/** @} eacl */
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 1b48df0d4862..d61b000dac2c 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -288,7 +288,7 @@ struct ptlrpc_connection {
 	/** Our own lnet nid for this connection */
 	lnet_nid_t	      c_self;
 	/** Remote side nid for this connection */
-	lnet_process_id_t       c_peer;
+	struct lnet_process_id	c_peer;
 	/** UUID of the other side */
 	struct obd_uuid	 c_remote_uuid;
 	/** reference counter for this connection */
@@ -400,7 +400,7 @@ struct ptlrpc_service;
  * ptlrpc callback & work item stuff
  */
 struct ptlrpc_cb_id {
-	void   (*cbid_fn)(lnet_event_t *ev);     /* specific callback fn */
+	void   (*cbid_fn)(struct lnet_event *ev); /* specific callback fn */
 	void    *cbid_arg;		      /* additional arg */
 };
 
@@ -457,7 +457,7 @@ struct ptlrpc_reply_state {
 	struct obd_export     *rs_export;
 	struct ptlrpc_service_part *rs_svcpt;
 	/** Lnet metadata handle for the reply */
-	lnet_handle_md_t       rs_md_h;
+	struct lnet_handle_md		rs_md_h;
 
 	/** Context for the service thread */
 	struct ptlrpc_svc_ctx *rs_svc_ctx;
@@ -586,11 +586,11 @@ struct ptlrpc_cli_req {
 	/** Link back to the request set */
 	struct ptlrpc_request_set	*cr_set;
 	/** outgoing request MD handle */
-	lnet_handle_md_t		 cr_req_md_h;
+	struct lnet_handle_md		 cr_req_md_h;
 	/** request-out callback parameter */
 	struct ptlrpc_cb_id		 cr_req_cbid;
 	/** incoming reply MD handle */
-	lnet_handle_md_t		 cr_reply_md_h;
+	struct lnet_handle_md		 cr_reply_md_h;
 	wait_queue_head_t		 cr_reply_waitq;
 	/** reply callback parameter */
 	struct ptlrpc_cb_id		 cr_reply_cbid;
@@ -876,7 +876,7 @@ struct ptlrpc_request {
 	/** our LNet NID */
 	lnet_nid_t	   rq_self;
 	/** Peer description (the other side) */
-	lnet_process_id_t    rq_peer;
+	struct lnet_process_id	rq_peer;
 	/**
 	 * service time estimate (secs)
 	 * If the request is not served by this time, it is marked as timed out.
@@ -1225,7 +1225,7 @@ struct ptlrpc_bulk_desc {
 	int			bd_md_count;	/* # valid entries in bd_mds */
 	int			bd_md_max_brw;	/* max entries in bd_mds */
 	/** array of associated MDs */
-	lnet_handle_md_t	bd_mds[PTLRPC_BULK_OPS_COUNT];
+	struct lnet_handle_md	bd_mds[PTLRPC_BULK_OPS_COUNT];
 
 	union {
 		struct {
@@ -1376,7 +1376,7 @@ struct ptlrpc_request_buffer_desc {
 	/** Back pointer to service for which this buffer is registered */
 	struct ptlrpc_service_part *rqbd_svcpt;
 	/** LNet descriptor */
-	lnet_handle_md_t       rqbd_md_h;
+	struct lnet_handle_md		rqbd_md_h;
 	int		    rqbd_refcount;
 	/** The buffer itself */
 	char		  *rqbd_buffer;
@@ -1749,23 +1749,23 @@ static inline bool nrs_policy_compat_one(const struct ptlrpc_service *svc,
 /** @} nrs */
 
 /* ptlrpc/events.c */
-extern lnet_handle_eq_t ptlrpc_eq_h;
+extern struct lnet_handle_eq ptlrpc_eq_h;
 int ptlrpc_uuid_to_peer(struct obd_uuid *uuid,
-			lnet_process_id_t *peer, lnet_nid_t *self);
+			struct lnet_process_id *peer, lnet_nid_t *self);
 /**
  * These callbacks are invoked by LNet when something happened to
  * underlying buffer
  * @{
  */
-void request_out_callback(lnet_event_t *ev);
-void reply_in_callback(lnet_event_t *ev);
-void client_bulk_callback(lnet_event_t *ev);
-void request_in_callback(lnet_event_t *ev);
-void reply_out_callback(lnet_event_t *ev);
+void request_out_callback(struct lnet_event *ev);
+void reply_in_callback(struct lnet_event *ev);
+void client_bulk_callback(struct lnet_event *ev);
+void request_in_callback(struct lnet_event *ev);
+void reply_out_callback(struct lnet_event *ev);
 /** @} */
 
 /* ptlrpc/connection.c */
-struct ptlrpc_connection *ptlrpc_connection_get(lnet_process_id_t peer,
+struct ptlrpc_connection *ptlrpc_connection_get(struct lnet_process_id peer,
 						lnet_nid_t self,
 						struct obd_uuid *uuid);
 int ptlrpc_connection_put(struct ptlrpc_connection *c);
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index dace6591a0a4..33304041bb63 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -318,6 +318,7 @@ extern char obd_jobid_var[];
 #define OBD_FAIL_LDLM_AGL_NOLOCK	 0x31b
 #define OBD_FAIL_LDLM_OST_LVB		 0x31c
 #define OBD_FAIL_LDLM_ENQUEUE_HANG	 0x31d
+#define OBD_FAIL_LDLM_PAUSE_CANCEL2	 0x31f
 #define OBD_FAIL_LDLM_CP_CB_WAIT2	 0x320
 #define OBD_FAIL_LDLM_CP_CB_WAIT3	 0x321
 #define OBD_FAIL_LDLM_CP_CB_WAIT4	 0x322
diff --git a/drivers/staging/lustre/lustre/ldlm/interval_tree.c b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
index e134ecd21bb2..e1069021420d 100644
--- a/drivers/staging/lustre/lustre/ldlm/interval_tree.c
+++ b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
@@ -101,11 +101,6 @@ static inline int node_equal(struct interval_node *n1, struct interval_node *n2)
 	return extent_equal(&n1->in_extent, &n2->in_extent);
 }
 
-static inline __u64 max_u64(__u64 x, __u64 y)
-{
-	return x > y ? x : y;
-}
-
 static struct interval_node *interval_first(struct interval_node *node)
 {
 	if (!node)
@@ -134,8 +129,8 @@ static void __rotate_change_maxhigh(struct interval_node *node,
 	rotate->in_max_high = node->in_max_high;
 	left_max = node->in_left ? node->in_left->in_max_high : 0;
 	right_max = node->in_right ? node->in_right->in_max_high : 0;
-	node->in_max_high  = max_u64(interval_high(node),
-				     max_u64(left_max, right_max));
+	node->in_max_high  = max(interval_high(node),
+				 max(left_max, right_max));
 }
 
 /* The left rotation "pivots" around the link from node to node->right, and
@@ -394,8 +389,8 @@ static void update_maxhigh(struct interval_node *node,
 	while (node) {
 		left_max = node->in_left ? node->in_left->in_max_high : 0;
 		right_max = node->in_right ? node->in_right->in_max_high : 0;
-		node->in_max_high = max_u64(interval_high(node),
-					    max_u64(left_max, right_max));
+		node->in_max_high = max(interval_high(node),
+					max(left_max, right_max));
 
 		if (node->in_max_high >= old_maxhigh)
 			break;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index 5c02501d0560..5d24b4825796 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -108,9 +108,7 @@ extern unsigned int ldlm_cancel_unused_locks_before_replay;
 
 /* ldlm_resource.c */
 int ldlm_resource_putref_locked(struct ldlm_resource *res);
-void ldlm_namespace_free_prior(struct ldlm_namespace *ns,
-			       struct obd_import *imp, int force);
-void ldlm_namespace_free_post(struct ldlm_namespace *ns);
+
 /* ldlm_lock.c */
 
 struct ldlm_cb_set_arg {
@@ -156,6 +154,7 @@ int ldlm_bl_to_thread_list(struct ldlm_namespace *ns,
 			   struct ldlm_lock_desc *ld,
 			   struct list_head *cancels, int count,
 			   enum ldlm_cancel_flags cancel_flags);
+int ldlm_bl_thread_wakeup(void);
 
 void ldlm_handle_bl_callback(struct ldlm_namespace *ns,
 			     struct ldlm_lock_desc *ld, struct ldlm_lock *lock);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 5a94265fe60b..ddb46428093f 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -435,7 +435,6 @@ static struct ldlm_lock *ldlm_lock_new(struct ldlm_resource *resource)
 	lock->l_exp_refs_nr = 0;
 	lock->l_exp_refs_target = NULL;
 #endif
-	INIT_LIST_HEAD(&lock->l_exp_list);
 
 	return lock;
 }
@@ -771,19 +770,11 @@ void ldlm_lock_decref_internal(struct ldlm_lock *lock, enum ldlm_mode mode)
 
 	ldlm_lock_decref_internal_nolock(lock, mode);
 
-	if (ldlm_is_local(lock) &&
+	if ((ldlm_is_local(lock) || lock->l_req_mode == LCK_GROUP) &&
 	    !lock->l_readers && !lock->l_writers) {
 		/* If this is a local lock on a server namespace and this was
 		 * the last reference, cancel the lock.
-		 */
-		CDEBUG(D_INFO, "forcing cancel of local lock\n");
-		ldlm_set_cbpending(lock);
-	}
-
-	if (!lock->l_readers && !lock->l_writers &&
-	    (ldlm_is_cbpending(lock) || lock->l_req_mode == LCK_GROUP)) {
-		/* If we received a blocked AST and this was the last reference,
-		 * run the callback.
+		 *
 		 * Group locks are special:
 		 * They must not go in LRU, but they are not called back
 		 * like non-group locks, instead they are manually released.
@@ -791,6 +782,13 @@ void ldlm_lock_decref_internal(struct ldlm_lock *lock, enum ldlm_mode mode)
 		 * they are manually released, so we remove them when they have
 		 * no more reader or writer references. - LU-6368
 		 */
+		ldlm_set_cbpending(lock);
+	}
+
+	if (!lock->l_readers && !lock->l_writers && ldlm_is_cbpending(lock)) {
+		/* If we received a blocked AST and this was the last reference,
+		 * run the callback.
+		 */
 		LDLM_DEBUG(lock, "final decref done on cbpending lock");
 
 		LDLM_LOCK_GET(lock); /* dropped by bl thread */
@@ -1882,6 +1880,19 @@ out:
 	return rc;
 }
 
+static bool is_bl_done(struct ldlm_lock *lock)
+{
+	bool bl_done = true;
+
+	if (!ldlm_is_bl_done(lock)) {
+		lock_res_and_lock(lock);
+		bl_done = ldlm_is_bl_done(lock);
+		unlock_res_and_lock(lock);
+	}
+
+	return bl_done;
+}
+
 /**
  * Helper function to call blocking AST for LDLM lock \a lock in a
  * "cancelling" mode.
@@ -1899,8 +1910,20 @@ void ldlm_cancel_callback(struct ldlm_lock *lock)
 		} else {
 			LDLM_DEBUG(lock, "no blocking ast");
 		}
+		/* only canceller can set bl_done bit */
+		ldlm_set_bl_done(lock);
+		wake_up_all(&lock->l_waitq);
+	} else if (!ldlm_is_bl_done(lock)) {
+		struct l_wait_info lwi = { 0 };
+
+		/*
+		 * The lock is guaranteed to have been canceled once
+		 * returning from this function.
+		 */
+		unlock_res_and_lock(lock);
+		l_wait_event(lock->l_waitq, is_bl_done(lock), &lwi);
+		lock_res_and_lock(lock);
 	}
-	ldlm_set_bl_done(lock);
 }
 
 /**
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index 12647af5a336..6f9d540a97ce 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -454,6 +454,12 @@ int ldlm_bl_to_thread_list(struct ldlm_namespace *ns, struct ldlm_lock_desc *ld,
 	return ldlm_bl_to_thread(ns, ld, NULL, cancels, count, cancel_flags);
 }
 
+int ldlm_bl_thread_wakeup(void)
+{
+	wake_up(&ldlm_state->ldlm_bl_pool->blp_waitq);
+	return 0;
+}
+
 /* Setinfo coming from Server (eg MDT) to Client (eg MDC)! */
 static int ldlm_handle_setinfo(struct ptlrpc_request *req)
 {
@@ -675,8 +681,11 @@ static int ldlm_callback_handler(struct ptlrpc_request *req)
 	return 0;
 }
 
-static struct ldlm_bl_work_item *ldlm_bl_get_work(struct ldlm_bl_pool *blp)
+static int ldlm_bl_get_work(struct ldlm_bl_pool *blp,
+			    struct ldlm_bl_work_item **p_blwi,
+			    struct obd_export **p_exp)
 {
+	int num_th = atomic_read(&blp->blp_num_threads);
 	struct ldlm_bl_work_item *blwi = NULL;
 	static unsigned int num_bl;
 
@@ -693,18 +702,18 @@ static struct ldlm_bl_work_item *ldlm_bl_get_work(struct ldlm_bl_pool *blp)
 					  blwi_entry);
 
 	if (blwi) {
-		if (++num_bl >= atomic_read(&blp->blp_num_threads))
+		if (++num_bl >= num_th)
 			num_bl = 0;
 		list_del(&blwi->blwi_entry);
 	}
 	spin_unlock(&blp->blp_lock);
+	*p_blwi = blwi;
 
-	return blwi;
+	return (*p_blwi || *p_exp) ? 1 : 0;
 }
 
 /* This only contains temporary data until the thread starts */
 struct ldlm_bl_thread_data {
-	char			bltd_name[CFS_CURPROC_COMM_MAX];
 	struct ldlm_bl_pool	*bltd_blp;
 	struct completion	bltd_comp;
 	int			bltd_num;
@@ -712,19 +721,32 @@ struct ldlm_bl_thread_data {
 
 static int ldlm_bl_thread_main(void *arg);
 
-static int ldlm_bl_thread_start(struct ldlm_bl_pool *blp)
+static int ldlm_bl_thread_start(struct ldlm_bl_pool *blp, bool check_busy)
 {
 	struct ldlm_bl_thread_data bltd = { .bltd_blp = blp };
 	struct task_struct *task;
 
 	init_completion(&bltd.bltd_comp);
-	bltd.bltd_num = atomic_read(&blp->blp_num_threads);
-	snprintf(bltd.bltd_name, sizeof(bltd.bltd_name),
-		 "ldlm_bl_%02d", bltd.bltd_num);
-	task = kthread_run(ldlm_bl_thread_main, &bltd, "%s", bltd.bltd_name);
+
+	bltd.bltd_num = atomic_inc_return(&blp->blp_num_threads);
+	if (bltd.bltd_num >= blp->blp_max_threads) {
+		atomic_dec(&blp->blp_num_threads);
+		return 0;
+	}
+
+	LASSERTF(bltd.bltd_num > 0, "thread num:%d\n", bltd.bltd_num);
+	if (check_busy &&
+	    atomic_read(&blp->blp_busy_threads) < (bltd.bltd_num - 1)) {
+		atomic_dec(&blp->blp_num_threads);
+		return 0;
+	}
+
+	task = kthread_run(ldlm_bl_thread_main, &bltd, "ldlm_bl_%02d",
+			   bltd.bltd_num);
 	if (IS_ERR(task)) {
 		CERROR("cannot start LDLM thread ldlm_bl_%02d: rc %ld\n",
-		       atomic_read(&blp->blp_num_threads), PTR_ERR(task));
+		       bltd.bltd_num, PTR_ERR(task));
+		atomic_dec(&blp->blp_num_threads);
 		return PTR_ERR(task);
 	}
 	wait_for_completion(&bltd.bltd_comp);
@@ -732,6 +754,64 @@ static int ldlm_bl_thread_start(struct ldlm_bl_pool *blp)
 	return 0;
 }
 
+/* Not fatal if racy and have a few too many threads */
+static int ldlm_bl_thread_need_create(struct ldlm_bl_pool *blp,
+				      struct ldlm_bl_work_item *blwi)
+{
+	if (atomic_read(&blp->blp_num_threads) >= blp->blp_max_threads)
+		return 0;
+
+	if (atomic_read(&blp->blp_busy_threads) <
+	    atomic_read(&blp->blp_num_threads))
+		return 0;
+
+	if (blwi && (!blwi->blwi_ns || blwi->blwi_mem_pressure))
+		return 0;
+
+	return 1;
+}
+
+static int ldlm_bl_thread_blwi(struct ldlm_bl_pool *blp,
+			       struct ldlm_bl_work_item *blwi)
+{
+	if (!blwi->blwi_ns)
+		/* added by ldlm_cleanup() */
+		return LDLM_ITER_STOP;
+
+	if (blwi->blwi_mem_pressure)
+		memory_pressure_set();
+
+	OBD_FAIL_TIMEOUT(OBD_FAIL_LDLM_PAUSE_CANCEL2, 4);
+
+	if (blwi->blwi_count) {
+		int count;
+
+		/*
+		 * The special case when we cancel locks in lru
+		 * asynchronously, we pass the list of locks here.
+		 * Thus locks are marked LDLM_FL_CANCELING, but NOT
+		 * canceled locally yet.
+		 */
+		count = ldlm_cli_cancel_list_local(&blwi->blwi_head,
+						   blwi->blwi_count,
+						   LCF_BL_AST);
+		ldlm_cli_cancel_list(&blwi->blwi_head, count, NULL,
+				     blwi->blwi_flags);
+	} else {
+		ldlm_handle_bl_callback(blwi->blwi_ns, &blwi->blwi_ld,
+					blwi->blwi_lock);
+	}
+	if (blwi->blwi_mem_pressure)
+		memory_pressure_clr();
+
+	if (blwi->blwi_flags & LCF_ASYNC)
+		kfree(blwi);
+	else
+		complete(&blwi->blwi_comp);
+
+	return 0;
+}
+
 /**
  * Main blocking requests processing thread.
  *
@@ -742,76 +822,40 @@ static int ldlm_bl_thread_start(struct ldlm_bl_pool *blp)
 static int ldlm_bl_thread_main(void *arg)
 {
 	struct ldlm_bl_pool *blp;
+	struct ldlm_bl_thread_data *bltd = arg;
 
-	{
-		struct ldlm_bl_thread_data *bltd = arg;
-
-		blp = bltd->bltd_blp;
-
-		atomic_inc(&blp->blp_num_threads);
-		atomic_inc(&blp->blp_busy_threads);
+	blp = bltd->bltd_blp;
 
-		complete(&bltd->bltd_comp);
-		/* cannot use bltd after this, it is only on caller's stack */
-	}
+	complete(&bltd->bltd_comp);
+	/* cannot use bltd after this, it is only on caller's stack */
 
 	while (1) {
 		struct l_wait_info lwi = { 0 };
 		struct ldlm_bl_work_item *blwi = NULL;
-		int busy;
-
-		blwi = ldlm_bl_get_work(blp);
+		struct obd_export *exp = NULL;
+		int rc;
 
-		if (!blwi) {
-			atomic_dec(&blp->blp_busy_threads);
+		rc = ldlm_bl_get_work(blp, &blwi, &exp);
+		if (!rc)
 			l_wait_event_exclusive(blp->blp_waitq,
-					       (blwi = ldlm_bl_get_work(blp)),
+					       ldlm_bl_get_work(blp, &blwi,
+								&exp),
 					       &lwi);
-			busy = atomic_inc_return(&blp->blp_busy_threads);
-		} else {
-			busy = atomic_read(&blp->blp_busy_threads);
-		}
-
-		if (!blwi->blwi_ns)
-			/* added by ldlm_cleanup() */
-			break;
+		atomic_inc(&blp->blp_busy_threads);
 
-		/* Not fatal if racy and have a few too many threads */
-		if (unlikely(busy < blp->blp_max_threads &&
-			     busy >= atomic_read(&blp->blp_num_threads) &&
-			     !blwi->blwi_mem_pressure))
+		if (ldlm_bl_thread_need_create(blp, blwi))
 			/* discard the return value, we tried */
-			ldlm_bl_thread_start(blp);
-
-		if (blwi->blwi_mem_pressure)
-			memory_pressure_set();
-
-		if (blwi->blwi_count) {
-			int count;
-			/* The special case when we cancel locks in LRU
-			 * asynchronously, we pass the list of locks here.
-			 * Thus locks are marked LDLM_FL_CANCELING, but NOT
-			 * canceled locally yet.
-			 */
-			count = ldlm_cli_cancel_list_local(&blwi->blwi_head,
-							   blwi->blwi_count,
-							   LCF_BL_AST);
-			ldlm_cli_cancel_list(&blwi->blwi_head, count, NULL,
-					     blwi->blwi_flags);
-		} else {
-			ldlm_handle_bl_callback(blwi->blwi_ns, &blwi->blwi_ld,
-						blwi->blwi_lock);
-		}
-		if (blwi->blwi_mem_pressure)
-			memory_pressure_clr();
+			ldlm_bl_thread_start(blp, true);
 
-		if (blwi->blwi_flags & LCF_ASYNC)
-			kfree(blwi);
-		else
-			complete(&blwi->blwi_comp);
+		if (blwi)
+			rc = ldlm_bl_thread_blwi(blp, blwi);
+
+		atomic_dec(&blp->blp_busy_threads);
+
+		if (rc == LDLM_ITER_STOP)
+			break;
 	}
 
-	atomic_dec(&blp->blp_busy_threads);
 	atomic_dec(&blp->blp_num_threads);
 	complete(&blp->blp_comp);
 	return 0;
@@ -991,7 +1035,7 @@ static int ldlm_setup(void)
 	}
 
 	for (i = 0; i < blp->blp_min_threads; i++) {
-		rc = ldlm_bl_thread_start(blp);
+		rc = ldlm_bl_thread_start(blp, false);
 		if (rc < 0)
 			goto out;
 	}
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index 8dfb3c8e6b7a..cf3fc5793377 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -900,8 +900,9 @@ static int ldlm_pools_recalc(enum ldlm_side client)
 {
 	struct ldlm_namespace *ns;
 	struct ldlm_namespace *ns_old = NULL;
+	/* seconds of sleep if no active namespaces */
+	int time = LDLM_POOL_CLI_DEF_RECALC_PERIOD;
 	int nr;
-	int time = 50; /* seconds of sleep if no active namespaces */
 
 	/*
 	 * Recalc at least ldlm_namespace_nr_read(client) namespaces.
@@ -974,6 +975,10 @@ static int ldlm_pools_recalc(enum ldlm_side client)
 			ldlm_namespace_put(ns);
 		}
 	}
+
+	/* Wake up the blocking threads from time to time. */
+	ldlm_bl_thread_wakeup();
+
 	return time;
 }
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index ebfda368b057..84eeaa552113 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -1029,13 +1029,23 @@ int ldlm_cli_cancel(const struct lustre_handle *lockh,
 	struct ldlm_lock *lock;
 	LIST_HEAD(cancels);
 
-	/* concurrent cancels on the same handle can happen */
-	lock = ldlm_handle2lock_long(lockh, LDLM_FL_CANCELING);
+	lock = ldlm_handle2lock_long(lockh, 0);
 	if (!lock) {
 		LDLM_DEBUG_NOLOCK("lock is already being destroyed");
 		return 0;
 	}
 
+	lock_res_and_lock(lock);
+	/* Lock is being canceled and the caller doesn't want to wait */
+	if (ldlm_is_canceling(lock) && (cancel_flags & LCF_ASYNC)) {
+		unlock_res_and_lock(lock);
+		LDLM_LOCK_RELEASE(lock);
+		return 0;
+	}
+
+	ldlm_set_canceling(lock);
+	unlock_res_and_lock(lock);
+
 	rc = ldlm_cli_cancel_local(lock);
 	if (rc == LDLM_FL_LOCAL_ONLY || cancel_flags & LCF_LOCAL) {
 		LDLM_LOCK_RELEASE(lock);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index d16f5e95ef0b..633f65b078eb 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -806,7 +806,7 @@ static void cleanup_resource(struct ldlm_resource *res, struct list_head *q,
 
 		unlock_res(res);
 		ldlm_lock2handle(lock, &lockh);
-		rc = ldlm_cli_cancel(&lockh, LCF_ASYNC);
+		rc = ldlm_cli_cancel(&lockh, LCF_LOCAL);
 		if (rc)
 			CERROR("ldlm_cli_cancel: %d\n", rc);
 		LDLM_LOCK_RELEASE(lock);
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 966f580e26fb..38f84662cf02 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -126,6 +126,7 @@ static int ll_ddelete(const struct dentry *de)
 static int ll_d_init(struct dentry *de)
 {
 	struct ll_dentry_data *lld = kzalloc(sizeof(*lld), GFP_KERNEL);
+
 	if (unlikely(!lld))
 		return -ENOMEM;
 	lld->lld_invalid = 1;
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 481c0d01d4c6..67c4b9cc6e75 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -116,13 +116,13 @@ static void ll_prepare_close(struct inode *inode, struct md_op_data *op_data,
  * If \a bias is MDS_CLOSE_LAYOUT_SWAP then \a data is a pointer to the inode to
  * swap layouts with.
  */
-static int ll_close_inode_openhandle(struct obd_export *md_exp,
+static int ll_close_inode_openhandle(struct inode *inode,
 				     struct obd_client_handle *och,
-				     struct inode *inode,
 				     enum mds_op_bias bias,
 				     void *data)
 {
 	const struct ll_inode_info *lli = ll_i2info(inode);
+	struct obd_export *md_exp = ll_i2mdexp(inode);
 	struct md_op_data *op_data;
 	struct ptlrpc_request *req = NULL;
 	int rc;
@@ -231,15 +231,13 @@ int ll_md_real_close(struct inode *inode, fmode_t fmode)
 		/* There might be a race and this handle may already
 		 * be closed.
 		 */
-		rc = ll_close_inode_openhandle(ll_i2sbi(inode)->ll_md_exp,
-					       och, inode, 0, NULL);
+		rc = ll_close_inode_openhandle(inode, och, 0, NULL);
 	}
 
 	return rc;
 }
 
-static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
-		       struct file *file)
+static int ll_md_close(struct inode *inode, struct file *file)
 {
 	struct ll_file_data *fd = LUSTRE_FPRIVATE(file);
 	struct ll_inode_info *lli = ll_i2info(inode);
@@ -270,8 +268,7 @@ static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
 	}
 
 	if (fd->fd_och) {
-		rc = ll_close_inode_openhandle(md_exp, fd->fd_och, inode, 0,
-					       NULL);
+		rc = ll_close_inode_openhandle(inode, fd->fd_och, 0, NULL);
 		fd->fd_och = NULL;
 		goto out;
 	}
@@ -296,7 +293,7 @@ static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
 	}
 	mutex_unlock(&lli->lli_och_mutex);
 
-	if (!md_lock_match(md_exp, flags, ll_inode2fid(inode),
+	if (!md_lock_match(ll_i2mdexp(inode), flags, ll_inode2fid(inode),
 			   LDLM_IBITS, &policy, lockmode, &lockh))
 		rc = ll_md_real_close(inode, fd->fd_omode);
 
@@ -345,7 +342,7 @@ int ll_file_release(struct inode *inode, struct file *file)
 		lli->lli_async_rc = 0;
 	}
 
-	rc = ll_md_close(sbi->ll_md_exp, inode, file);
+	rc = ll_md_close(inode, file);
 
 	if (CFS_FAIL_TIMEOUT_MS(OBD_FAIL_PTLRPC_DUMP_LOG, cfs_fail_val))
 		libcfs_debug_dumplog();
@@ -835,7 +832,7 @@ out_close:
 		it.it_lock_mode = 0;
 		och->och_lease_handle.cookie = 0ULL;
 	}
-	rc2 = ll_close_inode_openhandle(sbi->ll_md_exp, och, inode, 0, NULL);
+	rc2 = ll_close_inode_openhandle(inode, och, 0, NULL);
 	if (rc2 < 0)
 		CERROR("%s: error closing file "DFID": %d\n",
 		       ll_get_fsname(inode->i_sb, NULL, 0),
@@ -901,8 +898,8 @@ static int ll_swap_layouts_close(struct obd_client_handle *och,
 	 * NB: lease lock handle is released in mdc_close_layout_swap_pack()
 	 * because we still need it to pack l_remote_handle to MDT.
 	 */
-	rc = ll_close_inode_openhandle(ll_i2sbi(inode)->ll_md_exp, och, inode,
-				       MDS_CLOSE_LAYOUT_SWAP, inode2);
+	rc = ll_close_inode_openhandle(inode, och, MDS_CLOSE_LAYOUT_SWAP,
+				       inode2);
 
 	och = NULL; /* freed in ll_close_inode_openhandle() */
 
@@ -937,8 +934,7 @@ static int ll_lease_close(struct obd_client_handle *och, struct inode *inode,
 	if (lease_broken)
 		*lease_broken = cancelled;
 
-	return ll_close_inode_openhandle(ll_i2sbi(inode)->ll_md_exp,
-					 och, inode, 0, NULL);
+	return ll_close_inode_openhandle(inode, och, 0, NULL);
 }
 
 int ll_merge_attr(const struct lu_env *env, struct inode *inode)
@@ -1159,7 +1155,7 @@ static ssize_t ll_file_read_iter(struct kiocb *iocb, struct iov_iter *to)
 	struct lu_env      *env;
 	struct vvp_io_args *args;
 	ssize_t	     result;
-	int		 refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (IS_ERR(env))
@@ -1183,7 +1179,7 @@ static ssize_t ll_file_write_iter(struct kiocb *iocb, struct iov_iter *from)
 	struct lu_env      *env;
 	struct vvp_io_args *args;
 	ssize_t	     result;
-	int		 refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (IS_ERR(env))
@@ -1340,7 +1336,7 @@ static int ll_file_getstripe(struct inode *inode,
 			     struct lov_user_md __user *lum)
 {
 	struct lu_env *env;
-	int refcheck;
+	u16 refcheck;
 	int rc;
 
 	env = cl_env_get(&refcheck);
@@ -1494,8 +1490,7 @@ int ll_release_openhandle(struct inode *inode, struct lookup_intent *it)
 
 	ll_och_fill(ll_i2sbi(inode)->ll_md_exp, it, och);
 
-	rc = ll_close_inode_openhandle(ll_i2sbi(inode)->ll_md_exp,
-				       och, inode, 0, NULL);
+	rc = ll_close_inode_openhandle(inode, och, 0, NULL);
 out:
 	/* this one is in place of ll_file_open */
 	if (it_disposition(it, DISP_ENQ_OPEN_REF)) {
@@ -1517,7 +1512,7 @@ static int ll_do_fiemap(struct inode *inode, struct fiemap *fiemap,
 {
 	struct ll_fiemap_info_key fmkey = { .lfik_name = KEY_FIEMAP, };
 	struct lu_env *env;
-	int refcheck;
+	u16 refcheck;
 	int rc = 0;
 
 	/* Checks for fiemap flags */
@@ -1623,7 +1618,7 @@ int ll_data_version(struct inode *inode, __u64 *data_version, int flags)
 	struct cl_object *obj = ll_i2info(inode)->lli_clob;
 	struct lu_env *env;
 	struct cl_io *io;
-	int refcheck;
+	u16 refcheck;
 	int result;
 
 	/* If no file object initialized, we consider its version is 0. */
@@ -1668,7 +1663,7 @@ int ll_hsm_release(struct inode *inode)
 	struct obd_client_handle *och = NULL;
 	__u64 data_version = 0;
 	int rc;
-	int refcheck;
+	u16 refcheck;
 
 	CDEBUG(D_INODE, "%s: Releasing file "DFID".\n",
 	       ll_get_fsname(inode->i_sb, NULL, 0),
@@ -1698,8 +1693,8 @@ int ll_hsm_release(struct inode *inode)
 	 * NB: lease lock handle is released in mdc_hsm_release_pack() because
 	 * we still need it to pack l_remote_handle to MDT.
 	 */
-	rc = ll_close_inode_openhandle(ll_i2sbi(inode)->ll_md_exp, och, inode,
-				       MDS_HSM_RELEASE, &data_version);
+	rc = ll_close_inode_openhandle(inode, och, MDS_HSM_RELEASE,
+				       &data_version);
 	och = NULL;
 
 out:
@@ -2324,7 +2319,7 @@ int cl_sync_file_range(struct inode *inode, loff_t start, loff_t end,
 	struct cl_io *io;
 	struct cl_fsync_io *fio;
 	int result;
-	int refcheck;
+	u16 refcheck;
 
 	if (mode != CL_FSYNC_NONE && mode != CL_FSYNC_LOCAL &&
 	    mode != CL_FSYNC_DISCARD && mode != CL_FSYNC_ALL)
@@ -3272,7 +3267,7 @@ int ll_layout_conf(struct inode *inode, const struct cl_object_conf *conf)
 	struct cl_object *obj = lli->lli_clob;
 	struct lu_env *env;
 	int rc;
-	int refcheck;
+	u16 refcheck;
 
 	if (!obj)
 		return 0;
diff --git a/drivers/staging/lustre/lustre/llite/glimpse.c b/drivers/staging/lustre/lustre/llite/glimpse.c
index 504498de536e..0143112e672d 100644
--- a/drivers/staging/lustre/lustre/llite/glimpse.c
+++ b/drivers/staging/lustre/lustre/llite/glimpse.c
@@ -138,7 +138,7 @@ int cl_glimpse_lock(const struct lu_env *env, struct cl_io *io,
 }
 
 static int cl_io_get(struct inode *inode, struct lu_env **envout,
-		     struct cl_io **ioout, int *refcheck)
+		     struct cl_io **ioout, u16 *refcheck)
 {
 	struct lu_env	  *env;
 	struct cl_io	   *io;
@@ -178,7 +178,7 @@ int cl_glimpse_size0(struct inode *inode, int agl)
 	struct lu_env	  *env = NULL;
 	struct cl_io	   *io  = NULL;
 	int		     result;
-	int		     refcheck;
+	u16 refcheck;
 
 	result = cl_io_get(inode, &env, &io, &refcheck);
 	if (result > 0) {
diff --git a/drivers/staging/lustre/lustre/llite/lcommon_cl.c b/drivers/staging/lustre/lustre/llite/lcommon_cl.c
index f1036f477a51..8af611033e12 100644
--- a/drivers/staging/lustre/lustre/llite/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/llite/lcommon_cl.c
@@ -72,7 +72,7 @@
  * mutex.
  */
 struct lu_env *cl_inode_fini_env;
-int cl_inode_fini_refcheck;
+u16 cl_inode_fini_refcheck;
 
 /**
  * A mutex serializing calls to slp_inode_fini() under extreme memory
@@ -86,7 +86,7 @@ int cl_setattr_ost(struct cl_object *obj, const struct iattr *attr,
 	struct lu_env *env;
 	struct cl_io  *io;
 	int	    result;
-	int	    refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (IS_ERR(env))
@@ -149,7 +149,7 @@ int cl_file_inode_init(struct inode *inode, struct lustre_md *md)
 		}
 	};
 	int result = 0;
-	int refcheck;
+	u16 refcheck;
 
 	LASSERT(md->body->mbo_valid & OBD_MD_FLID);
 	LASSERT(S_ISREG(inode->i_mode));
@@ -237,7 +237,7 @@ void cl_inode_fini(struct inode *inode)
 	struct lu_env	   *env;
 	struct ll_inode_info    *lli  = ll_i2info(inode);
 	struct cl_object	*clob = lli->lli_clob;
-	int refcheck;
+	u16 refcheck;
 	int emergency;
 
 	if (clob) {
diff --git a/drivers/staging/lustre/lustre/llite/lcommon_misc.c b/drivers/staging/lustre/lustre/llite/lcommon_misc.c
index f0c132e2cf92..7f7f3f1648ef 100644
--- a/drivers/staging/lustre/lustre/llite/lcommon_misc.c
+++ b/drivers/staging/lustre/lustre/llite/lcommon_misc.c
@@ -124,7 +124,7 @@ int cl_get_grouplock(struct cl_object *obj, unsigned long gid, int nonblock,
 	struct cl_lock	 *lock;
 	struct cl_lock_descr   *descr;
 	__u32		   enqflags;
-	int		     refcheck;
+	u16 refcheck;
 	int		     rc;
 
 	env = cl_env_get(&refcheck);
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 55f68acd85d1..d2a0fabd8a69 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -35,7 +35,6 @@
 #include "../include/lustre_debug.h"
 #include "../include/lustre_ver.h"
 #include "../include/lustre_disk.h"	/* for s2sbi */
-#include "../include/lustre_eacl.h"
 #include "../include/lustre_linkea.h"
 
 /* for struct cl_lock_descr and struct cl_io */
@@ -1330,7 +1329,7 @@ int cl_setattr_ost(struct cl_object *obj, const struct iattr *attr,
 		   unsigned int attr_flags);
 
 extern struct lu_env *cl_inode_fini_env;
-extern int cl_inode_fini_refcheck;
+extern u16 cl_inode_fini_refcheck;
 
 int cl_file_inode_init(struct inode *inode, struct lustre_md *md);
 void cl_inode_fini(struct inode *inode);
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index d483c44aafe5..11b5a8d36415 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -1486,8 +1486,6 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		goto out;
 	}
 
-	op_data->op_attr = *attr;
-
 	if (!hsm_import && attr->ia_valid & ATTR_SIZE) {
 		/*
 		 * If we are changing file size, file content is
@@ -1495,8 +1493,11 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		 */
 		attr->ia_valid |= MDS_OPEN_OWNEROVERRIDE;
 		op_data->op_bias |= MDS_DATA_MODIFIED;
+		clear_bit(LLIF_DATA_MODIFIED, &lli->lli_flags);
 	}
 
+	op_data->op_attr = *attr;
+
 	rc = ll_md_setattr(dentry, op_data);
 	if (rc)
 		goto out;
@@ -1542,8 +1543,15 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		int rc2;
 
 		rc2 = ll_hsm_state_set(inode, &hss);
+		/*
+		 * truncate and write can happen at the same time, so that
+		 * the file can be set modified even though the file is not
+		 * restored from released state, and ll_hsm_state_set() is
+		 * not applicable for the file, and rc2 < 0 is normal in this
+		 * case.
+		 */
 		if (rc2 < 0)
-			CERROR(DFID "HSM set dirty failed: rc2 = %d\n",
+			CDEBUG(D_INFO, DFID "HSM set dirty failed: rc2 = %d\n",
 			       PFID(ll_inode2fid(inode)), rc2);
 	}
 
@@ -2486,7 +2494,7 @@ no_kbuf:
 void ll_compute_rootsquash_state(struct ll_sb_info *sbi)
 {
 	struct root_squash_info *squash = &sbi->ll_squash;
-	lnet_process_id_t id;
+	struct lnet_process_id id;
 	bool matched;
 	int i;
 
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 896196c74cd2..cbbfdaf127a7 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -150,7 +150,7 @@ static int ll_page_mkwrite0(struct vm_area_struct *vma, struct page *vmpage,
 	struct cl_io	    *io;
 	struct vvp_io	   *vio;
 	int		      result;
-	int refcheck;
+	u16 refcheck;
 	sigset_t	     set;
 	struct inode	     *inode;
 	struct ll_inode_info     *lli;
@@ -268,7 +268,7 @@ static int ll_fault0(struct vm_area_struct *vma, struct vm_fault *vmf)
 	unsigned long	    ra_flags;
 	int		      result = 0;
 	int		      fault_ret = 0;
-	int refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (IS_ERR(env))
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index f3ee584157e0..c742cba60199 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -386,7 +386,7 @@ static ssize_t ll_max_cached_mb_seq_write(struct file *file,
 	struct lu_env *env;
 	long diff = 0;
 	long nrpages = 0;
-	int refcheck;
+	u16 refcheck;
 	long pages_number;
 	int mult;
 	long rc;
@@ -1308,7 +1308,7 @@ static void ll_display_extents_info(struct ll_rw_extents_info *io_extents,
 			   r, pct(r, read_tot), pct(read_cum, read_tot),
 			   w, pct(w, write_tot), pct(write_cum, write_tot));
 		start = end;
-		if (start == 1 << 10) {
+		if (start == 1024) {
 			start = 1;
 			units += 10;
 			unitp++;
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index fc176540bb95..d583696e8378 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -434,6 +434,7 @@ struct dentry *ll_splice_alias(struct inode *inode, struct dentry *de)
 {
 	if (inode) {
 		struct dentry *new = ll_find_alias(inode, de);
+
 		if (new) {
 			d_move(new, de);
 			iput(inode);
diff --git a/drivers/staging/lustre/lustre/llite/range_lock.c b/drivers/staging/lustre/lustre/llite/range_lock.c
index 14148a097476..161391b6fb36 100644
--- a/drivers/staging/lustre/lustre/llite/range_lock.c
+++ b/drivers/staging/lustre/lustre/llite/range_lock.c
@@ -174,7 +174,7 @@ void range_unlock(struct range_lock_tree *tree, struct range_lock *lock)
  */
 static enum interval_iter range_lock_cb(struct interval_node *node, void *arg)
 {
-	struct range_lock *lock = (struct range_lock *)arg;
+	struct range_lock *lock = arg;
 	struct range_lock *overlap = node2rangelock(node);
 
 	lock->rl_blocking_ranges += overlap->rl_lock_count + 1;
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index 50d027e0cfab..1bac51f882a7 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -905,7 +905,7 @@ int ll_writepage(struct page *vmpage, struct writeback_control *wbc)
 	bool redirtied = false;
 	bool unlocked = false;
 	int result;
-	int refcheck;
+	u16 refcheck;
 
 	LASSERT(PageLocked(vmpage));
 	LASSERT(!PageWriteback(vmpage));
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index d89e79599199..420f296f9658 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -156,32 +156,6 @@ static int ll_releasepage(struct page *vmpage, gfp_t gfp_mask)
 
 #define MAX_DIRECTIO_SIZE (2 * 1024 * 1024 * 1024UL)
 
-static inline int ll_get_user_pages(int rw, unsigned long user_addr,
-				    size_t size, struct page ***pages,
-				    int *max_pages)
-{
-	int result = -ENOMEM;
-
-	/* set an arbitrary limit to prevent arithmetic overflow */
-	if (size > MAX_DIRECTIO_SIZE) {
-		*pages = NULL;
-		return -EFBIG;
-	}
-
-	*max_pages = (user_addr + size + PAGE_SIZE - 1) >> PAGE_SHIFT;
-	*max_pages -= user_addr >> PAGE_SHIFT;
-
-	*pages = libcfs_kvzalloc(*max_pages * sizeof(**pages), GFP_NOFS);
-	if (*pages) {
-		result = get_user_pages_fast(user_addr, *max_pages,
-					     (rw == READ), *pages);
-		if (unlikely(result <= 0))
-			kvfree(*pages);
-	}
-
-	return result;
-}
-
 /*  ll_free_user_pages - tear down page struct array
  *  @pages: array of page struct pointers underlying target buffer
  */
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index 4759802e062d..56f4b10624ce 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -84,7 +84,7 @@ MODULE_ALIAS_FS("lustre");
 
 static int __init lustre_init(void)
 {
-	lnet_process_id_t lnet_id;
+	struct lnet_process_id lnet_id;
 	struct timespec64 ts;
 	int i, rc, seed[2];
 
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index cd77b55d3895..60aac42e5344 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -129,6 +129,7 @@ static const char *ll_get_link(struct dentry *dentry,
 	struct ptlrpc_request *request = NULL;
 	int rc;
 	char *symname = NULL;
+
 	if (!dentry)
 		return ERR_PTR(-ECHILD);
 
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index 3669ea77ee93..6cb2db28eb60 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -313,7 +313,7 @@ int cl_sb_init(struct super_block *sb)
 	struct cl_device  *cl;
 	struct lu_env     *env;
 	int rc = 0;
-	int refcheck;
+	u16 refcheck;
 
 	sbi  = ll_s2sbi(sb);
 	env = cl_env_get(&refcheck);
@@ -336,7 +336,7 @@ int cl_sb_fini(struct super_block *sb)
 	struct ll_sb_info *sbi;
 	struct lu_env     *env;
 	struct cl_device  *cld;
-	int		refcheck;
+	u16 refcheck;
 	int		result;
 
 	sbi = ll_s2sbi(sb);
@@ -535,7 +535,7 @@ static int vvp_pgcache_show(struct seq_file *f, void *v)
 	struct cl_object	*clob;
 	struct lu_env	   *env;
 	struct vvp_pgcache_id    id;
-	int		      refcheck;
+	u16 refcheck;
 	int		      result;
 
 	env = cl_env_get(&refcheck);
@@ -584,7 +584,7 @@ static void *vvp_pgcache_start(struct seq_file *f, loff_t *pos)
 {
 	struct ll_sb_info *sbi;
 	struct lu_env     *env;
-	int		refcheck;
+	u16 refcheck;
 
 	sbi = f->private;
 
@@ -608,7 +608,7 @@ static void *vvp_pgcache_next(struct seq_file *f, void *v, loff_t *pos)
 {
 	struct ll_sb_info *sbi;
 	struct lu_env     *env;
-	int		refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (!IS_ERR(env)) {
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index 4c57755e06e7..aa31bc0a58a6 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -219,6 +219,7 @@ static int vvp_io_one_lock_index(const struct lu_env *env, struct cl_io *io,
 	if (vio->vui_fd && (vio->vui_fd->fd_flags & LL_FILE_GROUP_LOCKED)) {
 		descr->cld_mode = CLM_GROUP;
 		descr->cld_gid  = vio->vui_fd->fd_grouplock.lg_gid;
+		enqflags |= CEF_LOCK_MATCH;
 	} else {
 		descr->cld_mode  = mode;
 	}
@@ -449,6 +450,7 @@ static void vvp_io_advance(const struct lu_env *env,
 {
 	struct cl_object *obj = ios->cis_io->ci_obj;
 	struct vvp_io	 *vio = cl2vvp_io(env, ios);
+
 	CLOBINVRNT(env, obj, vvp_object_invariant(obj));
 
 	vio->vui_tot_count -= nob;
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index 421cc04ecf1e..6187bffec8c4 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -40,7 +40,6 @@
 #include "../include/obd_support.h"
 #include "../include/lustre_dlm.h"
 #include "../include/lustre_ver.h"
-#include "../include/lustre_eacl.h"
 
 #include "llite_internal.h"
 
@@ -427,7 +426,7 @@ static ssize_t ll_getxattr_lov(struct inode *inode, void *buf, size_t buf_size)
 			.cl_buf.lb_len = buf_size,
 		};
 		struct lu_env *env;
-		int refcheck;
+		u16 refcheck;
 
 		if (!obj)
 			return -ENODATA;
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 271e18966f50..732595125d8a 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -373,7 +373,6 @@ static void lmv_del_target(struct lmv_obd *lmv, int index)
 
 	kfree(lmv->tgts[index]);
 	lmv->tgts[index] = NULL;
-	return;
 }
 
 static int lmv_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
@@ -640,7 +639,7 @@ static int lmv_fid2path(struct obd_export *exp, int len, void *karg,
 	int			remote_gf_size = 0;
 	int			rc;
 
-	gf = (struct getinfo_fid2path *)karg;
+	gf = karg;
 	tgt = lmv_find_target(lmv, &gf->gf_fid);
 	if (IS_ERR(tgt))
 		return PTR_ERR(tgt);
@@ -657,7 +656,7 @@ repeat_fid2path:
 		struct getinfo_fid2path *ori_gf;
 		char *ptr;
 
-		ori_gf = (struct getinfo_fid2path *)karg;
+		ori_gf = karg;
 		if (strlen(ori_gf->gf_path) +
 		    strlen(gf->gf_path) > ori_gf->gf_pathlen) {
 			rc = -EOVERFLOW;
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index ff458020b96a..bf25f887062d 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -91,7 +91,6 @@ static void *lmv_tgt_seq_start(struct seq_file *p, loff_t *pos)
 
 static void lmv_tgt_seq_stop(struct seq_file *p, void *v)
 {
-	return;
 }
 
 static void *lmv_tgt_seq_next(struct seq_file *p, void *v, loff_t *pos)
diff --git a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
index c49a34bf10e5..391c632365ae 100644
--- a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
@@ -118,7 +118,7 @@ struct lov_device_emerg {
 	 *
 	 * \see cl_env_get()
 	 */
-	int		 emrg_refcheck;
+	u16		 emrg_refcheck;
 };
 
 struct lov_device {
@@ -378,40 +378,39 @@ struct lov_thread_info {
  * State that lov_io maintains for every sub-io.
  */
 struct lov_io_sub {
-	int		  sub_stripe;
-	/**
-	 * sub-io for a stripe. Ideally sub-io's can be stopped and resumed
-	 * independently, with lov acting as a scheduler to maximize overall
-	 * throughput.
-	 */
-	struct cl_io	*sub_io;
+	u16		 sub_stripe;
 	/**
-	 * Linkage into a list (hanging off lov_io::lis_active) of all
-	 * sub-io's active for the current IO iteration.
+	 * environment's refcheck.
+	 *
+	 * \see cl_env_get()
 	 */
-	struct list_head	   sub_linkage;
+	u16			 sub_refcheck;
+	u16			 sub_reenter;
 	/**
 	 * true, iff cl_io_init() was successfully executed against
 	 * lov_io_sub::sub_io.
 	 */
-	int		  sub_io_initialized;
+	u16			 sub_io_initialized:1,
 	/**
 	 * True, iff lov_io_sub::sub_io and lov_io_sub::sub_env weren't
 	 * allocated, but borrowed from a per-device emergency pool.
 	 */
-	int		  sub_borrowed;
+				 sub_borrowed:1;
 	/**
-	 * environment, in which sub-io executes.
+	 * Linkage into a list (hanging off lov_io::lis_active) of all
+	 * sub-io's active for the current IO iteration.
 	 */
-	struct lu_env *sub_env;
+	struct list_head	 sub_linkage;
 	/**
-	 * environment's refcheck.
-	 *
-	 * \see cl_env_get()
+	 * sub-io for a stripe. Ideally sub-io's can be stopped and resumed
+	 * independently, with lov acting as a scheduler to maximize overall
+	 * throughput.
+	 */
+	struct cl_io	*sub_io;
+	/**
+	 * environment, in which sub-io executes.
 	 */
-	int		  sub_refcheck;
-	int		  sub_refcheck2;
-	int		  sub_reenter;
+	struct lu_env *sub_env;
 };
 
 /**
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index e0f0756ee883..df77b2586612 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -424,21 +424,23 @@ static int lov_io_iter_init(const struct lu_env *env,
 
 		end = lov_offset_mod(end, 1);
 		sub = lov_sub_get(env, lio, stripe);
-		if (!IS_ERR(sub)) {
-			lov_io_sub_inherit(sub->sub_io, lio, stripe,
-					   start, end);
-			rc = cl_io_iter_init(sub->sub_env, sub->sub_io);
-			lov_sub_put(sub);
-			CDEBUG(D_VFSTRACE, "shrink: %d [%llu, %llu)\n",
-			       stripe, start, end);
-		} else {
+		if (IS_ERR(sub)) {
 			rc = PTR_ERR(sub);
+			break;
 		}
 
-		if (!rc)
-			list_add_tail(&sub->sub_linkage, &lio->lis_active);
-		else
+		lov_io_sub_inherit(sub->sub_io, lio, stripe, start, end);
+		rc = cl_io_iter_init(sub->sub_env, sub->sub_io);
+		if (rc)
+			cl_io_iter_fini(sub->sub_env, sub->sub_io);
+		lov_sub_put(sub);
+		if (rc)
 			break;
+
+		CDEBUG(D_VFSTRACE, "shrink: %d [%llu, %llu)\n",
+		       stripe, start, end);
+
+		list_add_tail(&sub->sub_linkage, &lio->lis_active);
 	}
 	return rc;
 }
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index b3161fb6d4b5..25f15da6e189 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -68,7 +68,6 @@ static void lov_getref(struct obd_device *obd)
 	mutex_lock(&lov->lov_lock);
 	atomic_inc(&lov->lov_refcount);
 	mutex_unlock(&lov->lov_lock);
-	return;
 }
 
 static void __lov_del_obd(struct obd_device *obd, struct lov_tgt_desc *tgt);
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index 977579c9c519..ab3ecfeeadc8 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -746,7 +746,7 @@ static int lov_layout_change(const struct lu_env *unused,
 	const struct lov_layout_operations *old_ops;
 	const struct lov_layout_operations *new_ops;
 	struct lu_env *env;
-	int refcheck;
+	u16 refcheck;
 	int rc;
 
 	LASSERT(0 <= lov->lo_type && lov->lo_type < ARRAY_SIZE(lov_dispatch));
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_object.c b/drivers/staging/lustre/lustre/obdclass/cl_object.c
index 703cb67ce42e..08e55d418537 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_object.c
@@ -688,7 +688,7 @@ static inline struct cl_env *cl_env_container(struct lu_env *env)
  *
  * \see cl_env_put()
  */
-struct lu_env *cl_env_get(int *refcheck)
+struct lu_env *cl_env_get(u16 *refcheck)
 {
 	struct lu_env *env;
 
@@ -709,7 +709,7 @@ EXPORT_SYMBOL(cl_env_get);
  *
  * \see cl_env_get()
  */
-struct lu_env *cl_env_alloc(int *refcheck, __u32 tags)
+struct lu_env *cl_env_alloc(u16 *refcheck, u32 tags)
 {
 	struct lu_env *env;
 
@@ -769,7 +769,7 @@ EXPORT_SYMBOL(cl_env_cache_purge);
  * this thread is using environment and it is returned to the allocation
  * cache, or freed straight away, if cache is large enough.
  */
-void cl_env_put(struct lu_env *env, int *refcheck)
+void cl_env_put(struct lu_env *env, u16 *refcheck)
 {
 	struct cl_env *cle;
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index cd9a40ca4448..71fcc4cc9e72 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -482,6 +482,7 @@ void cl_page_disown0(const struct lu_env *env,
 int cl_page_is_owned(const struct cl_page *pg, const struct cl_io *io)
 {
 	struct cl_io *top = cl_io_top((struct cl_io *)io);
+
 	LINVRNT(cl_object_same(pg->cp_obj, io->ci_obj));
 	return pg->cp_state == CPS_OWNED && pg->cp_owner == top;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index 2c99717b0aba..1ec6e3767d81 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -598,6 +598,93 @@ int lprocfs_rd_conn_uuid(struct seq_file *m, void *data)
 }
 EXPORT_SYMBOL(lprocfs_rd_conn_uuid);
 
+/**
+ * Lock statistics structure for access, possibly only on this CPU.
+ *
+ * The statistics struct may be allocated with per-CPU structures for
+ * efficient concurrent update (usually only on server-wide stats), or
+ * as a single global struct (e.g. for per-client or per-job statistics),
+ * so the required locking depends on the type of structure allocated.
+ *
+ * For per-CPU statistics, pin the thread to the current cpuid so that
+ * will only access the statistics for that CPU.  If the stats structure
+ * for the current CPU has not been allocated (or previously freed),
+ * allocate it now.  The per-CPU statistics do not need locking since
+ * the thread is pinned to the CPU during update.
+ *
+ * For global statistics, lock the stats structure to prevent concurrent update.
+ *
+ * \param[in] stats    statistics structure to lock
+ * \param[in] opc      type of operation:
+ *                     LPROCFS_GET_SMP_ID: "lock" and return current CPU index
+ *                             for incrementing statistics for that CPU
+ *                     LPROCFS_GET_NUM_CPU: "lock" and return number of used
+ *                             CPU indices to iterate over all indices
+ * \param[out] flags   CPU interrupt saved state for IRQ-safe locking
+ *
+ * \retval cpuid of current thread or number of allocated structs
+ * \retval negative on error (only for opc LPROCFS_GET_SMP_ID + per-CPU stats)
+ */
+int lprocfs_stats_lock(struct lprocfs_stats *stats,
+		       enum lprocfs_stats_lock_ops opc,
+		       unsigned long *flags)
+{
+	if (stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) {
+		if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
+			spin_lock_irqsave(&stats->ls_lock, *flags);
+		else
+			spin_lock(&stats->ls_lock);
+		return opc == LPROCFS_GET_NUM_CPU ? 1 : 0;
+	}
+
+	switch (opc) {
+	case LPROCFS_GET_SMP_ID: {
+		unsigned int cpuid = get_cpu();
+
+		if (unlikely(!stats->ls_percpu[cpuid])) {
+			int rc = lprocfs_stats_alloc_one(stats, cpuid);
+
+			if (rc < 0) {
+				put_cpu();
+				return rc;
+			}
+		}
+		return cpuid;
+	}
+	case LPROCFS_GET_NUM_CPU:
+		return stats->ls_biggest_alloc_num;
+	default:
+		LBUG();
+	}
+}
+
+/**
+ * Unlock statistics structure after access.
+ *
+ * Unlock the lock acquired via lprocfs_stats_lock() for global statistics,
+ * or unpin this thread from the current cpuid for per-CPU statistics.
+ *
+ * This function must be called using the same arguments as used when calling
+ * lprocfs_stats_lock() so that the correct operation can be performed.
+ *
+ * \param[in] stats    statistics structure to unlock
+ * \param[in] opc      type of operation (current cpuid or number of structs)
+ * \param[in] flags    CPU interrupt saved state for IRQ-safe locking
+ */
+void lprocfs_stats_unlock(struct lprocfs_stats *stats,
+			  enum lprocfs_stats_lock_ops opc,
+			  unsigned long *flags)
+{
+	if (stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) {
+		if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
+			spin_unlock_irqrestore(&stats->ls_lock, *flags);
+		else
+			spin_unlock(&stats->ls_lock);
+	} else if (opc == LPROCFS_GET_SMP_ID) {
+		put_cpu();
+	}
+}
+
 /** add up per-cpu counters */
 void lprocfs_stats_collect(struct lprocfs_stats *stats, int idx,
 			   struct lprocfs_counter *cnt)
@@ -1146,6 +1233,30 @@ void lprocfs_free_stats(struct lprocfs_stats **statsh)
 }
 EXPORT_SYMBOL(lprocfs_free_stats);
 
+__u64 lprocfs_stats_collector(struct lprocfs_stats *stats, int idx,
+			      enum lprocfs_fields_flags field)
+{
+	unsigned int i;
+	unsigned int  num_cpu;
+	unsigned long flags     = 0;
+	__u64         ret       = 0;
+
+	LASSERT(stats);
+
+	num_cpu = lprocfs_stats_lock(stats, LPROCFS_GET_NUM_CPU, &flags);
+	for (i = 0; i < num_cpu; i++) {
+		if (!stats->ls_percpu[i])
+			continue;
+		ret += lprocfs_read_helper(
+				lprocfs_stats_counter_get(stats, i, idx),
+				&stats->ls_cnt_header[idx], stats->ls_flags,
+				field);
+	}
+	lprocfs_stats_unlock(stats, LPROCFS_GET_NUM_CPU, &flags);
+	return ret;
+}
+EXPORT_SYMBOL(lprocfs_stats_collector);
+
 void lprocfs_clear_stats(struct lprocfs_stats *stats)
 {
 	struct lprocfs_counter		*percpu_cntr;
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_config.c b/drivers/staging/lustre/lustre/obdclass/obd_config.c
index 9ca84c7d49de..6a7e7a7d2af1 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_config.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_config.c
@@ -1052,7 +1052,8 @@ int class_process_proc_param(char *prefix, struct lprocfs_vars *lvars,
 
 					oldfs = get_fs();
 					set_fs(KERNEL_DS);
-					rc = var->fops->write(&fakefile, sval,
+					rc = var->fops->write(&fakefile,
+						(const char __user *)sval,
 								vallen, NULL);
 					set_fs(oldfs);
 				}
@@ -1426,8 +1427,8 @@ int class_manual_cleanup(struct obd_device *obd)
 	lustre_cfg_bufs_reset(&bufs, obd->obd_name);
 	lustre_cfg_bufs_set_string(&bufs, 1, flags);
 	lcfg = lustre_cfg_new(LCFG_CLEANUP, &bufs);
-	if (!lcfg)
-		return -ENOMEM;
+	if (IS_ERR(lcfg))
+		return PTR_ERR(lcfg);
 
 	rc = class_process_config(lcfg);
 	if (rc) {
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index 549076193bde..77b4c5504689 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -816,7 +816,7 @@ cl_echo_object_find(struct echo_device *d, const struct ost_id *oi)
 	struct echo_object *eco;
 	struct cl_object   *obj;
 	struct lu_fid *fid;
-	int refcheck;
+	u16 refcheck;
 	int rc;
 
 	LASSERTF(ostid_id(oi), DOSTID "\n", POSTID(oi));
@@ -882,7 +882,7 @@ static int cl_echo_object_put(struct echo_object *eco)
 {
 	struct lu_env *env;
 	struct cl_object *obj = echo_obj2cl(eco);
-	int refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	if (IS_ERR(env))
@@ -999,7 +999,7 @@ static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 	struct cl_page	  *clp;
 	struct lustre_handle    lh = { 0 };
 	size_t page_size = cl_page_size(obj);
-	int refcheck;
+	u16 refcheck;
 	int rc;
 	int i;
 
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 575b2969ad83..86f252d6adbd 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -229,7 +229,7 @@ static ssize_t osc_cached_mb_seq_write(struct file *file,
 	rc = atomic_long_read(&cli->cl_lru_in_list) - pages_number;
 	if (rc > 0) {
 		struct lu_env *env;
-		int refcheck;
+		u16 refcheck;
 
 		env = cl_env_get(&refcheck);
 		if (!IS_ERR(env)) {
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index 0490478393df..c5ccf568313a 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -898,6 +898,7 @@ int osc_extent_finish(const struct lu_env *env, struct osc_extent *ext,
 		int offset = last_off & ~PAGE_MASK;
 		int count = last_count + (offset & (blocksize - 1));
 		int end = (offset + last_count) & (blocksize - 1);
+
 		if (end)
 			count += blocksize - end;
 
@@ -988,7 +989,7 @@ static int osc_extent_truncate(struct osc_extent *ext, pgoff_t trunc_index,
 	int grants = 0;
 	int nr_pages = 0;
 	int rc = 0;
-	int refcheck;
+	u16 refcheck;
 
 	LASSERT(sanity_check(ext) == 0);
 	EASSERT(ext->oe_state == OES_TRUNC, ext);
@@ -2790,7 +2791,6 @@ again:
 			 * We have to wait for this extent because we can't
 			 * truncate that page.
 			 */
-			LASSERT(!ext->oe_hp);
 			OSC_EXTENT_DUMP(D_CACHE, ext,
 					"waiting for busy extent\n");
 			waiting = osc_extent_get(ext);
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index c09ab97d64ae..270212f4e5cf 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -62,7 +62,9 @@ struct osc_io {
 	/** super class */
 	struct cl_io_slice oi_cl;
 	/** true if this io is lockless. */
-	unsigned int		oi_lockless;
+	unsigned int		oi_lockless:1,
+	/** true if this io is counted as active IO */
+				oi_is_active:1;
 	/** how many LRU pages are reserved for this IO */
 	unsigned long		oi_lru_reserved;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_internal.h b/drivers/staging/lustre/lustre/osc/osc_internal.h
index 8abd83f26716..845e795d8795 100644
--- a/drivers/staging/lustre/lustre/osc/osc_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_internal.h
@@ -133,7 +133,8 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 		  struct list_head *ext_list, int cmd);
 long osc_lru_shrink(const struct lu_env *env, struct client_obd *cli,
 		    long target, bool force);
-long osc_lru_reclaim(struct client_obd *cli, unsigned long npages);
+unsigned long osc_lru_reserve(struct client_obd *cli, unsigned long npages);
+void osc_lru_unreserve(struct client_obd *cli, unsigned long npages);
 
 unsigned long osc_ldlm_weigh_ast(struct ldlm_lock *dlmlock);
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index 0b4cc4283b05..f991bee81b37 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -354,7 +354,10 @@ static int osc_io_iter_init(const struct lu_env *env,
 
 	spin_lock(&imp->imp_lock);
 	if (likely(!imp->imp_invalid)) {
+		struct osc_io *oio = osc_env_io(env);
+
 		atomic_inc(&osc->oo_nr_ios);
+		oio->oi_is_active = 1;
 		rc = 0;
 	}
 	spin_unlock(&imp->imp_lock);
@@ -368,10 +371,7 @@ static int osc_io_write_iter_init(const struct lu_env *env,
 	struct cl_io *io = ios->cis_io;
 	struct osc_io *oio = osc_env_io(env);
 	struct osc_object *osc = cl2osc(ios->cis_obj);
-	struct client_obd *cli = osc_cli(osc);
-	unsigned long c;
 	unsigned long npages;
-	unsigned long max_pages;
 
 	if (cl_io_is_append(io))
 		return osc_io_iter_init(env, ios);
@@ -380,31 +380,7 @@ static int osc_io_write_iter_init(const struct lu_env *env,
 	if (io->u.ci_rw.crw_pos & ~PAGE_MASK)
 		++npages;
 
-	max_pages = cli->cl_max_pages_per_rpc * cli->cl_max_rpcs_in_flight;
-	if (npages > max_pages)
-		npages = max_pages;
-
-	c = atomic_long_read(cli->cl_lru_left);
-	if (c < npages && osc_lru_reclaim(cli, npages) > 0)
-		c = atomic_long_read(cli->cl_lru_left);
-	while (c >= npages) {
-		if (c == atomic_long_cmpxchg(cli->cl_lru_left, c, c - npages)) {
-			oio->oi_lru_reserved = npages;
-			break;
-		}
-		c = atomic_long_read(cli->cl_lru_left);
-	}
-	if (atomic_long_read(cli->cl_lru_left) < max_pages) {
-		/*
-		 * If there aren't enough pages in the per-OSC LRU then
-		 * wake up the LRU thread to try and clear out space, so
-		 * we don't block if pages are being dirtied quickly.
-		 */
-		CDEBUG(D_CACHE, "%s: queue LRU, left: %lu/%ld.\n",
-		       cli_name(cli), atomic_long_read(cli->cl_lru_left),
-		       max_pages);
-		(void)ptlrpcd_queue_work(cli->cl_lru_work);
-	}
+	oio->oi_lru_reserved = osc_lru_reserve(osc_cli(osc), npages);
 
 	return osc_io_iter_init(env, ios);
 }
@@ -412,11 +388,16 @@ static int osc_io_write_iter_init(const struct lu_env *env,
 static void osc_io_iter_fini(const struct lu_env *env,
 			     const struct cl_io_slice *ios)
 {
-	struct osc_object *osc = cl2osc(ios->cis_obj);
+	struct osc_io *oio = osc_env_io(env);
 
-	LASSERT(atomic_read(&osc->oo_nr_ios) > 0);
-	if (atomic_dec_and_test(&osc->oo_nr_ios))
-		wake_up_all(&osc->oo_io_waitq);
+	if (oio->oi_is_active) {
+		struct osc_object *osc = cl2osc(ios->cis_obj);
+
+		oio->oi_is_active = 0;
+		LASSERT(atomic_read(&osc->oo_nr_ios) > 0);
+		if (atomic_dec_and_test(&osc->oo_nr_ios))
+			wake_up_all(&osc->oo_io_waitq);
+	}
 }
 
 static void osc_io_write_iter_fini(const struct lu_env *env,
@@ -424,10 +405,9 @@ static void osc_io_write_iter_fini(const struct lu_env *env,
 {
 	struct osc_io *oio = osc_env_io(env);
 	struct osc_object *osc = cl2osc(ios->cis_obj);
-	struct client_obd *cli = osc_cli(osc);
 
 	if (oio->oi_lru_reserved > 0) {
-		atomic_long_add(oio->oi_lru_reserved, cli->cl_lru_left);
+		osc_lru_unreserve(osc_cli(osc), oio->oi_lru_reserved);
 		oio->oi_lru_reserved = 0;
 	}
 	oio->oi_write_osclock = NULL;
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index 5f799a4c78f9..940c10c1d7a1 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -167,6 +167,8 @@ static __u64 osc_enq2ldlm_flags(__u32 enqflags)
 		result |= LDLM_FL_AST_DISCARD_DATA;
 	if (enqflags & CEF_PEEK)
 		result |= LDLM_FL_TEST_LOCK;
+	if (enqflags & CEF_LOCK_MATCH)
+		result |= LDLM_FL_MATCH_LOCK;
 	return result;
 }
 
@@ -295,7 +297,7 @@ static int osc_lock_upcall(void *cookie, struct lustre_handle *lockh,
 	struct cl_lock_slice *slice = &oscl->ols_cl;
 	struct lu_env *env;
 	int rc;
-	int refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	/* should never happen, similar to osc_ldlm_blocking_ast(). */
@@ -347,7 +349,7 @@ static int osc_lock_upcall_agl(void *cookie, struct lustre_handle *lockh,
 	struct osc_object *osc = cookie;
 	struct ldlm_lock *dlmlock;
 	struct lu_env *env;
-	int refcheck;
+	u16 refcheck;
 
 	env = cl_env_get(&refcheck);
 	LASSERT(!IS_ERR(env));
@@ -382,7 +384,7 @@ static int osc_lock_flush(struct osc_object *obj, pgoff_t start, pgoff_t end,
 			  enum cl_lock_mode mode, int discard)
 {
 	struct lu_env *env;
-	int refcheck;
+	u16 refcheck;
 	int rc = 0;
 	int rc2 = 0;
 
@@ -536,7 +538,7 @@ static int osc_ldlm_blocking_ast(struct ldlm_lock *dlmlock,
 	}
 	case LDLM_CB_CANCELING: {
 		struct lu_env *env;
-		int refcheck;
+		u16 refcheck;
 
 		/*
 		 * This can be called in the context of outer IO, e.g.,
@@ -573,7 +575,7 @@ static int osc_ldlm_glimpse_ast(struct ldlm_lock *dlmlock, void *data)
 	struct req_capsule *cap;
 	struct cl_object *obj = NULL;
 	int result;
-	int refcheck;
+	u16 refcheck;
 
 	LASSERT(lustre_msg_get_opc(req->rq_reqmsg) == LDLM_GL_CALLBACK);
 
@@ -684,7 +686,7 @@ unsigned long osc_ldlm_weigh_ast(struct ldlm_lock *dlmlock)
 	struct osc_lock		*oscl;
 	unsigned long            weight;
 	bool			 found = false;
-	int refcheck;
+	u16 refcheck;
 
 	might_sleep();
 	/*
@@ -838,13 +840,14 @@ static void osc_lock_wake_waiters(const struct lu_env *env,
 	spin_unlock(&oscl->ols_lock);
 }
 
-static void osc_lock_enqueue_wait(const struct lu_env *env,
-				  struct osc_object *obj,
-				  struct osc_lock *oscl)
+static int osc_lock_enqueue_wait(const struct lu_env *env,
+				 struct osc_object *obj,
+				 struct osc_lock *oscl)
 {
 	struct osc_lock *tmp_oscl;
 	struct cl_lock_descr *need = &oscl->ols_cl.cls_lock->cll_descr;
 	struct cl_sync_io *waiter = &osc_env_info(env)->oti_anchor;
+	int rc = 0;
 
 	spin_lock(&obj->oo_ol_spin);
 	list_add_tail(&oscl->ols_nextlock_oscobj, &obj->oo_ol_list);
@@ -881,13 +884,17 @@ restart:
 		spin_unlock(&tmp_oscl->ols_lock);
 
 		spin_unlock(&obj->oo_ol_spin);
-		(void)cl_sync_io_wait(env, waiter, 0);
-
+		rc = cl_sync_io_wait(env, waiter, 0);
 		spin_lock(&obj->oo_ol_spin);
+		if (rc < 0)
+			break;
+
 		oscl->ols_owner = NULL;
 		goto restart;
 	}
 	spin_unlock(&obj->oo_ol_spin);
+
+	return rc;
 }
 
 /**
@@ -935,7 +942,9 @@ static int osc_lock_enqueue(const struct lu_env *env,
 		goto enqueue_base;
 	}
 
-	osc_lock_enqueue_wait(env, osc, oscl);
+	result = osc_lock_enqueue_wait(env, osc, oscl);
+	if (result < 0)
+		goto out;
 
 	/* we can grant lockless lock right after all conflicting locks
 	 * are canceled.
@@ -960,7 +969,6 @@ enqueue_base:
 	 * osc_lock.
 	 */
 	ostid_build_res_name(&osc->oo_oinfo->loi_oi, resname);
-	osc_lock_build_einfo(env, lock, osc, &oscl->ols_einfo);
 	osc_lock_build_policy(env, lock, policy);
 	if (oscl->ols_agl) {
 		oscl->ols_einfo.ei_cbdata = NULL;
@@ -975,18 +983,7 @@ enqueue_base:
 				  upcall, cookie,
 				  &oscl->ols_einfo, PTLRPCD_SET, async,
 				  oscl->ols_agl);
-	if (result != 0) {
-		oscl->ols_state = OLS_CANCELLED;
-		osc_lock_wake_waiters(env, osc, oscl);
-
-		/* hide error for AGL lock. */
-		if (oscl->ols_agl) {
-			cl_object_put(env, osc2cl(osc));
-			result = 0;
-		}
-		if (anchor)
-			cl_sync_io_note(env, anchor, result);
-	} else {
+	if (!result) {
 		if (osc_lock_is_lockless(oscl)) {
 			oio->oi_lockless = 1;
 		} else if (!async) {
@@ -994,6 +991,18 @@ enqueue_base:
 			LASSERT(oscl->ols_hold);
 			LASSERT(oscl->ols_dlmlock);
 		}
+	} else if (oscl->ols_agl) {
+		cl_object_put(env, osc2cl(osc));
+		result = 0;
+	}
+
+out:
+	if (result < 0) {
+		oscl->ols_state = OLS_CANCELLED;
+		osc_lock_wake_waiters(env, osc, oscl);
+
+		if (anchor)
+			cl_sync_io_note(env, anchor, result);
 	}
 	return result;
 }
@@ -1157,6 +1166,7 @@ int osc_lock_init(const struct lu_env *env,
 		oscl->ols_flags |= LDLM_FL_BLOCK_GRANTED;
 		oscl->ols_glimpse = 1;
 	}
+	osc_lock_build_einfo(env, lock, cl2osc(obj), &oscl->ols_einfo);
 
 	cl_lock_slice_add(lock, &oscl->ols_cl, obj, &osc_lock_ops);
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index d3e5ca7db7b2..fa621bda1ffe 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -200,10 +200,6 @@ static int osc_object_prune(const struct lu_env *env, struct cl_object *obj)
 	struct osc_object       *osc = cl2osc(obj);
 	struct ldlm_res_id      *resname = &osc_env_info(env)->oti_resname;
 
-	LASSERTF(osc->oo_npages == 0,
-		 DFID "still have %lu pages, obj: %p, osc: %p\n",
-		 PFID(lu_object_fid(&obj->co_lu)), osc->oo_npages, obj, osc);
-
 	/* DLM locks don't hold a reference of osc_object so we have to
 	 * clear it before the object is being destroyed.
 	 */
@@ -457,9 +453,15 @@ int osc_object_invalidate(const struct lu_env *env, struct osc_object *osc)
 
 	l_wait_event(osc->oo_io_waitq, !atomic_read(&osc->oo_nr_ios), &lwi);
 
-	/* Discard all pages of this object. */
+	/* Discard all dirty pages of this object. */
 	osc_cache_truncate_start(env, osc, 0, NULL);
 
+	/* Discard all caching pages */
+	osc_lock_discard_pages(env, osc, 0, CL_PAGE_EOF, CLM_WRITE);
+
+	/* Clear ast data of dlm lock. Do this after discarding all pages */
+	osc_object_prune(env, osc2cl(osc));
+
 	return 0;
 }
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_page.c b/drivers/staging/lustre/lustre/osc/osc_page.c
index ab9d0d7bb943..ed8a0dc18ee5 100644
--- a/drivers/staging/lustre/lustre/osc/osc_page.c
+++ b/drivers/staging/lustre/lustre/osc/osc_page.c
@@ -42,8 +42,8 @@
 
 static void osc_lru_del(struct client_obd *cli, struct osc_page *opg);
 static void osc_lru_use(struct client_obd *cli, struct osc_page *opg);
-static int osc_lru_reserve(const struct lu_env *env, struct osc_object *obj,
-			   struct osc_page *opg);
+static int osc_lru_alloc(const struct lu_env *env, struct client_obd *cli,
+			 struct osc_page *opg);
 
 /** \addtogroup osc
  *  @{
@@ -273,7 +273,7 @@ int osc_page_init(const struct lu_env *env, struct cl_object *obj,
 
 	/* reserve an LRU space for this page */
 	if (page->cp_type == CPT_CACHEABLE && result == 0) {
-		result = osc_lru_reserve(env, osc, opg);
+		result = osc_lru_alloc(env, osc_cli(osc), opg);
 		if (result == 0) {
 			spin_lock(&osc->oo_tree_lock);
 			result = radix_tree_insert(&osc->oo_tree, index, opg);
@@ -676,12 +676,12 @@ long osc_lru_shrink(const struct lu_env *env, struct client_obd *cli,
  * LRU pages in batch. Therefore, the actual number is adjusted at least
  * max_pages_per_rpc.
  */
-long osc_lru_reclaim(struct client_obd *cli, unsigned long npages)
+static long osc_lru_reclaim(struct client_obd *cli, unsigned long npages)
 {
 	struct lu_env *env;
 	struct cl_client_cache *cache = cli->cl_cache;
 	int max_scans;
-	int refcheck;
+	u16 refcheck;
 	long rc = 0;
 
 	LASSERT(cache);
@@ -749,18 +749,17 @@ out:
 }
 
 /**
- * osc_lru_reserve() is called to reserve an LRU slot for a cl_page.
+ * osc_lru_alloc() is called to reserve an LRU slot for a cl_page.
  *
  * Usually the LRU slots are reserved in osc_io_iter_rw_init().
  * Only in the case that the LRU slots are in extreme shortage, it should
  * have reserved enough slots for an IO.
  */
-static int osc_lru_reserve(const struct lu_env *env, struct osc_object *obj,
-			   struct osc_page *opg)
+static int osc_lru_alloc(const struct lu_env *env, struct client_obd *cli,
+			 struct osc_page *opg)
 {
 	struct l_wait_info lwi = LWI_INTR(LWI_ON_SIGNAL_NOOP, NULL);
 	struct osc_io *oio = osc_env_io(env);
-	struct client_obd *cli = osc_cli(obj);
 	int rc = 0;
 
 	if (!cli->cl_cache) /* shall not be in LRU */
@@ -801,6 +800,64 @@ out:
 }
 
 /**
+ * osc_lru_reserve() is called to reserve enough LRU slots for I/O.
+ *
+ * The benefit of doing this is to reduce contention against atomic counter
+ * cl_lru_left by changing it from per-page access to per-IO access.
+ */
+unsigned long osc_lru_reserve(struct client_obd *cli, unsigned long npages)
+{
+	unsigned long reserved = 0;
+	unsigned long max_pages;
+	unsigned long c;
+
+	/*
+	 * reserve a full RPC window at most to avoid that a thread accidentally
+	 * consumes too many LRU slots
+	 */
+	max_pages = cli->cl_max_pages_per_rpc * cli->cl_max_rpcs_in_flight;
+	if (npages > max_pages)
+		npages = max_pages;
+
+	c = atomic_long_read(cli->cl_lru_left);
+	if (c < npages && osc_lru_reclaim(cli, npages) > 0)
+		c = atomic_long_read(cli->cl_lru_left);
+	while (c >= npages) {
+		if (c == atomic_long_cmpxchg(cli->cl_lru_left, c, c - npages)) {
+			reserved = npages;
+			break;
+		}
+		c = atomic_long_read(cli->cl_lru_left);
+	}
+	if (atomic_long_read(cli->cl_lru_left) < max_pages) {
+		/*
+		 * If there aren't enough pages in the per-OSC LRU then
+		 * wake up the LRU thread to try and clear out space, so
+		 * we don't block if pages are being dirtied quickly.
+		 */
+		CDEBUG(D_CACHE, "%s: queue LRU, left: %lu/%ld.\n",
+		       cli_name(cli), atomic_long_read(cli->cl_lru_left),
+		       max_pages);
+		(void)ptlrpcd_queue_work(cli->cl_lru_work);
+	}
+
+	return reserved;
+}
+
+/**
+ * osc_lru_unreserve() is called to unreserve LRU slots.
+ *
+ * LRU slots reserved by osc_lru_reserve() may have entries left due to several
+ * reasons such as page already existing or I/O error. Those reserved slots
+ * should be freed by calling this function.
+ */
+void osc_lru_unreserve(struct client_obd *cli, unsigned long npages)
+{
+	atomic_long_add(npages, cli->cl_lru_left);
+	wake_up_all(&osc_lru_waitq);
+}
+
+/**
  * Atomic operations are expensive. We accumulate the accounting for the
  * same page pgdat to get better performance.
  * In practice this can work pretty good because the pages in the same RPC
@@ -988,7 +1045,7 @@ unsigned long osc_cache_shrink_scan(struct shrinker *sk,
 	struct client_obd *cli;
 	struct lu_env *env;
 	long shrank = 0;
-	int refcheck;
+	u16 refcheck;
 	int rc;
 
 	if (!sc->nr_to_scan)
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index c4cfe18c3294..d8aa3fb468c7 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -1195,7 +1195,8 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,
 	return rc;
 }
 
-static int check_write_checksum(struct obdo *oa, const lnet_process_id_t *peer,
+static int check_write_checksum(struct obdo *oa,
+				const struct lnet_process_id *peer,
 				__u32 client_cksum, __u32 server_cksum, int nob,
 				u32 page_count, struct brw_page **pga,
 				enum cksum_type client_cksum_type)
@@ -1245,7 +1246,7 @@ static int check_write_checksum(struct obdo *oa, const lnet_process_id_t *peer,
 static int osc_brw_fini_request(struct ptlrpc_request *req, int rc)
 {
 	struct osc_brw_async_args *aa = (void *)&req->rq_async_args;
-	const lnet_process_id_t *peer =
+	const struct lnet_process_id *peer =
 			&req->rq_import->imp_connection->c_peer;
 	struct client_obd *cli = aa->aa_cli;
 	struct ost_body *body;
@@ -2011,7 +2012,7 @@ int osc_enqueue_base(struct obd_export *exp, struct ldlm_res_id *res_id,
 	}
 
 no_match:
-	if (*flags & LDLM_FL_TEST_LOCK)
+	if (*flags & (LDLM_FL_TEST_LOCK | LDLM_FL_MATCH_LOCK))
 		return -ENOLCK;
 	if (intent) {
 		req = ptlrpc_request_alloc(class_exp2cliimp(exp),
@@ -2495,7 +2496,13 @@ static int osc_ldlm_resource_invalidate(struct cfs_hash *hs,
 			osc = lock->l_ast_data;
 			cl_object_get(osc2cl(osc));
 		}
-		lock->l_ast_data = NULL;
+
+		/*
+		 * clear LDLM_FL_CLEANED flag to make sure it will be canceled
+		 * by the 2nd round of ldlm_namespace_clean() call in
+		 * osc_import_event().
+		 */
+		ldlm_clear_cleaned(lock);
 	}
 	unlock_res(res);
 
@@ -2532,7 +2539,7 @@ static int osc_import_event(struct obd_device *obd,
 	case IMP_EVENT_INVALIDATE: {
 		struct ldlm_namespace *ns = obd->obd_namespace;
 		struct lu_env *env;
-		int refcheck;
+		u16 refcheck;
 
 		ldlm_namespace_cleanup(ns, LDLM_FL_LOCAL_ONLY);
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 04a98a08ece1..6466974a43e7 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -78,7 +78,7 @@ struct ptlrpc_connection *ptlrpc_uuid_to_connection(struct obd_uuid *uuid)
 {
 	struct ptlrpc_connection *c;
 	lnet_nid_t self;
-	lnet_process_id_t peer;
+	struct lnet_process_id peer;
 	int err;
 
 	/*
@@ -151,7 +151,7 @@ struct ptlrpc_bulk_desc *ptlrpc_new_bulk(unsigned int nfrags,
 	 * node. Negotiated ocd_brw_size will always be <= this number.
 	 */
 	for (i = 0; i < PTLRPC_BULK_OPS_COUNT; i++)
-		LNetInvalidateHandle(&desc->bd_mds[i]);
+		LNetInvalidateMDHandle(&desc->bd_mds[i]);
 
 	return desc;
 free_desc:
@@ -3144,8 +3144,7 @@ void ptlrpc_set_bulk_mbits(struct ptlrpc_request *req)
 	 * that server can infer the number of bulks that were prepared,
 	 * see LU-1431
 	 */
-	req->rq_mbits += ((bd->bd_iov_count + LNET_MAX_IOV - 1) /
-			  LNET_MAX_IOV) - 1;
+	req->rq_mbits += DIV_ROUND_UP(bd->bd_iov_count, LNET_MAX_IOV) - 1;
 }
 
 /**
diff --git a/drivers/staging/lustre/lustre/ptlrpc/connection.c b/drivers/staging/lustre/lustre/ptlrpc/connection.c
index 6c7c8b68a909..73a2dbbeb7e6 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/connection.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/connection.c
@@ -41,7 +41,7 @@ static struct cfs_hash *conn_hash;
 static struct cfs_hash_ops conn_hash_ops;
 
 struct ptlrpc_connection *
-ptlrpc_connection_get(lnet_process_id_t peer, lnet_nid_t self,
+ptlrpc_connection_get(struct lnet_process_id peer, lnet_nid_t self,
 		      struct obd_uuid *uuid)
 {
 	struct ptlrpc_connection *conn, *conn2;
@@ -155,14 +155,14 @@ void ptlrpc_connection_fini(void)
 static unsigned int
 conn_hashfn(struct cfs_hash *hs, const void *key, unsigned int mask)
 {
-	return cfs_hash_djb2_hash(key, sizeof(lnet_process_id_t), mask);
+	return cfs_hash_djb2_hash(key, sizeof(struct lnet_process_id), mask);
 }
 
 static int
 conn_keycmp(const void *key, struct hlist_node *hnode)
 {
 	struct ptlrpc_connection *conn;
-	const lnet_process_id_t *conn_key;
+	const struct lnet_process_id *conn_key;
 
 	LASSERT(key);
 	conn_key = key;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index dc0fe9d660da..978bdaca3cdd 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -42,12 +42,12 @@
 #include "../include/lustre_sec.h"
 #include "ptlrpc_internal.h"
 
-lnet_handle_eq_t   ptlrpc_eq_h;
+struct lnet_handle_eq ptlrpc_eq_h;
 
 /*
  *  Client's outgoing request callback
  */
-void request_out_callback(lnet_event_t *ev)
+void request_out_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request *req = cbid->cbid_arg;
@@ -86,7 +86,7 @@ void request_out_callback(lnet_event_t *ev)
 /*
  * Client's incoming reply callback
  */
-void reply_in_callback(lnet_event_t *ev)
+void reply_in_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request *req = cbid->cbid_arg;
@@ -176,7 +176,7 @@ out_wake:
 /*
  * Client's bulk has been written/read
  */
-void client_bulk_callback(lnet_event_t *ev)
+void client_bulk_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_bulk_desc *desc = cbid->cbid_arg;
@@ -289,7 +289,7 @@ static void ptlrpc_req_add_history(struct ptlrpc_service_part *svcpt,
 /*
  * Server's incoming request callback
  */
-void request_in_callback(lnet_event_t *ev)
+void request_in_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_request_buffer_desc *rqbd = cbid->cbid_arg;
@@ -389,7 +389,7 @@ void request_in_callback(lnet_event_t *ev)
 /*
  *  Server's outgoing reply callback
  */
-void reply_out_callback(lnet_event_t *ev)
+void reply_out_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
 	struct ptlrpc_reply_state *rs = cbid->cbid_arg;
@@ -429,10 +429,10 @@ void reply_out_callback(lnet_event_t *ev)
 	}
 }
 
-static void ptlrpc_master_callback(lnet_event_t *ev)
+static void ptlrpc_master_callback(struct lnet_event *ev)
 {
 	struct ptlrpc_cb_id *cbid = ev->md.user_ptr;
-	void (*callback)(lnet_event_t *ev) = cbid->cbid_fn;
+	void (*callback)(struct lnet_event *ev) = cbid->cbid_fn;
 
 	/* Honestly, it's best to find out early. */
 	LASSERT(cbid->cbid_arg != LP_POISON);
@@ -446,7 +446,7 @@ static void ptlrpc_master_callback(lnet_event_t *ev)
 }
 
 int ptlrpc_uuid_to_peer(struct obd_uuid *uuid,
-			lnet_process_id_t *peer, lnet_nid_t *self)
+			struct lnet_process_id *peer, lnet_nid_t *self)
 {
 	int best_dist = 0;
 	__u32 best_order = 0;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index 356d73511ea3..8177e1a31ca9 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -58,7 +58,6 @@
 /* struct ptlrpc_request, lustre_msg* */
 #include "../include/lustre_req_layout.h"
 #include "../include/lustre_acl.h"
-#include "../include/lustre_debug.h"
 
 /*
  * RQFs (see below) refer to two struct req_msg_field arrays describing the
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index b8701841ab4a..eddc1927a8d2 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -43,13 +43,13 @@
  * over \a conn connection to portal \a portal.
  * Returns 0 on success or error code.
  */
-static int ptl_send_buf(lnet_handle_md_t *mdh, void *base, int len,
-			lnet_ack_req_t ack, struct ptlrpc_cb_id *cbid,
+static int ptl_send_buf(struct lnet_handle_md *mdh, void *base, int len,
+			enum lnet_ack_req ack, struct ptlrpc_cb_id *cbid,
 			struct ptlrpc_connection *conn, int portal, __u64 xid,
 			unsigned int offset)
 {
 	int rc;
-	lnet_md_t md;
+	struct lnet_md md;
 
 	LASSERT(portal != 0);
 	CDEBUG(D_INFO, "conn=%p id %s\n", conn, libcfs_id2str(conn->c_peer));
@@ -94,7 +94,7 @@ static int ptl_send_buf(lnet_handle_md_t *mdh, void *base, int len,
 	return 0;
 }
 
-static void mdunlink_iterate_helper(lnet_handle_md_t *bd_mds, int count)
+static void mdunlink_iterate_helper(struct lnet_handle_md *bd_mds, int count)
 {
 	int i;
 
@@ -109,14 +109,14 @@ static void mdunlink_iterate_helper(lnet_handle_md_t *bd_mds, int count)
 static int ptlrpc_register_bulk(struct ptlrpc_request *req)
 {
 	struct ptlrpc_bulk_desc *desc = req->rq_bulk;
-	lnet_process_id_t peer;
+	struct lnet_process_id peer;
 	int rc = 0;
 	int rc2;
 	int posted_md;
 	int total_md;
 	u64 mbits;
-	lnet_handle_me_t me_h;
-	lnet_md_t md;
+	struct lnet_handle_me me_h;
+	struct lnet_md md;
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_PTLRPC_BULK_GET_NET))
 		return 0;
@@ -142,7 +142,7 @@ static int ptlrpc_register_bulk(struct ptlrpc_request *req)
 	LASSERT(desc->bd_cbid.cbid_fn == client_bulk_callback);
 	LASSERT(desc->bd_cbid.cbid_arg == desc);
 
-	total_md = (desc->bd_iov_count + LNET_MAX_IOV - 1) / LNET_MAX_IOV;
+	total_md = DIV_ROUND_UP(desc->bd_iov_count, LNET_MAX_IOV);
 	/* rq_mbits is matchbits of the final bulk */
 	mbits = req->rq_mbits - total_md + 1;
 
@@ -472,8 +472,8 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 	int rc2;
 	int mpflag = 0;
 	struct ptlrpc_connection *connection;
-	lnet_handle_me_t reply_me_h;
-	lnet_md_t reply_md;
+	struct lnet_handle_me reply_me_h;
+	struct lnet_md reply_md;
 	struct obd_import *imp = request->rq_import;
 	struct obd_device *obd = imp->imp_obd;
 
@@ -719,10 +719,10 @@ EXPORT_SYMBOL(ptl_send_rpc);
 int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 {
 	struct ptlrpc_service *service = rqbd->rqbd_svcpt->scp_service;
-	static lnet_process_id_t match_id = {LNET_NID_ANY, LNET_PID_ANY};
+	static struct lnet_process_id match_id = {LNET_NID_ANY, LNET_PID_ANY};
 	int rc;
-	lnet_md_t md;
-	lnet_handle_me_t me_h;
+	struct lnet_md md;
+	struct lnet_handle_me me_h;
 
 	CDEBUG(D_NET, "LNetMEAttach: portal %d\n",
 	       service->srv_req_portal);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pers.c b/drivers/staging/lustre/lustre/ptlrpc/pers.c
index 601acb84f343..df4994f406e9 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pers.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pers.c
@@ -40,7 +40,7 @@
 
 #include "ptlrpc_internal.h"
 
-void ptlrpc_fill_bulk_md(lnet_md_t *md, struct ptlrpc_bulk_desc *desc,
+void ptlrpc_fill_bulk_md(struct lnet_md *md, struct ptlrpc_bulk_desc *desc,
 			 int mdidx)
 {
 	int offset = mdidx * LNET_MAX_IOV;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index 8e6a805487ec..d2707a323c47 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -234,7 +234,7 @@ extern struct ptlrpc_nrs_pol_conf nrs_conf_fifo;
 int ptlrpc_expire_one_request(struct ptlrpc_request *req, int async_unlink);
 
 /* pers.c */
-void ptlrpc_fill_bulk_md(lnet_md_t *md, struct ptlrpc_bulk_desc *desc,
+void ptlrpc_fill_bulk_md(struct lnet_md *md, struct ptlrpc_bulk_desc *desc,
 			 int mdcnt);
 
 /* pack_generic.c */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index 2fe9085e2034..128838a695e0 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -272,7 +272,7 @@ static unsigned long enc_pools_shrink_scan(struct shrinker *s,
 static inline
 int npages_to_npools(unsigned long npages)
 {
-	return (int)((npages + PAGES_PER_POOL - 1) / PAGES_PER_POOL);
+	return (int)DIV_ROUND_UP(npages, PAGES_PER_POOL);
 }
 
 /*
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index 8ed8202da57a..dbda4d9a08e7 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -19,6 +19,8 @@ menuconfig STAGING_MEDIA
 if STAGING_MEDIA && MEDIA_SUPPORT
 
 # Please keep them in alphabetic order
+source "drivers/staging/media/atomisp/Kconfig"
+
 source "drivers/staging/media/bcm2048/Kconfig"
 
 source "drivers/staging/media/cxd2099/Kconfig"
@@ -27,8 +29,6 @@ source "drivers/staging/media/davinci_vpfe/Kconfig"
 
 source "drivers/staging/media/omap4iss/Kconfig"
 
-source "drivers/staging/media/platform/bcm2835/Kconfig"
-
 # Keep LIRC at the end, as it has sub-menus
 source "drivers/staging/media/lirc/Kconfig"
 
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index 3a6adeabede1..c04600c81264 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,6 +1,6 @@
 obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
-obj-$(CONFIG_VIDEO_BCM2835)	+= platform/bcm2835/
 obj-$(CONFIG_VIDEO_DM365_VPFE)	+= davinci_vpfe/
 obj-$(CONFIG_VIDEO_OMAP4)	+= omap4iss/
+obj-$(CONFIG_INTEL_ATOMISP)     += atomisp/
diff --git a/drivers/staging/media/atomisp/Kconfig b/drivers/staging/media/atomisp/Kconfig
new file mode 100644
index 000000000000..8eb13c3ba29c
--- /dev/null
+++ b/drivers/staging/media/atomisp/Kconfig
@@ -0,0 +1,11 @@
+menuconfig INTEL_ATOMISP
+        bool "Enable support to Intel MIPI camera drivers"
+        depends on X86 && EFI && MEDIA_CONTROLLER && PCI && ACPI
+        help
+          Enable support for the Intel ISP2 camera interfaces and MIPI
+          sensor drivers.
+
+if INTEL_ATOMISP
+source "drivers/staging/media/atomisp/pci/Kconfig"
+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 000000000000..403fe5edff6d
--- /dev/null
+++ b/drivers/staging/media/atomisp/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for camera drivers.
+#
+obj-$(CONFIG_INTEL_ATOMISP) += pci/
+obj-$(CONFIG_INTEL_ATOMISP) += i2c/
+obj-$(CONFIG_INTEL_ATOMISP) += platform/
diff --git a/drivers/staging/media/atomisp/TODO b/drivers/staging/media/atomisp/TODO
new file mode 100644
index 000000000000..737452cbf8a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/TODO
@@ -0,0 +1,64 @@
+1. A single AtomISP driver needs to be implemented to support both BYT and
+   CHT platforms. The current driver is a mechanical and hand combined merge
+   of the two using an ifdef ISP2401 to select the CHT version, which at the
+   moment is not enabled. Eventually this should become a runtime if check,
+   but there are some quite tricky things that need sorting out before that
+   will be possible.
+
+2. The file structure needs to get tidied up to resemble a normal Linux
+   driver.
+
+3. Lots of the midlayer glue. unused code and abstraction needs removing.
+
+3. The sensor drivers read MIPI settings from EFI variables or default to the
+   settings hard-coded in the platform data file for different platforms.
+   This isn't ideal but may be hard to improve as this is how existing
+   platforms work.
+
+4. The sensor drivers use the regulator framework API. In the ideal world it
+   would be using ACPI but that's not how the existing devices work.
+
+5. The AtomISP driver includes some special IOCTLS (ATOMISP_IOC_XXXX_XXXX)
+   that may need some cleaning up.
+
+6. Correct Coding Style. Please don't send coding style patches for this
+   driver until the other work is done.
+
+7. The ISP code depends on the exact FW version. The version defined in
+   BYT: 
+   drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
+   static const char *release_version = STR(irci_stable_candrpv_0415_20150521_0458);
+   CHT:
+   drivers/staging/media/atomisp/pci/atomisp2/css/sh_css_firmware.c
+   static const char *release_version = STR(irci_ecr-master_20150911_0724);
+
+   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.
+
+
+Limitations:
+
+1. Currently the patch only support some camera sensors
+   gc2235/gc0310/0v2680/ov2722/ov5693/mt9m114...
+
+2. 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.
+
+3. Without a 3A libary the capture behaviour is not very good. To take a good
+   picture, you need tune ISP parameters by IOCTL functions or use a 3A libary
+   such as libxcam.
+
+4. 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.
+
+5. 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 000000000000..b80d29d53e65
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/Kconfig
@@ -0,0 +1,106 @@
+#
+# Kconfig for sensor drivers
+#
+
+source "drivers/staging/media/atomisp/i2c/ov5693/Kconfig"
+source "drivers/staging/media/atomisp/i2c/imx/Kconfig"
+
+config VIDEO_OV2722
+       tristate "OVT ov2722 sensor support"
+       depends on I2C && VIDEO_V4L2
+       ---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_GC2235
+       tristate "Galaxy gc2235 sensor support"
+       depends on I2C && VIDEO_V4L2
+       ---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_OV8858
+       tristate "Omnivision ov8858 sensor support"
+       depends on I2C && VIDEO_V4L2 && VIDEO_ATOMISP
+       ---help---
+         This is a Video4Linux2 sensor-level driver for the Omnivision
+         ov8858 RAW sensor.
+
+	 OV8858 is a 8M raw sensor.
+
+         It currently only works with the atomisp driver.
+
+config VIDEO_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_MT9M114
+       tristate "Aptina mt9m114 sensor support"
+       depends on I2C && VIDEO_V4L2
+       ---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_AP1302
+       tristate "AP1302 external ISP support"
+       depends on I2C && VIDEO_V4L2
+       select REGMAP_I2C
+       ---help---
+         This is a Video4Linux2 sensor-level driver for the external
+         ISP AP1302.
+
+         AP1302 is an exteral ISP.
+
+         It currently only works with the atomisp driver.
+
+config VIDEO_GC0310
+	tristate "GC0310 sensor support"
+        depends on I2C && VIDEO_V4L2
+        ---help---
+         This is a Video4Linux2 sensor-level driver for the Galaxycore
+         GC0310 0.3MP sensor.
+	 
+config VIDEO_OV2680
+       tristate "Omnivision OV2680 sensor support"
+       depends on I2C && VIDEO_V4L2
+       ---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.
+
+#
+# Kconfig for flash drivers
+#
+
+config VIDEO_LM3554
+       tristate "LM3554 flash light driver"
+       depends on VIDEO_V4L2 && 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 000000000000..8ea01904c0ea
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/Makefile
@@ -0,0 +1,23 @@
+#
+# Makefile for sensor drivers
+#
+
+obj-$(CONFIG_VIDEO_IMX)        += imx/
+obj-$(CONFIG_VIDEO_OV5693)     += ov5693/
+obj-$(CONFIG_VIDEO_MT9M114)    += mt9m114.o
+obj-$(CONFIG_VIDEO_GC2235)     += gc2235.o
+obj-$(CONFIG_VIDEO_OV2722)     += ov2722.o
+obj-$(CONFIG_VIDEO_OV2680)     += ov2680.o
+obj-$(CONFIG_VIDEO_GC0310)     += gc0310.o
+
+obj-$(CONFIG_VIDEO_MSRLIST_HELPER) += libmsrlisthelper.o
+
+obj-$(CONFIG_VIDEO_AP1302)     += ap1302.o
+
+# Makefile for flash drivers
+#
+
+obj-$(CONFIG_VIDEO_LM3554) += lm3554.o
+
+ccflags-y += -Werror
+
diff --git a/drivers/staging/media/atomisp/i2c/ap1302.c b/drivers/staging/media/atomisp/i2c/ap1302.c
new file mode 100644
index 000000000000..bacffbe962d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ap1302.c
@@ -0,0 +1,1260 @@
+/*
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include "../include/linux/atomisp.h"
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include "ap1302.h"
+
+#define to_ap1302_device(sub_dev) \
+		container_of(sub_dev, struct ap1302_device, sd)
+
+/* Static definitions */
+static struct regmap_config ap1302_reg16_config = {
+	.reg_bits = 16,
+	.val_bits = 16,
+	.reg_format_endian = REGMAP_ENDIAN_BIG,
+	.val_format_endian = REGMAP_ENDIAN_BIG,
+};
+
+static struct regmap_config ap1302_reg32_config = {
+	.reg_bits = 16,
+	.val_bits = 32,
+	.reg_format_endian = REGMAP_ENDIAN_BIG,
+	.val_format_endian = REGMAP_ENDIAN_BIG,
+};
+
+static enum ap1302_contexts ap1302_cntx_mapping[] = {
+	CONTEXT_PREVIEW,	/* Invalid atomisp run mode */
+	CONTEXT_VIDEO,		/* ATOMISP_RUN_MODE_VIDEO */
+	CONTEXT_SNAPSHOT,	/* ATOMISP_RUN_MODE_STILL_CAPTURE */
+	CONTEXT_SNAPSHOT,	/* ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE */
+	CONTEXT_PREVIEW,	/* ATOMISP_RUN_MODE_PREVIEW */
+};
+
+static struct ap1302_res_struct ap1302_preview_res[] = {
+	{
+		.width = 640,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 720,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fps = 30,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fps = 30,
+	}
+};
+
+static struct ap1302_res_struct ap1302_snapshot_res[] = {
+	{
+		.width = 640,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 720,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fps = 30,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fps = 30,
+	}
+};
+
+static struct ap1302_res_struct ap1302_video_res[] = {
+	{
+		.width = 640,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 720,
+		.height = 480,
+		.fps = 30,
+	},
+	{
+		.width = 1280,
+		.height = 720,
+		.fps = 30,
+	},
+	{
+		.width = 1920,
+		.height = 1080,
+		.fps = 30,
+	}
+};
+
+static enum ap1302_contexts stream_to_context[] = {
+	CONTEXT_SNAPSHOT,
+	CONTEXT_PREVIEW,
+	CONTEXT_PREVIEW,
+	CONTEXT_VIDEO
+};
+
+static u16 aux_stream_config[CONTEXT_NUM][CONTEXT_NUM] = {
+	{0, 0, 0},	/* Preview: No aux streams. */
+	{1, 0, 2},	/* Snapshot: 1 for postview. 2 for video */
+	{1, 0, 0},	/* Video: 1 for preview. */
+};
+
+static struct ap1302_context_info context_info[] = {
+	{CNTX_WIDTH, AP1302_REG16, "width"},
+	{CNTX_HEIGHT, AP1302_REG16, "height"},
+	{CNTX_ROI_X0, AP1302_REG16, "roi_x0"},
+	{CNTX_ROI_X1, AP1302_REG16, "roi_x1"},
+	{CNTX_ROI_Y0, AP1302_REG16, "roi_y0"},
+	{CNTX_ROI_Y1, AP1302_REG16, "roi_y1"},
+	{CNTX_ASPECT, AP1302_REG16, "aspect"},
+	{CNTX_LOCK, AP1302_REG16, "lock"},
+	{CNTX_ENABLE, AP1302_REG16, "enable"},
+	{CNTX_OUT_FMT, AP1302_REG16, "out_fmt"},
+	{CNTX_SENSOR_MODE, AP1302_REG16, "sensor_mode"},
+	{CNTX_MIPI_CTRL, AP1302_REG16, "mipi_ctrl"},
+	{CNTX_MIPI_II_CTRL, AP1302_REG16, "mipi_ii_ctrl"},
+	{CNTX_LINE_TIME, AP1302_REG32, "line_time"},
+	{CNTX_MAX_FPS, AP1302_REG16, "max_fps"},
+	{CNTX_AE_USG, AP1302_REG16, "ae_usg"},
+	{CNTX_AE_UPPER_ET, AP1302_REG32, "ae_upper_et"},
+	{CNTX_AE_MAX_ET, AP1302_REG32, "ae_max_et"},
+	{CNTX_SS, AP1302_REG16, "ss"},
+	{CNTX_S1_SENSOR_MODE, AP1302_REG16, "s1_sensor_mode"},
+	{CNTX_HINF_CTRL, AP1302_REG16, "hinf_ctrl"},
+};
+
+/* This array stores the description list for metadata.
+   The metadata contains exposure settings and face
+   detection results. */
+static u16 ap1302_ss_list[] = {
+	0xb01c, /* From 0x0186 with size 0x1C are exposure settings. */
+	0x0186,
+	0xb002, /* 0x71c0 is for F-number */
+	0x71c0,
+	0xb010, /* From 0x03dc with size 0x10 are face general infos. */
+	0x03dc,
+	0xb0a0, /* From 0x03e4 with size 0xa0 are face detail infos. */
+	0x03e4,
+	0xb020, /* From 0x0604 with size 0x20 are smile rate infos. */
+	0x0604,
+	0x0000
+};
+
+/* End of static definitions */
+
+static int ap1302_i2c_read_reg(struct v4l2_subdev *sd,
+				u16 reg, u16 len, void *val)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (len == AP1302_REG16)
+		ret = regmap_read(dev->regmap16, reg, val);
+	else if (len == AP1302_REG32)
+		ret = regmap_read(dev->regmap32, reg, val);
+	else
+		ret = -EINVAL;
+	if (ret) {
+		dev_dbg(&client->dev, "Read reg failed. reg=0x%04X\n", reg);
+		return ret;
+	}
+	if (len == AP1302_REG16)
+		dev_dbg(&client->dev, "read_reg[0x%04X] = 0x%04X\n",
+			reg, *(u16 *)val);
+	else
+		dev_dbg(&client->dev, "read_reg[0x%04X] = 0x%08X\n",
+			reg, *(u32 *)val);
+	return ret;
+}
+
+static int ap1302_i2c_write_reg(struct v4l2_subdev *sd,
+				u16 reg, u16 len, u32 val)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	if (len == AP1302_REG16)
+		ret = regmap_write(dev->regmap16, reg, val);
+	else if (len == AP1302_REG32)
+		ret = regmap_write(dev->regmap32, reg, val);
+	else
+		ret = -EINVAL;
+	if (ret) {
+		dev_dbg(&client->dev, "Write reg failed. reg=0x%04X\n", reg);
+		return ret;
+	}
+	if (len == AP1302_REG16)
+		dev_dbg(&client->dev, "write_reg[0x%04X] = 0x%04X\n",
+			reg, (u16)val);
+	else
+		dev_dbg(&client->dev, "write_reg[0x%04X] = 0x%08X\n",
+			reg, (u32)val);
+	return ret;
+}
+
+static u16
+ap1302_calculate_context_reg_addr(enum ap1302_contexts context, u16 offset)
+{
+	u16 reg_addr;
+	/* The register offset is defined according to preview/video registers.
+	   Preview and video context have the same register definition.
+	   But snapshot context does not have register S1_SENSOR_MODE.
+	   When setting snapshot registers, if the offset exceeds
+	   S1_SENSOR_MODE, the actual offset needs to minus 2. */
+	if (context == CONTEXT_SNAPSHOT) {
+		if (offset == CNTX_S1_SENSOR_MODE)
+			return 0;
+		if (offset > CNTX_S1_SENSOR_MODE)
+			offset -= 2;
+	}
+	if (context == CONTEXT_PREVIEW)
+		reg_addr = REG_PREVIEW_BASE + offset;
+	else if (context == CONTEXT_VIDEO)
+		reg_addr = REG_VIDEO_BASE + offset;
+	else
+		reg_addr = REG_SNAPSHOT_BASE + offset;
+	return reg_addr;
+}
+
+static int ap1302_read_context_reg(struct v4l2_subdev *sd,
+		enum ap1302_contexts context, u16 offset, u16 len)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	u16 reg_addr = ap1302_calculate_context_reg_addr(context, offset);
+	if (reg_addr == 0)
+		return -EINVAL;
+	return ap1302_i2c_read_reg(sd, reg_addr, len,
+			    ((u8 *)&dev->cntx_config[context]) + offset);
+}
+
+static int ap1302_write_context_reg(struct v4l2_subdev *sd,
+		enum ap1302_contexts context, u16 offset, u16 len)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	u16 reg_addr = ap1302_calculate_context_reg_addr(context, offset);
+	if (reg_addr == 0)
+		return -EINVAL;
+	return ap1302_i2c_write_reg(sd, reg_addr, len,
+			*(u32 *)(((u8 *)&dev->cntx_config[context]) + offset));
+}
+
+static int ap1302_dump_context_reg(struct v4l2_subdev *sd,
+				   enum ap1302_contexts context)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int i;
+	dev_dbg(&client->dev, "Dump registers for context[%d]:\n", context);
+	for (i = 0; i < ARRAY_SIZE(context_info); i++) {
+		struct ap1302_context_info *info = &context_info[i];
+		u8 *var = (u8 *)&dev->cntx_config[context] + info->offset;
+		/* Snapshot context does not have s1_sensor_mode register. */
+		if (context == CONTEXT_SNAPSHOT &&
+			info->offset == CNTX_S1_SENSOR_MODE)
+			continue;
+		ap1302_read_context_reg(sd, context, info->offset, info->len);
+		if (info->len == AP1302_REG16)
+			dev_dbg(&client->dev, "context.%s = 0x%04X (%d)\n",
+				info->name, *(u16 *)var, *(u16 *)var);
+		else
+			dev_dbg(&client->dev, "context.%s = 0x%08X (%d)\n",
+				info->name, *(u32 *)var, *(u32 *)var);
+	}
+	return 0;
+}
+
+static int ap1302_request_firmware(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int ret;
+	ret = request_firmware(&dev->fw, "ap1302_fw.bin", &client->dev);
+	if (ret)
+		dev_err(&client->dev,
+			"ap1302_request_firmware failed. ret=%d\n", ret);
+	return ret;
+}
+
+/* When loading firmware, host writes firmware data from address 0x8000.
+   When the address reaches 0x9FFF, the next address should return to 0x8000.
+   This function handles this address window and load firmware data to AP1302.
+   win_pos indicates the offset within this window. Firmware loading procedure
+   may call this function several times. win_pos records the current position
+   that has been written to.*/
+static int ap1302_write_fw_window(struct v4l2_subdev *sd,
+				  u16 *win_pos, const u8 *buf, u32 len)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int ret;
+	u32 pos;
+	u32 sub_len;
+	for (pos = 0; pos < len; pos += sub_len) {
+		if (len - pos < AP1302_FW_WINDOW_SIZE - *win_pos)
+			sub_len = len - pos;
+		else
+			sub_len = AP1302_FW_WINDOW_SIZE - *win_pos;
+		ret = regmap_raw_write(dev->regmap16,
+					*win_pos + AP1302_FW_WINDOW_OFFSET,
+					buf + pos, sub_len);
+		if (ret)
+			return ret;
+		*win_pos += sub_len;
+		if (*win_pos >= AP1302_FW_WINDOW_SIZE)
+			*win_pos = 0;
+	}
+	return 0;
+}
+
+static int ap1302_load_firmware(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	const struct ap1302_firmware *fw;
+	const u8 *fw_data;
+	u16 reg_val = 0;
+	u16 win_pos = 0;
+	int ret;
+
+	dev_info(&client->dev, "Start to load firmware.\n");
+	if (!dev->fw) {
+		dev_err(&client->dev, "firmware not requested.\n");
+		return -EINVAL;
+	}
+	fw = (const struct ap1302_firmware *) dev->fw->data;
+	if (dev->fw->size != (sizeof(*fw) + fw->total_size)) {
+		dev_err(&client->dev, "firmware size does not match.\n");
+		return -EINVAL;
+	}
+	/* The fw binary contains a header of struct ap1302_firmware.
+	   Following the header is the bootdata of AP1302.
+	   The bootdata pointer can be referenced as &fw[1]. */
+	fw_data = (u8 *)&fw[1];
+
+	/* Clear crc register. */
+	ret = ap1302_i2c_write_reg(sd, REG_SIP_CRC, AP1302_REG16, 0xFFFF);
+	if (ret)
+		return ret;
+
+	/* Load FW data for PLL init stage. */
+	ret = ap1302_write_fw_window(sd, &win_pos, fw_data, fw->pll_init_size);
+	if (ret)
+		return ret;
+
+	/* Write 2 to bootdata_stage register to apply basic_init_hp
+	   settings and enable PLL. */
+	ret = ap1302_i2c_write_reg(sd, REG_BOOTDATA_STAGE,
+				   AP1302_REG16, 0x0002);
+	if (ret)
+		return ret;
+
+	/* Wait 1ms for PLL to lock. */
+	msleep(20);
+
+	/* Load the rest of bootdata content. */
+	ret = ap1302_write_fw_window(sd, &win_pos, fw_data + fw->pll_init_size,
+				     fw->total_size - fw->pll_init_size);
+	if (ret)
+		return ret;
+
+	/* Check crc. */
+	ret = ap1302_i2c_read_reg(sd, REG_SIP_CRC, AP1302_REG16, &reg_val);
+	if (ret)
+		return ret;
+	if (reg_val != fw->crc) {
+		dev_err(&client->dev,
+			"crc does not match. T:0x%04X F:0x%04X\n",
+			fw->crc, reg_val);
+		return -EAGAIN;
+	}
+
+	/* Write 0xFFFF to bootdata_stage register to indicate AP1302 that
+	   the whole bootdata content has been loaded. */
+	ret = ap1302_i2c_write_reg(sd, REG_BOOTDATA_STAGE,
+				   AP1302_REG16, 0xFFFF);
+	if (ret)
+		return ret;
+	dev_info(&client->dev, "Load firmware successfully.\n");
+
+	return 0;
+}
+
+static int __ap1302_s_power(struct v4l2_subdev *sd, int on, int load_fw)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret, i;
+	u16 ss_ptr;
+
+	dev_info(&client->dev, "ap1302_s_power is called.\n");
+	ret = dev->platform_data->power_ctrl(sd, on);
+	if (ret) {
+		dev_err(&client->dev,
+			"ap1302_s_power error. on=%d ret=%d\n", on, ret);
+		return ret;
+	}
+	dev->power_on = on;
+	if (!on || !load_fw)
+		return 0;
+	/* Load firmware after power on. */
+	ret = ap1302_load_firmware(sd);
+	if (ret) {
+		dev_err(&client->dev,
+			"ap1302_load_firmware failed. ret=%d\n", ret);
+		return ret;
+	}
+	ret = ap1302_i2c_read_reg(sd, REG_SS_HEAD_PT0, AP1302_REG16, &ss_ptr);
+	if (ret)
+		return ret;
+	for (i = 0; i < ARRAY_SIZE(ap1302_ss_list); i++) {
+		ret = ap1302_i2c_write_reg(sd, ss_ptr + i * 2,
+			AP1302_REG16, ap1302_ss_list[i]);
+		if (ret)
+			return ret;
+	}
+	return ret;
+}
+
+static int ap1302_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __ap1302_s_power(sd, on, 1);
+	dev->sys_activated = 0;
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ap1302_s_config(struct v4l2_subdev *sd, void *pdata)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct camera_mipi_info *mipi_info;
+	u16 reg_val = 0;
+	int ret;
+
+	dev_info(&client->dev, "ap1302_s_config is called.\n");
+	if (pdata == NULL)
+		return -ENODEV;
+
+	dev->platform_data = pdata;
+
+	mutex_lock(&dev->input_lock);
+
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret)
+			goto fail_power;
+	}
+
+	ret = __ap1302_s_power(sd, 1, 0);
+	if (ret)
+		goto fail_power;
+
+	/* Detect for AP1302 */
+	ret = ap1302_i2c_read_reg(sd, REG_CHIP_VERSION, AP1302_REG16, &reg_val);
+	if (ret || (reg_val != AP1302_CHIP_ID)) {
+		dev_err(&client->dev,
+			"Chip version does no match. ret=%d ver=0x%04x\n",
+			ret, reg_val);
+		goto fail_config;
+	}
+	dev_info(&client->dev, "AP1302 Chip ID is 0x%X\n", reg_val);
+
+	/* Detect revision for AP1302 */
+	ret = ap1302_i2c_read_reg(sd, REG_CHIP_REV, AP1302_REG16, &reg_val);
+	if (ret)
+		goto fail_config;
+	dev_info(&client->dev, "AP1302 Chip Rev is 0x%X\n", reg_val);
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_config;
+
+	mipi_info = v4l2_get_subdev_hostdata(sd);
+	if (!mipi_info)
+		goto fail_config;
+	dev->num_lanes = mipi_info->num_lanes;
+
+	ret = __ap1302_s_power(sd, 0, 0);
+	if (ret)
+		goto fail_power;
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+
+fail_config:
+	__ap1302_s_power(sd, 0, 0);
+fail_power:
+	mutex_unlock(&dev->input_lock);
+	dev_err(&client->dev, "ap1302_s_config failed\n");
+	return ret;
+}
+
+static enum ap1302_contexts ap1302_get_context(struct v4l2_subdev *sd)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	return dev->cur_context;
+}
+
+static int ap1302_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+	return 0;
+}
+
+static int ap1302_match_resolution(struct ap1302_context_res *res,
+				   struct v4l2_mbus_framefmt *fmt)
+{
+	s32 w0, h0, mismatch, distance;
+	s32 w1 = fmt->width;
+	s32 h1 = fmt->height;
+	s32 min_distance = INT_MAX;
+	s32 i, idx = -1;
+
+	if (w1 == 0 || h1 == 0)
+		return -1;
+
+	for (i = 0; i < res->res_num; i++) {
+		w0 = res->res_table[i].width;
+		h0 = res->res_table[i].height;
+		if (w0 < w1 || h0 < h1)
+			continue;
+		mismatch = abs(w0 * h1 - w1 * h0) * 8192 / w1 / h0;
+		if (mismatch > 8192 * AP1302_MAX_RATIO_MISMATCH / 100)
+			continue;
+		distance = (w0 * h1 + w1 * h0) * 8192 / w1 / h1;
+		if (distance < min_distance) {
+			min_distance = distance;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static s32 ap1302_try_mbus_fmt_locked(struct v4l2_subdev *sd,
+				enum ap1302_contexts context,
+				struct v4l2_mbus_framefmt *fmt)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct ap1302_res_struct *res_table;
+	s32 res_num, idx = -1;
+
+	res_table = dev->cntx_res[context].res_table;
+	res_num = dev->cntx_res[context].res_num;
+
+	if ((fmt->width <= res_table[res_num - 1].width) &&
+		(fmt->height <= res_table[res_num - 1].height))
+		idx = ap1302_match_resolution(&dev->cntx_res[context], fmt);
+	if (idx == -1)
+		idx = res_num - 1;
+
+	fmt->width = res_table[idx].width;
+	fmt->height = res_table[idx].height;
+	fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	return idx;
+}
+
+
+static int ap1302_get_fmt(struct v4l2_subdev *sd,
+	                 struct v4l2_subdev_pad_config *cfg,
+					 struct v4l2_subdev_format *format)
+
+{
+    struct v4l2_mbus_framefmt *fmt = &format->format;
+    struct ap1302_device *dev = to_ap1302_device(sd);
+	enum ap1302_contexts context;
+	struct ap1302_res_struct *res_table;
+	s32 cur_res;
+     if (format->pad)
+		return -EINVAL;
+	mutex_lock(&dev->input_lock);
+	context = ap1302_get_context(sd);
+	res_table = dev->cntx_res[context].res_table;
+	cur_res = dev->cntx_res[context].cur_res;
+	fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	fmt->width = res_table[cur_res].width;
+	fmt->height = res_table[cur_res].height;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int ap1302_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct atomisp_input_stream_info *stream_info =
+		(struct atomisp_input_stream_info *)fmt->reserved;
+	enum ap1302_contexts context, main_context;
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+	mutex_lock(&dev->input_lock);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		context = ap1302_get_context(sd);
+		ap1302_try_mbus_fmt_locked(sd, context, fmt);
+		cfg->try_fmt = *fmt;
+	    mutex_unlock(&dev->input_lock);
+		return 0;
+		}
+	context = stream_to_context[stream_info->stream];
+	dev_dbg(&client->dev, "ap1302_set_mbus_fmt. stream=%d context=%d\n",
+		stream_info->stream, context);
+	dev->cntx_res[context].cur_res =
+		ap1302_try_mbus_fmt_locked(sd, context, fmt);
+	dev->cntx_config[context].width = fmt->width;
+	dev->cntx_config[context].height = fmt->height;
+	ap1302_write_context_reg(sd, context, CNTX_WIDTH, AP1302_REG16);
+	ap1302_write_context_reg(sd, context, CNTX_HEIGHT, AP1302_REG16);
+	ap1302_read_context_reg(sd, context, CNTX_OUT_FMT, AP1302_REG16);
+	dev->cntx_config[context].out_fmt &= ~OUT_FMT_TYPE_MASK;
+	dev->cntx_config[context].out_fmt |= AP1302_FMT_UYVY422;
+	ap1302_write_context_reg(sd, context, CNTX_OUT_FMT, AP1302_REG16);
+
+	main_context = ap1302_get_context(sd);
+	if (context == main_context) {
+		ap1302_read_context_reg(sd, context,
+			CNTX_MIPI_CTRL, AP1302_REG16);
+		dev->cntx_config[context].mipi_ctrl &= ~MIPI_CTRL_IMGVC_MASK;
+		dev->cntx_config[context].mipi_ctrl |=
+			(context << MIPI_CTRL_IMGVC_OFFSET);
+		dev->cntx_config[context].mipi_ctrl &= ~MIPI_CTRL_SSVC_MASK;
+		dev->cntx_config[context].mipi_ctrl |=
+			(context << MIPI_CTRL_SSVC_OFFSET);
+		dev->cntx_config[context].mipi_ctrl &= ~MIPI_CTRL_SSTYPE_MASK;
+		dev->cntx_config[context].mipi_ctrl |=
+			(0x12 << MIPI_CTRL_SSTYPE_OFFSET);
+		ap1302_write_context_reg(sd, context,
+			CNTX_MIPI_CTRL, AP1302_REG16);
+		ap1302_read_context_reg(sd, context,
+			CNTX_SS, AP1302_REG16);
+		dev->cntx_config[context].ss = AP1302_SS_CTRL;
+		ap1302_write_context_reg(sd, context,
+			CNTX_SS, AP1302_REG16);
+	} else {
+		/* Configure aux stream */
+		ap1302_read_context_reg(sd, context,
+			CNTX_MIPI_II_CTRL, AP1302_REG16);
+		dev->cntx_config[context].mipi_ii_ctrl &= ~MIPI_CTRL_IMGVC_MASK;
+		dev->cntx_config[context].mipi_ii_ctrl |=
+			(context << MIPI_CTRL_IMGVC_OFFSET);
+		ap1302_write_context_reg(sd, context,
+			CNTX_MIPI_II_CTRL, AP1302_REG16);
+		if (stream_info->enable) {
+			ap1302_read_context_reg(sd, main_context,
+				CNTX_OUT_FMT, AP1302_REG16);
+			dev->cntx_config[context].out_fmt |=
+				(aux_stream_config[main_context][context]
+				 << OUT_FMT_IIS_OFFSET);
+			ap1302_write_context_reg(sd, main_context,
+				CNTX_OUT_FMT, AP1302_REG16);
+		}
+	}
+	stream_info->ch_id = context;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+
+static int ap1302_g_frame_interval(struct v4l2_subdev *sd,
+			struct v4l2_subdev_frame_interval *interval)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	enum ap1302_contexts context;
+	struct ap1302_res_struct *res_table;
+	u32 cur_res;
+
+	mutex_lock(&dev->input_lock);
+	context = ap1302_get_context(sd);
+	res_table = dev->cntx_res[context].res_table;
+	cur_res = dev->cntx_res[context].cur_res;
+	interval->interval.denominator = res_table[cur_res].fps;
+	interval->interval.numerator = 1;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int ap1302_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	enum ap1302_contexts context;
+	struct ap1302_res_struct *res_table;
+	int index = fse->index;
+
+	mutex_lock(&dev->input_lock);
+	context = ap1302_get_context(sd);
+	if (index >= dev->cntx_res[context].res_num) {
+		mutex_unlock(&dev->input_lock);
+		return -EINVAL;
+	}
+
+	res_table = dev->cntx_res[context].res_table;
+	fse->min_width = res_table[index].width;
+	fse->min_height = res_table[index].height;
+	fse->max_width = res_table[index].width;
+	fse->max_height = res_table[index].height;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+
+static int ap1302_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	*frames = 0;
+	return 0;
+}
+
+static int ap1302_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	enum ap1302_contexts context;
+	u32 reg_val;
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	context = ap1302_get_context(sd);
+	dev_dbg(&client->dev, "ap1302_s_stream. context=%d enable=%d\n",
+			context, enable);
+	/* Switch context */
+	ap1302_i2c_read_reg(sd, REG_CTRL,
+			    AP1302_REG16, &reg_val);
+	reg_val &= ~CTRL_CNTX_MASK;
+	reg_val |= (context<<CTRL_CNTX_OFFSET);
+	ap1302_i2c_write_reg(sd, REG_CTRL,
+			    AP1302_REG16, reg_val);
+	/* Select sensor */
+	ap1302_i2c_read_reg(sd, REG_SENSOR_SELECT,
+			    AP1302_REG16, &reg_val);
+	reg_val &= ~SENSOR_SELECT_MASK;
+	reg_val |= (AP1302_SENSOR_PRI<<SENSOR_SELECT_OFFSET);
+	ap1302_i2c_write_reg(sd, REG_SENSOR_SELECT,
+			    AP1302_REG16, reg_val);
+	if (enable) {
+		dev_info(&client->dev, "Start stream. context=%d\n", context);
+		ap1302_dump_context_reg(sd, context);
+		if (!dev->sys_activated) {
+			reg_val = AP1302_SYS_ACTIVATE;
+			dev->sys_activated = 1;
+		} else {
+			reg_val = AP1302_SYS_SWITCH;
+		}
+	} else {
+		dev_info(&client->dev, "Stop stream. context=%d\n", context);
+		reg_val = AP1302_SYS_SWITCH;
+	}
+	ret = ap1302_i2c_write_reg(sd, REG_SYS_START, AP1302_REG16, reg_val);
+	if (ret)
+		dev_err(&client->dev,
+			"AP1302 set stream failed. enable=%d\n", enable);
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static u16 ap1302_ev_values[] = {0xfd00, 0xfe80, 0x0, 0x180, 0x300};
+
+static int ap1302_set_exposure_off(struct v4l2_subdev *sd, s32 val)
+{
+	val -= AP1302_MIN_EV;
+	return ap1302_i2c_write_reg(sd, REG_AE_BV_OFF, AP1302_REG16,
+				ap1302_ev_values[val]);
+}
+
+static u16 ap1302_wb_values[] = {
+	0, /* V4L2_WHITE_BALANCE_MANUAL */
+	0xf, /* V4L2_WHITE_BALANCE_AUTO */
+	0x2, /* V4L2_WHITE_BALANCE_INCANDESCENT */
+	0x4, /* V4L2_WHITE_BALANCE_FLUORESCENT */
+	0x5, /* V4L2_WHITE_BALANCE_FLUORESCENT_H */
+	0x1, /* V4L2_WHITE_BALANCE_HORIZON */
+	0x5, /* V4L2_WHITE_BALANCE_DAYLIGHT */
+	0xf, /* V4L2_WHITE_BALANCE_FLASH */
+	0x6, /* V4L2_WHITE_BALANCE_CLOUDY */
+	0x6, /* V4L2_WHITE_BALANCE_SHADE */
+};
+
+static int ap1302_set_wb_mode(struct v4l2_subdev *sd, s32 val)
+{
+	int ret = 0;
+	u16 reg_val;
+
+	ret = ap1302_i2c_read_reg(sd, REG_AWB_CTRL, AP1302_REG16, &reg_val);
+	if (ret)
+		return ret;
+	reg_val &= ~AWB_CTRL_MODE_MASK;
+	reg_val |= ap1302_wb_values[val] << AWB_CTRL_MODE_OFFSET;
+	if (val == V4L2_WHITE_BALANCE_FLASH)
+		reg_val |= AWB_CTRL_FLASH_MASK;
+	else
+		reg_val &= ~AWB_CTRL_FLASH_MASK;
+	ret = ap1302_i2c_write_reg(sd, REG_AWB_CTRL, AP1302_REG16, reg_val);
+	return ret;
+}
+
+static int ap1302_set_zoom(struct v4l2_subdev *sd, s32 val)
+{
+	ap1302_i2c_write_reg(sd, REG_DZ_TGT_FCT, AP1302_REG16,
+		val * 4 + 0x100);
+	return 0;
+}
+
+static u16 ap1302_sfx_values[] = {
+	0x00, /* V4L2_COLORFX_NONE */
+	0x03, /* V4L2_COLORFX_BW */
+	0x0d, /* V4L2_COLORFX_SEPIA */
+	0x07, /* V4L2_COLORFX_NEGATIVE */
+	0x04, /* V4L2_COLORFX_EMBOSS */
+	0x0f, /* V4L2_COLORFX_SKETCH */
+	0x08, /* V4L2_COLORFX_SKY_BLUE */
+	0x09, /* V4L2_COLORFX_GRASS_GREEN */
+	0x0a, /* V4L2_COLORFX_SKIN_WHITEN */
+	0x00, /* V4L2_COLORFX_VIVID */
+	0x00, /* V4L2_COLORFX_AQUA */
+	0x00, /* V4L2_COLORFX_ART_FREEZE */
+	0x00, /* V4L2_COLORFX_SILHOUETTE */
+	0x10, /* V4L2_COLORFX_SOLARIZATION */
+	0x02, /* V4L2_COLORFX_ANTIQUE */
+	0x00, /* V4L2_COLORFX_SET_CBCR */
+};
+
+static int ap1302_set_special_effect(struct v4l2_subdev *sd, s32 val)
+{
+	ap1302_i2c_write_reg(sd, REG_SFX_MODE, AP1302_REG16,
+		ap1302_sfx_values[val]);
+	return 0;
+}
+
+static u16 ap1302_scene_mode_values[] = {
+	0x00, /* V4L2_SCENE_MODE_NONE */
+	0x07, /* V4L2_SCENE_MODE_BACKLIGHT */
+	0x0a, /* V4L2_SCENE_MODE_BEACH_SNOW */
+	0x06, /* V4L2_SCENE_MODE_CANDLE_LIGHT */
+	0x00, /* V4L2_SCENE_MODE_DAWN_DUSK */
+	0x00, /* V4L2_SCENE_MODE_FALL_COLORS */
+	0x0d, /* V4L2_SCENE_MODE_FIREWORKS */
+	0x02, /* V4L2_SCENE_MODE_LANDSCAPE */
+	0x05, /* V4L2_SCENE_MODE_NIGHT */
+	0x0c, /* V4L2_SCENE_MODE_PARTY_INDOOR */
+	0x01, /* V4L2_SCENE_MODE_PORTRAIT */
+	0x03, /* V4L2_SCENE_MODE_SPORTS */
+	0x0e, /* V4L2_SCENE_MODE_SUNSET */
+	0x0b, /* V4L2_SCENE_MODE_TEXT */
+};
+
+static int ap1302_set_scene_mode(struct v4l2_subdev *sd, s32 val)
+{
+	ap1302_i2c_write_reg(sd, REG_SCENE_CTRL, AP1302_REG16,
+		ap1302_scene_mode_values[val]);
+	return 0;
+}
+
+static u16 ap1302_flicker_values[] = {
+	0x0,    /* OFF */
+	0x3201, /* 50HZ */
+	0x3c01, /* 60HZ */
+	0x2     /* AUTO */
+};
+
+static int ap1302_set_flicker_freq(struct v4l2_subdev *sd, s32 val)
+{
+	ap1302_i2c_write_reg(sd, REG_FLICK_CTRL, AP1302_REG16,
+		ap1302_flicker_values[val]);
+	return 0;
+}
+
+static int ap1302_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ap1302_device *dev = container_of(
+		ctrl->handler, struct ap1302_device, ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_RUN_MODE:
+		dev->cur_context = ap1302_cntx_mapping[ctrl->val];
+		break;
+	case V4L2_CID_EXPOSURE:
+		ap1302_set_exposure_off(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE:
+		ap1302_set_wb_mode(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_ZOOM_ABSOLUTE:
+		ap1302_set_zoom(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_COLORFX:
+		ap1302_set_special_effect(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_SCENE_MODE:
+		ap1302_set_scene_mode(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_POWER_LINE_FREQUENCY:
+		ap1302_set_flicker_freq(&dev->sd, ctrl->val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int ap1302_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int ret;
+	u32 reg_val;
+
+	if (reg->size != AP1302_REG16 &&
+	    reg->size != AP1302_REG32)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	if (dev->power_on)
+		ret = ap1302_i2c_read_reg(sd, reg->reg, reg->size, &reg_val);
+	else
+		ret = -EIO;
+	mutex_unlock(&dev->input_lock);
+	if (ret)
+		return ret;
+
+	reg->val = reg_val;
+
+	return 0;
+}
+
+static int ap1302_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct ap1302_device *dev = to_ap1302_device(sd);
+	int ret;
+
+	if (reg->size != AP1302_REG16 &&
+	    reg->size != AP1302_REG32)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	if (dev->power_on)
+		ret = ap1302_i2c_write_reg(sd, reg->reg, reg->size, reg->val);
+	else
+		ret = -EIO;
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static long ap1302_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	long ret = 0;
+	switch (cmd) {
+	case VIDIOC_DBG_G_REGISTER:
+		ret = ap1302_g_register(sd, arg);
+		break;
+	case VIDIOC_DBG_S_REGISTER:
+		ret = ap1302_s_register(sd, arg);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = ap1302_s_ctrl,
+};
+
+static const char * const ctrl_run_mode_menu[] = {
+	NULL,
+	"Video",
+	"Still capture",
+	"Continuous capture",
+	"Preview",
+};
+
+static const struct v4l2_ctrl_config ctrls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_RUN_MODE,
+		.name = "Run Mode",
+		.type = V4L2_CTRL_TYPE_MENU,
+		.min = 1,
+		.def = 4,
+		.max = 4,
+		.qmenu = ctrl_run_mode_menu,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE,
+		.name = "Exposure",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = AP1302_MIN_EV,
+		.def = 0,
+		.max = AP1302_MAX_EV,
+		.step = 1,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
+		.name = "White Balance",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.def = 0,
+		.max = 9,
+		.step = 1,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_ZOOM_ABSOLUTE,
+		.name = "Zoom Absolute",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.def = 0,
+		.max = 1024,
+		.step = 1,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_COLORFX,
+		.name = "Color Special Effect",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.def = 0,
+		.max = 15,
+		.step = 1,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_SCENE_MODE,
+		.name = "Scene Mode",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.def = 0,
+		.max = 13,
+		.step = 1,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_POWER_LINE_FREQUENCY,
+		.name = "Light frequency filter",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.def = 3,
+		.max = 3,
+		.step = 1,
+	},
+};
+
+static struct v4l2_subdev_sensor_ops ap1302_sensor_ops = {
+	.g_skip_frames	= ap1302_g_skip_frames,
+};
+
+static const struct v4l2_subdev_video_ops ap1302_video_ops = {
+	.s_stream = ap1302_s_stream,
+	.g_frame_interval = ap1302_g_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops ap1302_core_ops = {
+	.s_power = ap1302_s_power,
+	.ioctl = ap1302_ioctl,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = ap1302_g_register,
+	.s_register = ap1302_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_pad_ops ap1302_pad_ops = {
+	.enum_mbus_code = ap1302_enum_mbus_code,
+	.enum_frame_size = ap1302_enum_frame_size,
+	.get_fmt = ap1302_get_fmt,
+	.set_fmt = ap1302_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ap1302_ops = {
+	.core = &ap1302_core_ops,
+	.pad = &ap1302_pad_ops,
+	.video = &ap1302_video_ops,
+	.sensor = &ap1302_sensor_ops
+};
+
+static int ap1302_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ap1302_device *dev = to_ap1302_device(sd);
+
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	release_firmware(dev->fw);
+
+	media_entity_cleanup(&dev->sd.entity);
+	dev->platform_data->csi_cfg(sd, 0);
+	v4l2_device_unregister_subdev(sd);
+
+	return 0;
+}
+
+static int ap1302_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ap1302_device *dev;
+	int ret;
+	unsigned int i;
+
+	dev_info(&client->dev, "ap1302 probe called.\n");
+
+	/* allocate device & init sub device */
+	dev = devm_kzalloc(&client->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "%s: out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	v4l2_i2c_subdev_init(&(dev->sd), client, &ap1302_ops);
+
+	ret = ap1302_request_firmware(&(dev->sd));
+	if (ret) {
+		dev_err(&client->dev, "Cannot request ap1302 firmware.\n");
+		goto out_free;
+	}
+
+	dev->regmap16 = devm_regmap_init_i2c(client, &ap1302_reg16_config);
+	if (IS_ERR(dev->regmap16)) {
+		ret = PTR_ERR(dev->regmap16);
+		dev_err(&client->dev,
+			"Failed to allocate 16bit register map: %d\n", ret);
+		return ret;
+	}
+
+	dev->regmap32 = devm_regmap_init_i2c(client, &ap1302_reg32_config);
+	if (IS_ERR(dev->regmap32)) {
+		ret = PTR_ERR(dev->regmap32);
+		dev_err(&client->dev,
+			"Failed to allocate 32bit register map: %d\n", ret);
+		return ret;
+	}
+
+	if (client->dev.platform_data) {
+		ret = ap1302_s_config(&dev->sd, client->dev.platform_data);
+		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;
+
+	dev->cntx_res[CONTEXT_PREVIEW].res_num = ARRAY_SIZE(ap1302_preview_res);
+	dev->cntx_res[CONTEXT_PREVIEW].res_table = ap1302_preview_res;
+	dev->cntx_res[CONTEXT_SNAPSHOT].res_num =
+		ARRAY_SIZE(ap1302_snapshot_res);
+	dev->cntx_res[CONTEXT_SNAPSHOT].res_table = ap1302_snapshot_res;
+	dev->cntx_res[CONTEXT_VIDEO].res_num = ARRAY_SIZE(ap1302_video_res);
+	dev->cntx_res[CONTEXT_VIDEO].res_table = ap1302_video_res;
+
+	ret = v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(ctrls));
+	if (ret) {
+		ap1302_remove(client);
+		return ret;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(ctrls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &ctrls[i], NULL);
+
+	if (dev->ctrl_handler.error) {
+		ap1302_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;
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
+
+	dev->run_mode = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_RUN_MODE);
+	v4l2_ctrl_s_ctrl(dev->run_mode, ATOMISP_RUN_MODE_PREVIEW);
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret)
+		ap1302_remove(client);
+	return ret;
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	return ret;
+}
+
+static const struct i2c_device_id ap1302_id[] = {
+	{AP1302_NAME, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, ap1302_id);
+
+static struct i2c_driver ap1302_driver = {
+	.driver = {
+		.name = AP1302_NAME,
+	},
+	.probe = ap1302_probe,
+	.remove = ap1302_remove,
+	.id_table = ap1302_id,
+};
+
+module_i2c_driver(ap1302_driver);
+
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_DESCRIPTION("AP1302 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/ap1302.h b/drivers/staging/media/atomisp/i2c/ap1302.h
new file mode 100644
index 000000000000..9341232c580d
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ap1302.h
@@ -0,0 +1,198 @@
+/*
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __AP1302_H__
+#define __AP1302_H__
+
+#include "../include/linux/atomisp_platform.h"
+#include <linux/regmap.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#define AP1302_NAME		"ap1302"
+#define AP1302_CHIP_ID		0x265
+#define AP1302_I2C_MAX_LEN	65534
+#define AP1302_FW_WINDOW_OFFSET	0x8000
+#define AP1302_FW_WINDOW_SIZE	0x2000
+
+#define AP1302_REG16		2
+#define AP1302_REG32		4
+
+#define REG_CHIP_VERSION	0x0000
+#define REG_CHIP_REV		0x0050
+#define REG_MF_ID		0x0004
+#define REG_ERROR		0x0006
+#define REG_CTRL		0x1000
+#define REG_DZ_TGT_FCT		0x1010
+#define REG_SFX_MODE		0x1016
+#define REG_SS_HEAD_PT0		0x1174
+#define REG_AE_BV_OFF		0x5014
+#define REG_AE_BV_BIAS		0x5016
+#define REG_AWB_CTRL		0x5100
+#define REG_FLICK_CTRL		0x5440
+#define REG_SCENE_CTRL		0x5454
+#define REG_BOOTDATA_STAGE	0x6002
+#define REG_SENSOR_SELECT	0x600C
+#define REG_SYS_START		0x601A
+#define REG_SIP_CRC		0xF052
+
+#define REG_PREVIEW_BASE	0x2000
+#define REG_SNAPSHOT_BASE	0x3000
+#define REG_VIDEO_BASE		0x4000
+#define CNTX_WIDTH		0x00
+#define CNTX_HEIGHT		0x02
+#define CNTX_ROI_X0		0x04
+#define CNTX_ROI_Y0		0x06
+#define CNTX_ROI_X1		0x08
+#define CNTX_ROI_Y1		0x0A
+#define CNTX_ASPECT		0x0C
+#define CNTX_LOCK		0x0E
+#define CNTX_ENABLE		0x10
+#define CNTX_OUT_FMT		0x12
+#define CNTX_SENSOR_MODE	0x14
+#define CNTX_MIPI_CTRL		0x16
+#define CNTX_MIPI_II_CTRL	0x18
+#define CNTX_LINE_TIME		0x1C
+#define CNTX_MAX_FPS		0x20
+#define CNTX_AE_USG		0x22
+#define CNTX_AE_UPPER_ET	0x24
+#define CNTX_AE_MAX_ET		0x28
+#define CNTX_SS			0x2C
+#define CNTX_S1_SENSOR_MODE	0x2E
+#define CNTX_HINF_CTRL		0x30
+
+#define CTRL_CNTX_MASK		0x03
+#define CTRL_CNTX_OFFSET	0x00
+#define HINF_CTRL_LANE_MASK	0x07
+#define HINF_CTRL_LANE_OFFSET	0x00
+#define MIPI_CTRL_IMGVC_MASK	0xC0
+#define MIPI_CTRL_IMGVC_OFFSET	0x06
+#define MIPI_CTRL_IMGTYPE_AUTO	0x3F
+#define MIPI_CTRL_SSVC_MASK	0xC000
+#define MIPI_CTRL_SSVC_OFFSET	0x0E
+#define MIPI_CTRL_SSTYPE_MASK	0x3F00
+#define MIPI_CTRL_SSTYPE_OFFSET	0x08
+#define OUT_FMT_IIS_MASK	0x30
+#define OUT_FMT_IIS_OFFSET	0x08
+#define OUT_FMT_SS_MASK		0x1000
+#define OUT_FMT_SS_OFFSET	0x12
+#define OUT_FMT_TYPE_MASK	0xFF
+#define SENSOR_SELECT_MASK	0x03
+#define SENSOR_SELECT_OFFSET	0x00
+#define AWB_CTRL_MODE_MASK	0x0F
+#define AWB_CTRL_MODE_OFFSET	0x00
+#define AWB_CTRL_FLASH_MASK	0x100
+
+#define AP1302_FMT_UYVY422	0x50
+
+#define AP1302_SYS_ACTIVATE	0x8010
+#define AP1302_SYS_SWITCH	0x8140
+#define AP1302_SENSOR_PRI	0x01
+#define AP1302_SENSOR_SEC	0x02
+#define AP1302_SS_CTRL		0x31
+
+#define AP1302_MAX_RATIO_MISMATCH	10 /* Unit in percentage */
+#define AP1302_MAX_EV		2
+#define AP1302_MIN_EV		-2
+
+enum ap1302_contexts {
+	CONTEXT_PREVIEW = 0,
+	CONTEXT_SNAPSHOT,
+	CONTEXT_VIDEO,
+	CONTEXT_NUM
+};
+
+/* The context registers are defined according to preview/video registers.
+   Preview and video context have the same register definition.
+   But snapshot context does not have register S1_SENSOR_MODE.
+   When setting snapshot registers, if the offset exceeds
+   S1_SENSOR_MODE, the actual offset needs to minus 2. */
+struct ap1302_context_config {
+	u16 width;
+	u16 height;
+	u16 roi_x0;
+	u16 roi_y0;
+	u16 roi_x1;
+	u16 roi_y1;
+	u16 aspect_factor;
+	u16 lock;
+	u16 enable;
+	u16 out_fmt;
+	u16 sensor_mode;
+	u16 mipi_ctrl;
+	u16 mipi_ii_ctrl;
+	u16 padding;
+	u32 line_time;
+	u16 max_fps;
+	u16 ae_usg;
+	u32 ae_upper_et;
+	u32 ae_max_et;
+	u16 ss;
+	u16 s1_sensor_mode;
+	u16 hinf_ctrl;
+	u32 reserved;
+};
+
+struct ap1302_res_struct {
+	u16 width;
+	u16 height;
+	u16 fps;
+};
+
+struct ap1302_context_res {
+	s32 res_num;
+	s32 cur_res;
+	struct ap1302_res_struct *res_table;
+};
+
+struct ap1302_device {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct camera_sensor_platform_data *platform_data;
+	const struct firmware *fw;
+	struct mutex input_lock; /* serialize sensor's ioctl */
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *run_mode;
+	struct ap1302_context_config cntx_config[CONTEXT_NUM];
+	struct ap1302_context_res cntx_res[CONTEXT_NUM];
+	enum ap1302_contexts cur_context;
+	unsigned int num_lanes;
+	struct regmap *regmap16;
+	struct regmap *regmap32;
+	bool sys_activated;
+	bool power_on;
+};
+
+struct ap1302_firmware {
+	u32 crc;
+	u32 pll_init_size;
+	u32 total_size;
+	u32 reserved;
+};
+
+struct ap1302_context_info {
+	u16 offset;
+	u16 len;
+	char *name;
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/gc0310.c b/drivers/staging/media/atomisp/i2c/gc0310.c
new file mode 100644
index 000000000000..1ec616a15086
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc0310.c
@@ -0,0 +1,1490 @@
+/*
+ * 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/gpio.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 = gc0310_res[dev->fmt_idx].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 = gc0310_res[dev->fmt_idx].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;
+	pr_info("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);
+	pr_info("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);
+	pr_info("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);
+	pr_info("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);
+	pr_info("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;
+	pr_info("crop_horizontal_end=%d\n", buf->crop_horizontal_end);
+	pr_info("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;
+	pr_info("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;
+	pr_info("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;
+	}
+
+	pr_info("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;
+
+	pr_info("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
+};
+
+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);
+
+	pr_info("%s S\n", __func__);
+	mutex_lock(&dev->input_lock);
+
+	/* set inital 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);
+
+	pr_info("%s E\n", __func__);
+	return 0;
+}
+
+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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	/* 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;
+
+	pr_info("%s S\n", __func__);
+	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);
+
+	pr_info("%s E\n", __func__);
+	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);
+	else {
+		ret = power_up(sd);
+		if (!ret)
+			return gc0310_init(sd);
+	}
+	return ret;
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 800
+static int distance(struct gc0310_resolution *res, u32 w, u32 h)
+{
+	unsigned int w_ratio = (res->width << 13) / w;
+	unsigned int h_ratio;
+	int match;
+
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - ((int)8192));
+
+	if ((w_ratio < (int)8192) || (h_ratio < (int)8192)  ||
+		(match > LARGEST_ALLOWED_RATIO_MISMATCH))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	struct gc0310_resolution *tmp_res = NULL;
+
+	for (i = 0; i < N_RES; i++) {
+		tmp_res = &gc0310_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static int get_resolution_index(int w, int h)
+{
+	int i;
+
+	for (i = 0; i < N_RES; i++) {
+		if (w != gc0310_res[i].width)
+			continue;
+		if (h != gc0310_res[i].height)
+			continue;
+
+		return i;
+	}
+
+	return -1;
+}
+
+
+/* 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;
+
+	pr_info("%s S\n", __func__);
+
+	ret = gc0310_write_reg_array(client, gc0310_res[dev->fmt_idx].regs);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 write register err.\n");
+		return ret;
+	}
+
+	pr_info("%s E\n", __func__);
+	return ret;
+}
+
+static int gc0310_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  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;
+	int ret = 0;
+	int idx = 0;
+	pr_info("%s S\n", __func__);
+
+	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);
+
+	idx = nearest_resolution_index(fmt->width, fmt->height);
+	if (idx == -1) {
+		/* return the largest resolution */
+		fmt->width = gc0310_res[N_RES - 1].width;
+		fmt->height = gc0310_res[N_RES - 1].height;
+	} else {
+		fmt->width = gc0310_res[idx].width;
+		fmt->height = gc0310_res[idx].height;
+	}
+	fmt->code = MEDIA_BUS_FMT_SGRBG8_1X8;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->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;
+	}
+
+	printk("%s: before gc0310_write_reg_array %s\n", __FUNCTION__,
+	       gc0310_res[dev->fmt_idx].desc);
+	ret = startup(sd);
+	if (ret) {
+		dev_err(&client->dev, "gc0310 startup err\n");
+		goto err;
+	}
+
+	ret = gc0310_get_intg_factor(client, gc0310_info,
+				     &gc0310_res[dev->fmt_idx]);
+	if (ret) {
+		dev_err(&client->dev, "failed to get integration_factor\n");
+		goto err;
+	}
+
+	pr_info("%s E\n", __func__);
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc0310_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  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 = gc0310_res[dev->fmt_idx].width;
+	fmt->height = gc0310_res[dev->fmt_idx].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;
+
+	pr_info("%s S\n", __func__);
+	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);
+	pr_info("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");
+
+	pr_info("%s E\n", __func__);
+
+	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;
+
+	pr_info("%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);
+	pr_info("%s E\n", __func__);
+	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;
+
+	pr_info("%s S\n", __func__);
+	if (!platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+		(struct camera_sensor_platform_data *)platform_data;
+
+	mutex_lock(&dev->input_lock);
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			dev_err(&client->dev, "platform init err\n");
+			goto platform_init_failed;
+		}
+	}
+	/* 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);
+
+	pr_info("%s E\n", __func__);
+	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:
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+platform_init_failed:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc0310_g_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!param)
+		return -EINVAL;
+
+	if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(&client->dev,  "unsupported buffer type.\n");
+		return -EINVAL;
+	}
+
+	memset(param, 0, sizeof(*param));
+	param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (dev->fmt_idx >= 0 && dev->fmt_idx < N_RES) {
+		param->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+		param->parm.capture.timeperframe.numerator = 1;
+		param->parm.capture.capturemode = dev->run_mode;
+		param->parm.capture.timeperframe.denominator =
+			gc0310_res[dev->fmt_idx].fps;
+	}
+	return 0;
+}
+
+static int gc0310_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct gc0310_device *dev = to_gc0310_sensor(sd);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	mutex_lock(&dev->input_lock);
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		gc0310_res = gc0310_res_video;
+		N_RES = N_RES_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		gc0310_res = gc0310_res_still;
+		N_RES = N_RES_STILL;
+		break;
+	default:
+		gc0310_res = gc0310_res_preview;
+		N_RES = N_RES_PREVIEW;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+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 = gc0310_res[dev->fmt_idx].fps;
+
+	return 0;
+}
+
+static int gc0310_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 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_pad_config *cfg,
+				  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 = gc0310_res[dev->fmt_idx].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_parm = gc0310_g_parm,
+	.s_parm = gc0310_s_parm,
+	.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 int 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");
+
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	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);
+
+	return 0;
+}
+
+static int gc0310_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct gc0310_device *dev;
+	int ret;
+	void *pdata;
+	unsigned int i;
+
+	pr_info("%s S\n", __func__);
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&(dev->sd), client, &gc0310_ops);
+
+	if (ACPI_COMPANION(&client->dev))
+		pdata = gmin_camera_platform_data(&dev->sd,
+						  ATOMISP_INPUT_FORMAT_RAW_8,
+						  atomisp_bayer_order_grbg);
+	else
+		pdata = client->dev.platform_data;
+
+	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);
+
+	pr_info("%s E\n", __func__);
+	return ret;
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static struct acpi_device_id gc0310_acpi_match[] = {
+	{"XXGC0310"},
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, gc0310_acpi_match);
+
+MODULE_DEVICE_TABLE(i2c, gc0310_id);
+static struct i2c_driver gc0310_driver = {
+	.driver = {
+		.name = GC0310_NAME,
+		.acpi_match_table = ACPI_PTR(gc0310_acpi_match),
+	},
+	.probe = gc0310_probe,
+	.remove = gc0310_remove,
+	.id_table = gc0310_id,
+};
+
+static int init_gc0310(void)
+{
+	return i2c_add_driver(&gc0310_driver);
+}
+
+static void exit_gc0310(void)
+{
+
+	i2c_del_driver(&gc0310_driver);
+}
+
+module_init(init_gc0310);
+module_exit(exit_gc0310);
+
+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/gc0310.h b/drivers/staging/media/atomisp/i2c/gc0310.h
new file mode 100644
index 000000000000..f31eb277f542
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc0310.h
@@ -0,0 +1,459 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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"
+
+#define GC0310_NAME		"gc0310"
+
+/* 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;
+	int fmt_idx;
+	int run_mode;
+	u8 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;
+};
+
+static const struct i2c_device_id gc0310_id[] = {
+	{GC0310_NAME, 0},
+	{}
+};
+
+/*
+ * 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 vaule 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},
+};
+
+
+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))
+
+struct gc0310_resolution gc0310_res_still[] = {
+	{
+		.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_STILL (ARRAY_SIZE(gc0310_res_still))
+
+struct gc0310_resolution gc0310_res_video[] = {
+	{
+		.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_VIDEO (ARRAY_SIZE(gc0310_res_video))
+
+static struct gc0310_resolution *gc0310_res = gc0310_res_preview;
+static int N_RES = N_RES_PREVIEW;
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/gc2235.c b/drivers/staging/media/atomisp/i2c/gc2235.c
new file mode 100644
index 000000000000..50f431729b6c
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc2235.c
@@ -0,0 +1,1219 @@
+/*
+ * 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/gpio.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, dummy;
+	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;
+
+	dummy = (reg_val_h << 8) | reg_val;
+
+	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
+};
+
+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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	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 = -1;
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	ret |= dev->platform_data->gpio1_ctrl(sd, !flag);
+	usleep_range(60, 90);
+	return dev->platform_data->gpio0_ctrl(sd, flag);
+}
+
+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;
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 800
+static int distance(struct gc2235_resolution *res, u32 w, u32 h)
+{
+	unsigned int w_ratio = (res->width << 13) / w;
+	unsigned int h_ratio;
+	int match;
+
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - 8192);
+
+	if ((w_ratio < 8192) || (h_ratio < 8192) ||
+	    (match > LARGEST_ALLOWED_RATIO_MISMATCH))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	struct gc2235_resolution *tmp_res = NULL;
+
+	for (i = 0; i < N_RES; i++) {
+		tmp_res = &gc2235_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static int get_resolution_index(int w, int h)
+{
+	int i;
+
+	for (i = 0; i < N_RES; i++) {
+		if (w != gc2235_res[i].width)
+			continue;
+		if (h != gc2235_res[i].height)
+			continue;
+
+		return i;
+	}
+
+	return -1;
+}
+
+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, gc2235_res[dev->fmt_idx].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_pad_config *cfg,
+			  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;
+	int ret = 0;
+	int idx;
+
+	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);
+	idx = nearest_resolution_index(fmt->width, fmt->height);
+	if (idx == -1) {
+		/* return the largest resolution */
+		fmt->width = gc2235_res[N_RES - 1].width;
+		fmt->height = gc2235_res[N_RES - 1].height;
+	} else {
+		fmt->width = gc2235_res[idx].width;
+		fmt->height = gc2235_res[idx].height;
+	}
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->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) {
+		dev_err(&client->dev, "gc2235 startup err\n");
+		goto err;
+	}
+
+	ret = gc2235_get_intg_factor(client, gc2235_info,
+				     &gc2235_res[dev->fmt_idx]);
+	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_pad_config *cfg,
+			  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 = gc2235_res[dev->fmt_idx].width;
+	fmt->height = gc2235_res[dev->fmt_idx].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, low;
+	int ret;
+	u16 id;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ret = gc2235_read_reg(client, GC2235_8BIT,
+					GC2235_SENSOR_ID_H, &high);
+	if (ret) {
+		dev_err(&client->dev, "sensor_id_high = 0x%x\n", high);
+		return -ENODEV;
+	}
+	ret = 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);
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			dev_err(&client->dev, "platform init err\n");
+			goto platform_init_failed;
+		}
+	}
+	/* 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:
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+platform_init_failed:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int gc2235_g_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!param)
+		return -EINVAL;
+
+	if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(&client->dev,  "unsupported buffer type.\n");
+		return -EINVAL;
+	}
+
+	memset(param, 0, sizeof(*param));
+	param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (dev->fmt_idx >= 0 && dev->fmt_idx < N_RES) {
+		param->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+		param->parm.capture.timeperframe.numerator = 1;
+		param->parm.capture.capturemode = dev->run_mode;
+		param->parm.capture.timeperframe.denominator =
+			gc2235_res[dev->fmt_idx].fps;
+	}
+	return 0;
+}
+
+static int gc2235_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct gc2235_device *dev = to_gc2235_sensor(sd);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	mutex_lock(&dev->input_lock);
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		gc2235_res = gc2235_res_video;
+		N_RES = N_RES_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		gc2235_res = gc2235_res_still;
+		N_RES = N_RES_STILL;
+		break;
+	default:
+		gc2235_res = gc2235_res_preview;
+		N_RES = N_RES_PREVIEW;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+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 = gc2235_res[dev->fmt_idx].fps;
+
+	return 0;
+}
+
+static int gc2235_enum_mbus_code(struct v4l2_subdev *sd,
+				struct v4l2_subdev_pad_config *cfg,
+				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_pad_config *cfg,
+				  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 = gc2235_res[dev->fmt_idx].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_parm = gc2235_g_parm,
+	.s_parm = gc2235_s_parm,
+	.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 int 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");
+
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	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);
+
+	return 0;
+}
+
+static int gc2235_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct gc2235_device *dev;
+	void *gcpdev;
+	int ret;
+	unsigned int i;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&(dev->sd), client, &gc2235_ops);
+
+	gcpdev = client->dev.platform_data;
+	if (ACPI_COMPANION(&client->dev))
+		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);
+
+	if (ACPI_HANDLE(&client->dev))
+		ret = atomisp_register_i2c_module(&dev->sd, gcpdev, RAW_CAMERA);
+
+	return ret;
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+
+	return ret;
+}
+
+static struct acpi_device_id gc2235_acpi_match[] = {
+	{ "INT33F8" },
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, gc2235_acpi_match);
+MODULE_DEVICE_TABLE(i2c, gc2235_id);
+static struct i2c_driver gc2235_driver = {
+	.driver = {
+		.name = GC2235_NAME,
+		.acpi_match_table = ACPI_PTR(gc2235_acpi_match),
+	},
+	.probe = gc2235_probe,
+	.remove = gc2235_remove,
+	.id_table = gc2235_id,
+};
+
+static int init_gc2235(void)
+{
+	return i2c_add_driver(&gc2235_driver);
+}
+
+static void exit_gc2235(void)
+{
+
+	i2c_del_driver(&gc2235_driver);
+}
+
+module_init(init_gc2235);
+module_exit(exit_gc2235);
+
+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/gc2235.h b/drivers/staging/media/atomisp/i2c/gc2235.h
new file mode 100644
index 000000000000..ccbc757045a5
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/gc2235.h
@@ -0,0 +1,672 @@
+/*
+ * 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"
+
+#define GC2235_NAME		"gc2235"
+
+/* 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 camera_sensor_platform_data *platform_data;
+	int vt_pix_clk_freq_mhz;
+	int fmt_idx;
+	int run_mode;
+	u8 res;
+	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 const struct i2c_device_id gc2235_id[] = {
+	{GC2235_NAME, 0},
+	{}
+};
+
+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[] = {
+	/* Sysytem */
+	{ 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
+ */
+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 }
+};
+
+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 }
+};
+
+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))
+
+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))
+
+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))
+
+static struct gc2235_resolution *gc2235_res = gc2235_res_preview;
+static int N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/Kconfig b/drivers/staging/media/atomisp/i2c/imx/Kconfig
new file mode 100644
index 000000000000..a39eeb3b6ad4
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/Kconfig
@@ -0,0 +1,9 @@
+config VIDEO_IMX
+	tristate "sony imx sensor support"
+	depends on I2C && VIDEO_V4L2 && VIDEO_MSRLIST_HELPER && m
+	---help---
+	  This is a Video4Linux2 sensor-level driver for the Sony
+	  IMX RAW sensor.
+
+	  It currently depends on internal V4L2 extensions defined in
+	  atomisp driver.
diff --git a/drivers/staging/media/atomisp/i2c/imx/Makefile b/drivers/staging/media/atomisp/i2c/imx/Makefile
new file mode 100644
index 000000000000..1d7f7ab94cac
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_VIDEO_IMX) += imx1x5.o
+
+imx1x5-objs := imx.o drv201.o ad5816g.o dw9714.o dw9719.o dw9718.o vcm.o otp.o otp_imx.o otp_brcc064_e2prom.o otp_e2prom.o
+
+ov8858_driver-objs := ../ov8858.o dw9718.o vcm.o
+obj-$(CONFIG_VIDEO_OV8858)     += ov8858_driver.o
+
+ccflags-y += -Werror
diff --git a/drivers/staging/media/atomisp/i2c/imx/ad5816g.c b/drivers/staging/media/atomisp/i2c/imx/ad5816g.c
new file mode 100644
index 000000000000..d68ebb49f002
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/ad5816g.c
@@ -0,0 +1,225 @@
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+#include "ad5816g.h"
+
+struct ad5816g_device ad5816g_dev;
+
+static int ad5816g_i2c_rd8(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 = AD5816G_VCM_ADDR;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+
+	msg[1].addr = AD5816G_VCM_ADDR;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+	return 0;
+}
+
+static int ad5816g_i2c_wr8(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = AD5816G_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int ad5816g_i2c_wr16(struct i2c_client *client, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3];
+	buf[0] = reg;
+	buf[1] = (u8)(val >> 8);
+	buf[2] = (u8)(val & 0xff);
+	msg.addr = AD5816G_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = &buf[0];
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int ad5816g_set_arc_mode(struct i2c_client *client)
+{
+	int ret;
+
+	ret = ad5816g_i2c_wr8(client, AD5816G_CONTROL, AD5816G_ARC_EN);
+	if (ret)
+		return ret;
+
+	ret = ad5816g_i2c_wr8(client, AD5816G_MODE,
+				AD5816G_MODE_2_5M_SWITCH_CLOCK);
+	if (ret)
+		return ret;
+
+	ret = ad5816g_i2c_wr8(client, AD5816G_VCM_FREQ, AD5816G_DEF_FREQ);
+	return ret;
+}
+
+int ad5816g_vcm_power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 ad5816g_id;
+
+	/* Enable power */
+	ret = ad5816g_dev.platform_data->power_ctrl(sd, 1);
+	if (ret)
+		return ret;
+	/* waiting time AD5816G(vcm) - t1 + t2
+	  * t1(1ms) -Time from VDD high to first i2c cmd
+	  * t2(100us) - exit power-down mode time
+	  */
+	usleep_range(1100, 2200);
+	/* Detect device */
+	ret = ad5816g_i2c_rd8(client, AD5816G_IC_INFO, &ad5816g_id);
+	if (ret < 0)
+		goto fail_powerdown;
+	if (ad5816g_id != AD5816G_ID) {
+		ret = -ENXIO;
+		goto fail_powerdown;
+	}
+	ret = ad5816g_set_arc_mode(client);
+	if (ret)
+		return ret;
+
+	/* set the VCM_THRESHOLD */
+	ret = ad5816g_i2c_wr8(client, AD5816G_VCM_THRESHOLD,
+		AD5816G_DEF_THRESHOLD);
+
+	return ret;
+
+fail_powerdown:
+	ad5816g_dev.platform_data->power_ctrl(sd, 0);
+	return ret;
+}
+
+int ad5816g_vcm_power_down(struct v4l2_subdev *sd)
+{
+	return ad5816g_dev.platform_data->power_ctrl(sd, 0);
+}
+
+
+int ad5816g_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 data = val & VCM_CODE_MASK;
+
+	return ad5816g_i2c_wr16(client, AD5816G_VCM_CODE_MSB, data);
+}
+
+int ad5816g_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	int ret;
+
+	value = clamp(value, 0, AD5816G_MAX_FOCUS_POS);
+	ret = ad5816g_t_focus_vcm(sd, value);
+	if (ret == 0) {
+		ad5816g_dev.number_of_steps = value - ad5816g_dev.focus;
+		ad5816g_dev.focus = value;
+		getnstimeofday(&(ad5816g_dev.timestamp_t_focus_abs));
+	}
+
+	return ret;
+}
+
+int ad5816g_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+
+	return ad5816g_t_focus_abs(sd, ad5816g_dev.focus + value);
+}
+
+int ad5816g_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	u32 status = 0;
+	struct timespec temptime;
+	const struct timespec timedelay = {
+		0,
+		min_t(u32, abs(ad5816g_dev.number_of_steps) * DELAY_PER_STEP_NS,
+			DELAY_MAX_PER_STEP_NS),
+	};
+
+	ktime_get_ts(&temptime);
+
+	temptime = timespec_sub(temptime, (ad5816g_dev.timestamp_t_focus_abs));
+
+	if (timespec_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;
+}
+
+int ad5816g_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	s32 val;
+
+	ad5816g_q_focus_status(sd, &val);
+
+	if (val & ATOMISP_FOCUS_STATUS_MOVING)
+		*value  = ad5816g_dev.focus - ad5816g_dev.number_of_steps;
+	else
+		*value = ad5816g_dev.focus;
+
+	return 0;
+}
+
+int ad5816g_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int ad5816g_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int ad5816g_vcm_init(struct v4l2_subdev *sd)
+{
+	ad5816g_dev.platform_data = camera_get_af_platform_data();
+	return (NULL == ad5816g_dev.platform_data) ? -ENODEV : 0;
+
+}
+
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/ad5816g.h b/drivers/staging/media/atomisp/i2c/imx/ad5816g.h
new file mode 100644
index 000000000000..f995c2eeada4
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/ad5816g.h
@@ -0,0 +1,49 @@
+#ifndef __AD5816G_H__
+#define __AD5816G_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/types.h>
+#include <linux/time.h>
+
+#define AD5816G_VCM_ADDR	0x0e
+
+/* ad5816g device structure */
+struct ad5816g_device {
+	const struct camera_af_platform_data *platform_data;
+	struct timespec timestamp_t_focus_abs;
+	struct timespec focus_time;	/* Time when focus was last time set */
+	s32 focus;			/* Current focus value */
+	s16 number_of_steps;
+};
+
+#define AD5816G_INVALID_CONFIG	0xffffffff
+#define AD5816G_MAX_FOCUS_POS	1023
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 1023)
+
+/* Register Definitions */
+#define AD5816G_IC_INFO			0x00
+#define AD5816G_IC_VERSION		0x01
+#define AD5816G_CONTROL			0x02
+#define AD5816G_VCM_CODE_MSB	0x03
+#define AD5816G_VCM_CODE_LSB	0x04
+#define AD5816G_STATUS			0x05
+#define AD5816G_MODE			0x06
+#define AD5816G_VCM_FREQ		0x07
+#define AD5816G_VCM_THRESHOLD	0x08
+
+/* ARC MODE ENABLE */
+#define AD5816G_ARC_EN			0x02
+/* ARC RES2 MODE */
+#define AD5816G_ARC_RES2			0x01
+/* ARC VCM FREQ - 78.1Hz */
+#define AD5816G_DEF_FREQ			0x7a
+/* ARC VCM THRESHOLD - 0x08 << 1 */
+#define AD5816G_DEF_THRESHOLD		0x64
+#define AD5816G_ID			0x24
+#define VCM_CODE_MASK	0x03ff
+
+#define AD5816G_MODE_2_5M_SWITCH_CLOCK	0x14
+
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/common.h b/drivers/staging/media/atomisp/i2c/imx/common.h
new file mode 100644
index 000000000000..7e525cef56ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/common.h
@@ -0,0 +1,65 @@
+#ifndef __COMMON_H__
+#define __COMMON_H__
+
+#define MAX_FPS_OPTIONS_SUPPORTED	3
+#define I2C_MSG_LENGTH		0x2
+#define E2PROM_2ADDR 0x80000000
+#define E2PROM_ADDR_MASK 0x7fffffff
+
+/* Defines for register writes and register array processing */
+#define IMX_BYTE_MAX	32
+#define IMX_SHORT_MAX	16
+#define I2C_RETRY_COUNT		5
+#define IMX_TOK_MASK	0xfff0
+
+enum imx_tok_type {
+	IMX_8BIT  = 0x0001,
+	IMX_16BIT = 0x0002,
+	IMX_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+	IMX_TOK_DELAY  = 0xfe00	/* delay token for reg list */
+};
+
+/**
+ * struct imx_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 imx_reg {
+	enum imx_tok_type type;
+	u16 sreg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+struct imx_fps_setting {
+	int fps;
+	unsigned short pixels_per_line;
+	unsigned short lines_per_frame;
+	int mipi_freq;			/* MIPI lane frequency in kHz */
+	const struct imx_reg *regs; /* regs that the fps setting needs */
+};
+
+struct imx_resolution {
+	const struct imx_fps_setting fps_options[MAX_FPS_OPTIONS_SUPPORTED];
+	u8 *desc;
+	const struct imx_reg *regs;
+	int res;
+	int width;
+	int height;
+	int fps;
+	unsigned short pixels_per_line;
+	unsigned short lines_per_frame;
+	int mipi_freq;			/* MIPI lane frequency in kHz */
+	unsigned short skip_frames;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	bool used;
+};
+
+#define GROUPED_PARAMETER_HOLD_ENABLE  {IMX_8BIT, 0x0104, 0x1}
+#define GROUPED_PARAMETER_HOLD_DISABLE  {IMX_8BIT, 0x0104, 0x0}
+
+int imx_write_reg(struct i2c_client *client, u16 data_length, u16 reg, u16 val);
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/drv201.c b/drivers/staging/media/atomisp/i2c/imx/drv201.c
new file mode 100644
index 000000000000..915e4019cfeb
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/drv201.c
@@ -0,0 +1,218 @@
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include <asm/intel-mid.h>
+
+#include "drv201.h"
+
+static struct drv201_device drv201_dev;
+
+static int drv201_i2c_rd8(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 = DRV201_VCM_ADDR;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &buf[0];
+
+	msg[1].addr = DRV201_VCM_ADDR;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+	return 0;
+}
+
+static int drv201_i2c_wr8(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2];
+	buf[0] = reg;
+	buf[1] = val;
+	msg.addr = DRV201_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 2;
+	msg.buf = &buf[0];
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+static int drv201_i2c_wr16(struct i2c_client *client, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3];
+	buf[0] = reg;
+	buf[1] = (u8)(val >> 8);
+	buf[2] = (u8)(val & 0xff);
+	msg.addr = DRV201_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = 3;
+	msg.buf = &buf[0];
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+	return 0;
+}
+
+int drv201_vcm_power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 value;
+
+	/* Enable power */
+	ret = drv201_dev.platform_data->power_ctrl(sd, 1);
+	if (ret)
+		return ret;
+	/* Wait for VBAT to stabilize */
+	udelay(1);
+	/*
+	 * Jiggle SCL pin to wake up device.
+	 * Drv201 expect SCL from low to high to wake device up.
+	 * So the 1st access to i2c would fail.
+	 * Using following function to wake device up.
+	 */
+	drv201_i2c_wr8(client, DRV201_CONTROL, DRV201_RESET);
+
+	/* Need 100us to transit from SHUTDOWN to STANDBY*/
+	usleep_range(WAKEUP_DELAY_US, WAKEUP_DELAY_US * 10);
+
+	/* Reset device */
+	ret = drv201_i2c_wr8(client, DRV201_CONTROL, DRV201_RESET);
+	if (ret < 0)
+		goto fail_powerdown;
+
+	/* Detect device */
+	ret = drv201_i2c_rd8(client, DRV201_CONTROL, &value);
+	if (ret < 0)
+		goto fail_powerdown;
+	if (value != DEFAULT_CONTROL_VAL) {
+		ret = -ENXIO;
+		goto fail_powerdown;
+	}
+
+	drv201_dev.focus = DRV201_MAX_FOCUS_POS;
+	drv201_dev.initialized = true;
+
+	return 0;
+fail_powerdown:
+	drv201_dev.platform_data->power_ctrl(sd, 0);
+	return ret;
+}
+
+int drv201_vcm_power_down(struct v4l2_subdev *sd)
+{
+	return drv201_dev.platform_data->power_ctrl(sd, 0);
+}
+
+
+int drv201_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 data = val & VCM_CODE_MASK;
+
+	if (!drv201_dev.initialized)
+		return -ENODEV;
+	return drv201_i2c_wr16(client, DRV201_VCM_CURRENT, data);
+}
+
+int drv201_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	int ret;
+
+	value = clamp(value, 0, DRV201_MAX_FOCUS_POS);
+	ret = drv201_t_focus_vcm(sd, value);
+	if (ret == 0) {
+		drv201_dev.number_of_steps = value - drv201_dev.focus;
+		drv201_dev.focus = value;
+		getnstimeofday(&(drv201_dev.timestamp_t_focus_abs));
+	}
+
+	return ret;
+}
+
+int drv201_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+	return drv201_t_focus_abs(sd, drv201_dev.focus + value);
+}
+
+int drv201_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	u32 status = 0;
+	struct timespec temptime;
+	const struct timespec timedelay = {
+		0,
+		min_t(u32, abs(drv201_dev.number_of_steps)*DELAY_PER_STEP_NS,
+			DELAY_MAX_PER_STEP_NS),
+	};
+
+	ktime_get_ts(&temptime);
+
+	temptime = timespec_sub(temptime, (drv201_dev.timestamp_t_focus_abs));
+
+	if (timespec_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;
+}
+
+int drv201_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	s32 val;
+
+	drv201_q_focus_status(sd, &val);
+
+	if (val & ATOMISP_FOCUS_STATUS_MOVING)
+		*value  = drv201_dev.focus - drv201_dev.number_of_steps;
+	else
+		*value  = drv201_dev.focus;
+
+	return 0;
+}
+
+int drv201_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int drv201_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int drv201_vcm_init(struct v4l2_subdev *sd)
+{
+	drv201_dev.platform_data = camera_get_af_platform_data();
+	return (NULL == drv201_dev.platform_data) ? -ENODEV : 0;
+}
+
+
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/drv201.h b/drivers/staging/media/atomisp/i2c/imx/drv201.h
new file mode 100644
index 000000000000..8fc0ad116630
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/drv201.h
@@ -0,0 +1,38 @@
+#ifndef __DRV201_H__
+#define __DRV201_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/types.h>
+#include <linux/time.h>
+
+#define DRV201_VCM_ADDR	0x0e
+
+/* drv201 device structure */
+struct drv201_device {
+	const struct camera_af_platform_data *platform_data;
+	struct timespec timestamp_t_focus_abs;
+	struct timespec focus_time;	/* Time when focus was last time set */
+	s32 focus;			/* Current focus value */
+	s16 number_of_steps;
+	bool initialized;		/* true if drv201 is detected */
+};
+
+#define DRV201_INVALID_CONFIG	0xffffffff
+#define DRV201_MAX_FOCUS_POS	1023
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 1023)
+
+#define DRV201_CONTROL				2
+#define DRV201_VCM_CURRENT		3
+#define DRV201_STATUS				5
+#define DRV201_MODE				6
+#define DRV201_VCM_FREQ			7
+
+#define DEFAULT_CONTROL_VAL		2
+#define DRV201_RESET				1
+#define WAKEUP_DELAY_US			100
+#define VCM_CODE_MASK	0x03ff
+
+#endif
+
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9714.c b/drivers/staging/media/atomisp/i2c/imx/dw9714.c
new file mode 100644
index 000000000000..b7dee1b6bb37
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9714.c
@@ -0,0 +1,235 @@
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include <asm/intel-mid.h>
+
+#include "dw9714.h"
+
+static struct dw9714_device dw9714_dev;
+static int dw9714_i2c_write(struct i2c_client *client, u16 data)
+{
+	struct i2c_msg msg;
+	const int num_msg = 1;
+	int ret;
+	u16 val;
+
+	val = cpu_to_be16(data);
+	msg.addr = DW9714_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = DW9714_16BIT;
+	msg.buf = (u8 *)&val;
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return ret == num_msg ? 0 : -EIO;
+}
+
+int dw9714_vcm_power_up(struct v4l2_subdev *sd)
+{
+	int ret;
+
+	/* Enable power */
+	ret = dw9714_dev.platform_data->power_ctrl(sd, 1);
+	/* waiting time requested by DW9714A(vcm) */
+	usleep_range(12000, 12500);
+	return ret;
+}
+
+int dw9714_vcm_power_down(struct v4l2_subdev *sd)
+{
+	return dw9714_dev.platform_data->power_ctrl(sd, 0);
+}
+
+
+int dw9714_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = -EINVAL;
+	u8 mclk = vcm_step_mclk(dw9714_dev.vcm_settings.step_setting);
+	u8 s = vcm_step_s(dw9714_dev.vcm_settings.step_setting);
+
+	/*
+	 * For different mode, VCM_PROTECTION_OFF/ON required by the
+	 * control procedure. For DW9714_DIRECT/DLC mode, slew value is
+	 * VCM_DEFAULT_S(0).
+	 */
+	switch (dw9714_dev.vcm_mode) {
+	case DW9714_DIRECT:
+		if (dw9714_dev.vcm_settings.update) {
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_OFF);
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client, DIRECT_VCM);
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_ON);
+			if (ret)
+				return ret;
+			dw9714_dev.vcm_settings.update = false;
+		}
+		ret = dw9714_i2c_write(client,
+					vcm_val(val, VCM_DEFAULT_S));
+		break;
+	case DW9714_LSC:
+		if (dw9714_dev.vcm_settings.update) {
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_OFF);
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client,
+				vcm_dlc_mclk(DLC_DISABLE, mclk));
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client,
+				vcm_tsrc(dw9714_dev.vcm_settings.t_src));
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_ON);
+			if (ret)
+				return ret;
+			dw9714_dev.vcm_settings.update = false;
+		}
+		ret = dw9714_i2c_write(client, vcm_val(val, s));
+		break;
+	case DW9714_DLC:
+		if (dw9714_dev.vcm_settings.update) {
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_OFF);
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client,
+					vcm_dlc_mclk(DLC_ENABLE, mclk));
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client,
+				vcm_tsrc(dw9714_dev.vcm_settings.t_src));
+			if (ret)
+				return ret;
+			ret = dw9714_i2c_write(client, VCM_PROTECTION_ON);
+			if (ret)
+				return ret;
+			dw9714_dev.vcm_settings.update = false;
+		}
+		ret = dw9714_i2c_write(client,
+					vcm_val(val, VCM_DEFAULT_S));
+		break;
+	}
+	return ret;
+}
+
+int dw9714_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	int ret;
+
+	value = clamp(value, 0, DW9714_MAX_FOCUS_POS);
+	ret = dw9714_t_focus_vcm(sd, value);
+	if (ret == 0) {
+		dw9714_dev.number_of_steps = value - dw9714_dev.focus;
+		dw9714_dev.focus = value;
+		getnstimeofday(&(dw9714_dev.timestamp_t_focus_abs));
+	}
+
+	return ret;
+}
+
+int dw9714_t_focus_abs_init(struct v4l2_subdev *sd)
+{
+	int ret;
+
+	ret = dw9714_t_focus_vcm(sd, DW9714_DEFAULT_FOCUS_POS);
+	if (ret == 0) {
+		dw9714_dev.number_of_steps =
+			DW9714_DEFAULT_FOCUS_POS - dw9714_dev.focus;
+		dw9714_dev.focus = DW9714_DEFAULT_FOCUS_POS;
+		getnstimeofday(&(dw9714_dev.timestamp_t_focus_abs));
+	}
+
+	return ret;
+}
+
+int dw9714_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+
+	return dw9714_t_focus_abs(sd, dw9714_dev.focus + value);
+}
+
+int dw9714_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	u32 status = 0;
+	struct timespec temptime;
+	const struct timespec timedelay = {
+		0,
+		min_t(u32, abs(dw9714_dev.number_of_steps)*DELAY_PER_STEP_NS,
+			DELAY_MAX_PER_STEP_NS),
+	};
+
+	ktime_get_ts(&temptime);
+
+	temptime = timespec_sub(temptime, (dw9714_dev.timestamp_t_focus_abs));
+
+	if (timespec_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;
+}
+
+int dw9714_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	s32 val;
+
+	dw9714_q_focus_status(sd, &val);
+
+	if (val & ATOMISP_FOCUS_STATUS_MOVING)
+		*value  = dw9714_dev.focus - dw9714_dev.number_of_steps;
+	else
+		*value  = dw9714_dev.focus;
+
+	return 0;
+}
+
+int dw9714_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	dw9714_dev.vcm_settings.step_setting = value;
+	dw9714_dev.vcm_settings.update = true;
+
+	return 0;
+}
+
+int dw9714_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	dw9714_dev.vcm_settings.t_src = value;
+	dw9714_dev.vcm_settings.update = true;
+
+	return 0;
+}
+
+int dw9714_vcm_init(struct v4l2_subdev *sd)
+{
+
+	/* set VCM to home position and vcm mode to direct*/
+	dw9714_dev.vcm_mode = DW9714_DIRECT;
+	dw9714_dev.vcm_settings.update = false;
+	dw9714_dev.platform_data = camera_get_af_platform_data();
+	return (NULL == dw9714_dev.platform_data) ? -ENODEV : 0;
+
+}
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9714.h b/drivers/staging/media/atomisp/i2c/imx/dw9714.h
new file mode 100644
index 000000000000..5a98a9c97182
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9714.h
@@ -0,0 +1,63 @@
+#ifndef __DW9714_H__
+#define __DW9714_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/types.h>
+
+
+#define DW9714_VCM_ADDR	0x0c
+
+enum dw9714_tok_type {
+	DW9714_8BIT  = 0x0001,
+	DW9714_16BIT = 0x0002,
+};
+
+struct dw9714_vcm_settings {
+	u16 code;	/* bit[9:0]: Data[9:0] */
+	u8 t_src;	/* bit[4:0]: T_SRC[4:0] */
+	u8 step_setting;	/* bit[3:0]: S[3:0]/bit[5:4]: MCLK[1:0] */
+	bool update;
+};
+
+enum dw9714_vcm_mode {
+	DW9714_DIRECT = 0x1,	/* direct control */
+	DW9714_LSC = 0x2,	/* linear slope control */
+	DW9714_DLC = 0x3,	/* dual level control */
+};
+
+/* dw9714 device structure */
+struct dw9714_device {
+	struct dw9714_vcm_settings vcm_settings;
+	struct timespec timestamp_t_focus_abs;
+	enum dw9714_vcm_mode vcm_mode;
+	s16 number_of_steps;
+	bool initialized;		/* true if dw9714 is detected */
+	s32 focus;			/* Current focus value */
+	struct timespec focus_time;	/* Time when focus was last time set */
+	__u8 buffer[4];			/* Used for i2c transactions */
+	const struct camera_af_platform_data *platform_data;
+};
+
+#define DW9714_INVALID_CONFIG	0xffffffff
+#define DW9714_MAX_FOCUS_POS	1023
+#define DW9714_DEFAULT_FOCUS_POS	290
+
+
+/* MCLK[1:0] = 01 T_SRC[4:0] = 00001 S[3:0] = 0111 */
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 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)
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9718.c b/drivers/staging/media/atomisp/i2c/imx/dw9718.c
new file mode 100644
index 000000000000..65a1fcf187d5
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9718.c
@@ -0,0 +1,238 @@
+/*
+ * Support for dw9718 vcm driver.
+ *
+ * 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <linux/delay.h>
+#include "dw9718.h"
+
+static struct dw9718_device dw9718_dev;
+
+static int dw9718_i2c_rd8(struct i2c_client *client, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2] = { reg };
+
+	msg[0].addr = DW9718_VCM_ADDR;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = buf;
+
+	msg[1].addr = DW9718_VCM_ADDR;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+
+	return 0;
+}
+
+static int dw9718_i2c_wr8(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2] = { reg, val};
+
+	msg.addr = DW9718_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = sizeof(buf);
+	msg.buf = buf;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static int dw9718_i2c_wr16(struct i2c_client *client, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3] = { reg, (u8)(val >> 8), (u8)(val & 0xff)};
+
+	msg.addr = DW9718_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = sizeof(buf);
+	msg.buf = buf;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+int dw9718_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	value = clamp(value, 0, DW9718_MAX_FOCUS_POS);
+	ret = dw9718_i2c_wr16(client, DW9718_DATA_M, value);
+	/*pr_info("%s: value = %d\n", __func__, value);*/
+	if (ret < 0)
+		return ret;
+
+	getnstimeofday(&dw9718_dev.focus_time);
+	dw9718_dev.focus = value;
+
+	return 0;
+}
+
+int dw9718_vcm_power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 value;
+
+	if (dw9718_dev.power_on)
+		return 0;
+
+	/* Enable power */
+	ret = dw9718_dev.platform_data->power_ctrl(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "DW9718_PD power_ctrl failed %d\n", ret);
+		return ret;
+	}
+	/* Wait for VBAT to stabilize */
+	udelay(100);
+
+	/* Detect device */
+	ret = dw9718_i2c_rd8(client, DW9718_SACT, &value);
+	if (ret < 0) {
+		dev_err(&client->dev, "read DW9718_SACT failed %d\n", ret);
+		goto fail_powerdown;
+	}
+	/*
+	 * WORKAROUND: for module P8V12F-203 which are used on
+	 * Cherrytrail Refresh Davis Reef AoB, register SACT is not
+	 * returning default value as spec. But VCM works as expected and
+	 * root cause is still under discussion with vendor.
+	 * workaround here to avoid aborting the power up sequence and just
+	 * give a warning about this error.
+	 */
+	if (value != DW9718_SACT_DEFAULT_VAL)
+		dev_warn(&client->dev, "%s error, incorrect ID\n", __func__);
+
+	/* Initialize according to recommended settings */
+	ret = dw9718_i2c_wr8(client, DW9718_CONTROL,
+			     DW9718_CONTROL_SW_LINEAR |
+			     DW9718_CONTROL_S_SAC4 |
+			     DW9718_CONTROL_OCP_DISABLE |
+			     DW9718_CONTROL_UVLO_DISABLE);
+	if (ret < 0) {
+		dev_err(&client->dev, "write DW9718_CONTROL  failed %d\n", ret);
+		goto fail_powerdown;
+	}
+	ret = dw9718_i2c_wr8(client, DW9718_SACT,
+			     DW9718_SACT_MULT_TWO |
+			     DW9718_SACT_PERIOD_8_8MS);
+	if (ret < 0) {
+		dev_err(&client->dev, "write DW9718_SACT  failed %d\n", ret);
+		goto fail_powerdown;
+	}
+
+	ret = dw9718_t_focus_abs(sd, dw9718_dev.focus);
+	if (ret)
+		return ret;
+	dw9718_dev.initialized = true;
+	dw9718_dev.power_on = 1;
+
+	return 0;
+
+fail_powerdown:
+	dev_err(&client->dev, "%s error, powerup failed\n", __func__);
+	dw9718_dev.platform_data->power_ctrl(sd, 0);
+	return ret;
+}
+
+int dw9718_vcm_power_down(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	if (!dw9718_dev.power_on)
+		return 0;
+
+	ret =  dw9718_dev.platform_data->power_ctrl(sd, 0);
+	if (ret) {
+		dev_err(&client->dev, "%s power_ctrl failed\n",
+				__func__);
+		return ret;
+	}
+	dw9718_dev.power_on = 0;
+
+	return 0;
+}
+
+int dw9718_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	static const struct timespec move_time = {
+		.tv_sec = 0,
+		.tv_nsec = 60000000
+	};
+	struct timespec current_time, finish_time, delta_time;
+
+	getnstimeofday(&current_time);
+	finish_time = timespec_add(dw9718_dev.focus_time, move_time);
+	delta_time = timespec_sub(current_time, finish_time);
+	if (delta_time.tv_sec >= 0 && delta_time.tv_nsec >= 0) {
+		*value = ATOMISP_FOCUS_HP_COMPLETE |
+			 ATOMISP_FOCUS_STATUS_ACCEPTS_NEW_MOVE;
+	} else {
+		*value = ATOMISP_FOCUS_STATUS_MOVING |
+			 ATOMISP_FOCUS_HP_IN_PROGRESS;
+	}
+
+	return 0;
+}
+
+int dw9718_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	return -EINVAL;
+}
+
+int dw9718_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+	return dw9718_t_focus_abs(sd, dw9718_dev.focus + value);
+}
+
+int dw9718_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	*value  = dw9718_dev.focus;
+	return 0;
+}
+int dw9718_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int dw9718_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int dw9718_vcm_init(struct v4l2_subdev *sd)
+{
+	dw9718_dev.platform_data = camera_get_af_platform_data();
+	dw9718_dev.focus = DW9718_DEFAULT_FOCUS_POSITION;
+	dw9718_dev.power_on = 0;
+	return (NULL == dw9718_dev.platform_data) ? -ENODEV : 0;
+}
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9718.h b/drivers/staging/media/atomisp/i2c/imx/dw9718.h
new file mode 100644
index 000000000000..4a1040c3149f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9718.h
@@ -0,0 +1,64 @@
+/*
+ * Support for dw9719 vcm driver.
+ *
+ * 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __DW9718_H__
+#define __DW9718_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/types.h>
+
+#define DW9718_VCM_ADDR	 (0x18 >> 1)
+
+/* dw9718 device structure */
+struct dw9718_device {
+	struct timespec timestamp_t_focus_abs;
+	s16 number_of_steps;
+	bool initialized;		/* true if dw9718 is detected */
+	s32 focus;			/* Current focus value */
+	struct timespec focus_time;	/* Time when focus was last time set */
+	__u8 buffer[4];			/* Used for i2c transactions */
+	const struct camera_af_platform_data *platform_data;
+	__u8 power_on;
+};
+
+#define DW9718_MAX_FOCUS_POS	1023
+
+/* Register addresses */
+#define DW9718_PD			0x00
+#define DW9718_CONTROL			0x01
+#define DW9718_DATA_M			0x02
+#define DW9718_DATA_L			0x03
+#define DW9718_SW			0x04
+#define DW9718_SACT			0x05
+#define DW9718_FLAG			0x10
+
+#define DW9718_CONTROL_SW_LINEAR	BIT(0)
+#define DW9718_CONTROL_S_SAC4		(BIT(1) | BIT(3))
+#define DW9718_CONTROL_OCP_DISABLE	BIT(4)
+#define DW9718_CONTROL_UVLO_DISABLE	BIT(5)
+
+#define DW9718_SACT_MULT_TWO		0x00
+#define DW9718_SACT_PERIOD_8_8MS	0x19
+#define DW9718_SACT_DEFAULT_VAL		0x60
+
+#define DW9718_DEFAULT_FOCUS_POSITION	300
+
+#endif /* __DW9718_H__ */
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9719.c b/drivers/staging/media/atomisp/i2c/imx/dw9719.c
new file mode 100644
index 000000000000..eca2d7640030
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9719.c
@@ -0,0 +1,209 @@
+/*
+ * Support for dw9719 vcm driver.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <linux/delay.h>
+#include "dw9719.h"
+
+static struct dw9719_device dw9719_dev;
+
+static int dw9719_i2c_rd8(struct i2c_client *client, u8 reg, u8 *val)
+{
+	struct i2c_msg msg[2];
+	u8 buf[2] = { reg };
+
+	msg[0].addr = DW9719_VCM_ADDR;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = buf;
+
+	msg[1].addr = DW9719_VCM_ADDR;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 1;
+	msg[1].buf = &buf[1];
+	*val = 0;
+
+	if (i2c_transfer(client->adapter, msg, 2) != 2)
+		return -EIO;
+	*val = buf[1];
+
+	return 0;
+}
+
+static int dw9719_i2c_wr8(struct i2c_client *client, u8 reg, u8 val)
+{
+	struct i2c_msg msg;
+	u8 buf[2] = { reg, val };
+
+	msg.addr = DW9719_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = sizeof(buf);
+	msg.buf = buf;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+static int dw9719_i2c_wr16(struct i2c_client *client, u8 reg, u16 val)
+{
+	struct i2c_msg msg;
+	u8 buf[3] = { reg, (u8)(val >> 8), (u8)(val & 0xff)};
+
+	msg.addr = DW9719_VCM_ADDR;
+	msg.flags = 0;
+	msg.len = sizeof(buf);
+	msg.buf = buf;
+
+	if (i2c_transfer(client->adapter, &msg, 1) != 1)
+		return -EIO;
+
+	return 0;
+}
+
+int dw9719_vcm_power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u8 value;
+
+	/* Enable power */
+	ret = dw9719_dev.platform_data->power_ctrl(sd, 1);
+	/* waiting time requested by DW9714A(vcm) */
+	if (ret)
+		return ret;
+	/* Wait for VBAT to stabilize */
+	udelay(1);
+
+	/*
+	 * Jiggle SCL pin to wake up device.
+	 */
+	ret = dw9719_i2c_wr8(client, DW9719_CONTROL, 1);
+	/* Need 100us to transit from SHUTDOWN to STANDBY*/
+	usleep_range(100, 1000);
+
+	/* Enable the ringing compensation */
+	ret = dw9719_i2c_wr8(client, DW9719_CONTROL, DW9719_ENABLE_RINGING);
+	if (ret < 0)
+		goto fail_powerdown;
+
+	/* Use SAC3 mode */
+	ret = dw9719_i2c_wr8(client, DW9719_MODE, DW9719_MODE_SAC3);
+	if (ret < 0)
+		goto fail_powerdown;
+
+	/* Set the resonance frequency */
+	ret = dw9719_i2c_wr8(client, DW9719_VCM_FREQ, DW9719_DEFAULT_VCM_FREQ);
+	if (ret < 0)
+		goto fail_powerdown;
+
+	/* Detect device */
+	ret = dw9719_i2c_rd8(client, DW9719_INFO, &value);
+	if (ret < 0)
+		goto fail_powerdown;
+	if (value != DW9719_ID) {
+		ret = -ENXIO;
+		goto fail_powerdown;
+	}
+	dw9719_dev.focus = 0;
+	dw9719_dev.initialized = true;
+
+	return 0;
+
+fail_powerdown:
+	dw9719_dev.platform_data->power_ctrl(sd, 0);
+	return ret;
+}
+
+int dw9719_vcm_power_down(struct v4l2_subdev *sd)
+{
+	return dw9719_dev.platform_data->power_ctrl(sd, 0);
+}
+
+int dw9719_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	static const struct timespec move_time = {
+
+		.tv_sec = 0,
+		.tv_nsec = 60000000
+	};
+	struct timespec current_time, finish_time, delta_time;
+
+	getnstimeofday(&current_time);
+	finish_time = timespec_add(dw9719_dev.focus_time, move_time);
+	delta_time = timespec_sub(current_time, finish_time);
+	if (delta_time.tv_sec >= 0 && delta_time.tv_nsec >= 0) {
+		*value = ATOMISP_FOCUS_HP_COMPLETE |
+			 ATOMISP_FOCUS_STATUS_ACCEPTS_NEW_MOVE;
+	} else {
+		*value = ATOMISP_FOCUS_STATUS_MOVING |
+			 ATOMISP_FOCUS_HP_IN_PROGRESS;
+	}
+
+	return 0;
+}
+
+int dw9719_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	return -EINVAL;
+}
+
+int dw9719_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	value = clamp(value, 0, DW9719_MAX_FOCUS_POS);
+	ret = dw9719_i2c_wr16(client, DW9719_VCM_CURRENT, value);
+	if (ret < 0)
+		return ret;
+
+	getnstimeofday(&dw9719_dev.focus_time);
+	dw9719_dev.focus = value;
+
+	return 0;
+}
+
+int dw9719_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+	return dw9719_t_focus_abs(sd, dw9719_dev.focus + value);
+}
+
+int dw9719_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	*value = dw9719_dev.focus;
+	return 0;
+}
+int dw9719_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int dw9719_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	return 0;
+}
+
+int dw9719_vcm_init(struct v4l2_subdev *sd)
+{
+	dw9719_dev.platform_data = camera_get_af_platform_data();
+	return (NULL == dw9719_dev.platform_data) ? -ENODEV : 0;
+}
diff --git a/drivers/staging/media/atomisp/i2c/imx/dw9719.h b/drivers/staging/media/atomisp/i2c/imx/dw9719.h
new file mode 100644
index 000000000000..711f412aef2a
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/dw9719.h
@@ -0,0 +1,58 @@
+/*
+ * Support for dw9719 vcm driver.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __DW9719_H__
+#define __DW9719_H__
+
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/types.h>
+
+#define DW9719_VCM_ADDR	 (0x18 >> 1)
+
+/* dw9719 device structure */
+struct dw9719_device {
+	struct timespec timestamp_t_focus_abs;
+	s16 number_of_steps;
+	bool initialized;		/* true if dw9719 is detected */
+	s32 focus;			/* Current focus value */
+	struct timespec focus_time;	/* Time when focus was last time set */
+	__u8 buffer[4];			/* Used for i2c transactions */
+	const struct camera_af_platform_data *platform_data;
+};
+
+#define DW9719_INVALID_CONFIG	0xffffffff
+#define DW9719_MAX_FOCUS_POS	1023
+#define DELAY_PER_STEP_NS	1000000
+#define DELAY_MAX_PER_STEP_NS	(1000000 * 1023)
+
+#define DW9719_INFO			0
+#define DW9719_ID			0xF1
+#define DW9719_CONTROL			2
+#define DW9719_VCM_CURRENT		3
+
+#define DW9719_MODE			6
+#define DW9719_VCM_FREQ			7
+
+#define DW9719_MODE_SAC3		0x40
+#define DW9719_DEFAULT_VCM_FREQ		0x04
+#define DW9719_ENABLE_RINGING		0x02
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx.c b/drivers/staging/media/atomisp/i2c/imx/imx.c
new file mode 100644
index 000000000000..408a7b945153
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx.c
@@ -0,0 +1,2512 @@
+/*
+ * Support for Sony imx 8MP camera sensor.
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <asm/intel-mid.h>
+#include "../../include/linux/atomisp_platform.h"
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include "../../include/linux/libmsrlisthelper.h"
+#include <linux/mm.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include "imx.h"
+
+/*
+ * The imx135 embedded data info:
+ * embedded data line num: 2
+ * line 0 effective data size(byte): 76
+ * line 1 effective data size(byte): 113
+ */
+static const uint32_t
+	imx135_embedded_effective_size[IMX135_EMBEDDED_DATA_LINE_NUM]
+	=  {76, 113};
+
+static enum atomisp_bayer_order imx_bayer_order_mapping[] = {
+	atomisp_bayer_order_rggb,
+	atomisp_bayer_order_grbg,
+	atomisp_bayer_order_gbrg,
+	atomisp_bayer_order_bggr
+};
+
+static const unsigned int
+IMX227_BRACKETING_LUT_FRAME_ENTRY[IMX_MAX_AE_LUT_LENGTH] = {
+	0x0E10, 0x0E1E, 0x0E2C, 0x0E3A, 0x0E48};
+
+static int
+imx_read_reg(struct i2c_client *client, u16 len, u16 reg, u16 *val)
+{
+	struct i2c_msg msg[2];
+	u16 data[IMX_SHORT_MAX];
+	int ret, i;
+	int retry = 0;
+
+	if (len > IMX_BYTE_MAX) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	do {
+		memset(msg, 0 , sizeof(msg));
+		memset(data, 0 , sizeof(data));
+
+		msg[0].addr = client->addr;
+		msg[0].flags = 0;
+		msg[0].len = I2C_MSG_LENGTH;
+		msg[0].buf = (u8 *)data;
+		/* high byte goes first */
+		data[0] = cpu_to_be16(reg);
+
+		msg[1].addr = client->addr;
+		msg[1].len = len;
+		msg[1].flags = I2C_M_RD;
+		msg[1].buf = (u8 *)data;
+
+		ret = i2c_transfer(client->adapter, msg, 2);
+		if (ret != 2) {
+			dev_err(&client->dev,
+			  "retrying i2c read from offset 0x%x error %d... %d\n",
+			  reg, ret, retry);
+			msleep(20);
+		}
+	} while (ret != 2 && retry++ < I2C_RETRY_COUNT);
+
+	if (ret != 2)
+		return -EIO;
+
+	/* high byte comes first */
+	if (len == IMX_8BIT) {
+		*val = (u8)data[0];
+	} else {
+		/* 16-bit access is default when len > 1 */
+		for (i = 0; i < (len >> 1); i++)
+			val[i] = be16_to_cpu(data[i]);
+	}
+
+	return 0;
+}
+
+static int imx_i2c_write(struct i2c_client *client, u16 len, u8 *data)
+{
+	struct i2c_msg msg;
+	int ret;
+	int retry = 0;
+
+	do {
+		msg.addr = client->addr;
+		msg.flags = 0;
+		msg.len = len;
+		msg.buf = data;
+
+		ret = i2c_transfer(client->adapter, &msg, 1);
+		if (ret != 1) {
+			dev_err(&client->dev,
+				"retrying i2c write transfer... %d\n", retry);
+				msleep(20);
+		}
+	} while (ret != 1 && retry++ < I2C_RETRY_COUNT);
+
+	return ret == 1 ? 0 : -EIO;
+}
+
+int
+imx_write_reg(struct i2c_client *client, u16 data_length, u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	u16 *wreg = (u16 *)data;
+	const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
+
+	if (data_length != IMX_8BIT && data_length != IMX_16BIT) {
+		v4l2_err(client, "%s error, invalid data_length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == IMX_8BIT)
+		data[2] = (u8)(val);
+	else {
+		/* IMX_16BIT */
+		u16 *wdata = (u16 *)&data[2];
+		*wdata = cpu_to_be16(val);
+	}
+
+	ret = imx_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;
+}
+
+/*
+ * imx_write_reg_array - Initializes a list of imx 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().
+ *
+ * __imx_flush_reg_array, __imx_buf_reg_array() and
+ * __imx_write_reg_is_consecutive() are internal functions to
+ * imx_write_reg_array_fast() and should be not used anywhere else.
+ *
+ */
+
+static int __imx_flush_reg_array(struct i2c_client *client,
+				     struct imx_write_ctrl *ctrl)
+{
+	u16 size;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	ctrl->buffer.addr = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return imx_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __imx_buf_reg_array(struct i2c_client *client,
+				   struct imx_write_ctrl *ctrl,
+				   const struct imx_reg *next)
+{
+	int size;
+	u16 *data16;
+
+	switch (next->type) {
+	case IMX_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case IMX_16BIT:
+		size = 2;
+		data16 = (u16 *)&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->sreg;
+
+	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) >= IMX_MAX_WRITE_BUF_SIZE)
+		return __imx_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int
+__imx_write_reg_is_consecutive(struct i2c_client *client,
+				   struct imx_write_ctrl *ctrl,
+				   const struct imx_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->sreg;
+}
+
+static int imx_write_reg_array(struct i2c_client *client,
+				   const struct imx_reg *reglist)
+{
+	const struct imx_reg *next = reglist;
+	struct imx_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != IMX_TOK_TERM; next++) {
+		switch (next->type & IMX_TOK_MASK) {
+		case IMX_TOK_DELAY:
+			err = __imx_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 (!__imx_write_reg_is_consecutive(client, &ctrl,
+								next)) {
+				err = __imx_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __imx_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				v4l2_err(client, "%s: write error, aborted\n",
+					 __func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __imx_flush_reg_array(client, &ctrl);
+}
+
+static int __imx_min_fps_diff(int fps, const struct imx_fps_setting *fps_list)
+{
+	int diff = INT_MAX;
+	int i;
+
+	if (fps == 0)
+		return 0;
+
+	for (i = 0; i < MAX_FPS_OPTIONS_SUPPORTED; i++) {
+		if (!fps_list[i].fps)
+			break;
+		if (abs(fps_list[i].fps - fps) < diff)
+			diff = abs(fps_list[i].fps - fps);
+	}
+
+	return diff;
+}
+
+static int __imx_nearest_fps_index(int fps,
+					const struct imx_fps_setting *fps_list)
+{
+	int fps_index = 0;
+	int i;
+
+	for (i = 0; i < MAX_FPS_OPTIONS_SUPPORTED; i++) {
+		if (!fps_list[i].fps)
+			break;
+		if (abs(fps_list[i].fps - fps)
+		    < abs(fps_list[fps_index].fps - fps))
+			fps_index = i;
+	}
+	return fps_index;
+}
+
+/*
+ * This is to choose the nearest fps setting above the requested fps
+ * fps_list should be in ascendant order.
+ */
+static int __imx_above_nearest_fps_index(int fps,
+					const struct imx_fps_setting *fps_list)
+{
+	int fps_index = 0;
+	int i;
+
+	for (i = 0; i < MAX_FPS_OPTIONS_SUPPORTED; i++) {
+		if (!fps_list[i].fps)
+			break;
+		if (fps <= fps_list[i].fps) {
+			fps_index = i;
+			break;
+		}
+	}
+
+	return fps_index;
+}
+
+static int imx_get_lanes(struct v4l2_subdev *sd)
+{
+	struct camera_mipi_info *imx_info = v4l2_get_subdev_hostdata(sd);
+
+	if (!imx_info)
+		return -ENOSYS;
+	if (imx_info->num_lanes < 1 || imx_info->num_lanes > 4 ||
+	    imx_info->num_lanes == 3)
+		return -EINVAL;
+
+	return imx_info->num_lanes;
+}
+
+static int __imx_update_exposure_timing(struct i2c_client *client, u16 exposure,
+			u16 llp, u16 fll)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret = 0;
+
+	if (dev->sensor_id != IMX227_ID) {
+		/* Increase the VTS to match exposure + margin */
+		if (exposure > fll - IMX_INTEGRATION_TIME_MARGIN)
+			fll = exposure + IMX_INTEGRATION_TIME_MARGIN;
+	}
+
+	ret = imx_write_reg(client, IMX_16BIT,
+		dev->reg_addr->line_length_pixels, llp);
+	if (ret)
+		return ret;
+
+	ret = imx_write_reg(client, IMX_16BIT,
+		dev->reg_addr->frame_length_lines, fll);
+	if (ret)
+		return ret;
+
+	if (exposure)
+		ret = imx_write_reg(client, IMX_16BIT,
+			dev->reg_addr->coarse_integration_time, exposure);
+
+	return ret;
+}
+
+static int __imx_update_gain(struct v4l2_subdev *sd, u16 gain)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	/* set global gain */
+	ret = imx_write_reg(client, IMX_8BIT, dev->reg_addr->global_gain, gain);
+	if (ret)
+		return ret;
+
+	/* set short analog gain */
+	if (dev->sensor_id == IMX135_ID)
+		ret = imx_write_reg(client, IMX_8BIT, IMX_SHORT_AGC_GAIN, gain);
+
+	return ret;
+}
+
+static int __imx_update_digital_gain(struct i2c_client *client, u16 digitgain)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct imx_write_buffer digit_gain;
+
+	digit_gain.addr = cpu_to_be16(dev->reg_addr->dgc_adj);
+	digit_gain.data[0] = (digitgain >> 8) & 0xFF;
+	digit_gain.data[1] = digitgain & 0xFF;
+
+	if (dev->sensor_id == IMX219_ID) {
+		return imx_i2c_write(client, IMX219_DGC_LEN, (u8 *)&digit_gain);
+	} else if (dev->sensor_id == IMX227_ID) {
+		return imx_i2c_write(client, IMX227_DGC_LEN, (u8 *)&digit_gain);
+	} else {
+		digit_gain.data[2] = (digitgain >> 8) & 0xFF;
+		digit_gain.data[3] = digitgain & 0xFF;
+		digit_gain.data[4] = (digitgain >> 8) & 0xFF;
+		digit_gain.data[5] = digitgain & 0xFF;
+		digit_gain.data[6] = (digitgain >> 8) & 0xFF;
+		digit_gain.data[7] = digitgain & 0xFF;
+		return imx_i2c_write(client, IMX_DGC_LEN, (u8 *)&digit_gain);
+	}
+	return 0;
+}
+
+static int imx_set_exposure_gain(struct v4l2_subdev *sd, u16 coarse_itg,
+	u16 gain, u16 digitgain)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int lanes = imx_get_lanes(sd);
+	unsigned int digitgain_scaled;
+	int ret = 0;
+
+	/* Validate exposure:  cannot exceed VTS-4 where VTS is 16bit */
+	coarse_itg = clamp_t(u16, coarse_itg, 0, IMX_MAX_EXPOSURE_SUPPORTED);
+
+	/* Validate gain: must not exceed maximum 8bit value */
+	gain = clamp_t(u16, gain, 0, IMX_MAX_GLOBAL_GAIN_SUPPORTED);
+
+	mutex_lock(&dev->input_lock);
+
+	if (dev->sensor_id == IMX227_ID) {
+		ret = imx_write_reg_array(client, imx_param_hold);
+		if (ret) {
+			mutex_unlock(&dev->input_lock);
+			return ret;
+		}
+	}
+
+	/* For imx175, setting gain must be delayed by one */
+	if ((dev->sensor_id == IMX175_ID) && dev->digital_gain)
+		digitgain_scaled = dev->digital_gain;
+	else
+		digitgain_scaled = digitgain;
+	/* imx132 with two lanes needs more gain to saturate at max */
+	if (dev->sensor_id == IMX132_ID && lanes > 1) {
+		digitgain_scaled *= IMX132_2LANES_GAINFACT;
+		digitgain_scaled >>= IMX132_2LANES_GAINFACT_SHIFT;
+	}
+	/* Validate digital gain: must not exceed 12 bit value*/
+	digitgain_scaled = clamp_t(unsigned int, digitgain_scaled,
+				   0, IMX_MAX_DIGITAL_GAIN_SUPPORTED);
+
+	ret = __imx_update_exposure_timing(client, coarse_itg,
+			dev->pixels_per_line, dev->lines_per_frame);
+	if (ret)
+		goto out;
+	dev->coarse_itg = coarse_itg;
+
+	if (dev->sensor_id == IMX175_ID)
+		ret = __imx_update_gain(sd, dev->gain);
+	else
+		ret = __imx_update_gain(sd, gain);
+	if (ret)
+		goto out;
+	dev->gain = gain;
+
+	ret = __imx_update_digital_gain(client, digitgain_scaled);
+	if (ret)
+		goto out;
+	dev->digital_gain = digitgain;
+
+out:
+	if (dev->sensor_id == IMX227_ID)
+		ret = imx_write_reg_array(client, imx_param_update);
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static long imx_s_exposure(struct v4l2_subdev *sd,
+			       struct atomisp_exposure *exposure)
+{
+	return imx_set_exposure_gain(sd, exposure->integration_time[0],
+				exposure->gain[0], exposure->gain[1]);
+}
+
+/* FIXME -To be updated with real OTP reading */
+static int imx_g_priv_int_data(struct v4l2_subdev *sd,
+				   struct v4l2_private_int_data *priv)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_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 > dev->otp_driver->size)
+		read_size = dev->otp_driver->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;
+	}
+out:
+	/* Return correct size */
+	priv->size = dev->otp_driver->size;
+
+	return 0;
+}
+
+static int __imx_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+	int lanes = imx_get_lanes(sd);
+	int ret;
+
+	if (dev->sensor_id == IMX_ID_DEFAULT)
+		return 0;
+
+	/* The default is no flip at sensor initialization */
+	dev->h_flip->cur.val = 0;
+	dev->v_flip->cur.val = 0;
+	/* Sets the default FPS */
+	dev->fps_index = 0;
+	dev->curr_res_table = dev->mode_tables->res_preview;
+	dev->entries_curr_table = dev->mode_tables->n_res_preview;
+
+	ret = imx_write_reg_array(client, dev->mode_tables->init_settings);
+	if (ret)
+		return ret;
+
+	if (dev->sensor_id == IMX132_ID && lanes > 0) {
+		static const u8 imx132_rglanesel[] = {
+			IMX132_RGLANESEL_1LANE,		/* 1 lane */
+			IMX132_RGLANESEL_2LANES,	/* 2 lanes */
+			IMX132_RGLANESEL_1LANE,		/* undefined */
+			IMX132_RGLANESEL_4LANES,	/* 4 lanes */
+		};
+		ret = imx_write_reg(client, IMX_8BIT,
+				IMX132_RGLANESEL, imx132_rglanesel[lanes - 1]);
+	}
+
+	return ret;
+}
+
+static int imx_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret = 0;
+
+	mutex_lock(&dev->input_lock);
+	ret = __imx_init(sd, val);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static long imx_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return imx_s_exposure(sd, arg);
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+		return imx_g_priv_int_data(sd, arg);
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret;
+
+       /* power control */
+	ret = dev->platform_data->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 = dev->platform_data->gpio_ctrl(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "gpio failed\n");
+		goto fail_gpio;
+	}
+
+	return 0;
+fail_gpio:
+	dev->platform_data->gpio_ctrl(sd, 0);
+fail_clk:
+	dev->platform_data->flisclk_ctrl(sd, 0);
+fail_power:
+	dev->platform_data->power_ctrl(sd, 0);
+	dev_err(&client->dev, "sensor power-up failed\n");
+
+	return ret;
+}
+
+static int power_down(struct v4l2_subdev *sd)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk failed\n");
+
+	/* gpio ctrl */
+	ret = dev->platform_data->gpio_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "gpio failed\n");
+
+	/* power control */
+	ret = dev->platform_data->power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "vprog failed.\n");
+
+	return ret;
+}
+
+static int __imx_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret = 0;
+	int r = 0;
+
+	if (on == 0) {
+		ret = power_down(sd);
+		if (dev->vcm_driver && dev->vcm_driver->power_down)
+			r = dev->vcm_driver->power_down(sd);
+		if (ret == 0)
+			ret = r;
+		dev->power = 0;
+	} else {
+		if (dev->vcm_driver && dev->vcm_driver->power_up)
+			ret = dev->vcm_driver->power_up(sd);
+		if (ret)
+			return ret;
+		ret = power_up(sd);
+		if (!ret) {
+			dev->power = 1;
+			return __imx_init(sd, 0);
+		}
+	}
+
+	return ret;
+}
+
+static int imx_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	ret = __imx_s_power(sd, on);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int imx_get_intg_factor(struct i2c_client *client,
+				struct camera_mipi_info *info,
+				const struct imx_reg *reglist)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx_device *dev = to_imx_sensor(sd);
+	int lanes = imx_get_lanes(sd);
+	u32 vt_pix_clk_div;
+	u32 vt_sys_clk_div;
+	u32 pre_pll_clk_div;
+	u32 pll_multiplier;
+
+	const int ext_clk_freq_hz = 19200000;
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	int ret;
+	u16 data[IMX_INTG_BUF_COUNT];
+
+	u32 vt_pix_clk_freq_mhz;
+	u32 coarse_integration_time_min;
+	u32 coarse_integration_time_max_margin;
+	u32 read_mode;
+	u32 div;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	memset(data, 0, IMX_INTG_BUF_COUNT * sizeof(u16));
+	ret = imx_read_reg(client, 1, IMX_VT_PIX_CLK_DIV, data);
+	if (ret)
+		return ret;
+	vt_pix_clk_div = data[0] & IMX_MASK_5BIT;
+
+	if (dev->sensor_id == IMX132_ID || dev->sensor_id == IMX208_ID) {
+		static const int rgpltd[] = { 2, 4, 1, 1 };
+		ret = imx_read_reg(client, 1, IMX132_208_VT_RGPLTD, data);
+		if (ret)
+			return ret;
+		vt_sys_clk_div = rgpltd[data[0] & IMX_MASK_2BIT];
+	} else {
+		ret = imx_read_reg(client, 1, IMX_VT_SYS_CLK_DIV, data);
+		if (ret)
+			return ret;
+		vt_sys_clk_div = data[0] & IMX_MASK_2BIT;
+	}
+	ret = imx_read_reg(client, 1, IMX_PRE_PLL_CLK_DIV, data);
+	if (ret)
+		return ret;
+	pre_pll_clk_div = data[0] & IMX_MASK_4BIT;
+
+	ret = imx_read_reg(client, 2,
+		(dev->sensor_id == IMX132_ID ||
+		 dev->sensor_id == IMX219_ID ||
+		 dev->sensor_id == IMX208_ID) ?
+		IMX132_208_219_PLL_MULTIPLIER : IMX_PLL_MULTIPLIER, data);
+	if (ret)
+		return ret;
+	pll_multiplier = data[0] & IMX_MASK_11BIT;
+
+	memset(data, 0, IMX_INTG_BUF_COUNT * sizeof(u16));
+	ret = imx_read_reg(client, 4, IMX_COARSE_INTG_TIME_MIN, data);
+	if (ret)
+		return ret;
+	coarse_integration_time_min = data[0];
+	coarse_integration_time_max_margin = data[1];
+
+	/* Get the cropping and output resolution to ISP for this mode. */
+	ret =  imx_read_reg(client, 2, dev->reg_addr->horizontal_start_h, data);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = data[0];
+
+	ret = imx_read_reg(client, 2, dev->reg_addr->vertical_start_h, data);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = data[0];
+
+	ret = imx_read_reg(client, 2, dev->reg_addr->horizontal_end_h, data);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = data[0];
+
+	ret = imx_read_reg(client, 2, dev->reg_addr->vertical_end_h, data);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = data[0];
+
+	ret = imx_read_reg(client, 2,
+		dev->reg_addr->horizontal_output_size_h, data);
+	if (ret)
+		return ret;
+	buf->output_width = data[0];
+
+	ret = imx_read_reg(client, 2,
+		dev->reg_addr->vertical_output_size_h, data);
+	if (ret)
+		return ret;
+	buf->output_height = data[0];
+
+	memset(data, 0, IMX_INTG_BUF_COUNT * sizeof(u16));
+	if (dev->sensor_id == IMX132_ID || dev->sensor_id == IMX208_ID ||
+		dev->sensor_id == IMX219_ID)
+		read_mode = 0;
+	else {
+		if (dev->sensor_id == IMX227_ID)
+			ret = imx_read_reg(client, 1, IMX227_READ_MODE, data);
+		else
+			ret = imx_read_reg(client, 1, IMX_READ_MODE, data);
+
+		if (ret)
+			return ret;
+		read_mode = data[0] & IMX_MASK_2BIT;
+	}
+
+	div = pre_pll_clk_div*vt_sys_clk_div*vt_pix_clk_div;
+	if (div == 0)
+		return -EINVAL;
+
+	if (dev->sensor_id == IMX132_ID || dev->sensor_id == IMX208_ID)
+		vt_pix_clk_freq_mhz = ext_clk_freq_hz / div;
+	else if (dev->sensor_id == IMX227_ID) {
+		/* according to IMX227 datasheet:
+		 * vt_pix_freq_mhz = * num_of_vt_lanes(4) * ivt_pix_clk_freq_mhz
+		 */
+		vt_pix_clk_freq_mhz =
+			(u64)4 * ext_clk_freq_hz * pll_multiplier;
+		do_div(vt_pix_clk_freq_mhz, div);
+	} else
+		vt_pix_clk_freq_mhz = 2 * ext_clk_freq_hz / div;
+
+	vt_pix_clk_freq_mhz *= pll_multiplier;
+	if (dev->sensor_id == IMX132_ID && lanes > 0)
+		vt_pix_clk_freq_mhz *= lanes;
+
+	dev->vt_pix_clk_freq_mhz = vt_pix_clk_freq_mhz;
+
+	buf->vt_pix_clk_freq_mhz = vt_pix_clk_freq_mhz;
+	buf->coarse_integration_time_min = coarse_integration_time_min;
+	buf->coarse_integration_time_max_margin =
+				coarse_integration_time_max_margin;
+
+	buf->fine_integration_time_min = IMX_FINE_INTG_TIME;
+	buf->fine_integration_time_max_margin = IMX_FINE_INTG_TIME;
+	buf->fine_integration_time_def = IMX_FINE_INTG_TIME;
+	buf->frame_length_lines = dev->lines_per_frame;
+	buf->line_length_pck = dev->pixels_per_line;
+	buf->read_mode = read_mode;
+
+	if (dev->sensor_id == IMX132_ID || dev->sensor_id == IMX208_ID ||
+		dev->sensor_id == IMX219_ID) {
+		buf->binning_factor_x = 1;
+		buf->binning_factor_y = 1;
+	} else {
+		if (dev->sensor_id == IMX227_ID)
+			ret = imx_read_reg(client, 1, IMX227_BINNING_ENABLE,
+				data);
+		else
+			ret = imx_read_reg(client, 1, IMX_BINNING_ENABLE, data);
+
+		if (ret)
+			return ret;
+		/* 1:binning enabled, 0:disabled */
+		if (data[0] == 1) {
+			if (dev->sensor_id == IMX227_ID)
+				ret = imx_read_reg(client, 1,
+					IMX227_BINNING_TYPE, data);
+			else
+				ret = imx_read_reg(client, 1,
+					IMX_BINNING_TYPE, data);
+
+			if (ret)
+				return ret;
+			buf->binning_factor_x = data[0] >> 4 & 0x0f;
+			if (!buf->binning_factor_x)
+				buf->binning_factor_x = 1;
+			buf->binning_factor_y = data[0] & 0xf;
+			if (!buf->binning_factor_y)
+				buf->binning_factor_y = 1;
+			/* WOWRKAROUND, NHD setting for IMX227 should have 4x4
+			 * binning but the register setting does not reflect
+			 * this, I am asking vendor why this happens. this is
+			 * workaround for INTEL BZ 216560.
+			 */
+			if (dev->sensor_id == IMX227_ID) {
+				if (dev->curr_res_table[dev->fmt_idx].width ==
+					376 &&
+				    dev->curr_res_table[dev->fmt_idx].height ==
+					656) {
+					buf->binning_factor_x = 4;
+					buf->binning_factor_y = 4;
+				}
+			}
+		} else {
+			buf->binning_factor_x = 1;
+			buf->binning_factor_y = 1;
+		}
+	}
+
+	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 imx_q_exposure(struct v4l2_subdev *sd, s32 *value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+	u16 coarse;
+	int ret;
+
+	/* the fine integration time is currently not calculated */
+	ret = imx_read_reg(client, IMX_16BIT,
+		dev->reg_addr->coarse_integration_time, &coarse);
+	*value = coarse;
+
+	return ret;
+}
+
+static int imx_test_pattern(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret;
+
+	if (dev->power == 0)
+		return 0;
+
+	ret = imx_write_reg(client, IMX_16BIT, IMX_TEST_PATTERN_COLOR_R,
+		(u16)(dev->tp_r->val >> 22));
+	if (ret)
+		return ret;
+
+	ret = imx_write_reg(client, IMX_16BIT, IMX_TEST_PATTERN_COLOR_GR,
+		(u16)(dev->tp_gr->val >> 22));
+	if (ret)
+		return ret;
+
+	ret = imx_write_reg(client, IMX_16BIT, IMX_TEST_PATTERN_COLOR_GB,
+		(u16)(dev->tp_gb->val >> 22));
+	if (ret)
+		return ret;
+
+	ret = imx_write_reg(client, IMX_16BIT, IMX_TEST_PATTERN_COLOR_B,
+		(u16)(dev->tp_b->val >> 22));
+	if (ret)
+		return ret;
+
+	return imx_write_reg(client, IMX_16BIT, IMX_TEST_PATTERN_MODE,
+		(u16)(dev->tp_mode->val));
+}
+
+static u32 imx_translate_bayer_order(enum atomisp_bayer_order code)
+{
+	switch (code) {
+	case atomisp_bayer_order_rggb:
+		return MEDIA_BUS_FMT_SRGGB10_1X10;
+	case atomisp_bayer_order_grbg:
+		return MEDIA_BUS_FMT_SGRBG10_1X10;
+	case atomisp_bayer_order_bggr:
+		return MEDIA_BUS_FMT_SBGGR10_1X10;
+	case atomisp_bayer_order_gbrg:
+		return MEDIA_BUS_FMT_SGBRG10_1X10;
+	}
+	return 0;
+}
+
+static int imx_v_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct camera_mipi_info *imx_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u16 val;
+
+	if (dev->power == 0)
+		return -EIO;
+
+	ret = imx_write_reg_array(client, dev->param_hold);
+	if (ret)
+		return ret;
+
+	ret = imx_read_reg(client, IMX_8BIT,
+		dev->reg_addr->img_orientation, &val);
+	if (ret)
+		return ret;
+	if (value)
+		val |= IMX_VFLIP_BIT;
+	else
+		val &= ~IMX_VFLIP_BIT;
+
+	ret = imx_write_reg(client, IMX_8BIT,
+		dev->reg_addr->img_orientation, val);
+	if (ret)
+		return ret;
+
+	imx_info = v4l2_get_subdev_hostdata(sd);
+	if (imx_info) {
+		val &= (IMX_VFLIP_BIT|IMX_HFLIP_BIT);
+		imx_info->raw_bayer_order = imx_bayer_order_mapping[val];
+		dev->format.code = imx_translate_bayer_order(
+			imx_info->raw_bayer_order);
+	}
+
+	return imx_write_reg_array(client, dev->param_update);
+}
+
+static int imx_h_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct camera_mipi_info *imx_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u16 val;
+
+	if (dev->power == 0)
+		return -EIO;
+
+	ret = imx_write_reg_array(client, dev->param_hold);
+	if (ret)
+		return ret;
+	ret = imx_read_reg(client, IMX_8BIT,
+		dev->reg_addr->img_orientation, &val);
+	if (ret)
+		return ret;
+	if (value)
+		val |= IMX_HFLIP_BIT;
+	else
+		val &= ~IMX_HFLIP_BIT;
+	ret = imx_write_reg(client, IMX_8BIT,
+		dev->reg_addr->img_orientation, val);
+	if (ret)
+		return ret;
+
+	imx_info = v4l2_get_subdev_hostdata(sd);
+	if (imx_info) {
+		val &= (IMX_VFLIP_BIT|IMX_HFLIP_BIT);
+		imx_info->raw_bayer_order = imx_bayer_order_mapping[val];
+		dev->format.code = imx_translate_bayer_order(
+		imx_info->raw_bayer_order);
+	}
+
+	return imx_write_reg_array(client, dev->param_update);
+}
+
+static int imx_g_focal(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (IMX_FOCAL_LENGTH_NUM << 16) | IMX_FOCAL_LENGTH_DEM;
+	return 0;
+}
+
+static int imx_g_fnumber(struct v4l2_subdev *sd, s32 *val)
+{
+	/*const f number for imx*/
+	*val = (IMX_F_NUMBER_DEFAULT_NUM << 16) | IMX_F_NUMBER_DEM;
+	return 0;
+}
+
+static int imx_g_fnumber_range(struct v4l2_subdev *sd, s32 *val)
+{
+	*val = (IMX_F_NUMBER_DEFAULT_NUM << 24) |
+		(IMX_F_NUMBER_DEM << 16) |
+		(IMX_F_NUMBER_DEFAULT_NUM << 8) | IMX_F_NUMBER_DEM;
+	return 0;
+}
+
+static int imx_g_bin_factor_x(struct v4l2_subdev *sd, s32 *val)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	*val = dev->curr_res_table[dev->fmt_idx].bin_factor_x;
+
+	return 0;
+}
+
+static int imx_g_bin_factor_y(struct v4l2_subdev *sd, s32 *val)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	*val = dev->curr_res_table[dev->fmt_idx].bin_factor_y;
+
+	return 0;
+}
+
+int imx_vcm_power_up(struct v4l2_subdev *sd)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->power_up)
+		return dev->vcm_driver->power_up(sd);
+	return 0;
+}
+
+int imx_vcm_power_down(struct v4l2_subdev *sd)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->power_down)
+		return dev->vcm_driver->power_down(sd);
+	return 0;
+}
+
+int imx_vcm_init(struct v4l2_subdev *sd)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->init)
+		return dev->vcm_driver->init(sd);
+	return 0;
+}
+
+int imx_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->t_focus_vcm)
+		return dev->vcm_driver->t_focus_vcm(sd, val);
+	return 0;
+}
+
+int imx_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->t_focus_abs)
+		return dev->vcm_driver->t_focus_abs(sd, value);
+	return 0;
+}
+int imx_t_focus_rel(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->t_focus_rel)
+		return dev->vcm_driver->t_focus_rel(sd, value);
+	return 0;
+}
+
+int imx_q_focus_status(struct v4l2_subdev *sd, s32 *value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->q_focus_status)
+		return dev->vcm_driver->q_focus_status(sd, value);
+	return 0;
+}
+
+int imx_q_focus_abs(struct v4l2_subdev *sd, s32 *value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->q_focus_abs)
+		return dev->vcm_driver->q_focus_abs(sd, value);
+	return 0;
+}
+
+int imx_t_vcm_slew(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->t_vcm_slew)
+		return dev->vcm_driver->t_vcm_slew(sd, value);
+	return 0;
+}
+
+int imx_t_vcm_timing(struct v4l2_subdev *sd, s32 value)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (dev->vcm_driver && dev->vcm_driver->t_vcm_timing)
+		return dev->vcm_driver->t_vcm_timing(sd, value);
+	return 0;
+}
+
+static int imx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx_device *dev = container_of(
+		ctrl->handler, struct imx_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int ret = 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx_test_pattern(&dev->sd);
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&client->dev, "%s: CID_VFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = imx_v_flip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&client->dev, "%s: CID_HFLIP:%d.\n",
+			__func__, ctrl->val);
+		ret = imx_h_flip(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		ret = imx_t_focus_abs(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_RELATIVE:
+		ret = imx_t_focus_rel(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_VCM_SLEW:
+		ret = imx_t_vcm_slew(&dev->sd, ctrl->val);
+		break;
+	case V4L2_CID_VCM_TIMEING:
+		ret = imx_t_vcm_timing(&dev->sd, ctrl->val);
+		break;
+	}
+
+	return ret;
+}
+
+static int imx_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx_device *dev = container_of(
+		ctrl->handler, struct imx_device, ctrl_handler);
+	int ret = 0;
+	unsigned int val;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ret = imx_q_exposure(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		ret = imx_q_focus_abs(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCUS_STATUS:
+		ret = imx_q_focus_status(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FOCAL_ABSOLUTE:
+		ret = imx_g_focal(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_ABSOLUTE:
+		ret = imx_g_fnumber(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_FNUMBER_RANGE:
+		ret = imx_g_fnumber_range(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		ret = imx_g_bin_factor_x(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_BIN_FACTOR_VERT:
+		ret = imx_g_bin_factor_y(&dev->sd, &ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ctrl->val = dev->lines_per_frame -
+			dev->curr_res_table[dev->fmt_idx].height;
+		break;
+	case V4L2_CID_HBLANK:
+		ctrl->val = dev->pixels_per_line -
+			dev->curr_res_table[dev->fmt_idx].width;
+		break;
+	case V4L2_CID_PIXEL_RATE:
+		ctrl->val = dev->vt_pix_clk_freq_mhz;
+		break;
+	case V4L2_CID_LINK_FREQ:
+		val = dev->curr_res_table[dev->fmt_idx].
+					fps_options[dev->fps_index].mipi_freq;
+		if (val == 0)
+			val = dev->curr_res_table[dev->fmt_idx].mipi_freq;
+		if (val == 0)
+			return -EINVAL;
+		ctrl->val = val * 1000;			/* To Hz */
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = imx_s_ctrl,
+	.g_volatile_ctrl = imx_g_volatile_ctrl
+};
+
+static const struct v4l2_ctrl_config imx_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 = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_TEST_PATTERN,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test pattern",
+		.min = 0,
+		.max = 0xffff,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_TEST_PATTERN_COLOR_R,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test pattern solid color R",
+		.min = INT_MIN,
+		.max = INT_MAX,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_TEST_PATTERN_COLOR_GR,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test pattern solid color GR",
+		.min = INT_MIN,
+		.max = INT_MAX,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_TEST_PATTERN_COLOR_GB,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test pattern solid color GB",
+		.min = INT_MIN,
+		.max = INT_MAX,
+		.step = 1,
+		.def = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_TEST_PATTERN_COLOR_B,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Test pattern solid color B",
+		.min = INT_MIN,
+		.max = INT_MAX,
+		.step = 1,
+		.def = 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_FOCUS_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focus move absolute",
+		.min = 0,
+		.max = IMX_MAX_FOCUS_POS,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_RELATIVE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focus move relative",
+		.min = IMX_MAX_FOCUS_NEG,
+		.max = IMX_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 = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VCM_SLEW,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vcm slew",
+		.min = 0,
+		.max = IMX_VCM_SLEW_STEP_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VCM_TIMEING,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vcm step time",
+		.min = 0,
+		.max = IMX_VCM_SLEW_TIME_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "focal length",
+		.min = IMX_FOCAL_LENGTH_DEFAULT,
+		.max = IMX_FOCAL_LENGTH_DEFAULT,
+		.step = 0x01,
+		.def = IMX_FOCAL_LENGTH_DEFAULT,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number",
+		.min = IMX_F_NUMBER_DEFAULT,
+		.max = IMX_F_NUMBER_DEFAULT,
+		.step = 0x01,
+		.def = IMX_F_NUMBER_DEFAULT,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "f-number range",
+		.min = IMX_F_NUMBER_RANGE,
+		.max =  IMX_F_NUMBER_RANGE,
+		.step = 0x01,
+		.def = IMX_F_NUMBER_RANGE,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "horizontal binning factor",
+		.min = 0,
+		.max = IMX_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "vertical binning factor",
+		.min = 0,
+		.max = IMX_BIN_FACTOR_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.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,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_PIXEL_RATE,
+		.name = "Pixel Rate",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = INT_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HBLANK,
+		.name = "Horizontal Blanking",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = SHRT_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VBLANK,
+		.name = "Vertical Blanking",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = SHRT_MAX,
+		.step = 1,
+		.def = 0,
+		.flags = V4L2_CTRL_FLAG_VOLATILE,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HFLIP,
+		.name = "Horizontal Flip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VFLIP,
+		.name = "Vertical Flip",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = 0,
+		.max = 1,
+		.step = 1,
+		.def = 0,
+		.flags = 0,
+	},
+};
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 600
+static int distance(struct imx_resolution const *res, u32 w, u32 h,
+		bool keep_ratio)
+{
+	unsigned int w_ratio;
+	unsigned int h_ratio;
+	int match;
+	unsigned int allowed_ratio_mismatch = LARGEST_ALLOWED_RATIO_MISMATCH;
+
+	if (!keep_ratio)
+		allowed_ratio_mismatch = ~0;
+
+	if (w == 0)
+		return -1;
+	w_ratio = (res->width << 13) / w;
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - ((int)8192));
+
+	if ((w_ratio < (int)8192) || (h_ratio < (int)8192)  ||
+		(match > allowed_ratio_mismatch))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(struct v4l2_subdev *sd, int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int fps_diff;
+	int min_fps_diff = INT_MAX;
+	int min_dist = INT_MAX;
+	const struct imx_resolution *tmp_res = NULL;
+	struct imx_device *dev = to_imx_sensor(sd);
+	bool again = 1;
+retry:
+	for (i = 0; i < dev->entries_curr_table; i++) {
+		tmp_res = &dev->curr_res_table[i];
+		dist = distance(tmp_res, w, h, again);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+		if (dist == min_dist) {
+			fps_diff = __imx_min_fps_diff(dev->targetfps,
+						tmp_res->fps_options);
+			if (fps_diff < min_fps_diff) {
+				min_fps_diff = fps_diff;
+				idx = i;
+			}
+		}
+	}
+
+	/*
+	 * FIXME!
+	 * only IMX135 for Saltbay and IMX227 use this algorithm
+	 */
+	if (idx == -1 && again == true && dev->new_res_sel_method) {
+		again = false;
+		goto retry;
+	}
+	return idx;
+}
+
+/* Call with ctrl_handler.lock hold */
+static int __adjust_hvblank(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+	u16 new_frame_length_lines, new_line_length_pck;
+	int ret;
+
+	/*
+	 * No need to adjust h/v blank if not set dbg value
+	 * Note that there is no other checking on the h/v blank value,
+	 * as h/v blank can be set to any value above zero for debug purpose
+	 */
+	if (!dev->v_blank->val || !dev->h_blank->val)
+		return 0;
+
+	new_frame_length_lines = dev->curr_res_table[dev->fmt_idx].height +
+		dev->v_blank->val;
+	new_line_length_pck = dev->curr_res_table[dev->fmt_idx].width +
+		dev->h_blank->val;
+
+	ret = imx_write_reg(client, IMX_16BIT,
+		dev->reg_addr->line_length_pixels, new_line_length_pck);
+	if (ret)
+		return ret;
+	ret = imx_write_reg(client, IMX_16BIT,
+		dev->reg_addr->frame_length_lines, new_frame_length_lines);
+	if (ret)
+		return ret;
+
+	dev->lines_per_frame = new_frame_length_lines;
+	dev->pixels_per_line = new_line_length_pck;
+
+	return 0;
+}
+
+static int imx_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct camera_mipi_info *imx_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct imx_resolution *res;
+	int lanes = imx_get_lanes(sd);
+	int ret;
+	u16 data, val;
+	 int idx;
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	imx_info = v4l2_get_subdev_hostdata(sd);
+	if (imx_info == NULL)
+		return -EINVAL;
+	if ((fmt->width > imx_max_res[dev->sensor_id].res_max_width)
+		|| (fmt->height > imx_max_res[dev->sensor_id].res_max_height)) {
+		fmt->width =  imx_max_res[dev->sensor_id].res_max_width;
+		fmt->height = imx_max_res[dev->sensor_id].res_max_height;
+	} else {
+		idx = nearest_resolution_index(sd, fmt->width, fmt->height);
+
+		/*
+		 * nearest_resolution_index() doesn't return smaller
+		 *  resolutions. If it fails, it means the requested
+		 *  resolution is higher than wecan support. Fallback
+		 *  to highest possible resolution in this case.
+		 */
+		if (idx == -1)
+			idx = dev->entries_curr_table - 1;
+
+		fmt->width = dev->curr_res_table[idx].width;
+		fmt->height = dev->curr_res_table[idx].height;
+	}
+
+	fmt->code = dev->format.code;
+    if(format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *fmt;
+		return 0;
+	}
+	mutex_lock(&dev->input_lock);
+
+	dev->fmt_idx = nearest_resolution_index(sd, fmt->width, fmt->height);
+	if (dev->fmt_idx == -1) {
+		ret = -EINVAL;
+		goto out;
+	}
+	res = &dev->curr_res_table[dev->fmt_idx];
+
+	/* Adjust the FPS selection based on the resolution selected */
+	dev->fps_index = __imx_nearest_fps_index(dev->targetfps,
+						res->fps_options);
+	dev->fps = res->fps_options[dev->fps_index].fps;
+	dev->regs = res->fps_options[dev->fps_index].regs;
+	if (!dev->regs)
+		dev->regs = res->regs;
+
+	ret = imx_write_reg_array(client, dev->regs);
+	if (ret)
+		goto out;
+
+	if (dev->sensor_id == IMX132_ID && lanes > 0) {
+		static const u8 imx132_rgpltd[] = {
+			2,		/* 1 lane:  /1 */
+			0,		/* 2 lanes: /2 */
+			0,		/* undefined   */
+			1,		/* 4 lanes: /4 */
+		};
+		ret = imx_write_reg(client, IMX_8BIT, IMX132_208_VT_RGPLTD,
+				    imx132_rgpltd[lanes - 1]);
+		if (ret)
+			goto out;
+	}
+
+	dev->pixels_per_line = res->fps_options[dev->fps_index].pixels_per_line;
+	dev->lines_per_frame = res->fps_options[dev->fps_index].lines_per_frame;
+
+	/* dbg h/v blank time */
+	__adjust_hvblank(sd);
+
+	ret = __imx_update_exposure_timing(client, dev->coarse_itg,
+		dev->pixels_per_line, dev->lines_per_frame);
+	if (ret)
+		goto out;
+
+	ret = __imx_update_gain(sd, dev->gain);
+	if (ret)
+		goto out;
+
+	ret = __imx_update_digital_gain(client, dev->digital_gain);
+	if (ret)
+		goto out;
+
+	ret = imx_write_reg_array(client, dev->param_update);
+	if (ret)
+		goto out;
+
+	ret = imx_get_intg_factor(client, imx_info, dev->regs);
+	if (ret)
+		goto out;
+
+	ret = imx_read_reg(client, IMX_8BIT,
+		dev->reg_addr->img_orientation, &val);
+	if (ret)
+		goto out;
+	val &= (IMX_VFLIP_BIT|IMX_HFLIP_BIT);
+	imx_info->raw_bayer_order = imx_bayer_order_mapping[val];
+	dev->format.code = imx_translate_bayer_order(
+		imx_info->raw_bayer_order);
+
+	/*
+	 * Fill meta data info. add imx135 metadata setting for RAW10 format
+	 */
+	switch (dev->sensor_id) {
+	case IMX135_ID:
+		ret = imx_read_reg(client, 2,
+				IMX135_OUTPUT_DATA_FORMAT_REG, &data);
+		if (ret)
+			goto out;
+		/*
+		 * The IMX135 can support various resolutions like
+		 * RAW6/8/10/12/14.
+		 * 1.The data format is RAW10:
+		 *   matadata width = current resolution width(pixel) * 10 / 8
+		 * 2.The data format is RAW6 or RAW8:
+		 *   matadata width = current resolution width(pixel);
+		 * 3.other data format(RAW12/14 etc):
+		 *   TBD.
+		 */
+		if (data == IMX135_OUTPUT_FORMAT_RAW10)
+			/* the data format is RAW10. */
+			imx_info->metadata_width = res->width * 10 / 8;
+		else
+			/* The data format is RAW6/8/12/14/ etc. */
+			imx_info->metadata_width = res->width;
+
+		imx_info->metadata_height = IMX135_EMBEDDED_DATA_LINE_NUM;
+
+		if (imx_info->metadata_effective_width == NULL)
+			imx_info->metadata_effective_width =
+				imx135_embedded_effective_size;
+
+		break;
+	case IMX227_ID:
+		ret = imx_read_reg(client, 2, IMX227_OUTPUT_DATA_FORMAT_REG,
+			&data);
+		if (ret)
+			goto out;
+		if (data == IMX227_OUTPUT_FORMAT_RAW10)
+			/* the data format is RAW10. */
+			imx_info->metadata_width = res->width * 10 / 8;
+		else
+			/* The data format is RAW6/8/12/14/ etc. */
+			imx_info->metadata_width = res->width;
+
+		imx_info->metadata_height = IMX227_EMBEDDED_DATA_LINE_NUM;
+
+		if (imx_info->metadata_effective_width == NULL)
+			imx_info->metadata_effective_width =
+				imx227_embedded_effective_size;
+
+		break;
+	default:
+		imx_info->metadata_width = 0;
+		imx_info->metadata_height = 0;
+		imx_info->metadata_effective_width = NULL;
+		break;
+	}
+
+out:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+
+static int imx_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	fmt->width = dev->curr_res_table[dev->fmt_idx].width;
+	fmt->height = dev->curr_res_table[dev->fmt_idx].height;
+	fmt->code = dev->format.code;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int imx_detect(struct i2c_client *client, u16 *id, u8 *revision)
+{
+	struct i2c_adapter *adapter = client->adapter;
+
+	/* i2c check */
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	/* check sensor chip ID	 */
+	if (imx_read_reg(client, IMX_16BIT, IMX132_175_208_219_CHIP_ID, id)) {
+		v4l2_err(client, "sensor_id = 0x%x\n", *id);
+		return -ENODEV;
+	}
+
+	if (*id == IMX132_ID || *id == IMX175_ID ||
+		*id == IMX208_ID || *id == IMX219_ID)
+		goto found;
+
+	if (imx_read_reg(client, IMX_16BIT, IMX134_135_227_CHIP_ID, id)) {
+		v4l2_err(client, "sensor_id = 0x%x\n", *id);
+		return -ENODEV;
+	}
+	if (*id != IMX134_ID && *id != IMX135_ID && *id != IMX227_ID) {
+		v4l2_err(client, "no imx sensor found\n");
+		return -ENODEV;
+	}
+found:
+	v4l2_info(client, "sensor_id = 0x%x\n", *id);
+
+	/* TODO - need to be updated */
+	*revision = 0;
+
+	return 0;
+}
+
+static void __imx_print_timing(struct v4l2_subdev *sd)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 width = dev->curr_res_table[dev->fmt_idx].width;
+	u16 height = dev->curr_res_table[dev->fmt_idx].height;
+
+	dev_dbg(&client->dev, "Dump imx timing in stream on:\n");
+	dev_dbg(&client->dev, "width: %d:\n", width);
+	dev_dbg(&client->dev, "height: %d:\n", height);
+	dev_dbg(&client->dev, "pixels_per_line: %d:\n", dev->pixels_per_line);
+	dev_dbg(&client->dev, "line per frame: %d:\n", dev->lines_per_frame);
+	dev_dbg(&client->dev, "pix freq: %d:\n", dev->vt_pix_clk_freq_mhz);
+	dev_dbg(&client->dev, "init fps: %d:\n", dev->vt_pix_clk_freq_mhz /
+			dev->pixels_per_line / dev->lines_per_frame);
+	dev_dbg(&client->dev, "HBlank: %d nS:\n",
+			1000 * (dev->pixels_per_line - width) /
+			(dev->vt_pix_clk_freq_mhz / 1000000));
+	dev_dbg(&client->dev, "VBlank: %d uS:\n",
+			(dev->lines_per_frame - height) * dev->pixels_per_line /
+			(dev->vt_pix_clk_freq_mhz / 1000000));
+}
+
+/*
+ * imx stream on/off
+ */
+static int imx_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	int ret;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	if (enable) {
+		/* Noise reduction & dead pixel applied before streaming */
+		if (dev->fw == NULL) {
+			dev_warn(&client->dev, "No MSR loaded from library");
+		} else {
+			ret = apply_msr_data(client, dev->fw);
+			if (ret) {
+				mutex_unlock(&dev->input_lock);
+				return ret;
+			}
+		}
+		ret = imx_test_pattern(sd);
+		if (ret) {
+			v4l2_err(client, "Configure test pattern failed.\n");
+			mutex_unlock(&dev->input_lock);
+			return ret;
+		}
+		__imx_print_timing(sd);
+		ret = imx_write_reg_array(client, imx_streaming);
+		if (ret != 0) {
+			v4l2_err(client, "write_reg_array err\n");
+			mutex_unlock(&dev->input_lock);
+			return ret;
+		}
+		dev->streaming = 1;
+		if (dev->vcm_driver && dev->vcm_driver->t_focus_abs_init)
+			dev->vcm_driver->t_focus_abs_init(sd);
+	} else {
+		ret = imx_write_reg_array(client, imx_soft_standby);
+		if (ret != 0) {
+			v4l2_err(client, "write_reg_array err\n");
+			mutex_unlock(&dev->input_lock);
+			return ret;
+		}
+		dev->streaming = 0;
+		dev->targetfps = 0;
+	}
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int __update_imx_device_settings(struct imx_device *dev, u16 sensor_id)
+{
+	/* IMX on other platform is not supported yet */
+	return -EINVAL;
+}
+
+static int imx_s_config(struct v4l2_subdev *sd,
+			    int irq, void *pdata)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 sensor_revision;
+	u16 sensor_id;
+	int ret;
+	if (pdata == NULL)
+		return -ENODEV;
+
+	dev->platform_data = pdata;
+
+	mutex_lock(&dev->input_lock);
+
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			mutex_unlock(&dev->input_lock);
+			dev_err(&client->dev, "imx platform init err\n");
+			return ret;
+		}
+	}
+	/*
+	 * power off the module first.
+	 *
+	 * As first power on by board have undecided state of power/gpio pins.
+	 */
+	ret = __imx_s_power(sd, 0);
+	if (ret) {
+		v4l2_err(client, "imx power-down err.\n");
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	ret = __imx_s_power(sd, 1);
+	if (ret) {
+		v4l2_err(client, "imx power-up err.\n");
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = imx_detect(client, &sensor_id, &sensor_revision);
+	if (ret) {
+		v4l2_err(client, "imx_detect err s_config.\n");
+		goto fail_detect;
+	}
+
+	dev->sensor_id = sensor_id;
+	dev->sensor_revision = sensor_revision;
+
+	/* Resolution settings depend on sensor type and platform */
+	ret = __update_imx_device_settings(dev, dev->sensor_id);
+	if (ret)
+		goto fail_detect;
+	/* Read sensor's OTP data */
+	dev->otp_data = dev->otp_driver->otp_read(sd,
+		dev->otp_driver->dev_addr, dev->otp_driver->start_addr,
+		dev->otp_driver->size);
+
+	/* power off sensor */
+	ret = __imx_s_power(sd, 0);
+
+	mutex_unlock(&dev->input_lock);
+	if (ret)
+		v4l2_err(client, "imx power-down err.\n");
+
+	return ret;
+
+fail_detect:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_csi_cfg:
+	__imx_s_power(sd, 0);
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+	mutex_unlock(&dev->input_lock);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+	return ret;
+}
+
+static int
+imx_enum_mbus_code(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+		   struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	if (code->index >= MAX_FMTS)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	code->code = dev->format.code;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int
+imx_enum_frame_size(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
+		    struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	if (index >= dev->entries_curr_table) {
+		mutex_unlock(&dev->input_lock);
+		return -EINVAL;
+	}
+
+	fse->min_width = dev->curr_res_table[index].width;
+	fse->min_height = dev->curr_res_table[index].height;
+	fse->max_width = dev->curr_res_table[index].width;
+	fse->max_height = dev->curr_res_table[index].height;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int
+imx_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *param)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		dev->curr_res_table = dev->mode_tables->res_video;
+		dev->entries_curr_table = dev->mode_tables->n_res_video;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		dev->curr_res_table = dev->mode_tables->res_still;
+		dev->entries_curr_table = dev->mode_tables->n_res_still;
+		break;
+	default:
+		dev->curr_res_table = dev->mode_tables->res_preview;
+		dev->entries_curr_table = dev->mode_tables->n_res_preview;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+int
+imx_g_frame_interval(struct v4l2_subdev *sd,
+				struct v4l2_subdev_frame_interval *interval)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	interval->interval.denominator = dev->fps;
+	interval->interval.numerator = 1;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int __imx_s_frame_interval(struct v4l2_subdev *sd,
+			struct v4l2_subdev_frame_interval *interval)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct imx_resolution *res =
+				&dev->curr_res_table[dev->fmt_idx];
+	struct camera_mipi_info *imx_info = NULL;
+	unsigned short pixels_per_line;
+	unsigned short lines_per_frame;
+	unsigned int fps_index;
+	int fps;
+	int ret = 0;
+
+
+	imx_info = v4l2_get_subdev_hostdata(sd);
+	if (imx_info == NULL)
+		return -EINVAL;
+
+	if (!interval->interval.numerator)
+		interval->interval.numerator = 1;
+
+	fps = interval->interval.denominator / interval->interval.numerator;
+
+	if (!fps)
+		return -EINVAL;
+
+	dev->targetfps = fps;
+	/* No need to proceed further if we are not streaming */
+	if (!dev->streaming)
+		return 0;
+
+	 /* Ignore if we are already using the required FPS. */
+	if (fps == dev->fps)
+		return 0;
+
+	/*
+	 * Start here, sensor is already streaming, so adjust fps dynamically
+	 */
+	fps_index = __imx_above_nearest_fps_index(fps, res->fps_options);
+	if (fps > res->fps_options[fps_index].fps) {
+		/*
+		 * if does not have high fps setting, not support increase fps
+		 * by adjust lines per frame.
+		 */
+		dev_err(&client->dev, "Could not support fps: %d.\n", fps);
+		return -EINVAL;
+	}
+
+	if (res->fps_options[fps_index].regs &&
+	    res->fps_options[fps_index].regs != dev->regs) {
+		/*
+		 * if need a new setting, but the new setting has difference
+		 * with current setting, not use this one, as may have
+		 * unexpected result, e.g. PLL, IQ.
+		 */
+		dev_dbg(&client->dev,
+			"Sensor is streaming, not apply new sensor setting\n");
+		if (fps > res->fps_options[dev->fps_index].fps) {
+			/*
+			 * Does not support increase fps based on low fps
+			 * setting, as the high fps setting could not be used,
+			 * and fps requested is above current setting fps.
+			 */
+			dev_warn(&client->dev,
+			"Could not support fps: %d, keep current: %d.\n",
+			fps, dev->fps);
+			return 0;
+		}
+	} else {
+		dev->fps_index = fps_index;
+		dev->fps = res->fps_options[dev->fps_index].fps;
+	}
+
+	/* Update the new frametimings based on FPS */
+	pixels_per_line = res->fps_options[dev->fps_index].pixels_per_line;
+	lines_per_frame = res->fps_options[dev->fps_index].lines_per_frame;
+
+	if (fps > res->fps_options[fps_index].fps) {
+		/*
+		 * if does not have high fps setting, not support increase fps
+		 * by adjust lines per frame.
+		 */
+		dev_warn(&client->dev, "Could not support fps: %d. Use:%d.\n",
+				fps, res->fps_options[fps_index].fps);
+		goto done;
+	}
+
+	/* if the new setting does not match exactly */
+	if (dev->fps != fps) {
+#define MAX_LINES_PER_FRAME	0xffff
+		dev_dbg(&client->dev, "adjusting fps using lines_per_frame\n");
+		/*
+		 * FIXME!
+		 * 1: check DS on max value of lines_per_frame
+		 * 2: consider use pixel per line for more range?
+		 */
+		if (dev->lines_per_frame * dev->fps / fps >
+			MAX_LINES_PER_FRAME) {
+			dev_warn(&client->dev,
+		"adjust lines_per_frame out of range, try to use max value.\n");
+			lines_per_frame = MAX_LINES_PER_FRAME;
+		} else {
+			lines_per_frame = lines_per_frame * dev->fps / fps;
+		}
+	}
+done:
+	/* Update the new frametimings based on FPS */
+	dev->pixels_per_line = pixels_per_line;
+	dev->lines_per_frame = lines_per_frame;
+
+	/* Update the new values so that user side knows the current settings */
+	ret = __imx_update_exposure_timing(client,
+		dev->coarse_itg, dev->pixels_per_line, dev->lines_per_frame);
+	if (ret)
+		return ret;
+
+	dev->fps = fps;
+
+	ret = imx_get_intg_factor(client, imx_info, dev->regs);
+	if (ret)
+		return ret;
+
+	interval->interval.denominator = res->fps_options[dev->fps_index].fps;
+	interval->interval.numerator = 1;
+	__imx_print_timing(sd);
+
+	return ret;
+}
+
+static int imx_s_frame_interval(struct v4l2_subdev *sd,
+			struct v4l2_subdev_frame_interval *interval)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __imx_s_frame_interval(sd, interval);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+static int imx_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->curr_res_table[dev->fmt_idx].skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_sensor_ops imx_sensor_ops = {
+	.g_skip_frames	= imx_g_skip_frames,
+};
+
+static const struct v4l2_subdev_video_ops imx_video_ops = {
+	.s_stream = imx_s_stream,
+	.s_parm = imx_s_parm,
+	.g_frame_interval = imx_g_frame_interval,
+	.s_frame_interval = imx_s_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops imx_core_ops = {
+	.s_power = imx_s_power,
+	.ioctl = imx_ioctl,
+	.init = imx_init,
+};
+
+static const struct v4l2_subdev_pad_ops imx_pad_ops = {
+	.enum_mbus_code = imx_enum_mbus_code,
+	.enum_frame_size = imx_enum_frame_size,
+	.get_fmt = imx_get_fmt,
+	.set_fmt = imx_set_fmt,
+};
+
+static const struct v4l2_subdev_ops imx_ops = {
+	.core = &imx_core_ops,
+	.video = &imx_video_ops,
+	.pad = &imx_pad_ops,
+	.sensor = &imx_sensor_ops,
+};
+
+static const struct media_entity_operations imx_entity_ops = {
+	.link_setup = NULL,
+};
+
+static int imx_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx_device *dev = to_imx_sensor(sd);
+
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	dev->platform_data->csi_cfg(sd, 0);
+	v4l2_device_unregister_subdev(sd);
+	release_msr_list(client, dev->fw);
+	kfree(dev);
+
+	return 0;
+}
+
+static int __imx_init_ctrl_handler(struct imx_device *dev)
+{
+	struct v4l2_ctrl_handler *hdl;
+	int i;
+
+	hdl = &dev->ctrl_handler;
+
+	v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(imx_controls));
+
+	for (i = 0; i < ARRAY_SIZE(imx_controls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler,
+				&imx_controls[i], NULL);
+
+	dev->pixel_rate = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_PIXEL_RATE);
+	dev->h_blank = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_HBLANK);
+	dev->v_blank = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_VBLANK);
+	dev->link_freq = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_LINK_FREQ);
+	dev->h_flip = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_HFLIP);
+	dev->v_flip = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_VFLIP);
+	dev->tp_mode = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_TEST_PATTERN);
+	dev->tp_r = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_TEST_PATTERN_COLOR_R);
+	dev->tp_gr = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_TEST_PATTERN_COLOR_GR);
+	dev->tp_gb = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_TEST_PATTERN_COLOR_GB);
+	dev->tp_b = v4l2_ctrl_find(&dev->ctrl_handler,
+				V4L2_CID_TEST_PATTERN_COLOR_B);
+
+	if (dev->ctrl_handler.error || dev->pixel_rate == NULL
+		|| dev->h_blank == NULL || dev->v_blank == NULL
+		|| dev->h_flip == NULL || dev->v_flip == NULL
+		|| dev->link_freq == NULL) {
+		return dev->ctrl_handler.error;
+	}
+
+	dev->ctrl_handler.lock = &dev->input_lock;
+	dev->sd.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
+
+	return 0;
+}
+
+static void imx_update_reg_info(struct imx_device *dev)
+{
+	if (dev->sensor_id == IMX219_ID) {
+		dev->reg_addr = &imx219_addr;
+		dev->param_hold = imx219_param_hold;
+		dev->param_update = imx219_param_update;
+	} else {
+		dev->reg_addr = &imx_addr;
+		dev->param_hold = imx_param_hold;
+		dev->param_update = imx_param_update;
+	}
+}
+
+static int imx_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct imx_device *dev;
+	struct camera_mipi_info *imx_info = NULL;
+	int ret;
+	char *msr_file_name = NULL;
+
+	/* allocate sensor device & init sub device */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		v4l2_err(client, "%s: out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->i2c_id = id->driver_data;
+	dev->fmt_idx = 0;
+	dev->sensor_id = IMX_ID_DEFAULT;
+	dev->vcm_driver = &imx_vcms[IMX_ID_DEFAULT];
+	dev->digital_gain = 256;
+
+	v4l2_i2c_subdev_init(&(dev->sd), client, &imx_ops);
+
+	if (client->dev.platform_data) {
+		ret = imx_s_config(&dev->sd, client->irq,
+				       client->dev.platform_data);
+		if (ret)
+			goto out_free;
+	}
+	imx_info = v4l2_get_subdev_hostdata(&dev->sd);
+
+	/*
+	 * sd->name is updated with sensor driver name by the v4l2.
+	 * change it to sensor name in this case.
+	 */
+	imx_update_reg_info(dev);
+	snprintf(dev->sd.name, sizeof(dev->sd.name), "%s%x %d-%04x",
+		IMX_SUBDEV_PREFIX, dev->sensor_id,
+		i2c_adapter_id(client->adapter), client->addr);
+
+	ret = __imx_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 = imx_translate_bayer_order(
+		imx_info->raw_bayer_order);
+	dev->sd.entity.ops = &imx_entity_ops;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret) {
+		imx_remove(client);
+		return ret;
+	}
+
+	/* Load the Noise reduction, Dead pixel registers from cpf file*/
+	if (dev->platform_data->msr_file_name != NULL)
+		msr_file_name = dev->platform_data->msr_file_name();
+	if (msr_file_name) {
+		ret = load_msr_list(client, msr_file_name, &dev->fw);
+		if (ret) {
+			imx_remove(client);
+			return ret;
+		}
+	} else {
+		dev_warn(&client->dev, "Drvb file not present");
+	}
+
+	return ret;
+
+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 i2c_device_id imx_ids[] = {
+	{IMX_NAME_175, IMX175_ID},
+	{IMX_NAME_135, IMX135_ID},
+	{IMX_NAME_135_FUJI, IMX135_FUJI_ID},
+	{IMX_NAME_134, IMX134_ID},
+	{IMX_NAME_132, IMX132_ID},
+	{IMX_NAME_208, IMX208_ID},
+	{IMX_NAME_219, IMX219_ID},
+	{IMX_NAME_227, IMX227_ID},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, imx_ids);
+
+static struct i2c_driver imx_driver = {
+	.driver = {
+		.name = IMX_DRIVER,
+	},
+	.probe = imx_probe,
+	.remove = imx_remove,
+	.id_table = imx_ids,
+};
+
+static __init int init_imx(void)
+{
+	return i2c_add_driver(&imx_driver);
+}
+
+static __exit void exit_imx(void)
+{
+	i2c_del_driver(&imx_driver);
+}
+
+module_init(init_imx);
+module_exit(exit_imx);
+
+MODULE_DESCRIPTION("A low-level driver for Sony IMX sensors");
+MODULE_AUTHOR("Shenbo Huang <shenbo.huang@intel.com>");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx.h b/drivers/staging/media/atomisp/i2c/imx/imx.h
new file mode 100644
index 000000000000..36b3f3a5a41f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx.h
@@ -0,0 +1,766 @@
+/*
+ * Support for Sony IMX 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __IMX_H__
+#define __IMX_H__
+#include "../../include/linux/atomisp_platform.h"
+#include "../../include/linux/atomisp.h"
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include "imx175.h"
+#include "imx135.h"
+#include "imx134.h"
+#include "imx132.h"
+#include "imx208.h"
+#include "imx219.h"
+#include "imx227.h"
+
+#define IMX_MCLK		192
+
+/* TODO - This should be added into include/linux/videodev2.h */
+#ifndef V4L2_IDENT_IMX
+#define V4L2_IDENT_IMX	8245
+#endif
+
+#define IMX_MAX_AE_LUT_LENGTH	5
+/*
+ * imx System control registers
+ */
+#define IMX_MASK_5BIT	0x1F
+#define IMX_MASK_4BIT	0xF
+#define IMX_MASK_3BIT	0x7
+#define IMX_MASK_2BIT	0x3
+#define IMX_MASK_8BIT	0xFF
+#define IMX_MASK_11BIT	0x7FF
+#define IMX_INTG_BUF_COUNT		2
+
+#define IMX_FINE_INTG_TIME		0x1E8
+
+#define IMX_VT_PIX_CLK_DIV		0x0301
+#define IMX_VT_SYS_CLK_DIV		0x0303
+#define IMX_PRE_PLL_CLK_DIV		0x0305
+#define IMX227_IOP_PRE_PLL_CLK_DIV	0x030D
+#define IMX227_PLL_MULTIPLIER		0x0306
+#define IMX227_IOP_PLL_MULTIPLIER	0x030E
+#define IMX227_PLL_MULTI_DRIVE		0x0310
+#define IMX227_OP_PIX_CLK_DIV		0x0309
+#define IMX227_OP_SYS_CLK_DIV		0x030B
+#define IMX_PLL_MULTIPLIER		0x030C
+#define IMX_OP_PIX_DIV			0x0309
+#define IMX_OP_SYS_DIV			0x030B
+#define IMX_FRAME_LENGTH_LINES		0x0340
+#define IMX_LINE_LENGTH_PIXELS		0x0342
+#define IMX_COARSE_INTG_TIME_MIN	0x1004
+#define IMX_COARSE_INTG_TIME_MAX	0x1006
+#define IMX_BINNING_ENABLE		0x0390
+#define IMX227_BINNING_ENABLE		0x0900
+#define IMX_BINNING_TYPE		0x0391
+#define IMX227_BINNING_TYPE		0x0901
+#define IMX_READ_MODE			0x0390
+#define IMX227_READ_MODE		0x0900
+
+#define IMX_HORIZONTAL_START_H 0x0344
+#define IMX_VERTICAL_START_H 0x0346
+#define IMX_HORIZONTAL_END_H 0x0348
+#define IMX_VERTICAL_END_H 0x034a
+#define IMX_HORIZONTAL_OUTPUT_SIZE_H 0x034c
+#define IMX_VERTICAL_OUTPUT_SIZE_H 0x034e
+
+/* Post Divider setting register for imx132 and imx208 */
+#define IMX132_208_VT_RGPLTD		0x30A4
+
+/* Multiplier setting register for imx132, imx208, and imx219 */
+#define IMX132_208_219_PLL_MULTIPLIER		0x0306
+
+#define IMX_COARSE_INTEGRATION_TIME		0x0202
+#define IMX_TEST_PATTERN_MODE			0x0600
+#define IMX_TEST_PATTERN_COLOR_R		0x0602
+#define IMX_TEST_PATTERN_COLOR_GR		0x0604
+#define IMX_TEST_PATTERN_COLOR_B		0x0606
+#define IMX_TEST_PATTERN_COLOR_GB		0x0608
+#define IMX_IMG_ORIENTATION			0x0101
+#define IMX_VFLIP_BIT			2
+#define IMX_HFLIP_BIT			1
+#define IMX_GLOBAL_GAIN			0x0205
+#define IMX_SHORT_AGC_GAIN		0x0233
+#define IMX_DGC_ADJ		0x020E
+#define IMX_DGC_LEN		10
+#define IMX227_DGC_LEN		4
+#define IMX_MAX_EXPOSURE_SUPPORTED 0xfffb
+#define IMX_MAX_GLOBAL_GAIN_SUPPORTED 0x00ff
+#define IMX_MAX_DIGITAL_GAIN_SUPPORTED 0x0fff
+
+#define MAX_FMTS 1
+#define IMX_OTP_DATA_SIZE		1280
+
+#define IMX_SUBDEV_PREFIX "imx"
+#define IMX_DRIVER	"imx1x5"
+
+/* Sensor ids from identification register */
+#define IMX_NAME_134	"imx134"
+#define IMX_NAME_135	"imx135"
+#define IMX_NAME_175	"imx175"
+#define IMX_NAME_132	"imx132"
+#define IMX_NAME_208	"imx208"
+#define IMX_NAME_219	"imx219"
+#define IMX_NAME_227	"imx227"
+#define IMX175_ID	0x0175
+#define IMX135_ID	0x0135
+#define IMX134_ID	0x0134
+#define IMX132_ID	0x0132
+#define IMX208_ID	0x0208
+#define IMX219_ID	0x0219
+#define IMX227_ID	0x0227
+
+/* Sensor id based on i2c_device_id table
+ * (Fuji module can not be detected based on sensor registers) */
+#define IMX135_FUJI_ID			0x0136
+#define IMX_NAME_135_FUJI		"imx135fuji"
+
+/* imx175 - use dw9714 vcm */
+#define IMX175_MERRFLD 0x175
+#define IMX175_VALLEYVIEW 0x176
+#define IMX135_SALTBAY 0x135
+#define IMX135_VICTORIABAY 0x136
+#define IMX132_SALTBAY 0x132
+#define IMX134_VALLEYVIEW 0x134
+#define IMX208_MOFD_PD2 0x208
+#define IMX219_MFV0_PRH 0x219
+#define IMX227_SAND 0x227
+
+/* otp - specific settings */
+#define E2PROM_ADDR 0xa0
+#define E2PROM_LITEON_12P1BA869D_ADDR 0xa0
+#define E2PROM_ABICO_SS89A839_ADDR 0xa8
+#define DEFAULT_OTP_SIZE 1280
+#define IMX135_OTP_SIZE 1280
+#define IMX219_OTP_SIZE 2048
+#define IMX227_OTP_SIZE 2560
+#define E2PROM_LITEON_12P1BA869D_SIZE 544
+
+#define IMX_ID_DEFAULT	0x0000
+#define IMX132_175_208_219_CHIP_ID	0x0000
+#define IMX134_135_CHIP_ID	0x0016
+#define IMX134_135_227_CHIP_ID	0x0016
+
+#define IMX175_RES_WIDTH_MAX	3280
+#define IMX175_RES_HEIGHT_MAX	2464
+#define IMX135_RES_WIDTH_MAX	4208
+#define IMX135_RES_HEIGHT_MAX	3120
+#define IMX132_RES_WIDTH_MAX	1936
+#define IMX132_RES_HEIGHT_MAX	1096
+#define IMX134_RES_WIDTH_MAX	3280
+#define IMX134_RES_HEIGHT_MAX	2464
+#define IMX208_RES_WIDTH_MAX	1936
+#define IMX208_RES_HEIGHT_MAX	1096
+#define IMX219_RES_WIDTH_MAX	3280
+#define IMX219_RES_HEIGHT_MAX	2464
+#define IMX227_RES_WIDTH_MAX	2400
+#define IMX227_RES_HEIGHT_MAX	2720
+
+/* Defines for lens/VCM */
+#define IMX_FOCAL_LENGTH_NUM	369	/*3.69mm*/
+#define IMX_FOCAL_LENGTH_DEM	100
+#define IMX_F_NUMBER_DEFAULT_NUM	22
+#define IMX_F_NUMBER_DEM	10
+#define IMX_INVALID_CONFIG	0xffffffff
+#define IMX_MAX_FOCUS_POS	1023
+#define IMX_MAX_FOCUS_NEG	(-1023)
+#define IMX_VCM_SLEW_STEP_MAX	0x3f
+#define IMX_VCM_SLEW_TIME_MAX	0x1f
+
+#define IMX_BIN_FACTOR_MAX			4
+#define IMX_INTEGRATION_TIME_MARGIN	4
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define IMX_FOCAL_LENGTH_DEFAULT 0x1710064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define IMX_F_NUMBER_DEFAULT 0x16000a
+
+/*
+ * 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 IMX_F_NUMBER_RANGE 0x160a160a
+
+struct imx_vcm {
+	int (*power_up)(struct v4l2_subdev *sd);
+	int (*power_down)(struct v4l2_subdev *sd);
+	int (*init)(struct v4l2_subdev *sd);
+	int (*t_focus_vcm)(struct v4l2_subdev *sd, u16 val);
+	int (*t_focus_abs)(struct v4l2_subdev *sd, s32 value);
+	int (*t_focus_abs_init)(struct v4l2_subdev *sd);
+	int (*t_focus_rel)(struct v4l2_subdev *sd, s32 value);
+	int (*q_focus_status)(struct v4l2_subdev *sd, s32 *value);
+	int (*q_focus_abs)(struct v4l2_subdev *sd, s32 *value);
+	int (*t_vcm_slew)(struct v4l2_subdev *sd, s32 value);
+	int (*t_vcm_timing)(struct v4l2_subdev *sd, s32 value);
+};
+
+struct imx_otp {
+	void * (*otp_read)(struct v4l2_subdev *sd, u8 dev_addr,
+		u32 start_addr, u32 size);
+	u32 start_addr;
+	u32 size;
+	u8 dev_addr;
+};
+
+struct max_res {
+	int res_max_width;
+	int res_max_height;
+};
+
+struct max_res imx_max_res[] = {
+	[IMX175_ID] = {
+		.res_max_width = IMX175_RES_WIDTH_MAX,
+		.res_max_height = IMX175_RES_HEIGHT_MAX,
+	},
+	[IMX135_ID] = {
+		.res_max_width = IMX135_RES_WIDTH_MAX,
+		.res_max_height = IMX135_RES_HEIGHT_MAX,
+	},
+	[IMX132_ID] = {
+		.res_max_width = IMX132_RES_WIDTH_MAX,
+		.res_max_height = IMX132_RES_HEIGHT_MAX,
+	},
+	[IMX134_ID] = {
+		.res_max_width = IMX134_RES_WIDTH_MAX,
+		.res_max_height = IMX134_RES_HEIGHT_MAX,
+	},
+	[IMX208_ID] = {
+		.res_max_width = IMX208_RES_WIDTH_MAX,
+		.res_max_height = IMX208_RES_HEIGHT_MAX,
+	},
+	[IMX219_ID] = {
+		.res_max_width = IMX219_RES_WIDTH_MAX,
+		.res_max_height = IMX219_RES_HEIGHT_MAX,
+	},
+	[IMX227_ID] = {
+		.res_max_width = IMX227_RES_WIDTH_MAX,
+		.res_max_height = IMX227_RES_HEIGHT_MAX,
+	},
+};
+
+struct imx_settings {
+	struct imx_reg const *init_settings;
+	struct imx_resolution *res_preview;
+	struct imx_resolution *res_still;
+	struct imx_resolution *res_video;
+	int n_res_preview;
+	int n_res_still;
+	int n_res_video;
+};
+
+struct imx_settings imx_sets[] = {
+	[IMX175_MERRFLD] = {
+		.init_settings = imx175_init_settings,
+		.res_preview = imx175_res_preview,
+		.res_still = imx175_res_still,
+		.res_video = imx175_res_video,
+		.n_res_preview = ARRAY_SIZE(imx175_res_preview),
+		.n_res_still = ARRAY_SIZE(imx175_res_still),
+		.n_res_video = ARRAY_SIZE(imx175_res_video),
+	},
+	[IMX175_VALLEYVIEW] = {
+		.init_settings = imx175_init_settings,
+		.res_preview = imx175_res_preview,
+		.res_still = imx175_res_still,
+		.res_video = imx175_res_video,
+		.n_res_preview = ARRAY_SIZE(imx175_res_preview),
+		.n_res_still = ARRAY_SIZE(imx175_res_still),
+		.n_res_video = ARRAY_SIZE(imx175_res_video),
+	},
+	[IMX135_SALTBAY] = {
+		.init_settings = imx135_init_settings,
+		.res_preview = imx135_res_preview,
+		.res_still = imx135_res_still,
+		.res_video = imx135_res_video,
+		.n_res_preview = ARRAY_SIZE(imx135_res_preview),
+		.n_res_still = ARRAY_SIZE(imx135_res_still),
+		.n_res_video = ARRAY_SIZE(imx135_res_video),
+	},
+	[IMX135_VICTORIABAY] = {
+		.init_settings = imx135_init_settings,
+		.res_preview = imx135_res_preview_mofd,
+		.res_still = imx135_res_still_mofd,
+		.res_video = imx135_res_video,
+		.n_res_preview = ARRAY_SIZE(imx135_res_preview_mofd),
+		.n_res_still = ARRAY_SIZE(imx135_res_still_mofd),
+		.n_res_video = ARRAY_SIZE(imx135_res_video),
+	},
+	[IMX132_SALTBAY] = {
+		.init_settings = imx132_init_settings,
+		.res_preview = imx132_res_preview,
+		.res_still = imx132_res_still,
+		.res_video = imx132_res_video,
+		.n_res_preview = ARRAY_SIZE(imx132_res_preview),
+		.n_res_still = ARRAY_SIZE(imx132_res_still),
+		.n_res_video = ARRAY_SIZE(imx132_res_video),
+	},
+	[IMX134_VALLEYVIEW] = {
+		.init_settings = imx134_init_settings,
+		.res_preview = imx134_res_preview,
+		.res_still = imx134_res_still,
+		.res_video = imx134_res_video,
+		.n_res_preview = ARRAY_SIZE(imx134_res_preview),
+		.n_res_still = ARRAY_SIZE(imx134_res_still),
+		.n_res_video = ARRAY_SIZE(imx134_res_video),
+	},
+	[IMX208_MOFD_PD2] = {
+		.init_settings = imx208_init_settings,
+		.res_preview = imx208_res_preview,
+		.res_still = imx208_res_still,
+		.res_video = imx208_res_video,
+		.n_res_preview = ARRAY_SIZE(imx208_res_preview),
+		.n_res_still = ARRAY_SIZE(imx208_res_still),
+		.n_res_video = ARRAY_SIZE(imx208_res_video),
+	},
+	[IMX219_MFV0_PRH] = {
+		.init_settings = imx219_init_settings,
+		.res_preview = imx219_res_preview,
+		.res_still = imx219_res_still,
+		.res_video = imx219_res_video,
+		.n_res_preview = ARRAY_SIZE(imx219_res_preview),
+		.n_res_still = ARRAY_SIZE(imx219_res_still),
+		.n_res_video = ARRAY_SIZE(imx219_res_video),
+	},
+	[IMX227_SAND] = {
+		.init_settings = imx227_init_settings,
+		.res_preview = imx227_res_preview,
+		.res_still = imx227_res_still,
+		.res_video = imx227_res_video,
+		.n_res_preview = ARRAY_SIZE(imx227_res_preview),
+		.n_res_still = ARRAY_SIZE(imx227_res_still),
+		.n_res_video = ARRAY_SIZE(imx227_res_video),
+	},
+};
+
+struct imx_reg_addr {
+	u16 frame_length_lines;
+	u16 line_length_pixels;
+	u16 horizontal_start_h;
+	u16 vertical_start_h;
+	u16 horizontal_end_h;
+	u16 vertical_end_h;
+	u16 horizontal_output_size_h;
+	u16 vertical_output_size_h;
+	u16 coarse_integration_time;
+	u16 img_orientation;
+	u16 global_gain;
+	u16 dgc_adj;
+};
+
+struct imx_reg_addr imx_addr = {
+	IMX_FRAME_LENGTH_LINES,
+	IMX_LINE_LENGTH_PIXELS,
+	IMX_HORIZONTAL_START_H,
+	IMX_VERTICAL_START_H,
+	IMX_HORIZONTAL_END_H,
+	IMX_VERTICAL_END_H,
+	IMX_HORIZONTAL_OUTPUT_SIZE_H,
+	IMX_VERTICAL_OUTPUT_SIZE_H,
+	IMX_COARSE_INTEGRATION_TIME,
+	IMX_IMG_ORIENTATION,
+	IMX_GLOBAL_GAIN,
+	IMX_DGC_ADJ,
+};
+
+struct imx_reg_addr imx219_addr = {
+	IMX219_FRAME_LENGTH_LINES,
+	IMX219_LINE_LENGTH_PIXELS,
+	IMX219_HORIZONTAL_START_H,
+	IMX219_VERTICAL_START_H,
+	IMX219_HORIZONTAL_END_H,
+	IMX219_VERTICAL_END_H,
+	IMX219_HORIZONTAL_OUTPUT_SIZE_H,
+	IMX219_VERTICAL_OUTPUT_SIZE_H,
+	IMX219_COARSE_INTEGRATION_TIME,
+	IMX219_IMG_ORIENTATION,
+	IMX219_GLOBAL_GAIN,
+	IMX219_DGC_ADJ,
+};
+
+#define	v4l2_format_capture_type_entry(_width, _height, \
+		_pixelformat, _bytesperline, _colorspace) \
+	{\
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,\
+		.fmt.pix.width = (_width),\
+		.fmt.pix.height = (_height),\
+		.fmt.pix.pixelformat = (_pixelformat),\
+		.fmt.pix.bytesperline = (_bytesperline),\
+		.fmt.pix.colorspace = (_colorspace),\
+		.fmt.pix.sizeimage = (_height)*(_bytesperline),\
+	}
+
+#define	s_output_format_entry(_width, _height, _pixelformat, \
+		_bytesperline, _colorspace, _fps) \
+	{\
+		.v4l2_fmt = v4l2_format_capture_type_entry(_width, \
+			_height, _pixelformat, _bytesperline, \
+				_colorspace),\
+		.fps = (_fps),\
+	}
+
+#define	s_output_format_reg_entry(_width, _height, _pixelformat, \
+		_bytesperline, _colorspace, _fps, _reg_setting) \
+	{\
+		.s_fmt = s_output_format_entry(_width, _height,\
+				_pixelformat, _bytesperline, \
+				_colorspace, _fps),\
+		.reg_setting = (_reg_setting),\
+	}
+
+/* imx device structure */
+struct imx_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 */
+	int fmt_idx;
+	int status;
+	int streaming;
+	int power;
+	int run_mode;
+	int vt_pix_clk_freq_mhz;
+	int fps_index;
+	u32 focus;
+	u16 sensor_id;			/* Sensor id from registers */
+	u16 i2c_id;			/* Sensor id from i2c_device_id */
+	u16 coarse_itg;
+	u16 fine_itg;
+	u16 digital_gain;
+	u16 gain;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 targetfps;
+	u8 fps;
+	const struct imx_reg *regs;
+	u8 res;
+	u8 type;
+	u8 sensor_revision;
+	u8 *otp_data;
+	struct imx_settings *mode_tables;
+	struct imx_vcm *vcm_driver;
+	struct imx_otp *otp_driver;
+	const struct imx_resolution *curr_res_table;
+	int entries_curr_table;
+	const struct firmware *fw;
+	struct imx_reg_addr *reg_addr;
+	const struct imx_reg *param_hold;
+	const struct imx_reg *param_update;
+
+	/* used for h/b blank tuning */
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *h_blank;
+	struct v4l2_ctrl *v_blank;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *h_flip;
+	struct v4l2_ctrl *v_flip;
+
+	/* Test pattern control */
+	struct v4l2_ctrl *tp_mode;
+	struct v4l2_ctrl *tp_r;
+	struct v4l2_ctrl *tp_gr;
+	struct v4l2_ctrl *tp_gb;
+	struct v4l2_ctrl *tp_b;
+
+	/* FIXME! */
+	bool new_res_sel_method;
+};
+
+#define to_imx_sensor(x) container_of(x, struct imx_device, sd)
+
+#define IMX_MAX_WRITE_BUF_SIZE	32
+struct imx_write_buffer {
+	u16 addr;
+	u8 data[IMX_MAX_WRITE_BUF_SIZE];
+};
+
+struct imx_write_ctrl {
+	int index;
+	struct imx_write_buffer buffer;
+};
+
+static const struct imx_reg imx_soft_standby[] = {
+	{IMX_8BIT, 0x0100, 0x00},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx_streaming[] = {
+	{IMX_8BIT, 0x0100, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx_param_hold[] = {
+	{IMX_8BIT, 0x0104, 0x01},	/* GROUPED_PARAMETER_HOLD */
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx_param_update[] = {
+	{IMX_8BIT, 0x0104, 0x00},	/* GROUPED_PARAMETER_HOLD */
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx219_param_hold[] = {
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx219_param_update[] = {
+	{IMX_TOK_TERM, 0, 0}
+};
+
+extern int ad5816g_vcm_power_up(struct v4l2_subdev *sd);
+extern int ad5816g_vcm_power_down(struct v4l2_subdev *sd);
+extern int ad5816g_vcm_init(struct v4l2_subdev *sd);
+
+extern int ad5816g_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int ad5816g_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int ad5816g_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int ad5816g_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int ad5816g_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int ad5816g_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int ad5816g_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int drv201_vcm_power_up(struct v4l2_subdev *sd);
+extern int drv201_vcm_power_down(struct v4l2_subdev *sd);
+extern int drv201_vcm_init(struct v4l2_subdev *sd);
+
+extern int drv201_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int drv201_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int drv201_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int drv201_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int drv201_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int drv201_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int drv201_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int dw9714_vcm_power_up(struct v4l2_subdev *sd);
+extern int dw9714_vcm_power_down(struct v4l2_subdev *sd);
+extern int dw9714_vcm_init(struct v4l2_subdev *sd);
+
+extern int dw9714_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int dw9714_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int dw9714_t_focus_abs_init(struct v4l2_subdev *sd);
+extern int dw9714_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int dw9714_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int dw9714_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int dw9714_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int dw9714_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int dw9719_vcm_power_up(struct v4l2_subdev *sd);
+extern int dw9719_vcm_power_down(struct v4l2_subdev *sd);
+extern int dw9719_vcm_init(struct v4l2_subdev *sd);
+
+extern int dw9719_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int dw9719_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int dw9719_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int dw9719_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int dw9719_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int dw9719_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int dw9719_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int dw9718_vcm_power_up(struct v4l2_subdev *sd);
+extern int dw9718_vcm_power_down(struct v4l2_subdev *sd);
+extern int dw9718_vcm_init(struct v4l2_subdev *sd);
+
+extern int dw9718_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int dw9718_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int vcm_power_up(struct v4l2_subdev *sd);
+extern int vcm_power_down(struct v4l2_subdev *sd);
+
+struct imx_vcm imx_vcms[] = {
+	[IMX175_MERRFLD] = {
+		.power_up = drv201_vcm_power_up,
+		.power_down = drv201_vcm_power_down,
+		.init = drv201_vcm_init,
+		.t_focus_vcm = drv201_t_focus_vcm,
+		.t_focus_abs = drv201_t_focus_abs,
+		.t_focus_abs_init = NULL,
+		.t_focus_rel = drv201_t_focus_rel,
+		.q_focus_status = drv201_q_focus_status,
+		.q_focus_abs = drv201_q_focus_abs,
+		.t_vcm_slew = drv201_t_vcm_slew,
+		.t_vcm_timing = drv201_t_vcm_timing,
+	},
+	[IMX175_VALLEYVIEW] = {
+		.power_up = dw9714_vcm_power_up,
+		.power_down = dw9714_vcm_power_down,
+		.init = dw9714_vcm_init,
+		.t_focus_vcm = dw9714_t_focus_vcm,
+		.t_focus_abs = dw9714_t_focus_abs,
+		.t_focus_abs_init = NULL,
+		.t_focus_rel = dw9714_t_focus_rel,
+		.q_focus_status = dw9714_q_focus_status,
+		.q_focus_abs = dw9714_q_focus_abs,
+		.t_vcm_slew = dw9714_t_vcm_slew,
+		.t_vcm_timing = dw9714_t_vcm_timing,
+	},
+	[IMX135_SALTBAY] = {
+		.power_up = ad5816g_vcm_power_up,
+		.power_down = ad5816g_vcm_power_down,
+		.init = ad5816g_vcm_init,
+		.t_focus_vcm = ad5816g_t_focus_vcm,
+		.t_focus_abs = ad5816g_t_focus_abs,
+		.t_focus_abs_init = NULL,
+		.t_focus_rel = ad5816g_t_focus_rel,
+		.q_focus_status = ad5816g_q_focus_status,
+		.q_focus_abs = ad5816g_q_focus_abs,
+		.t_vcm_slew = ad5816g_t_vcm_slew,
+		.t_vcm_timing = ad5816g_t_vcm_timing,
+	},
+	[IMX135_VICTORIABAY] = {
+		.power_up = dw9719_vcm_power_up,
+		.power_down = dw9719_vcm_power_down,
+		.init = dw9719_vcm_init,
+		.t_focus_vcm = dw9719_t_focus_vcm,
+		.t_focus_abs = dw9719_t_focus_abs,
+		.t_focus_abs_init = NULL,
+		.t_focus_rel = dw9719_t_focus_rel,
+		.q_focus_status = dw9719_q_focus_status,
+		.q_focus_abs = dw9719_q_focus_abs,
+		.t_vcm_slew = dw9719_t_vcm_slew,
+		.t_vcm_timing = dw9719_t_vcm_timing,
+	},
+	[IMX134_VALLEYVIEW] = {
+		.power_up = dw9714_vcm_power_up,
+		.power_down = dw9714_vcm_power_down,
+		.init = dw9714_vcm_init,
+		.t_focus_vcm = dw9714_t_focus_vcm,
+		.t_focus_abs = dw9714_t_focus_abs,
+		.t_focus_abs_init = dw9714_t_focus_abs_init,
+		.t_focus_rel = dw9714_t_focus_rel,
+		.q_focus_status = dw9714_q_focus_status,
+		.q_focus_abs = dw9714_q_focus_abs,
+		.t_vcm_slew = dw9714_t_vcm_slew,
+		.t_vcm_timing = dw9714_t_vcm_timing,
+	},
+	[IMX219_MFV0_PRH] = {
+		.power_up = dw9718_vcm_power_up,
+		.power_down = dw9718_vcm_power_down,
+		.init = dw9718_vcm_init,
+		.t_focus_vcm = dw9718_t_focus_vcm,
+		.t_focus_abs = dw9718_t_focus_abs,
+		.t_focus_abs_init = NULL,
+		.t_focus_rel = dw9718_t_focus_rel,
+		.q_focus_status = dw9718_q_focus_status,
+		.q_focus_abs = dw9718_q_focus_abs,
+		.t_vcm_slew = dw9718_t_vcm_slew,
+		.t_vcm_timing = dw9718_t_vcm_timing,
+	},
+	[IMX_ID_DEFAULT] = {
+		.power_up = NULL,
+		.power_down = NULL,
+		.t_focus_abs_init = NULL,
+	},
+};
+
+extern void *dummy_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+extern void *imx_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+extern void *e2prom_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+extern void *brcc064_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+extern void *imx227_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+extern void *e2prom_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size);
+struct imx_otp imx_otps[] = {
+	[IMX175_MERRFLD] = {
+		.otp_read = imx_otp_read,
+		.dev_addr = E2PROM_ADDR,
+		.start_addr = 0,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX175_VALLEYVIEW] = {
+		.otp_read = e2prom_otp_read,
+		.dev_addr = E2PROM_ABICO_SS89A839_ADDR,
+		.start_addr = E2PROM_2ADDR,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX135_SALTBAY] = {
+		.otp_read = e2prom_otp_read,
+		.dev_addr = E2PROM_ADDR,
+		.start_addr = 0,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX135_VICTORIABAY] = {
+		.otp_read = imx_otp_read,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX134_VALLEYVIEW] = {
+		.otp_read = e2prom_otp_read,
+		.dev_addr = E2PROM_LITEON_12P1BA869D_ADDR,
+		.start_addr = 0,
+		.size = E2PROM_LITEON_12P1BA869D_SIZE,
+	},
+	[IMX132_SALTBAY] = {
+		.otp_read = dummy_otp_read,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX208_MOFD_PD2] = {
+		.otp_read = dummy_otp_read,
+		.size = DEFAULT_OTP_SIZE,
+	},
+	[IMX219_MFV0_PRH] = {
+		.otp_read = brcc064_otp_read,
+		.dev_addr = E2PROM_ADDR,
+		.start_addr = 0,
+		.size = IMX219_OTP_SIZE,
+	},
+	[IMX227_SAND] = {
+		.otp_read = imx227_otp_read,
+		.size = IMX227_OTP_SIZE,
+	},
+	[IMX_ID_DEFAULT] = {
+		.otp_read = dummy_otp_read,
+		.size = DEFAULT_OTP_SIZE,
+	},
+};
+
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx132.h b/drivers/staging/media/atomisp/i2c/imx/imx132.h
new file mode 100644
index 000000000000..98f047b8a1ba
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx132.h
@@ -0,0 +1,566 @@
+/*
+ * Support for Sony IMX 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __IMX132_H__
+#define __IMX132_H__
+#include "common.h"
+
+/********************** registers define ********************************/
+#define IMX132_RGLANESEL			0x3301	/* Number of lanes */
+#define IMX132_RGLANESEL_1LANE			0x01
+#define IMX132_RGLANESEL_2LANES			0x00
+#define IMX132_RGLANESEL_4LANES			0x03
+
+#define IMX132_2LANES_GAINFACT			2096	/* 524/256 * 2^10 */
+#define IMX132_2LANES_GAINFACT_SHIFT		10
+
+/********************** settings for imx from vendor*********************/
+static struct imx_reg imx132_1080p_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Global Settings */
+	{IMX_8BIT, 0x3087, 0x53},
+	{IMX_8BIT, 0x308B, 0x5A},
+	{IMX_8BIT, 0x3094, 0x11},
+	{IMX_8BIT, 0x309D, 0xA4},
+	{IMX_8BIT, 0x30AA, 0x01},
+	{IMX_8BIT, 0x30C6, 0x00},
+	{IMX_8BIT, 0x30C7, 0x00},
+	{IMX_8BIT, 0x3118, 0x2F},
+	{IMX_8BIT, 0x312A, 0x00},
+	{IMX_8BIT, 0x312B, 0x0B},
+	{IMX_8BIT, 0x312C, 0x0B},
+	{IMX_8BIT, 0x312D, 0x13},
+	/* PLL setting */
+	{IMX_8BIT, 0x0305, 0x02},
+	{IMX_8BIT, 0x0307, 0x50},
+	{IMX_8BIT, 0x30A4, 0x02},
+	{IMX_8BIT, 0x303C, 0x3C},
+	/* Mode setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x14},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x32},
+	{IMX_8BIT, 0x0348, 0x07},
+	{IMX_8BIT, 0x0349, 0xA3},
+	{IMX_8BIT, 0x034A, 0x04},
+	{IMX_8BIT, 0x034B, 0x79},
+	{IMX_8BIT, 0x034C, 0x07},
+	{IMX_8BIT, 0x034D, 0x90},
+	{IMX_8BIT, 0x034E, 0x04},
+	{IMX_8BIT, 0x034F, 0x48},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x303D, 0x10},
+	{IMX_8BIT, 0x303E, 0x5A},
+	{IMX_8BIT, 0x3040, 0x00},
+	{IMX_8BIT, 0x3041, 0x00},
+	{IMX_8BIT, 0x3048, 0x00},
+	{IMX_8BIT, 0x304C, 0x2F},
+	{IMX_8BIT, 0x304D, 0x02},
+	{IMX_8BIT, 0x3064, 0x92},
+	{IMX_8BIT, 0x306A, 0x10},
+	{IMX_8BIT, 0x309B, 0x00},
+	{IMX_8BIT, 0x309E, 0x41},
+	{IMX_8BIT, 0x30A0, 0x10},
+	{IMX_8BIT, 0x30A1, 0x0B},
+	{IMX_8BIT, 0x30B2, 0x00},
+	{IMX_8BIT, 0x30D5, 0x00},
+	{IMX_8BIT, 0x30D6, 0x00},
+	{IMX_8BIT, 0x30D7, 0x00},
+	{IMX_8BIT, 0x30D8, 0x00},
+	{IMX_8BIT, 0x30D9, 0x00},
+	{IMX_8BIT, 0x30DA, 0x00},
+	{IMX_8BIT, 0x30DB, 0x00},
+	{IMX_8BIT, 0x30DC, 0x00},
+	{IMX_8BIT, 0x30DD, 0x00},
+	{IMX_8BIT, 0x30DE, 0x00},
+	{IMX_8BIT, 0x3102, 0x0C},
+	{IMX_8BIT, 0x3103, 0x33},
+	{IMX_8BIT, 0x3104, 0x18},
+	{IMX_8BIT, 0x3105, 0x00},
+	{IMX_8BIT, 0x3106, 0x65},
+	{IMX_8BIT, 0x3107, 0x00},
+	{IMX_8BIT, 0x3108, 0x06},
+	{IMX_8BIT, 0x3109, 0x04},
+	{IMX_8BIT, 0x310A, 0x04},
+	{IMX_8BIT, 0x315C, 0x3D},
+	{IMX_8BIT, 0x315D, 0x3C},
+	{IMX_8BIT, 0x316E, 0x3E},
+	{IMX_8BIT, 0x316F, 0x3D},
+	/* Global timing */
+	{IMX_8BIT, 0x3304, 0x07}, /* RGTLPX[5:0] TLPX */
+	{IMX_8BIT, 0x3305, 0x06}, /* RGTCLKPREPARE[3:0] TCLK-PREPARE */
+	{IMX_8BIT, 0x3306, 0x19}, /* RGTCLKZERO[5:0] TCLK-ZERO */
+	{IMX_8BIT, 0x3307, 0x03}, /* RGTCLKPRE[5:0] TCLK-PRE */
+	{IMX_8BIT, 0x3308, 0x0F}, /* RGTCLKPOST[5:0] TCLK-POST */
+	{IMX_8BIT, 0x3309, 0x07}, /* RGTCLKTRAIL[3:0] TCLK-TRAIL */
+	{IMX_8BIT, 0x330A, 0x0C}, /* RGTHSEXIT[5:0] THS-EXIT */
+	{IMX_8BIT, 0x330B, 0x06}, /* RGTHSPREPARE[3:0] THS-PREPARE */
+	{IMX_8BIT, 0x330C, 0x0B}, /* RGTHSZERO[5:0] THS-ZERO */
+	{IMX_8BIT, 0x330D, 0x07}, /* RGTHSTRAIL[3:0] THS-TRAIL */
+	{IMX_8BIT, 0x330E, 0x03},
+	{IMX_8BIT, 0x3318, 0x62},
+	{IMX_8BIT, 0x3322, 0x09},
+	{IMX_8BIT, 0x3342, 0x00},
+	{IMX_8BIT, 0x3348, 0xE0},
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx132_1456x1096_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Global Settings */
+	{IMX_8BIT, 0x3087, 0x53},
+	{IMX_8BIT, 0x308B, 0x5A},
+	{IMX_8BIT, 0x3094, 0x11},
+	{IMX_8BIT, 0x309D, 0xA4},
+	{IMX_8BIT, 0x30AA, 0x01},
+	{IMX_8BIT, 0x30C6, 0x00},
+	{IMX_8BIT, 0x30C7, 0x00},
+	{IMX_8BIT, 0x3118, 0x2F},
+	{IMX_8BIT, 0x312A, 0x00},
+	{IMX_8BIT, 0x312B, 0x0B},
+	{IMX_8BIT, 0x312C, 0x0B},
+	{IMX_8BIT, 0x312D, 0x13},
+	/* PLL setting */
+	{IMX_8BIT, 0x0305, 0x02},
+	{IMX_8BIT, 0x0307, 0x50},
+	{IMX_8BIT, 0x30A4, 0x02},
+	{IMX_8BIT, 0x303C, 0x3C},
+	/* Mode setting */
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0x04},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x32},
+	{IMX_8BIT, 0x0348, 0x06},
+	{IMX_8BIT, 0x0349, 0xB3},
+	{IMX_8BIT, 0x034A, 0x04},
+	{IMX_8BIT, 0x034B, 0x79},
+	{IMX_8BIT, 0x034C, 0x05},
+	{IMX_8BIT, 0x034D, 0xB0},
+	{IMX_8BIT, 0x034E, 0x04},
+	{IMX_8BIT, 0x034F, 0x48},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x303D, 0x10},
+	{IMX_8BIT, 0x303E, 0x5A},
+	{IMX_8BIT, 0x3040, 0x00},
+	{IMX_8BIT, 0x3041, 0x00},
+	{IMX_8BIT, 0x3048, 0x00},
+	{IMX_8BIT, 0x304C, 0x2F},
+	{IMX_8BIT, 0x304D, 0x02},
+	{IMX_8BIT, 0x3064, 0x92},
+	{IMX_8BIT, 0x306A, 0x10},
+	{IMX_8BIT, 0x309B, 0x00},
+	{IMX_8BIT, 0x309E, 0x41},
+	{IMX_8BIT, 0x30A0, 0x10},
+	{IMX_8BIT, 0x30A1, 0x0B},
+	{IMX_8BIT, 0x30B2, 0x00},
+	{IMX_8BIT, 0x30D5, 0x00},
+	{IMX_8BIT, 0x30D6, 0x00},
+	{IMX_8BIT, 0x30D7, 0x00},
+	{IMX_8BIT, 0x30D8, 0x00},
+	{IMX_8BIT, 0x30D9, 0x00},
+	{IMX_8BIT, 0x30DA, 0x00},
+	{IMX_8BIT, 0x30DB, 0x00},
+	{IMX_8BIT, 0x30DC, 0x00},
+	{IMX_8BIT, 0x30DD, 0x00},
+	{IMX_8BIT, 0x30DE, 0x00},
+	{IMX_8BIT, 0x3102, 0x0C},
+	{IMX_8BIT, 0x3103, 0x33},
+	{IMX_8BIT, 0x3104, 0x18},
+	{IMX_8BIT, 0x3105, 0x00},
+	{IMX_8BIT, 0x3106, 0x65},
+	{IMX_8BIT, 0x3107, 0x00},
+	{IMX_8BIT, 0x3108, 0x06},
+	{IMX_8BIT, 0x3109, 0x04},
+	{IMX_8BIT, 0x310A, 0x04},
+	{IMX_8BIT, 0x315C, 0x3D},
+	{IMX_8BIT, 0x315D, 0x3C},
+	{IMX_8BIT, 0x316E, 0x3E},
+	{IMX_8BIT, 0x316F, 0x3D},
+	/* Global timing */
+	{IMX_8BIT, 0x3304, 0x07}, /* RGTLPX[5:0] TLPX */
+	{IMX_8BIT, 0x3305, 0x06}, /* RGTCLKPREPARE[3:0] TCLK-PREPARE */
+	{IMX_8BIT, 0x3306, 0x19}, /* RGTCLKZERO[5:0] TCLK-ZERO */
+	{IMX_8BIT, 0x3307, 0x03}, /* RGTCLKPRE[5:0] TCLK-PRE */
+	{IMX_8BIT, 0x3308, 0x0F}, /* RGTCLKPOST[5:0] TCLK-POST */
+	{IMX_8BIT, 0x3309, 0x07}, /* RGTCLKTRAIL[3:0] TCLK-TRAIL */
+	{IMX_8BIT, 0x330A, 0x0C}, /* RGTHSEXIT[5:0] THS-EXIT */
+	{IMX_8BIT, 0x330B, 0x06}, /* RGTHSPREPARE[3:0] THS-PREPARE */
+	{IMX_8BIT, 0x330C, 0x0B}, /* RGTHSZERO[5:0] THS-ZERO */
+	{IMX_8BIT, 0x330D, 0x07}, /* RGTHSTRAIL[3:0] THS-TRAIL */
+	{IMX_8BIT, 0x330E, 0x03},
+	{IMX_8BIT, 0x3318, 0x62},
+	{IMX_8BIT, 0x3322, 0x09},
+	{IMX_8BIT, 0x3342, 0x00},
+	{IMX_8BIT, 0x3348, 0xE0},
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx132_1636x1096_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Global Settings */
+	{IMX_8BIT, 0x3087, 0x53},
+	{IMX_8BIT, 0x308B, 0x5A},
+	{IMX_8BIT, 0x3094, 0x11},
+	{IMX_8BIT, 0x309D, 0xA4},
+	{IMX_8BIT, 0x30AA, 0x01},
+	{IMX_8BIT, 0x30C6, 0x00},
+	{IMX_8BIT, 0x30C7, 0x00},
+	{IMX_8BIT, 0x3118, 0x2F},
+	{IMX_8BIT, 0x312A, 0x00},
+	{IMX_8BIT, 0x312B, 0x0B},
+	{IMX_8BIT, 0x312C, 0x0B},
+	{IMX_8BIT, 0x312D, 0x13},
+	/* PLL setting */
+	{IMX_8BIT, 0x0305, 0x02},
+	{IMX_8BIT, 0x0307, 0x50},
+	{IMX_8BIT, 0x30A4, 0x02},
+	{IMX_8BIT, 0x303C, 0x3C},
+	/* Mode setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0xAA},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x32},
+	{IMX_8BIT, 0x0348, 0x07},
+	{IMX_8BIT, 0x0349, 0x0D},
+	{IMX_8BIT, 0x034A, 0x04},
+	{IMX_8BIT, 0x034B, 0x79},
+	{IMX_8BIT, 0x034C, 0x06},
+	{IMX_8BIT, 0x034D, 0x64},
+	{IMX_8BIT, 0x034E, 0x04},
+	{IMX_8BIT, 0x034F, 0x48},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x303D, 0x10},
+	{IMX_8BIT, 0x303E, 0x5A},
+	{IMX_8BIT, 0x3040, 0x00},
+	{IMX_8BIT, 0x3041, 0x00},
+	{IMX_8BIT, 0x3048, 0x00},
+	{IMX_8BIT, 0x304C, 0x2F},
+	{IMX_8BIT, 0x304D, 0x02},
+	{IMX_8BIT, 0x3064, 0x92},
+	{IMX_8BIT, 0x306A, 0x10},
+	{IMX_8BIT, 0x309B, 0x00},
+	{IMX_8BIT, 0x309E, 0x41},
+	{IMX_8BIT, 0x30A0, 0x10},
+	{IMX_8BIT, 0x30A1, 0x0B},
+	{IMX_8BIT, 0x30B2, 0x00},
+	{IMX_8BIT, 0x30D5, 0x00},
+	{IMX_8BIT, 0x30D6, 0x00},
+	{IMX_8BIT, 0x30D7, 0x00},
+	{IMX_8BIT, 0x30D8, 0x00},
+	{IMX_8BIT, 0x30D9, 0x00},
+	{IMX_8BIT, 0x30DA, 0x00},
+	{IMX_8BIT, 0x30DB, 0x00},
+	{IMX_8BIT, 0x30DC, 0x00},
+	{IMX_8BIT, 0x30DD, 0x00},
+	{IMX_8BIT, 0x30DE, 0x00},
+	{IMX_8BIT, 0x3102, 0x0C},
+	{IMX_8BIT, 0x3103, 0x33},
+	{IMX_8BIT, 0x3104, 0x18},
+	{IMX_8BIT, 0x3105, 0x00},
+	{IMX_8BIT, 0x3106, 0x65},
+	{IMX_8BIT, 0x3107, 0x00},
+	{IMX_8BIT, 0x3108, 0x06},
+	{IMX_8BIT, 0x3109, 0x04},
+	{IMX_8BIT, 0x310A, 0x04},
+	{IMX_8BIT, 0x315C, 0x3D},
+	{IMX_8BIT, 0x315D, 0x3C},
+	{IMX_8BIT, 0x316E, 0x3E},
+	{IMX_8BIT, 0x316F, 0x3D},
+	/* Global timing */
+	{IMX_8BIT, 0x3304, 0x07}, /* RGTLPX[5:0] TLPX */
+	{IMX_8BIT, 0x3305, 0x06}, /* RGTCLKPREPARE[3:0] TCLK-PREPARE */
+	{IMX_8BIT, 0x3306, 0x19}, /* RGTCLKZERO[5:0] TCLK-ZERO */
+	{IMX_8BIT, 0x3307, 0x03}, /* RGTCLKPRE[5:0] TCLK-PRE */
+	{IMX_8BIT, 0x3308, 0x0F}, /* RGTCLKPOST[5:0] TCLK-POST */
+	{IMX_8BIT, 0x3309, 0x07}, /* RGTCLKTRAIL[3:0] TCLK-TRAIL */
+	{IMX_8BIT, 0x330A, 0x0C}, /* RGTHSEXIT[5:0] THS-EXIT */
+	{IMX_8BIT, 0x330B, 0x06}, /* RGTHSPREPARE[3:0] THS-PREPARE */
+	{IMX_8BIT, 0x330C, 0x0B}, /* RGTHSZERO[5:0] THS-ZERO */
+	{IMX_8BIT, 0x330D, 0x07}, /* RGTHSTRAIL[3:0] THS-TRAIL */
+	{IMX_8BIT, 0x330E, 0x03},
+	{IMX_8BIT, 0x3318, 0x62},
+	{IMX_8BIT, 0x3322, 0x09},
+	{IMX_8BIT, 0x3342, 0x00},
+	{IMX_8BIT, 0x3348, 0xE0},
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx132_1336x1096_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Global Settings */
+	{IMX_8BIT, 0x3087, 0x53},
+	{IMX_8BIT, 0x308B, 0x5A},
+	{IMX_8BIT, 0x3094, 0x11},
+	{IMX_8BIT, 0x309D, 0xA4},
+	{IMX_8BIT, 0x30AA, 0x01},
+	{IMX_8BIT, 0x30C6, 0x00},
+	{IMX_8BIT, 0x30C7, 0x00},
+	{IMX_8BIT, 0x3118, 0x2F},
+	{IMX_8BIT, 0x312A, 0x00},
+	{IMX_8BIT, 0x312B, 0x0B},
+	{IMX_8BIT, 0x312C, 0x0B},
+	{IMX_8BIT, 0x312D, 0x13},
+	/* PLL setting */
+	{IMX_8BIT, 0x0305, 0x02},
+	{IMX_8BIT, 0x0307, 0x50},
+	{IMX_8BIT, 0x30A4, 0x02},
+	{IMX_8BIT, 0x303C, 0x3C},
+	/* Mode setting */
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0x2C},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x32},
+	{IMX_8BIT, 0x0348, 0x06},
+	{IMX_8BIT, 0x0349, 0x77},
+	{IMX_8BIT, 0x034A, 0x04},
+	{IMX_8BIT, 0x034B, 0x79},
+	{IMX_8BIT, 0x034C, 0x05},
+	{IMX_8BIT, 0x034D, 0x38},
+	{IMX_8BIT, 0x034E, 0x04},
+	{IMX_8BIT, 0x034F, 0x48},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x303D, 0x10},
+	{IMX_8BIT, 0x303E, 0x5A},
+	{IMX_8BIT, 0x3040, 0x00},
+	{IMX_8BIT, 0x3041, 0x00},
+	{IMX_8BIT, 0x3048, 0x00},
+	{IMX_8BIT, 0x304C, 0x2F},
+	{IMX_8BIT, 0x304D, 0x02},
+	{IMX_8BIT, 0x3064, 0x92},
+	{IMX_8BIT, 0x306A, 0x10},
+	{IMX_8BIT, 0x309B, 0x00},
+	{IMX_8BIT, 0x309E, 0x41},
+	{IMX_8BIT, 0x30A0, 0x10},
+	{IMX_8BIT, 0x30A1, 0x0B},
+	{IMX_8BIT, 0x30B2, 0x00},
+	{IMX_8BIT, 0x30D5, 0x00},
+	{IMX_8BIT, 0x30D6, 0x00},
+	{IMX_8BIT, 0x30D7, 0x00},
+	{IMX_8BIT, 0x30D8, 0x00},
+	{IMX_8BIT, 0x30D9, 0x00},
+	{IMX_8BIT, 0x30DA, 0x00},
+	{IMX_8BIT, 0x30DB, 0x00},
+	{IMX_8BIT, 0x30DC, 0x00},
+	{IMX_8BIT, 0x30DD, 0x00},
+	{IMX_8BIT, 0x30DE, 0x00},
+	{IMX_8BIT, 0x3102, 0x0C},
+	{IMX_8BIT, 0x3103, 0x33},
+	{IMX_8BIT, 0x3104, 0x18},
+	{IMX_8BIT, 0x3105, 0x00},
+	{IMX_8BIT, 0x3106, 0x65},
+	{IMX_8BIT, 0x3107, 0x00},
+	{IMX_8BIT, 0x3108, 0x06},
+	{IMX_8BIT, 0x3109, 0x04},
+	{IMX_8BIT, 0x310A, 0x04},
+	{IMX_8BIT, 0x315C, 0x3D},
+	{IMX_8BIT, 0x315D, 0x3C},
+	{IMX_8BIT, 0x316E, 0x3E},
+	{IMX_8BIT, 0x316F, 0x3D},
+	/* Global timing */
+	{IMX_8BIT, 0x3304, 0x07}, /* RGTLPX[5:0] TLPX */
+	{IMX_8BIT, 0x3305, 0x06}, /* RGTCLKPREPARE[3:0] TCLK-PREPARE */
+	{IMX_8BIT, 0x3306, 0x19}, /* RGTCLKZERO[5:0] TCLK-ZERO */
+	{IMX_8BIT, 0x3307, 0x03}, /* RGTCLKPRE[5:0] TCLK-PRE */
+	{IMX_8BIT, 0x3308, 0x0F}, /* RGTCLKPOST[5:0] TCLK-POST */
+	{IMX_8BIT, 0x3309, 0x07}, /* RGTCLKTRAIL[3:0] TCLK-TRAIL */
+	{IMX_8BIT, 0x330A, 0x0C}, /* RGTHSEXIT[5:0] THS-EXIT */
+	{IMX_8BIT, 0x330B, 0x06}, /* RGTHSPREPARE[3:0] THS-PREPARE */
+	{IMX_8BIT, 0x330C, 0x0B}, /* RGTHSZERO[5:0] THS-ZERO */
+	{IMX_8BIT, 0x330D, 0x07}, /* RGTHSTRAIL[3:0] THS-TRAIL */
+	{IMX_8BIT, 0x330E, 0x03},
+	{IMX_8BIT, 0x3318, 0x62},
+	{IMX_8BIT, 0x3322, 0x09},
+	{IMX_8BIT, 0x3342, 0x00},
+	{IMX_8BIT, 0x3348, 0xE0},
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/********************** settings for imx - reference *********************/
+static struct imx_reg const imx132_init_settings[] = {
+	/* sw reset */
+	{ IMX_8BIT, 0x0100, 0x00 },
+	{ IMX_8BIT, 0x0103, 0x01 },
+	{ IMX_TOK_DELAY, 0, 5},
+	{ IMX_8BIT, 0x0103, 0x00 },
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Global Settings */
+	{IMX_8BIT, 0x3087, 0x53},
+	{IMX_8BIT, 0x308B, 0x5A},
+	{IMX_8BIT, 0x3094, 0x11},
+	{IMX_8BIT, 0x309D, 0xA4},
+	{IMX_8BIT, 0x30AA, 0x01},
+	{IMX_8BIT, 0x30C6, 0x00},
+	{IMX_8BIT, 0x30C7, 0x00},
+	{IMX_8BIT, 0x3118, 0x2F},
+	{IMX_8BIT, 0x312A, 0x00},
+	{IMX_8BIT, 0x312B, 0x0B},
+	{IMX_8BIT, 0x312C, 0x0B},
+	{IMX_8BIT, 0x312D, 0x13},
+	GROUPED_PARAMETER_HOLD_DISABLE,
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+struct imx_resolution imx132_res_preview[] = {
+	{
+		.desc = "imx132_1080p_30fps",
+		.regs = imx132_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+};
+
+struct imx_resolution imx132_res_still[] = {
+	{
+		.desc = "imx132_1080p_30fps",
+		.regs = imx132_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+};
+
+struct imx_resolution imx132_res_video[] = {
+	{
+		.desc = "imx132_1336x1096_30fps",
+		.regs = imx132_1336x1096_30fps,
+		.width = 1336,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+	{
+		.desc = "imx132_1456x1096_30fps",
+		.regs = imx132_1456x1096_30fps,
+		.width = 1456,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+	{
+		.desc = "imx132_1636x1096_30fps",
+		.regs = imx132_1636x1096_30fps,
+		.width = 1636,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+	{
+		.desc = "imx132_1080p_30fps",
+		.regs = imx132_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08F2,
+				.lines_per_frame = 0x045C,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 384000,
+	},
+};
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx134.h b/drivers/staging/media/atomisp/i2c/imx/imx134.h
new file mode 100644
index 000000000000..cf35197ed77f
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx134.h
@@ -0,0 +1,2464 @@
+#ifndef __IMX134_H__
+#define __IMX134_H__
+
+/********************** imx134 setting - version 1 *********************/
+static struct imx_reg const imx134_init_settings[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* Basic settings */
+	{ IMX_8BIT, 0x0105, 0x01 },
+	{ IMX_8BIT, 0x0220, 0x01 },
+	{ IMX_8BIT, 0x3302, 0x11 },
+	{ IMX_8BIT, 0x3833, 0x20 },
+	{ IMX_8BIT, 0x3893, 0x00 },
+	{ IMX_8BIT, 0x3906, 0x08 },
+	{ IMX_8BIT, 0x3907, 0x01 },
+	{ IMX_8BIT, 0x391B, 0x01 },
+	{ IMX_8BIT, 0x3C09, 0x01 },
+	{ IMX_8BIT, 0x600A, 0x00 },
+
+	/* Analog settings */
+	{ IMX_8BIT, 0x3008, 0xB0 },
+	{ IMX_8BIT, 0x320A, 0x01 },
+	{ IMX_8BIT, 0x320D, 0x10 },
+	{ IMX_8BIT, 0x3216, 0x2E },
+	{ IMX_8BIT, 0x322C, 0x02 },
+	{ IMX_8BIT, 0x3409, 0x0C },
+	{ IMX_8BIT, 0x340C, 0x2D },
+	{ IMX_8BIT, 0x3411, 0x39 },
+	{ IMX_8BIT, 0x3414, 0x1E },
+	{ IMX_8BIT, 0x3427, 0x04 },
+	{ IMX_8BIT, 0x3480, 0x1E },
+	{ IMX_8BIT, 0x3484, 0x1E },
+	{ IMX_8BIT, 0x3488, 0x1E },
+	{ IMX_8BIT, 0x348C, 0x1E },
+	{ IMX_8BIT, 0x3490, 0x1E },
+	{ IMX_8BIT, 0x3494, 0x1E },
+	{ IMX_8BIT, 0x3511, 0x8F },
+	{ IMX_8BIT, 0x3617, 0x2D },
+
+	GROUPED_PARAMETER_HOLD_DISABLE,
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 3280x2464 8M 30fps, vendor provide */
+static struct imx_reg const imx134_8M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* clock setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },
+	{ IMX_8BIT, 0x0391, 0x11 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 }, /* down scaling 16/16 = 1 */
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:0	*/
+	{ IMX_8BIT, 0x0345, 0x00 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x00 },      /*	y_addr_start[15:8]:0	*/
+	{ IMX_8BIT, 0x0347, 0x00 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3279	*/
+	{ IMX_8BIT, 0x0349, 0xCF },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x09 },      /*	y_addr_end[15:8]:2463	*/
+	{ IMX_8BIT, 0x034B, 0x9F },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x0C },      /*	x_output_size[15:8]: 3280*/
+	{ IMX_8BIT, 0x034D, 0xD0 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x09 },      /*	y_output_size[15:8]:2464 */
+	{ IMX_8BIT, 0x034F, 0xA0 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x0C },
+	{ IMX_8BIT, 0x0355, 0xD0 },
+	{ IMX_8BIT, 0x0356, 0x09 },
+	{ IMX_8BIT, 0x0357, 0xA0 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x0C },
+	{ IMX_8BIT, 0x3311, 0xD0 },
+	{ IMX_8BIT, 0x3312, 0x09 },
+	{ IMX_8BIT, 0x3313, 0xA0 },
+	{ IMX_8BIT, 0x331C, 0x01 },
+	{ IMX_8BIT, 0x331D, 0xAE },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global timing setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration time setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/2 binning 30fps 1640x1232, vendor provide */
+static struct imx_reg const imx134_1640_1232_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* no resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0345, 0x00 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0347, 0x00 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:3279 */
+	{ IMX_8BIT, 0x0349, 0xCF },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2463 */
+	{ IMX_8BIT, 0x034B, 0x9F },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x06 },      /* x_output_size[15:8]:1640 */
+	{ IMX_8BIT, 0x034D, 0x68 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x04 },      /* y_output_size[15:8]:1232 */
+	{ IMX_8BIT, 0x034F, 0xD0 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x68 },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0xD0 },
+
+	{ IMX_8BIT, 0x301D, 0x30 },
+
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x68 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0xD0 },
+
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x06 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/4 binning 30fps 820x616, vendor provide */
+static struct imx_reg const imx134_820_616_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x44 },	/* 4x4 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* no resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0345, 0x00 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0347, 0x00 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:3279 */
+	{ IMX_8BIT, 0x0349, 0xCF },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2463 */
+	{ IMX_8BIT, 0x034B, 0x9F },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x03 },      /* x_output_size[15:8]:820 */
+	{ IMX_8BIT, 0x034D, 0x34 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x02 },      /* y_output_size[15:8]:616 */
+	{ IMX_8BIT, 0x034F, 0x68 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x03 },
+	{ IMX_8BIT, 0x0355, 0x34 },
+	{ IMX_8BIT, 0x0356, 0x02 },
+	{ IMX_8BIT, 0x0357, 0x68 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x03 },
+	{ IMX_8BIT, 0x3311, 0x34 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x68 },
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/4 binning 30fps 820x552 */
+static struct imx_reg const imx134_820_552_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x44 },	/* 4x4 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* no resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0345, 0x00 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:128 */
+	{ IMX_8BIT, 0x0347, 0x80 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:3280-1 */
+	{ IMX_8BIT, 0x0349, 0xCF },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2208+128-1 */
+	{ IMX_8BIT, 0x034B, 0x1F },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x03 },      /* x_output_size[15:8]: */
+	{ IMX_8BIT, 0x034D, 0x34 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x02 },      /* y_output_size[15:8]:616 */
+	{ IMX_8BIT, 0x034F, 0x28 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x03 },
+	{ IMX_8BIT, 0x0355, 0x34 },
+	{ IMX_8BIT, 0x0356, 0x02 },
+	{ IMX_8BIT, 0x0357, 0x28 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x03 },
+	{ IMX_8BIT, 0x3311, 0x34 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x28 },
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/4 binning 30fps 720x592 */
+static struct imx_reg const imx134_720_592_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x44 },	/* 4x4 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* no resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:200 */
+	{ IMX_8BIT, 0x0345, 0xC8 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:40 */
+	{ IMX_8BIT, 0x0347, 0x28 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:2880+200-1 */
+	{ IMX_8BIT, 0x0349, 0x07 },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2368+40-1 */
+	{ IMX_8BIT, 0x034B, 0x67 },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x02 },      /* x_output_size[15:8]: */
+	{ IMX_8BIT, 0x034D, 0xD0 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x02 },      /* y_output_size[15:8]:616 */
+	{ IMX_8BIT, 0x034F, 0x50 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x02 },
+	{ IMX_8BIT, 0x0355, 0xD0 },
+	{ IMX_8BIT, 0x0356, 0x02 },
+	{ IMX_8BIT, 0x0357, 0x50 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x02 },
+	{ IMX_8BIT, 0x3311, 0xD0 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x50 },
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+static struct imx_reg const imx134_752_616_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x44 },	/* 4x4 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* no resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:136 */
+	{ IMX_8BIT, 0x0345, 0x88 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0347, 0x00 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:3145+134-1 */
+	{ IMX_8BIT, 0x0349, 0x47 },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2463 */
+	{ IMX_8BIT, 0x034B, 0x9F },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x02 },      /* x_output_size[15:8]: 752*/
+	{ IMX_8BIT, 0x034D, 0xF0 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x02 },      /* y_output_size[15:8]:616 */
+	{ IMX_8BIT, 0x034F, 0x68 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+
+	{ IMX_8BIT, 0x0354, 0x02 },
+	{ IMX_8BIT, 0x0355, 0xF0 },
+	{ IMX_8BIT, 0x0356, 0x02 },
+	{ IMX_8BIT, 0x0357, 0x68 },
+
+	{ IMX_8BIT, 0x301D, 0x30 },
+
+	{ IMX_8BIT, 0x3310, 0x02 },
+	{ IMX_8BIT, 0x3311, 0xF0 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x68 },
+
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 1424x1168  */
+static struct imx_reg const imx134_1424_1168_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x11 },	/* no binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x22 },	/* 34/16=2.125 */
+	{ IMX_8BIT, 0x4082, 0x00 },	/* ?? */
+	{ IMX_8BIT, 0x4083, 0x00 },	/* ?? */
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /* x_addr_start[15:8]:136 */
+	{ IMX_8BIT, 0x0345, 0x80 },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x00 },      /* y_addr_start[15:8]:0 */
+	{ IMX_8BIT, 0x0347, 0x00 },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0C },      /* x_addr_end[15:8]:3145+134-1 */
+	{ IMX_8BIT, 0x0349, 0x51 },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x09 },      /* y_addr_end[15:8]:2463 */
+	{ IMX_8BIT, 0x034B, 0xB1 },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x05 },      /* x_output_size[15:8]: 1424*/
+	{ IMX_8BIT, 0x034D, 0x90 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x04 },      /* y_output_size[15:8]:1168 */
+	{ IMX_8BIT, 0x034F, 0x90 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+
+	{ IMX_8BIT, 0x0354, 0x0B },
+	{ IMX_8BIT, 0x0355, 0xD2 },
+	{ IMX_8BIT, 0x0356, 0x09 },
+	{ IMX_8BIT, 0x0357, 0xB2 },
+
+	{ IMX_8BIT, 0x301D, 0x30 },
+
+	{ IMX_8BIT, 0x3310, 0x05 },
+	{ IMX_8BIT, 0x3311, 0x90 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0x90 },
+
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+	{ IMX_8BIT, 0x4084, 0x05 },
+	{ IMX_8BIT, 0x4085, 0x90 },
+	{ IMX_8BIT, 0x4086, 0x04 },
+	{ IMX_8BIT, 0x4087, 0x90 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/4 binning, 16/35 down scaling, 30fps, dvs */
+static struct imx_reg const imx134_240_196_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/*4x4 binning */
+	{ IMX_8BIT, 0x0391, 0x44 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x23 },	/* down scaling = 16/35 */
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x02 },      /* x_addr_start[15:8]:590 */
+	{ IMX_8BIT, 0x0345, 0x4E },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x01 },      /* y_addr_start[15:8]:366 */
+	{ IMX_8BIT, 0x0347, 0x6E },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0A },      /* x_addr_end[15:8]:2104+590-1 */
+	{ IMX_8BIT, 0x0349, 0x85 },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x08 },      /* y_addr_end[15:8]:1720+366-1 */
+	{ IMX_8BIT, 0x034B, 0x25 },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x00 },      /* x_output_size[15:8]: 240*/
+	{ IMX_8BIT, 0x034D, 0xF0 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x00 },      /* y_output_size[15:8]:196 */
+	{ IMX_8BIT, 0x034F, 0xC4 },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x02 },	/* crop_x: 526 */
+	{ IMX_8BIT, 0x0355, 0x0E },
+	{ IMX_8BIT, 0x0356, 0x01 },	/* crop_y: 430 */
+	{ IMX_8BIT, 0x0357, 0xAE },
+
+	{ IMX_8BIT, 0x301D, 0x30 },
+
+	{ IMX_8BIT, 0x3310, 0x00 },
+	{ IMX_8BIT, 0x3311, 0xF0 },
+	{ IMX_8BIT, 0x3312, 0x00 },
+	{ IMX_8BIT, 0x3313, 0xC4 },
+
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x4C },
+
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0xF0 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0xC4 },
+
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x0A },
+	{ IMX_8BIT, 0x0203, 0x88 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane, 1/2 binning, 16/38 downscaling, 30fps, dvs */
+static struct imx_reg const imx134_448_366_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x26 },	/* down scaling = 16/38 */
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x02 },      /* x_addr_start[15:8]:590 */
+	{ IMX_8BIT, 0x0345, 0x4E },      /* x_addr_start[7:0] */
+	{ IMX_8BIT, 0x0346, 0x01 },      /* y_addr_start[15:8]:366 */
+	{ IMX_8BIT, 0x0347, 0x6E },      /* y_addr_start[7:0] */
+	{ IMX_8BIT, 0x0348, 0x0A },      /* x_addr_end[15:8]:2128+590-1 */
+	{ IMX_8BIT, 0x0349, 0x9D },      /* x_addr_end[7:0] */
+	{ IMX_8BIT, 0x034A, 0x08 },      /* y_addr_end[15:8]:1740+366-1 */
+	{ IMX_8BIT, 0x034B, 0x39 },      /* y_addr_end[7:0] */
+	{ IMX_8BIT, 0x034C, 0x01 },      /* x_output_size[15:8]: 448*/
+	{ IMX_8BIT, 0x034D, 0xC0 },      /* x_output_size[7:0] */
+	{ IMX_8BIT, 0x034E, 0x01 },      /* y_output_size[15:8]:366 */
+	{ IMX_8BIT, 0x034F, 0x6E },      /* y_output_size[7:0] */
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x04 },	/* crop_x: 1064 */
+	{ IMX_8BIT, 0x0355, 0x28 },
+	{ IMX_8BIT, 0x0356, 0x03 },	/* crop_y: 870 */
+	{ IMX_8BIT, 0x0357, 0x66 },
+
+	{ IMX_8BIT, 0x301D, 0x30 },
+
+	{ IMX_8BIT, 0x3310, 0x01 },
+	{ IMX_8BIT, 0x3311, 0xC0 },
+	{ IMX_8BIT, 0x3312, 0x01 },
+	{ IMX_8BIT, 0x3313, 0x6E },
+
+	{ IMX_8BIT, 0x331C, 0x02 },
+	{ IMX_8BIT, 0x331D, 0xD0 },
+
+	{ IMX_8BIT, 0x4084, 0x01 },
+	{ IMX_8BIT, 0x4085, 0xC0 },
+	{ IMX_8BIT, 0x4086, 0x01 },
+	{ IMX_8BIT, 0x4087, 0x6E },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 2336x1312, 30fps, for 1080p dvs,  vendor provide */
+static struct imx_reg const imx134_2336_1312_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },	/* disable binning */
+	{ IMX_8BIT, 0x0391, 0x11 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x16 },	/* down scaling = 16/22 = 8/11 */
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },  /*	x_addr_start[15:8]:34	*/
+	{ IMX_8BIT, 0x0345, 0x22 },  /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },  /*	y_addr_start[15:8]:332	*/
+	{ IMX_8BIT, 0x0347, 0x4C },  /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },  /*	x_addr_end[15:8]:3245	*/
+	{ IMX_8BIT, 0x0349, 0xAD },  /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },  /*	y_addr_end[15:8]:2135	*/
+	{ IMX_8BIT, 0x034B, 0x57 },  /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x09 },  /*	x_output_size[15:8]:2336 */
+	{ IMX_8BIT, 0x034D, 0x20 },  /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x05 },  /*	y_output_size[15:8]:1312 */
+	{ IMX_8BIT, 0x034F, 0x20 },  /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x0C },
+	{ IMX_8BIT, 0x0355, 0x8C },
+	{ IMX_8BIT, 0x0356, 0x07 },
+	{ IMX_8BIT, 0x0357, 0x0C },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x09 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x05 },
+	{ IMX_8BIT, 0x3313, 0x20 },
+	{ IMX_8BIT, 0x331C, 0x03 },
+	{ IMX_8BIT, 0x331D, 0xEB },
+	{ IMX_8BIT, 0x4084, 0x09 },
+	{ IMX_8BIT, 0x4085, 0x20 },
+	{ IMX_8BIT, 0x4086, 0x05 },
+	{ IMX_8BIT, 0x4087, 0x20 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 1920x1080, 30fps, for 720p still capture */
+static struct imx_reg const imx134_1936_1096_30fps_v1[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },	/* disable binning */
+	{ IMX_8BIT, 0x0391, 0x11 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x1A },	/* downscaling 16/26*/
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },  /*	x_addr_start[15:8]:64	*/
+	{ IMX_8BIT, 0x0345, 0x40 },  /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },  /*	y_addr_start[15:8]:340	*/
+	{ IMX_8BIT, 0x0347, 0x54 },  /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },  /*	x_addr_end[15:8]:3209	*/
+	{ IMX_8BIT, 0x0349, 0x89 },  /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },  /*	y_addr_end[15:8]:2121	*/
+	{ IMX_8BIT, 0x034B, 0x49 },  /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x07 },  /*	x_output_size[15:8]:1936 */
+	{ IMX_8BIT, 0x034D, 0x90 },  /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x04 },  /*	y_output_size[15:8]:1096 */
+	{ IMX_8BIT, 0x034F, 0x48 },  /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x0C }, /* crop x:3146 */
+	{ IMX_8BIT, 0x0355, 0x4A },
+	{ IMX_8BIT, 0x0356, 0x06 }, /* xrop y:1782 */
+	{ IMX_8BIT, 0x0357, 0xF6 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x07 },
+	{ IMX_8BIT, 0x3311, 0x80 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0x38 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x1E },
+	{ IMX_8BIT, 0x4084, 0x07 },
+	{ IMX_8BIT, 0x4085, 0x80 },
+	{ IMX_8BIT, 0x4086, 0x04 },
+	{ IMX_8BIT, 0x4087, 0x38 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 1920x1080, 30fps, for 720p still capture,  vendor provide */
+static struct imx_reg const imx134_1936_1096_30fps_v2[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 }, /* disable binning */
+	{ IMX_8BIT, 0x0391, 0x11 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 }, /* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x1B }, /* downscaling 16/27*/
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* Optionnal Function setting */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },  /*	x_addr_start[15:8]:64	*/
+	{ IMX_8BIT, 0x0345, 0x06 },  /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },  /*	y_addr_start[15:8]:340	*/
+	{ IMX_8BIT, 0x0347, 0x34 },  /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },  /*	x_addr_end[15:8]:3209	*/
+	{ IMX_8BIT, 0x0349, 0xC9 },  /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },  /*	y_addr_end[15:8]:2121	*/
+	{ IMX_8BIT, 0x034B, 0x6F },  /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x07 },  /*	x_output_size[15:8]:1936 */
+	{ IMX_8BIT, 0x034D, 0x90 },  /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x04 },  /*	y_output_size[15:8]:1096 */
+	{ IMX_8BIT, 0x034F, 0x48 },  /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x0C }, /* crop x:3146 */
+	{ IMX_8BIT, 0x0355, 0xC4 },
+	{ IMX_8BIT, 0x0356, 0x07 }, /* xrop y:1782 */
+	{ IMX_8BIT, 0x0357, 0x3A },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x07 }, /* decide by mode and output size */
+	{ IMX_8BIT, 0x3311, 0x90 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0x48 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x1E },
+	{ IMX_8BIT, 0x4084, 0x07 },
+	{ IMX_8BIT, 0x4085, 0x90 },
+	{ IMX_8BIT, 0x4086, 0x04 },
+	{ IMX_8BIT, 0x4087, 0x48 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Setting */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 1296x736, 30fps, for 720p still capture,  vendor provide */
+static struct imx_reg const imx134_1296_736_30fps_v2[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning */
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x14 },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:40	*/
+	{ IMX_8BIT, 0x0345, 0x14 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },      /*	y_addr_start[15:8]:332	*/
+	{ IMX_8BIT, 0x0347, 0x38 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3239	*/
+	{ IMX_8BIT, 0x0349, 0xBB },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },      /*	y_addr_end[15:8]:2131	*/
+	{ IMX_8BIT, 0x034B, 0x67 },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x05 },      /*	x_output_size[15:8]:1280 */
+	{ IMX_8BIT, 0x034D, 0x10 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x02 },      /*	y_output_size[15:8]:720 */
+	{ IMX_8BIT, 0x034F, 0xE0 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x54 },
+	{ IMX_8BIT, 0x0356, 0x03 },
+	{ IMX_8BIT, 0x0357, 0x98 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x05 },
+	{ IMX_8BIT, 0x3311, 0x10 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0xE0 },
+	{ IMX_8BIT, 0x331C, 0x01 },
+	{ IMX_8BIT, 0x331D, 0x10 },
+	{ IMX_8BIT, 0x4084, 0x05 },
+	{ IMX_8BIT, 0x4085, 0x10 },
+	{ IMX_8BIT, 0x4086, 0x02 },
+	{ IMX_8BIT, 0x4087, 0xE0 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+/* 4 lane 1280x720, 30fps, for 720p dvs,  vendor provide */
+static struct imx_reg const imx134_1568_880_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xA9 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning*/
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },	/* down scaling 16/16 = 1 */
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:72	*/
+	{ IMX_8BIT, 0x0345, 0x48 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },      /*	y_addr_start[15:8]:356	*/
+	{ IMX_8BIT, 0x0347, 0x64 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3207	*/
+	{ IMX_8BIT, 0x0349, 0x87 },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },      /*	y_addr_end[15:8]:2115	*/
+	{ IMX_8BIT, 0x034B, 0x43 },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x06 },      /*	x_output_size[15:8]:1568 */
+	{ IMX_8BIT, 0x034D, 0x20 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x03 },      /*	y_output_size[15:8]:880 */
+	{ IMX_8BIT, 0x034F, 0x70 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x20 },
+	{ IMX_8BIT, 0x0356, 0x03 },
+	{ IMX_8BIT, 0x0357, 0x70 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x03 },
+	{ IMX_8BIT, 0x3313, 0x70 },
+	{ IMX_8BIT, 0x331C, 0x03 },
+	{ IMX_8BIT, 0x331D, 0xF2 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xAF },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+static struct imx_reg const imx134_1568_876_60fps_0625[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0x8F },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning*/
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },	/* down scaling 16/16 = 1 */
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:72	*/
+	{ IMX_8BIT, 0x0345, 0x48 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },      /*	y_addr_start[15:8]:356	*/
+	{ IMX_8BIT, 0x0347, 0x64 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3207	*/
+	{ IMX_8BIT, 0x0349, 0x87 },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },      /*	y_addr_end[15:8]:2115	*/
+	{ IMX_8BIT, 0x034B, 0x3B },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x06 },      /*	x_output_size[15:8]:1568 */
+	{ IMX_8BIT, 0x034D, 0x20 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x03 },      /*	y_output_size[15:8]:880 */
+	{ IMX_8BIT, 0x034F, 0x6C },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x20 },
+	{ IMX_8BIT, 0x0356, 0x03 },
+	{ IMX_8BIT, 0x0357, 0x6C },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x03 },
+	{ IMX_8BIT, 0x3313, 0x6C },
+	{ IMX_8BIT, 0x331C, 0x03 },
+	{ IMX_8BIT, 0x331D, 0xF2 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x6F },
+	{ IMX_8BIT, 0x0831, 0x27 },
+	{ IMX_8BIT, 0x0832, 0x4F },
+	{ IMX_8BIT, 0x0833, 0x2F },
+	{ IMX_8BIT, 0x0834, 0x2F },
+	{ IMX_8BIT, 0x0835, 0x2F },
+	{ IMX_8BIT, 0x0836, 0x9F },
+	{ IMX_8BIT, 0x0837, 0x37 },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+
+/* 4 lane for 720p dvs,  vendor provide */
+static struct imx_reg const imx134_1568_880[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xC8 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning*/
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },	/* down scaling 16/16 = 1 */
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:72	*/
+	{ IMX_8BIT, 0x0345, 0x48 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x01 },      /*	y_addr_start[15:8]:356	*/
+	{ IMX_8BIT, 0x0347, 0x64 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3207	*/
+	{ IMX_8BIT, 0x0349, 0x87 },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x08 },      /*	y_addr_end[15:8]:2115	*/
+	{ IMX_8BIT, 0x034B, 0x43 },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x06 },      /*	x_output_size[15:8]:1568 */
+	{ IMX_8BIT, 0x034D, 0x20 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x03 },      /*	y_output_size[15:8]:880 */
+	{ IMX_8BIT, 0x034F, 0x70 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x20 },
+	{ IMX_8BIT, 0x0356, 0x03 },
+	{ IMX_8BIT, 0x0357, 0x70 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x03 },
+	{ IMX_8BIT, 0x3313, 0x70 },
+	{ IMX_8BIT, 0x331C, 0x03 },
+	{ IMX_8BIT, 0x331D, 0xF2 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x5F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x37 },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xBF },
+	{ IMX_8BIT, 0x0837, 0x3F },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x09 },
+	{ IMX_8BIT, 0x0203, 0xD2 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+/* 4 lane for 480p dvs, default 60fps,  vendor provide */
+static struct imx_reg const imx134_880_592[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xC8 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },	/* binning*/
+	{ IMX_8BIT, 0x0391, 0x22 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x1D },	/* downscaling ratio = 16/29 */
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x00 },      /*	x_addr_start[15:8]:44	*/
+	{ IMX_8BIT, 0x0345, 0x2C },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x00 },      /*	y_addr_start[15:8]:160	*/
+	{ IMX_8BIT, 0x0347, 0xA0 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0C },      /*	x_addr_end[15:8]:3235	*/
+	{ IMX_8BIT, 0x0349, 0xA3 },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x09 },      /*	y_addr_end[15:8]:2307	*/
+	{ IMX_8BIT, 0x034B, 0x03 },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x03 },      /*	x_output_size[15:8]:880 */
+	{ IMX_8BIT, 0x034D, 0x70 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x02 },      /*	y_output_size[15:8]:592 */
+	{ IMX_8BIT, 0x034F, 0x50 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x06 },
+	{ IMX_8BIT, 0x0355, 0x3C },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0x32 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x03 },
+	{ IMX_8BIT, 0x3311, 0x70 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x50 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x4C },
+	{ IMX_8BIT, 0x4084, 0x03 },
+	{ IMX_8BIT, 0x4085, 0x70 },
+	{ IMX_8BIT, 0x4086, 0x02 },
+	{ IMX_8BIT, 0x4087, 0x50 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x5F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x37 },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xBF },
+	{ IMX_8BIT, 0x0837, 0x3F },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x05 },
+	{ IMX_8BIT, 0x0203, 0x42 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+static struct imx_reg const imx134_2336_1308_60fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	/* mode set clear */
+	{ IMX_8BIT, 0x3A43, 0x01 },
+	/* Clock Setting */
+	{ IMX_8BIT, 0x011E, 0x13 },
+	{ IMX_8BIT, 0x011F, 0x33 },
+	{ IMX_8BIT, 0x0301, 0x05 },
+	{ IMX_8BIT, 0x0303, 0x01 },
+	{ IMX_8BIT, 0x0305, 0x0C },
+	{ IMX_8BIT, 0x0309, 0x05 },
+	{ IMX_8BIT, 0x030B, 0x01 },
+	{ IMX_8BIT, 0x030C, 0x01 },
+	{ IMX_8BIT, 0x030D, 0xC8 },
+	{ IMX_8BIT, 0x030E, 0x01 },
+	{ IMX_8BIT, 0x3A06, 0x11 },
+
+	/* Mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },	/* binning*/
+	{ IMX_8BIT, 0x0391, 0x11 },	/* 2x2 binning */
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },	/* H/V resize */
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },	/* down scaling 16/16 = 1 */
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+
+	/* OptionnalFunction settig */
+	{ IMX_8BIT, 0x0700, 0x00 },
+	{ IMX_8BIT, 0x3A63, 0x00 },
+	{ IMX_8BIT, 0x4100, 0xF8 },
+	{ IMX_8BIT, 0x4203, 0xFF },
+	{ IMX_8BIT, 0x4344, 0x00 },
+	{ IMX_8BIT, 0x441C, 0x01 },
+
+	/* Size setting */
+	{ IMX_8BIT, 0x0344, 0x01 },      /*	x_addr_start[15:8]:72	*/
+	{ IMX_8BIT, 0x0345, 0xD8 },      /*	x_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0346, 0x02 },      /*	y_addr_start[15:8]:356	*/
+	{ IMX_8BIT, 0x0347, 0x44 },      /*	y_addr_start[7:0]	*/
+	{ IMX_8BIT, 0x0348, 0x0A },      /*	x_addr_end[15:8]:3207	*/
+	{ IMX_8BIT, 0x0349, 0xF7 },      /*	x_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034A, 0x07 },      /*	y_addr_end[15:8]:2107	*/
+	{ IMX_8BIT, 0x034B, 0x5F+4 },      /*	y_addr_end[7:0]		*/
+	{ IMX_8BIT, 0x034C, 0x09 },      /*	x_output_size[15:8]:1568 */
+	{ IMX_8BIT, 0x034D, 0x20 },      /*	x_output_size[7:0]	*/
+	{ IMX_8BIT, 0x034E, 0x05 },      /*	y_output_size[15:8]:876 */
+	{ IMX_8BIT, 0x034F, 0x1C+4 },      /*	y_output_size[7:0]	*/
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x09 },
+	{ IMX_8BIT, 0x0355, 0x20 },
+	{ IMX_8BIT, 0x0356, 0x05 },
+	{ IMX_8BIT, 0x0357, 0x1C+4 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x09 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x05 },
+	{ IMX_8BIT, 0x3313, 0x1C+4 },
+	{ IMX_8BIT, 0x331C, 0x03 },
+	{ IMX_8BIT, 0x331D, 0xE8 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+
+	/* Global Timing Setting */
+	{ IMX_8BIT, 0x0830, 0x77 },
+	{ IMX_8BIT, 0x0831, 0x2F },
+	{ IMX_8BIT, 0x0832, 0x5F },
+	{ IMX_8BIT, 0x0833, 0x37 },
+	{ IMX_8BIT, 0x0834, 0x37 },
+	{ IMX_8BIT, 0x0835, 0x37 },
+	{ IMX_8BIT, 0x0836, 0xBF },
+	{ IMX_8BIT, 0x0837, 0x3F },
+	{ IMX_8BIT, 0x0839, 0x1F },
+	{ IMX_8BIT, 0x083A, 0x17 },
+	{ IMX_8BIT, 0x083B, 0x02 },
+
+	/* Integration Time Settin */
+	{ IMX_8BIT, 0x0202, 0x05 },
+	{ IMX_8BIT, 0x0203, 0x42 },
+
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00 },
+	{ IMX_8BIT, 0x0231, 0x00 },
+	{ IMX_8BIT, 0x0233, 0x00 },
+	{ IMX_8BIT, 0x0234, 0x00 },
+	{ IMX_8BIT, 0x0235, 0x40 },
+	{ IMX_8BIT, 0x0238, 0x00 },
+	{ IMX_8BIT, 0x0239, 0x04 },
+	{ IMX_8BIT, 0x023B, 0x00 },
+	{ IMX_8BIT, 0x023C, 0x01 },
+	{ IMX_8BIT, 0x33B0, 0x04 },
+	{ IMX_8BIT, 0x33B1, 0x00 },
+	{ IMX_8BIT, 0x33B3, 0x00 },
+	{ IMX_8BIT, 0x33B4, 0x01 },
+	{ IMX_8BIT, 0x3800, 0x00 },
+	{ IMX_TOK_TERM, 0, 0 }
+};
+
+struct imx_resolution imx134_res_preview[] = {
+	{
+		.desc = "imx134_CIF_30fps",
+		.regs = imx134_720_592_30fps,
+		.width = 720,
+		.height = 592,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_820_552_30fps_preview",
+		.regs = imx134_820_552_30fps,
+		.width = 820,
+		.height = 552,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_820_616_preview_30fps",
+		.regs = imx134_820_616_30fps,
+		.width = 820,
+		.height = 616,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1080p_preview_30fps",
+		.regs = imx134_1936_1096_30fps_v2,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1640_1232_preview_30fps",
+		.regs = imx134_1640_1232_30fps,
+		.width = 1640,
+		.height = 1232,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_8M_preview_30fps",
+		.regs = imx134_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+};
+
+struct imx_resolution imx134_res_still[] = {
+	{
+		.desc = "imx134_CIF_30fps",
+		.regs = imx134_1424_1168_30fps,
+		.width = 1424,
+		.height = 1168,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_VGA_still_30fps",
+		.regs = imx134_1640_1232_30fps,
+		.width = 1640,
+		.height = 1232,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1080p_still_30fps",
+		.regs = imx134_1936_1096_30fps_v2,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1640_1232_still_30fps",
+		.regs = imx134_1640_1232_30fps,
+		.width = 1640,
+		.height = 1232,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_8M_still_30fps",
+		.regs = imx134_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{
+				/* WORKAROUND for FW performance limitation */
+				 .fps = 8,
+				 .pixels_per_line = 6400,
+				 .lines_per_frame = 5312,
+			},
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+};
+
+struct imx_resolution imx134_res_video[] = {
+	{
+		.desc = "imx134_QCIF_DVS_30fps",
+		.regs = imx134_240_196_30fps,
+		.width = 240,
+		.height = 196,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_CIF_DVS_30fps",
+		.regs = imx134_448_366_30fps,
+		.width = 448,
+		.height = 366,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_VGA_30fps",
+		.regs = imx134_820_616_30fps,
+		.width = 820,
+		.height = 616,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_480p",
+		.regs = imx134_880_592,
+		.width = 880,
+		.height = 592,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2700,
+			},
+			{
+				 .fps = 60,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 1350,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1568_880",
+		.regs = imx134_1568_880,
+		.width = 1568,
+		.height = 880,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2700,
+			},
+			{
+				 .fps = 60,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 1350,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1080p_dvs_30fps",
+		.regs = imx134_2336_1312_30fps,
+		.width = 2336,
+		.height = 1312,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+	{
+		.desc = "imx134_1080p_dvs_60fps",
+		.regs = imx134_2336_1308_60fps,
+		.width = 2336,
+		.height = 1312,
+		.fps_options = {
+			{
+				 .fps = 60,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 1350,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+	{
+		/*This setting only be used for SDV mode*/
+		.desc = "imx134_8M_sdv_30fps",
+		.regs = imx134_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 3600,
+				 .lines_per_frame = 2518,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+	},
+};
+
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx135.h b/drivers/staging/media/atomisp/i2c/imx/imx135.h
new file mode 100644
index 000000000000..58b43af909f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx135.h
@@ -0,0 +1,3374 @@
+/*
+ * Support for Sony IMX 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __IMX135_H__
+#define __IMX135_H__
+
+#include "common.h"
+
+#define IMX_SC_CMMN_CHIP_ID_H	0x0016
+#define IMX_SC_CMMN_CHIP_ID_L	0x0017
+
+/*
+ * focal length bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define IMX_FOCAL_LENGTH_DEFAULT 0x1710064
+
+/*
+ * current f-number bits definition:
+ * bits 31-16: numerator, bits 15-0: denominator
+ */
+#define IMX_F_NUMBER_DEFAULT 0x16000a
+
+/*
+ * 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 IMX_F_NUMBER_RANGE 0x160a160a
+
+#define GROUPED_PARAMETER_HOLD_ENABLE  {IMX_8BIT, 0x0104, 0x1}
+#define GROUPED_PARAMETER_HOLD_DISABLE  {IMX_8BIT, 0x0104, 0x0}
+
+#define IMX135_EMBEDDED_DATA_LINE_NUM 2
+#define IMX135_OUTPUT_DATA_FORMAT_REG  0x0112
+#define IMX135_OUTPUT_FORMAT_RAW10  0x0a0a
+/*
+ * We use three different MIPI rates for our modes based on the resolution and
+ * FPS requirements. So we have three PLL configurationa and these are based
+ * on the EMC friendly MIPI values.
+ *
+ * Maximum clock: Pix clock @ 360.96MHz MIPI @ 451.2MHz 902.4mbps
+ * Reduced clock: Pix clock @ 273.00MHz MIPI @ 342.0MHz 684.0mbps
+ * Binning modes: Pix clock @ 335.36MHz MIPI @ 209.6MHz 419.2mbps
+ * Global Timing registers are based on the data rates and these are part of
+ * the below clock definitions.
+ */
+/* MIPI 499.2MHz 998.4mbps PIXCLK: 399.36MHz */
+#define PLL_SETTINGS_FOR_MIPI_499_2MHZ_SALTBAY \
+	{IMX_8BIT, 0x011e, 0x13}, \
+	{IMX_8BIT, 0x011f, 0x33}, \
+	{IMX_8BIT, 0x0301, 0x05}, \
+	{IMX_8BIT, 0x0303, 0x01}, \
+	{IMX_8BIT, 0x0305, 0x0c}, \
+	{IMX_8BIT, 0x0309, 0x05}, \
+	{IMX_8BIT, 0x030b, 0x01}, \
+	{IMX_8BIT, 0x030c, 0x02}, \
+	{IMX_8BIT, 0x030d, 0x70}, \
+	{IMX_8BIT, 0x030e, 0x01}, \
+	{IMX_8BIT, 0x3a06, 0x11}, \
+	{IMX_8BIT, 0x0830, 0x7f}, \
+	{IMX_8BIT, 0x0831, 0x37}, \
+	{IMX_8BIT, 0x0832, 0x67}, \
+	{IMX_8BIT, 0x0833, 0x3f}, \
+	{IMX_8BIT, 0x0834, 0x3f}, \
+	{IMX_8BIT, 0x0835, 0x47}, \
+	{IMX_8BIT, 0x0836, 0xdf}, \
+	{IMX_8BIT, 0x0837, 0x47}, \
+	{IMX_8BIT, 0x0839, 0x1f}, \
+	{IMX_8BIT, 0x083a, 0x17}, \
+	{IMX_8BIT, 0x083b, 0x02}
+
+/* MIPI 451.2MHz 902.4mbps PIXCLK: 360.96MHz */
+#define PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY \
+	{IMX_8BIT, 0x011e, 0x13}, \
+	{IMX_8BIT, 0x011f, 0x33}, \
+	{IMX_8BIT, 0x0301, 0x05}, \
+	{IMX_8BIT, 0x0303, 0x01}, \
+	{IMX_8BIT, 0x0305, 0x0c}, \
+	{IMX_8BIT, 0x0309, 0x05}, \
+	{IMX_8BIT, 0x030b, 0x01}, \
+	{IMX_8BIT, 0x030c, 0x02}, \
+	{IMX_8BIT, 0x030d, 0x34}, \
+	{IMX_8BIT, 0x030e, 0x01}, \
+	{IMX_8BIT, 0x3a06, 0x11}, \
+	{IMX_8BIT, 0x0830, 0x7f}, \
+	{IMX_8BIT, 0x0831, 0x37}, \
+	{IMX_8BIT, 0x0832, 0x67}, \
+	{IMX_8BIT, 0x0833, 0x3f}, \
+	{IMX_8BIT, 0x0834, 0x3f}, \
+	{IMX_8BIT, 0x0835, 0x47}, \
+	{IMX_8BIT, 0x0836, 0xdf}, \
+	{IMX_8BIT, 0x0837, 0x47}, \
+	{IMX_8BIT, 0x0839, 0x1f}, \
+	{IMX_8BIT, 0x083a, 0x17}, \
+	{IMX_8BIT, 0x083b, 0x02}
+
+/* MIPI 209.6MHz, 419.2mbps PIXCLK: 335.36 MHz */
+#define PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY \
+	{IMX_8BIT, 0x011e, 0x13}, \
+	{IMX_8BIT, 0x011f, 0x33}, \
+	{IMX_8BIT, 0x0301, 0x05}, \
+	{IMX_8BIT, 0x0303, 0x01}, \
+	{IMX_8BIT, 0x0305, 0x06}, \
+	{IMX_8BIT, 0x0309, 0x05}, \
+	{IMX_8BIT, 0x030b, 0x02}, \
+	{IMX_8BIT, 0x030c, 0x01}, \
+	{IMX_8BIT, 0x030d, 0x06}, \
+	{IMX_8BIT, 0x030e, 0x01}, \
+	{IMX_8BIT, 0x3a06, 0x12}, \
+	{IMX_8BIT, 0x0830, 0x5f}, \
+	{IMX_8BIT, 0x0831, 0x1f}, \
+	{IMX_8BIT, 0x0832, 0x3f}, \
+	{IMX_8BIT, 0x0833, 0x1f}, \
+	{IMX_8BIT, 0x0834, 0x1f}, \
+	{IMX_8BIT, 0x0835, 0x17}, \
+	{IMX_8BIT, 0x0836, 0x67}, \
+	{IMX_8BIT, 0x0837, 0x27}, \
+	{IMX_8BIT, 0x0839, 0x1f}, \
+	{IMX_8BIT, 0x083a, 0x17}, \
+	{IMX_8BIT, 0x083b, 0x02}
+
+/* MIPI 342MHz 684mbps PIXCLK: 273.6MHz */
+#define PLL_SETTINGS_FOR_MIPI_342MHZ_SALTBAY \
+	{IMX_8BIT, 0x011e, 0x13}, \
+	{IMX_8BIT, 0x011f, 0x33}, \
+	{IMX_8BIT, 0x0301, 0x05}, \
+	{IMX_8BIT, 0x0303, 0x01}, \
+	{IMX_8BIT, 0x0305, 0x08}, \
+	{IMX_8BIT, 0x0309, 0x05}, \
+	{IMX_8BIT, 0x030b, 0x01}, \
+	{IMX_8BIT, 0x030c, 0x01}, \
+	{IMX_8BIT, 0x030d, 0x1d}, \
+	{IMX_8BIT, 0x030e, 0x01}, \
+	{IMX_8BIT, 0x3a06, 0x11}, \
+	{IMX_8BIT, 0x0830, 0x77}, \
+	{IMX_8BIT, 0x0831, 0x2f}, \
+	{IMX_8BIT, 0x0832, 0x4f}, \
+	{IMX_8BIT, 0x0833, 0x37}, \
+	{IMX_8BIT, 0x0834, 0x2f}, \
+	{IMX_8BIT, 0x0835, 0x37}, \
+	{IMX_8BIT, 0x0836, 0xa7}, \
+	{IMX_8BIT, 0x0837, 0x37}, \
+	{IMX_8BIT, 0x0839, 0x1f}, \
+	{IMX_8BIT, 0x083a, 0x17}, \
+	{IMX_8BIT, 0x083b, 0x02}
+
+/* Basic settings: Applied only once after the sensor power up */
+static struct imx_reg const imx135_init_settings[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{ IMX_8BIT, 0x0220, 0x01},
+	{ IMX_8BIT, 0x3008, 0xB0},
+	{ IMX_8BIT, 0x320A, 0x01},
+	{ IMX_8BIT, 0x320D, 0x10},
+	{ IMX_8BIT, 0x3216, 0x2E},
+	{ IMX_8BIT, 0x3230, 0x0A},
+	{ IMX_8BIT, 0x3228, 0x05},
+	{ IMX_8BIT, 0x3229, 0x02},
+	{ IMX_8BIT, 0x322C, 0x02},
+	{ IMX_8BIT, 0x3302, 0x10},
+	{ IMX_8BIT, 0x3390, 0x45},
+	{ IMX_8BIT, 0x3409, 0x0C},
+	{ IMX_8BIT, 0x340B, 0xF5},
+	{ IMX_8BIT, 0x340C, 0x2D},
+	{ IMX_8BIT, 0x3412, 0x41},
+	{ IMX_8BIT, 0x3413, 0xAD},
+	{ IMX_8BIT, 0x3414, 0x1E},
+	{ IMX_8BIT, 0x3427, 0x04},
+	{ IMX_8BIT, 0x3480, 0x1E},
+	{ IMX_8BIT, 0x3484, 0x1E},
+	{ IMX_8BIT, 0x3488, 0x1E},
+	{ IMX_8BIT, 0x348C, 0x1E},
+	{ IMX_8BIT, 0x3490, 0x1E},
+	{ IMX_8BIT, 0x3494, 0x1E},
+	{ IMX_8BIT, 0x349C, 0x38},
+	{ IMX_8BIT, 0x34A3, 0x38},
+	{ IMX_8BIT, 0x3511, 0x8F},
+	{ IMX_8BIT, 0x3518, 0x00},
+	{ IMX_8BIT, 0x3519, 0x94},
+	{ IMX_8BIT, 0x3833, 0x20},
+	{ IMX_8BIT, 0x3893, 0x01},
+	{ IMX_8BIT, 0x38C2, 0x08},
+	{ IMX_8BIT, 0x38C3, 0x08},
+	{ IMX_8BIT, 0x3C09, 0x01},
+	{ IMX_8BIT, 0x4000, 0x0E},
+	{ IMX_8BIT, 0x4300, 0x00},
+	{ IMX_8BIT, 0x4316, 0x12},
+	{ IMX_8BIT, 0x4317, 0x22},
+	{ IMX_8BIT, 0x4318, 0x00},
+	{ IMX_8BIT, 0x4319, 0x00},
+	{ IMX_8BIT, 0x431A, 0x00},
+	{ IMX_8BIT, 0x4324, 0x03},
+	{ IMX_8BIT, 0x4325, 0x20},
+	{ IMX_8BIT, 0x4326, 0x03},
+	{ IMX_8BIT, 0x4327, 0x84},
+	{ IMX_8BIT, 0x4328, 0x03},
+	{ IMX_8BIT, 0x4329, 0x20},
+	{ IMX_8BIT, 0x432A, 0x03},
+	{ IMX_8BIT, 0x432B, 0x84},
+	{ IMX_8BIT, 0x432C, 0x01},
+	{ IMX_8BIT, 0x4401, 0x3F},
+	{ IMX_8BIT, 0x4402, 0xFF},
+	{ IMX_8BIT, 0x4412, 0x3F},
+	{ IMX_8BIT, 0x4413, 0xFF},
+	{ IMX_8BIT, 0x441D, 0x28},
+	{ IMX_8BIT, 0x4444, 0x00},
+	{ IMX_8BIT, 0x4445, 0x00},
+	{ IMX_8BIT, 0x4446, 0x3F},
+	{ IMX_8BIT, 0x4447, 0xFF},
+	{ IMX_8BIT, 0x4452, 0x00},
+	{ IMX_8BIT, 0x4453, 0xA0},
+	{ IMX_8BIT, 0x4454, 0x08},
+	{ IMX_8BIT, 0x4455, 0x00},
+	{ IMX_8BIT, 0x4458, 0x18},
+	{ IMX_8BIT, 0x4459, 0x18},
+	{ IMX_8BIT, 0x445A, 0x3F},
+	{ IMX_8BIT, 0x445B, 0x3A},
+	{ IMX_8BIT, 0x4462, 0x00},
+	{ IMX_8BIT, 0x4463, 0x00},
+	{ IMX_8BIT, 0x4464, 0x00},
+	{ IMX_8BIT, 0x4465, 0x00},
+	{ IMX_8BIT, 0x446E, 0x01},
+	{ IMX_8BIT, 0x4500, 0x1F},
+	{ IMX_8BIT, 0x600a, 0x00},
+	{ IMX_8BIT, 0x380a, 0x00},
+	{ IMX_8BIT, 0x380b, 0x00},
+	{ IMX_8BIT, 0x4103, 0x00},
+	{ IMX_8BIT, 0x4243, 0x9a},
+	{ IMX_8BIT, 0x4330, 0x01},
+	{ IMX_8BIT, 0x4331, 0x90},
+	{ IMX_8BIT, 0x4332, 0x02},
+	{ IMX_8BIT, 0x4333, 0x58},
+	{ IMX_8BIT, 0x4334, 0x03},
+	{ IMX_8BIT, 0x4335, 0x20},
+	{ IMX_8BIT, 0x4336, 0x03},
+	{ IMX_8BIT, 0x4337, 0x84},
+	{ IMX_8BIT, 0x433C, 0x01},
+	{ IMX_8BIT, 0x4340, 0x02},
+	{ IMX_8BIT, 0x4341, 0x58},
+	{ IMX_8BIT, 0x4342, 0x03},
+	{ IMX_8BIT, 0x4343, 0x52},
+	{ IMX_8BIT, 0x4364, 0x0b},
+	{ IMX_8BIT, 0x4368, 0x00},
+	{ IMX_8BIT, 0x4369, 0x0f},
+	{ IMX_8BIT, 0x436a, 0x03},
+	{ IMX_8BIT, 0x436b, 0xa8},
+	{ IMX_8BIT, 0x436c, 0x00},
+	{ IMX_8BIT, 0x436d, 0x00},
+	{ IMX_8BIT, 0x436e, 0x00},
+	{ IMX_8BIT, 0x436f, 0x06},
+	{ IMX_8BIT, 0x4281, 0x21},
+	{ IMX_8BIT, 0x4282, 0x18},
+	{ IMX_8BIT, 0x4283, 0x04},
+	{ IMX_8BIT, 0x4284, 0x08},
+	{ IMX_8BIT, 0x4287, 0x7f},
+	{ IMX_8BIT, 0x4288, 0x08},
+	{ IMX_8BIT, 0x428c, 0x08},
+	{ IMX_8BIT, 0x4297, 0x00},
+	{ IMX_8BIT, 0x4299, 0x7E},
+	{ IMX_8BIT, 0x42A4, 0xFB},
+	{ IMX_8BIT, 0x42A5, 0x7E},
+	{ IMX_8BIT, 0x42A6, 0xDF},
+	{ IMX_8BIT, 0x42A7, 0xB7},
+	{ IMX_8BIT, 0x42AF, 0x03},
+	{ IMX_8BIT, 0x4207, 0x03},
+	{ IMX_8BIT, 0x4218, 0x00},
+	{ IMX_8BIT, 0x421B, 0x20},
+	{ IMX_8BIT, 0x421F, 0x04},
+	{ IMX_8BIT, 0x4222, 0x02},
+	{ IMX_8BIT, 0x4223, 0x22},
+	{ IMX_8BIT, 0x422E, 0x54},
+	{ IMX_8BIT, 0x422F, 0xFB},
+	{ IMX_8BIT, 0x4230, 0xFF},
+	{ IMX_8BIT, 0x4231, 0xFE},
+	{ IMX_8BIT, 0x4232, 0xFF},
+	{ IMX_8BIT, 0x4235, 0x58},
+	{ IMX_8BIT, 0x4236, 0xF7},
+	{ IMX_8BIT, 0x4237, 0xFD},
+	{ IMX_8BIT, 0x4239, 0x4E},
+	{ IMX_8BIT, 0x423A, 0xFC},
+	{ IMX_8BIT, 0x423B, 0xFD},
+	{ IMX_8BIT, 0x4300, 0x00},
+	{ IMX_8BIT, 0x4316, 0x12},
+	{ IMX_8BIT, 0x4317, 0x22},
+	{ IMX_8BIT, 0x4318, 0x00},
+	{ IMX_8BIT, 0x4319, 0x00},
+	{ IMX_8BIT, 0x431A, 0x00},
+	{ IMX_8BIT, 0x4324, 0x03},
+	{ IMX_8BIT, 0x4325, 0x20},
+	{ IMX_8BIT, 0x4326, 0x03},
+	{ IMX_8BIT, 0x4327, 0x84},
+	{ IMX_8BIT, 0x4328, 0x03},
+	{ IMX_8BIT, 0x4329, 0x20},
+	{ IMX_8BIT, 0x432A, 0x03},
+	{ IMX_8BIT, 0x432B, 0x20},
+	{ IMX_8BIT, 0x432C, 0x01},
+	{ IMX_8BIT, 0x432D, 0x01},
+	{ IMX_8BIT, 0x4338, 0x02},
+	{ IMX_8BIT, 0x4339, 0x00},
+	{ IMX_8BIT, 0x433A, 0x00},
+	{ IMX_8BIT, 0x433B, 0x02},
+	{ IMX_8BIT, 0x435A, 0x03},
+	{ IMX_8BIT, 0x435B, 0x84},
+	{ IMX_8BIT, 0x435E, 0x01},
+	{ IMX_8BIT, 0x435F, 0xFF},
+	{ IMX_8BIT, 0x4360, 0x01},
+	{ IMX_8BIT, 0x4361, 0xF4},
+	{ IMX_8BIT, 0x4362, 0x03},
+	{ IMX_8BIT, 0x4363, 0x84},
+	{ IMX_8BIT, 0x437B, 0x01},
+	{ IMX_8BIT, 0x4400, 0x00}, /* STATS off ISP do not support STATS*/
+	{ IMX_8BIT, 0x4401, 0x3F},
+	{ IMX_8BIT, 0x4402, 0xFF},
+	{ IMX_8BIT, 0x4404, 0x13},
+	{ IMX_8BIT, 0x4405, 0x26},
+	{ IMX_8BIT, 0x4406, 0x07},
+	{ IMX_8BIT, 0x4408, 0x20},
+	{ IMX_8BIT, 0x4409, 0xE5},
+	{ IMX_8BIT, 0x440A, 0xFB},
+	{ IMX_8BIT, 0x440C, 0xF6},
+	{ IMX_8BIT, 0x440D, 0xEA},
+	{ IMX_8BIT, 0x440E, 0x20},
+	{ IMX_8BIT, 0x4410, 0x00},
+	{ IMX_8BIT, 0x4411, 0x00},
+	{ IMX_8BIT, 0x4412, 0x3F},
+	{ IMX_8BIT, 0x4413, 0xFF},
+	{ IMX_8BIT, 0x4414, 0x1F},
+	{ IMX_8BIT, 0x4415, 0xFF},
+	{ IMX_8BIT, 0x4416, 0x20},
+	{ IMX_8BIT, 0x4417, 0x00},
+	{ IMX_8BIT, 0x4418, 0x1F},
+	{ IMX_8BIT, 0x4419, 0xFF},
+	{ IMX_8BIT, 0x441A, 0x20},
+	{ IMX_8BIT, 0x441B, 0x00},
+	{ IMX_8BIT, 0x441D, 0x40},
+	{ IMX_8BIT, 0x441E, 0x1E},
+	{ IMX_8BIT, 0x441F, 0x38},
+	{ IMX_8BIT, 0x4420, 0x01},
+	{ IMX_8BIT, 0x4444, 0x00},
+	{ IMX_8BIT, 0x4445, 0x00},
+	{ IMX_8BIT, 0x4446, 0x1D},
+	{ IMX_8BIT, 0x4447, 0xF9},
+	{ IMX_8BIT, 0x4452, 0x00},
+	{ IMX_8BIT, 0x4453, 0xA0},
+	{ IMX_8BIT, 0x4454, 0x08},
+	{ IMX_8BIT, 0x4455, 0x00},
+	{ IMX_8BIT, 0x4456, 0x0F},
+	{ IMX_8BIT, 0x4457, 0xFF},
+	{ IMX_8BIT, 0x4458, 0x18},
+	{ IMX_8BIT, 0x4459, 0x18},
+	{ IMX_8BIT, 0x445A, 0x3F},
+	{ IMX_8BIT, 0x445B, 0x3A},
+	{ IMX_8BIT, 0x445C, 0x00},
+	{ IMX_8BIT, 0x445D, 0x28},
+	{ IMX_8BIT, 0x445E, 0x01},
+	{ IMX_8BIT, 0x445F, 0x90},
+	{ IMX_8BIT, 0x4460, 0x00},
+	{ IMX_8BIT, 0x4461, 0x60},
+	{ IMX_8BIT, 0x4462, 0x00},
+	{ IMX_8BIT, 0x4463, 0x00},
+	{ IMX_8BIT, 0x4464, 0x00},
+	{ IMX_8BIT, 0x4465, 0x00},
+	{ IMX_8BIT, 0x446C, 0x00},
+	{ IMX_8BIT, 0x446D, 0x00},
+	{ IMX_8BIT, 0x446E, 0x00},
+	{ IMX_8BIT, 0x452A, 0x02},
+	{ IMX_8BIT, 0x0712, 0x01},
+	{ IMX_8BIT, 0x0713, 0x00},
+	{ IMX_8BIT, 0x0714, 0x01},
+	{ IMX_8BIT, 0x0715, 0x00},
+	{ IMX_8BIT, 0x0716, 0x01},
+	{ IMX_8BIT, 0x0717, 0x00},
+	{ IMX_8BIT, 0x0718, 0x01},
+	{ IMX_8BIT, 0x0719, 0x00},
+	{ IMX_8BIT, 0x4500, 0x1F },
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	{ IMX_8BIT, 0x0205, 0x00},
+	{ IMX_8BIT, 0x020E, 0x01},
+	{ IMX_8BIT, 0x020F, 0x00},
+	{ IMX_8BIT, 0x0210, 0x02},
+	{ IMX_8BIT, 0x0211, 0x00},
+	{ IMX_8BIT, 0x0212, 0x02},
+	{ IMX_8BIT, 0x0213, 0x00},
+	{ IMX_8BIT, 0x0214, 0x01},
+	{ IMX_8BIT, 0x0215, 0x00},
+	/* HDR Setting */
+	{ IMX_8BIT, 0x0230, 0x00},
+	{ IMX_8BIT, 0x0231, 0x00},
+	{ IMX_8BIT, 0x0233, 0x00},
+	{ IMX_8BIT, 0x0234, 0x00},
+	{ IMX_8BIT, 0x0235, 0x40},
+	{ IMX_8BIT, 0x0238, 0x00},
+	{ IMX_8BIT, 0x0239, 0x04},
+	{ IMX_8BIT, 0x023B, 0x00},
+	{ IMX_8BIT, 0x023C, 0x01},
+	{ IMX_8BIT, 0x33B0, 0x04},
+	{ IMX_8BIT, 0x33B1, 0x00},
+	{ IMX_8BIT, 0x33B3, 0x00},
+	{ IMX_8BIT, 0x33B4, 0x01},
+	{ IMX_8BIT, 0x3800, 0x00},
+	GROUPED_PARAMETER_HOLD_DISABLE,
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/********* Preview, continuous capture and still modes *****************/
+
+static struct imx_reg const imx135_13m[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size Setting */
+	{IMX_8BIT, 0x0344, 0x00}, /* 0, 0, 4207,3119 4208x3120 */
+	{IMX_8BIT, 0x0345, 0x00},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x00},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x6F},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x2F},
+	{IMX_8BIT, 0x034C, 0x10},
+	{IMX_8BIT, 0x034D, 0x70},
+	{IMX_8BIT, 0x034E, 0x0C},
+	{IMX_8BIT, 0x034F, 0x30},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10}, /* 4208x3120 */
+	{IMX_8BIT, 0x0355, 0x70},
+	{IMX_8BIT, 0x0356, 0x0C},
+	{IMX_8BIT, 0x0357, 0x30},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x10},
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x0C},
+	{IMX_8BIT, 0x3313, 0x30},
+	{IMX_8BIT, 0x331C, 0x00},
+	{IMX_8BIT, 0x331D, 0x10},
+	{IMX_8BIT, 0x4084, 0x00}, /* If scaling, Fill this */
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/* 13MP reduced pixel clock MIPI 342MHz is EMC friendly*/
+static struct imx_reg const imx135_13m_for_mipi_342[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_342MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size Setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x00},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x00},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x6F},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x2F},
+	{IMX_8BIT, 0x034C, 0x10},
+	{IMX_8BIT, 0x034D, 0x70},
+	{IMX_8BIT, 0x034E, 0x0C},
+	{IMX_8BIT, 0x034F, 0x30},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10},
+	{IMX_8BIT, 0x0355, 0x70},
+	{IMX_8BIT, 0x0356, 0x0C},
+	{IMX_8BIT, 0x0357, 0x30},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x10},
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x0C},
+	{IMX_8BIT, 0x3313, 0x30},
+	{IMX_8BIT, 0x331C, 0x00},
+	{IMX_8BIT, 0x331D, 0x10},
+	{IMX_8BIT, 0x4084, 0x00}, /* If scaling, Fill this */
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_10m[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00}, /* 0, 376, 4207, 2743 */
+	{IMX_8BIT, 0x0345, 0x00},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x78},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x6f},
+	{IMX_8BIT, 0x034A, 0x0a},
+	{IMX_8BIT, 0x034B, 0xb7},
+	{IMX_8BIT, 0x034C, 0x10}, /* 4208x2368 */
+	{IMX_8BIT, 0x034D, 0x70},
+	{IMX_8BIT, 0x034E, 0x09},
+	{IMX_8BIT, 0x034F, 0x40},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10},
+	{IMX_8BIT, 0x0355, 0x70},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0x40},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x10},
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x09},
+	{IMX_8BIT, 0x3313, 0x40},
+	{IMX_8BIT, 0x331C, 0x01},
+	{IMX_8BIT, 0x331D, 0x68},
+	{IMX_8BIT, 0x4084, 0x00},
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_10m_for_mipi_342[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_342MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00}, /* 0, 376, 4207, 2743 */
+	{IMX_8BIT, 0x0345, 0x00},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x78},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x6f},
+	{IMX_8BIT, 0x034A, 0x0a},
+	{IMX_8BIT, 0x034B, 0xb7},
+	{IMX_8BIT, 0x034C, 0x10}, /* 4208x2368 */
+	{IMX_8BIT, 0x034D, 0x70},
+	{IMX_8BIT, 0x034E, 0x09},
+	{IMX_8BIT, 0x034F, 0x40},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10},
+	{IMX_8BIT, 0x0355, 0x70},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0x40},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x10},
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x09},
+	{IMX_8BIT, 0x3313, 0x40},
+	{IMX_8BIT, 0x331C, 0x01},
+	{IMX_8BIT, 0x331D, 0x68},
+	{IMX_8BIT, 0x4084, 0x00},
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * It is 8.5 DS from (3:2)8m cropped setting.
+ *
+ * The 8m(3:2) cropped setting is 2992x2448 effective res.
+ * The ISP effect cropped setting should be 1408x1152 effect res.
+ *
+ * Consider ISP 16x16 padding:
+ * sensor outputs 368x304
+ * cropped region is 3128x2584
+ */
+static struct imx_reg const imx135_368x304_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11}, /* no binning */
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* resize */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x88}, /* 136/16=8.5 */
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x02}, /* X_ADD_STA */
+	{IMX_8BIT, 0x0345, 0x1C}, /* 540 */
+	{IMX_8BIT, 0x0346, 0x01}, /* Y_ADD_STA */
+	{IMX_8BIT, 0x0347, 0x0C}, /* 268 */
+	{IMX_8BIT, 0x0348, 0x0E}, /* X_ADD_END */
+	{IMX_8BIT, 0x0349, 0x53}, /* 3667 */
+	{IMX_8BIT, 0x034A, 0x0B}, /* Y_ADD_END */
+	{IMX_8BIT, 0x034B, 0x23}, /* 2851 */
+	{IMX_8BIT, 0x034C, 0x01}, /* X_OUT_SIZE */
+	{IMX_8BIT, 0x034D, 0x70}, /* 368 */
+	{IMX_8BIT, 0x034E, 0x01}, /* Y_OUT_SIZE */
+	{IMX_8BIT, 0x034F, 0x30}, /* 304 */
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x0C}, /* Cut out siz same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x38},
+	{IMX_8BIT, 0x0356, 0x0A},
+	{IMX_8BIT, 0x0357, 0x18},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x01}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x01},
+	{IMX_8BIT, 0x3313, 0x30},
+	{IMX_8BIT, 0x331C, 0x02}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xD0},
+	{IMX_8BIT, 0x4084, 0x01}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x70},
+	{IMX_8BIT, 0x4086, 0x01},
+	{IMX_8BIT, 0x4087, 0x30},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * It is 1/4 binning from 8m cropped setting.
+ *
+ * The 8m cropped setting is 3264x2448 effective res.
+ * The xga cropped setting should be 816x612 effect res.
+ *
+ * Consider ISP 16x16 padding:
+ * sensor outputs 832x628
+ * cropped region is 3328x2512
+ */
+static struct imx_reg const imx135_xga_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01},
+	{IMX_8BIT, 0x0391, 0x44},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+/*	{IMX_8BIT, 0x4203, 0xFF}, */
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01}, /* X_ADD_STA */
+	{IMX_8BIT, 0x0345, 0xB8}, /* 440 */
+	{IMX_8BIT, 0x0346, 0x01}, /* Y_ADD_STA */
+	{IMX_8BIT, 0x0347, 0x30}, /* 304 */
+	{IMX_8BIT, 0x0348, 0x0E}, /* X_ADD_END */
+	{IMX_8BIT, 0x0349, 0xB7}, /* 4207-440=3767 */
+	{IMX_8BIT, 0x034A, 0x0A}, /* Y_ADD_END */
+	{IMX_8BIT, 0x034B, 0xFF}, /* 3119-304=2815 */
+	{IMX_8BIT, 0x034C, 0x03}, /* X_OUT_SIZE */
+	{IMX_8BIT, 0x034D, 0x40}, /* 832 */
+	{IMX_8BIT, 0x034E, 0x02}, /* Y_OUT_SIZE */
+	{IMX_8BIT, 0x034F, 0x74}, /* 628 */
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x03}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x40},
+	{IMX_8BIT, 0x0356, 0x02},
+	{IMX_8BIT, 0x0357, 0x74},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x03}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0x40},
+	{IMX_8BIT, 0x3312, 0x02},
+	{IMX_8BIT, 0x3313, 0x74},
+	{IMX_8BIT, 0x331C, 0x02}, /* ?? */
+	{IMX_8BIT, 0x331D, 0x21},
+	{IMX_8BIT, 0x4084, 0x03}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x40},
+	{IMX_8BIT, 0x4086, 0x02},
+	{IMX_8BIT, 0x4087, 0x74},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * It is 28/16 DS from (16:9)8m cropped setting.
+ *
+ * The 8m(16:9) cropped setting is 3360x1890 effective res.
+ * - this is larger then the expected 3264x1836 FOV
+ *
+ * Consider ISP 16x16 padding:
+ * sensor outputs 1936x1096
+ * cropped region is 3388x1918
+ */
+static struct imx_reg const imx135_1936x1096_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11}, /* no binning */
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* resize */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x1C}, /* 28/16 */
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01}, /* X_ADD_STA */
+	{IMX_8BIT, 0x0345, 0x9A}, /* 410 */
+	{IMX_8BIT, 0x0346, 0x02}, /* Y_ADD_STA */
+	{IMX_8BIT, 0x0347, 0x58}, /* 600 */
+	{IMX_8BIT, 0x0348, 0x0E}, /* X_ADD_END */
+	{IMX_8BIT, 0x0349, 0xD5}, /* 3797 */
+	{IMX_8BIT, 0x034A, 0x09}, /* Y_ADD_END */
+	{IMX_8BIT, 0x034B, 0xD5}, /* 2517 */
+	{IMX_8BIT, 0x034C, 0x07}, /* X_OUT_SIZE */
+	{IMX_8BIT, 0x034D, 0x90}, /* 1936 */
+	{IMX_8BIT, 0x034E, 0x04}, /* Y_OUT_SIZE */
+	{IMX_8BIT, 0x034F, 0x48}, /* 1096 */
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x0D}, /* Cut out siz same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x3C},
+	{IMX_8BIT, 0x0356, 0x07},
+	{IMX_8BIT, 0x0357, 0x7E},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x07}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0x90},
+	{IMX_8BIT, 0x3312, 0x04},
+	{IMX_8BIT, 0x3313, 0x48},
+	{IMX_8BIT, 0x331C, 0x00}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xAA},
+	{IMX_8BIT, 0x4084, 0x07}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x90},
+	{IMX_8BIT, 0x4086, 0x04},
+	{IMX_8BIT, 0x4087, 0x48},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * It is 2.125 DS from (3:2)8m cropped setting.
+ *
+ * The 8m(3:2) cropped setting is 2992x2448 effective res.
+ * The ISP effect cropped setting should be 1408x1152 effect res.
+ *
+ * Consider ISP 16x16 padding:
+ * sensor outputs 1424x1168
+ * cropped region is 3026x2482
+ */
+static struct imx_reg const imx135_1424x1168_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11}, /* no binning */
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* resize */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x22}, /* 34/16=2.125 */
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x02}, /* X_ADD_STA */
+	{IMX_8BIT, 0x0345, 0x4E}, /* 590 */
+	{IMX_8BIT, 0x0346, 0x01}, /* Y_ADD_STA */
+	{IMX_8BIT, 0x0347, 0x3E}, /* 318 */
+	{IMX_8BIT, 0x0348, 0x0E}, /* X_ADD_END */
+	{IMX_8BIT, 0x0349, 0x1F}, /* 3615 */
+	{IMX_8BIT, 0x034A, 0x0A}, /* Y_ADD_END */
+	{IMX_8BIT, 0x034B, 0xEF}, /* 2799 */
+	{IMX_8BIT, 0x034C, 0x05}, /* X_OUT_SIZE */
+	{IMX_8BIT, 0x034D, 0x90}, /* 1424 */
+	{IMX_8BIT, 0x034E, 0x04}, /* Y_OUT_SIZE */
+	{IMX_8BIT, 0x034F, 0x90}, /* 1168 */
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x0B}, /* Cut out siz same as the size after crop */
+	{IMX_8BIT, 0x0355, 0xD2},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0xB2},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x05}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0x90},
+	{IMX_8BIT, 0x3312, 0x04},
+	{IMX_8BIT, 0x3313, 0x90},
+	{IMX_8BIT, 0x331C, 0x00}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xAA},
+	{IMX_8BIT, 0x4084, 0x05}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x90},
+	{IMX_8BIT, 0x4086, 0x04},
+	{IMX_8BIT, 0x4087, 0x90},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * It is 1/2 binning from 8m cropped setting.
+ *
+ * The 8m cropped setting is 3264x2448 effective res.
+ * The 2m cropped setting should be 1632x1224 effect res.
+ *
+ * Consider ISP 16x16 padding:
+ * sensor outputs 1648x1240
+ * cropped region is 3296x2480
+ */
+static struct imx_reg const imx135_2m_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01},
+	{IMX_8BIT, 0x0391, 0x22},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01}, /* X_ADD_STA */
+	{IMX_8BIT, 0x0345, 0xC8}, /* 464(1D0) -> 456(1C8)*/
+	{IMX_8BIT, 0x0346, 0x01}, /* Y_ADD_STA */
+	{IMX_8BIT, 0x0347, 0x40}, /* 320 */
+	{IMX_8BIT, 0x0348, 0x0E}, /* X_ADD_END */
+	{IMX_8BIT, 0x0349, 0xA7}, /* 4207-456=3751 */
+	{IMX_8BIT, 0x034A, 0x0A}, /* Y_ADD_END */
+	{IMX_8BIT, 0x034B, 0xEF}, /* 3119-320=2799 */
+	{IMX_8BIT, 0x034C, 0x06}, /* X_OUT_SIZE */
+	{IMX_8BIT, 0x034D, 0x70}, /* 1648 */
+	{IMX_8BIT, 0x034E, 0x04}, /* Y_OUT_SIZE */
+	{IMX_8BIT, 0x034F, 0xD8}, /* 1240 */
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x06}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x70},
+	{IMX_8BIT, 0x0356, 0x04},
+	{IMX_8BIT, 0x0357, 0xD8},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x06}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0x70},
+	{IMX_8BIT, 0x3312, 0x04},
+	{IMX_8BIT, 0x3313, 0xD8},
+	{IMX_8BIT, 0x331C, 0x00}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xAA},
+	{IMX_8BIT, 0x4084, 0x00}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * 8M Cropped 16:9 setting
+ *
+ * Effect res: 3264x1836
+ * Sensor out: 3280x1852
+ */
+static struct imx_reg const imx135_6m_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0xD0},
+	{IMX_8BIT, 0x0346, 0x02}, /* 634 */
+	{IMX_8BIT, 0x0347, 0x7A},
+	{IMX_8BIT, 0x0348, 0x0E},
+	{IMX_8BIT, 0x0349, 0x9F},
+	{IMX_8BIT, 0x034A, 0x09}, /* 2485 */
+	{IMX_8BIT, 0x034B, 0xB5},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x07}, /* 1852 */
+	{IMX_8BIT, 0x034F, 0x3C},
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x0C}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0xD0},
+	{IMX_8BIT, 0x0356, 0x07},
+	{IMX_8BIT, 0x0357, 0x3C},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x0C}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x07},
+	{IMX_8BIT, 0x3313, 0x3C},
+	{IMX_8BIT, 0x331C, 0x00}, /* ?? */
+	{IMX_8BIT, 0x331D, 0x10},
+	{IMX_8BIT, 0x4084, 0x00}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_8m_cropped[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0xD0},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x48},
+	{IMX_8BIT, 0x0348, 0x0E},
+	{IMX_8BIT, 0x0349, 0x9F},
+	{IMX_8BIT, 0x034A, 0x0A},
+	{IMX_8BIT, 0x034B, 0xE7},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x09}, /* 2464 */
+	{IMX_8BIT, 0x034F, 0xA0},
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x0C}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0xD0},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0xA0},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x0C}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x09},
+	{IMX_8BIT, 0x3313, 0xA0},
+	{IMX_8BIT, 0x331C, 0x00}, /* ?? */
+	{IMX_8BIT, 0x331D, 0x10},
+	{IMX_8BIT, 0x4084, 0x00}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_8m_scaled_from_12m[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* Scaling */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x14},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x36},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x14},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x39},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x1B},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280x2464 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x09},
+	{IMX_8BIT, 0x034F, 0xA0},
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x04},
+	{IMX_8BIT, 0x0356, 0x0C},
+	{IMX_8BIT, 0x0357, 0x08},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x0C}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x09},
+	{IMX_8BIT, 0x3313, 0xA0},
+	{IMX_8BIT, 0x331C, 0x02}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xA0},
+	{IMX_8BIT, 0x4084, 0x0C}, /* Scaling related? */
+	{IMX_8BIT, 0x4085, 0xD0},
+	{IMX_8BIT, 0x4086, 0x09},
+	{IMX_8BIT, 0x4087, 0xA0},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_8m_scaled_from_12m_for_mipi342[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_342MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* Scaling */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x14},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x36},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x14},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x39},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x1B},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280x2464 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x09},
+	{IMX_8BIT, 0x034F, 0xA0},
+	{IMX_8BIT, 0x0350, 0x00}, /* No Dig crop */
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10}, /* Cut out size same as the size after crop */
+	{IMX_8BIT, 0x0355, 0x04},
+	{IMX_8BIT, 0x0356, 0x0C},
+	{IMX_8BIT, 0x0357, 0x08},
+	{IMX_8BIT, 0x301D, 0x30}, /* ?? */
+	{IMX_8BIT, 0x3310, 0x0C}, /* Write H and V size  same as output size? */
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x09},
+	{IMX_8BIT, 0x3313, 0xA0},
+	{IMX_8BIT, 0x331C, 0x02}, /* ?? */
+	{IMX_8BIT, 0x331D, 0xA0},
+	{IMX_8BIT, 0x4084, 0x0C}, /* Resize IMG Hand V size-> Scaling related?*/
+	{IMX_8BIT, 0x4085, 0xD0},
+	{IMX_8BIT, 0x4086, 0x09},
+	{IMX_8BIT, 0x4087, 0xA0},
+	{IMX_8BIT, 0x4400, 0x00}, /* STATS off */
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_6m[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x14},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00}, /* 36, 194, 1039, a9f 4100x2316 */
+	{IMX_8BIT, 0x0345, 0x36},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x94},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x39},
+	{IMX_8BIT, 0x034A, 0x0A},
+	{IMX_8BIT, 0x034B, 0x9F},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280x1852 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x07},
+	{IMX_8BIT, 0x034F, 0x3C},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10}, /* 4100x2316 */
+	{IMX_8BIT, 0x0355, 0x04},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0x0C},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x0C},
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x07},
+	{IMX_8BIT, 0x3313, 0x3C},
+	{IMX_8BIT, 0x331C, 0x02},
+	{IMX_8BIT, 0x331D, 0xA0},
+	{IMX_8BIT, 0x4084, 0x0C},
+	{IMX_8BIT, 0x4085, 0xD0},
+	{IMX_8BIT, 0x4086, 0x07},
+	{IMX_8BIT, 0x4087, 0x3C},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_6m_for_mipi_342[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_342MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x00},
+	{IMX_8BIT, 0x0391, 0x11},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x14},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00}, /* 36, 194, 1039, a9f 4100x2316 */
+	{IMX_8BIT, 0x0345, 0x36},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x94},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x39},
+	{IMX_8BIT, 0x034A, 0x0A},
+	{IMX_8BIT, 0x034B, 0x9F},
+	{IMX_8BIT, 0x034C, 0x0C}, /* 3280x1852 */
+	{IMX_8BIT, 0x034D, 0xD0},
+	{IMX_8BIT, 0x034E, 0x07},
+	{IMX_8BIT, 0x034F, 0x3C},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x10}, /* 4100x2316 */
+	{IMX_8BIT, 0x0355, 0x04},
+	{IMX_8BIT, 0x0356, 0x09},
+	{IMX_8BIT, 0x0357, 0x0C},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x0C},
+	{IMX_8BIT, 0x3311, 0xD0},
+	{IMX_8BIT, 0x3312, 0x07},
+	{IMX_8BIT, 0x3313, 0x3C},
+	{IMX_8BIT, 0x331C, 0x02},
+	{IMX_8BIT, 0x331D, 0xA0},
+	{IMX_8BIT, 0x4084, 0x0C},
+	{IMX_8BIT, 0x4085, 0xD0},
+	{IMX_8BIT, 0x4086, 0x07},
+	{IMX_8BIT, 0x4087, 0x3C},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/*
+ * FOV is: 3280x2464, larger then 3264x2448.
+ * Sensor output: 336x256
+ * Cropping region: 3444x2624
+ */
+static struct imx_reg const imx135_336x256[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01},
+	{IMX_8BIT, 0x0391, 0x22},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02}, /* 2x binning */
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x52}, /* scaling: 82/16 */
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01}, /* x_start: 374 */
+	{IMX_8BIT, 0x0345, 0x76},
+	{IMX_8BIT, 0x0346, 0x00}, /* y_start: 248 */
+	{IMX_8BIT, 0x0347, 0xF8},
+	{IMX_8BIT, 0x0348, 0x0E}, /* x_end: 3817 */
+	{IMX_8BIT, 0x0349, 0xE9},
+	{IMX_8BIT, 0x034A, 0x0B}, /* y_end: 2871 */
+	{IMX_8BIT, 0x034B, 0x37},
+	{IMX_8BIT, 0x034C, 0x01}, /* x_out: 336 */
+	{IMX_8BIT, 0x034D, 0x50},
+	{IMX_8BIT, 0x034E, 0x01}, /* y_out: 256 */
+	{IMX_8BIT, 0x034F, 0x00},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x06}, /* dig x_out: 1722 */
+	{IMX_8BIT, 0x0355, 0xBA},
+	{IMX_8BIT, 0x0356, 0x05}, /* dig y_out: 1312  */
+	{IMX_8BIT, 0x0357, 0x20},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x01}, /* ?: x_out */
+	{IMX_8BIT, 0x3311, 0x50},
+	{IMX_8BIT, 0x3312, 0x01}, /* ?: y_out */
+	{IMX_8BIT, 0x3313, 0x00},
+	{IMX_8BIT, 0x331C, 0x02},
+	{IMX_8BIT, 0x331D, 0x4E},
+	{IMX_8BIT, 0x4084, 0x01}, /* ?: x_out */
+	{IMX_8BIT, 0x4085, 0x50},
+	{IMX_8BIT, 0x4086, 0x01}, /* ?: y_out */
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_1m[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01},
+	{IMX_8BIT, 0x0391, 0x22},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x1F},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x58},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x28},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x17},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x07},
+	{IMX_8BIT, 0x034C, 0x04},
+	{IMX_8BIT, 0x034D, 0x10},
+	{IMX_8BIT, 0x034E, 0x03},
+	{IMX_8BIT, 0x034F, 0x10},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x07},
+	{IMX_8BIT, 0x0355, 0xE0},
+	{IMX_8BIT, 0x0356, 0x05},
+	{IMX_8BIT, 0x0357, 0xF0},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x04},
+	{IMX_8BIT, 0x3311, 0x10},
+	{IMX_8BIT, 0x3312, 0x03},
+	{IMX_8BIT, 0x3313, 0x10},
+	{IMX_8BIT, 0x331C, 0x02},
+	{IMX_8BIT, 0x331D, 0x4E},
+	{IMX_8BIT, 0x4084, 0x04},
+	{IMX_8BIT, 0x4085, 0x10},
+	{IMX_8BIT, 0x4086, 0x03},
+	{IMX_8BIT, 0x4087, 0x10},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg const imx135_3m_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01}, /* Binning */
+	{IMX_8BIT, 0x0391, 0x22}, /* 2x2 binning */
+	{IMX_8BIT, 0x0392, 0x00}, /* average */
+	{IMX_8BIT, 0x0401, 0x00},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x10},
+	{IMX_8BIT, 0x4082, 0x01},
+	{IMX_8BIT, 0x4083, 0x01},
+	{IMX_8BIT, 0x4203, 0xFF},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x28},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x08},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x47},
+	{IMX_8BIT, 0x034A, 0x0C},
+	{IMX_8BIT, 0x034B, 0x27},
+	{IMX_8BIT, 0x034C, 0x08},
+	{IMX_8BIT, 0x034D, 0x10},
+	{IMX_8BIT, 0x034E, 0x06},
+	{IMX_8BIT, 0x034F, 0x10},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x08},
+	{IMX_8BIT, 0x0355, 0x10},
+	{IMX_8BIT, 0x0356, 0x06},
+	{IMX_8BIT, 0x0357, 0x10},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x08},
+	{IMX_8BIT, 0x3311, 0x10},
+	{IMX_8BIT, 0x3312, 0x06},
+	{IMX_8BIT, 0x3313, 0x10},
+	{IMX_8BIT, 0x331C, 0x00},
+	{IMX_8BIT, 0x331D, 0xAA},
+	{IMX_8BIT, 0x4084, 0x00},
+	{IMX_8BIT, 0x4085, 0x00},
+	{IMX_8BIT, 0x4086, 0x00},
+	{IMX_8BIT, 0x4087, 0x00},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+/* 1080P 1936x1104 */
+static struct imx_reg const imx135_1080p_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03},
+	{IMX_8BIT, 0x0112, 0x0A},
+	{IMX_8BIT, 0x0113, 0x0A},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0387, 0x01},
+	{IMX_8BIT, 0x0390, 0x01},
+	{IMX_8BIT, 0x0391, 0x22},
+	{IMX_8BIT, 0x0392, 0x00},
+	{IMX_8BIT, 0x0401, 0x02},
+	{IMX_8BIT, 0x0404, 0x00},
+	{IMX_8BIT, 0x0405, 0x11},
+	{IMX_8BIT, 0x4082, 0x00},
+	{IMX_8BIT, 0x4083, 0x00},
+	{IMX_8BIT, 0x7006, 0x04},
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x2E},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x84},
+	{IMX_8BIT, 0x0348, 0x10},
+	{IMX_8BIT, 0x0349, 0x41},
+	{IMX_8BIT, 0x034A, 0x0A},
+	{IMX_8BIT, 0x034B, 0xAF},
+	{IMX_8BIT, 0x034C, 0x07},
+	{IMX_8BIT, 0x034D, 0x90},
+	{IMX_8BIT, 0x034E, 0x04},
+	{IMX_8BIT, 0x034F, 0x50},
+	{IMX_8BIT, 0x0350, 0x00},
+	{IMX_8BIT, 0x0351, 0x00},
+	{IMX_8BIT, 0x0352, 0x00},
+	{IMX_8BIT, 0x0353, 0x00},
+	{IMX_8BIT, 0x0354, 0x08},
+	{IMX_8BIT, 0x0355, 0x0A},
+	{IMX_8BIT, 0x0356, 0x04},
+	{IMX_8BIT, 0x0357, 0x96},
+	{IMX_8BIT, 0x301D, 0x30},
+	{IMX_8BIT, 0x3310, 0x07},
+	{IMX_8BIT, 0x3311, 0x90},
+	{IMX_8BIT, 0x3312, 0x04},
+	{IMX_8BIT, 0x3313, 0x50},
+	{IMX_8BIT, 0x331C, 0x01},
+	{IMX_8BIT, 0x331D, 0x00},
+	{IMX_8BIT, 0x4084, 0x07},
+	{IMX_8BIT, 0x4085, 0x90},
+	{IMX_8BIT, 0x4086, 0x04},
+	{IMX_8BIT, 0x4087, 0x50},
+	{IMX_8BIT, 0x4400, 0x00},
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static const struct imx_reg imx135_1080p_nodvs_fullfov_max_clock[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 168,464,4039,2655: 3872x2192 */
+	{ IMX_8BIT, 0x0345, 0xA8 },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0xD0 },
+	{ IMX_8BIT, 0x0348, 0x0F },
+	{ IMX_8BIT, 0x0349, 0xC7 },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0x5F },
+	{ IMX_8BIT, 0x034C, 0x07 }, /*1936 x 1096 */
+	{ IMX_8BIT, 0x034D, 0x90 },
+	{ IMX_8BIT, 0x034E, 0x04 },
+	{ IMX_8BIT, 0x034F, 0x48 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x07 }, /*1936 x 1096 */
+	{ IMX_8BIT, 0x0355, 0x90 },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0x48 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x07 },
+	{ IMX_8BIT, 0x3311, 0x90 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0x48 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0xB0 },
+	{ IMX_8BIT, 0x4084, 0x07 },
+	{ IMX_8BIT, 0x4085, 0x90 },
+	{ IMX_8BIT, 0x4086, 0x04 },
+	{ IMX_8BIT, 0x4087, 0x48 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* 1080P NODVS 1936x1096 */
+static const struct imx_reg imx135_1080p_nodvs_max_clock[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x11 },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 46,396,4161,2727: 4116x2332 */
+	{ IMX_8BIT, 0x0345, 0x2E },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0x8C },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x41 },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0xA7 },
+	{ IMX_8BIT, 0x034C, 0x07 }, /*1936 x 1096 */
+	{ IMX_8BIT, 0x034D, 0x90 },
+	{ IMX_8BIT, 0x034E, 0x04 },
+	{ IMX_8BIT, 0x034F, 0x48 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x08 }, /* 2058x1166 */
+	{ IMX_8BIT, 0x0355, 0x0A },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0x8E },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x07 },
+	{ IMX_8BIT, 0x3311, 0x90 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0x48 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0xB0 },
+	{ IMX_8BIT, 0x4084, 0x07 },
+	{ IMX_8BIT, 0x4085, 0x90 },
+	{ IMX_8BIT, 0x4086, 0x04 },
+	{ IMX_8BIT, 0x4087, 0x48 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* 1080P 10%DVS 2104x1184 */
+static const struct imx_reg imx135_1080p_10_dvs_max_clock[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x00 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x10 },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 0,376,4207,2743: 4208x2368 */
+	{ IMX_8BIT, 0x0345, 0x00 },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0x78 },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x6F },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0xB7 },
+	{ IMX_8BIT, 0x034C, 0x08 }, /* 2104 x 1184 */
+	{ IMX_8BIT, 0x034D, 0x38 },
+	{ IMX_8BIT, 0x034E, 0x04 },
+	{ IMX_8BIT, 0x034F, 0xA0 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x08 }, /* 2104 x 1184 */
+	{ IMX_8BIT, 0x0355, 0x38 },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0xA0 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x08 },
+	{ IMX_8BIT, 0x3311, 0x38 },
+	{ IMX_8BIT, 0x3312, 0x04 },
+	{ IMX_8BIT, 0x3313, 0xA0 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0xB0 },
+	{ IMX_8BIT, 0x4084, 0x00 },
+	{ IMX_8BIT, 0x4085, 0x00 },
+	{ IMX_8BIT, 0x4086, 0x00 },
+	{ IMX_8BIT, 0x4087, 0x00 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+static const struct imx_reg imx135_720pdvs_max_clock[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x15 },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 46,404,4161,2715: 4116x2312 */
+	{ IMX_8BIT, 0x0345, 0x2E },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0x94 },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x41 },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0x9B },
+	{ IMX_8BIT, 0x034C, 0x06 }, /*1568 x 880 */
+	{ IMX_8BIT, 0x034D, 0x20 },
+	{ IMX_8BIT, 0x034E, 0x03 },
+	{ IMX_8BIT, 0x034F, 0x70 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x08 }, /*2058 x 1156 */
+	{ IMX_8BIT, 0x0355, 0x0A },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0x84 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x03 },
+	{ IMX_8BIT, 0x3313, 0x70 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0x4C },
+	{ IMX_8BIT, 0x4084, 0x06 },
+	{ IMX_8BIT, 0x4085, 0x20 },
+	{ IMX_8BIT, 0x4086, 0x03 },
+	{ IMX_8BIT, 0x4087, 0x70 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/******************* Video Modes ******************/
+
+/* 1080P DVS 2336x1320 */
+static const struct imx_reg imx135_2336x1320_max_clock[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_451_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },
+	{ IMX_8BIT, 0x0391, 0x11 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x1C },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 60,404,4147,2715: 4088x2312 */
+	{ IMX_8BIT, 0x0345, 0x3C },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0x94 },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x33 },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0x9B },
+	{ IMX_8BIT, 0x034C, 0x09 }, /*2336 x 1320 */
+	{ IMX_8BIT, 0x034D, 0x20 },
+	{ IMX_8BIT, 0x034E, 0x05 },
+	{ IMX_8BIT, 0x034F, 0x28 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x0F }, /* 4088x2312 */
+	{ IMX_8BIT, 0x0355, 0xF8 },
+	{ IMX_8BIT, 0x0356, 0x09 },
+	{ IMX_8BIT, 0x0357, 0x08 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x09 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x05 },
+	{ IMX_8BIT, 0x3313, 0x28 },
+	{ IMX_8BIT, 0x331C, 0x04 },
+	{ IMX_8BIT, 0x331D, 0xE2 },
+	{ IMX_8BIT, 0x4084, 0x09 },
+	{ IMX_8BIT, 0x4085, 0x20 },
+	{ IMX_8BIT, 0x4086, 0x05 },
+	{ IMX_8BIT, 0x4087, 0x28 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* 1080P DVS 2336x1320 Cropped */
+static const struct imx_reg imx135_2336x1320_cropped_mipi499[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_499_2MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x00 },
+	{ IMX_8BIT, 0x0391, 0x11 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x1C },
+	{ IMX_8BIT, 0x4082, 0x01 },
+	{ IMX_8BIT, 0x4083, 0x01 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x03 }, /* 936,900,3271,2219: 2336x1320 */
+	{ IMX_8BIT, 0x0345, 0xA8 },
+	{ IMX_8BIT, 0x0346, 0x03 },
+	{ IMX_8BIT, 0x0347, 0x84 },
+	{ IMX_8BIT, 0x0348, 0x0C },
+	{ IMX_8BIT, 0x0349, 0xC7 },
+	{ IMX_8BIT, 0x034A, 0x08 },
+	{ IMX_8BIT, 0x034B, 0xAB },
+	{ IMX_8BIT, 0x034C, 0x09 }, /* 2336 x 1320 */
+	{ IMX_8BIT, 0x034D, 0x20 },
+	{ IMX_8BIT, 0x034E, 0x05 },
+	{ IMX_8BIT, 0x034F, 0x28 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x09 }, /* 2336 x 1320 */
+	{ IMX_8BIT, 0x0355, 0x20 },
+	{ IMX_8BIT, 0x0356, 0x05 },
+	{ IMX_8BIT, 0x0357, 0x28 },
+	{ IMX_8BIT, 0x301D, 0x30 },
+	{ IMX_8BIT, 0x3310, 0x09 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x05 },
+	{ IMX_8BIT, 0x3313, 0x28 },
+	{ IMX_8BIT, 0x331C, 0x00 },
+	{ IMX_8BIT, 0x331D, 0xB4 },
+	{ IMX_8BIT, 0x4084, 0x09 },
+	{ IMX_8BIT, 0x4085, 0x20 },
+	{ IMX_8BIT, 0x4086, 0x05 },
+	{ IMX_8BIT, 0x4087, 0x28 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* 720P DVS 1568 x 880 */
+static const struct imx_reg imx135_720p_dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x15 },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 46,404,4161,2715: 4116x2312 */
+	{ IMX_8BIT, 0x0345, 0x2e },
+	{ IMX_8BIT, 0x0346, 0x01 },
+	{ IMX_8BIT, 0x0347, 0x94 },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x41 },
+	{ IMX_8BIT, 0x034A, 0x0A },
+	{ IMX_8BIT, 0x034B, 0x9B },
+	{ IMX_8BIT, 0x034C, 0x06 }, /*1568 x 880 */
+	{ IMX_8BIT, 0x034D, 0x20 },
+	{ IMX_8BIT, 0x034E, 0x03 },
+	{ IMX_8BIT, 0x034F, 0x70 },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x08 }, /* 2058x1156 */
+	{ IMX_8BIT, 0x0355, 0x0a },
+	{ IMX_8BIT, 0x0356, 0x04 },
+	{ IMX_8BIT, 0x0357, 0x84 },
+	{ IMX_8BIT, 0x301D, 0x30 }, /* TODO! */
+	{ IMX_8BIT, 0x3310, 0x06 },
+	{ IMX_8BIT, 0x3311, 0x20 },
+	{ IMX_8BIT, 0x3312, 0x03 },
+	{ IMX_8BIT, 0x3313, 0x70 },
+	{ IMX_8BIT, 0x331C, 0x01 }, /* TODO! */
+	{ IMX_8BIT, 0x331D, 0xd6 }, /* TODO! */
+	{ IMX_8BIT, 0x4084, 0x06 },
+	{ IMX_8BIT, 0x4085, 0x20 },
+	{ IMX_8BIT, 0x4086, 0x03 },
+	{ IMX_8BIT, 0x4087, 0x70 },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* wvga: H : 1640 V : 1024 */
+static const struct imx_reg imx135_wvga_dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x22 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x14 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 },
+	{IMX_8BIT, 0x0345, 0x36 },
+	{IMX_8BIT, 0x0346, 0x01 },
+	{IMX_8BIT, 0x0347, 0x18 },
+	{IMX_8BIT, 0x0348, 0x10 },
+	{IMX_8BIT, 0x0349, 0x39 },
+	{IMX_8BIT, 0x034A, 0x0B },
+	{IMX_8BIT, 0x034B, 0x17 },
+	{IMX_8BIT, 0x034C, 0x06 },
+	{IMX_8BIT, 0x034D, 0x68 },
+	{IMX_8BIT, 0x034E, 0x04 },
+	{IMX_8BIT, 0x034F, 0x00 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x08 },
+	{IMX_8BIT, 0x0355, 0x02 },
+	{IMX_8BIT, 0x0356, 0x05 },
+	{IMX_8BIT, 0x0357, 0x00 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x06 },
+	{IMX_8BIT, 0x3311, 0x68 },
+	{IMX_8BIT, 0x3312, 0x04 },
+	{IMX_8BIT, 0x3313, 0x00 },
+	{IMX_8BIT, 0x331C, 0x01 },
+	{IMX_8BIT, 0x331D, 0xBD },
+	{IMX_8BIT, 0x4084, 0x06 },
+	{IMX_8BIT, 0x4085, 0x68 },
+	{IMX_8BIT, 0x4086, 0x04 },
+	{IMX_8BIT, 0x4087, 0x00 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* 480P 1036 x 696 */
+static const struct imx_reg imx135_480p_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x00 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x10 },/* No scal */
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 52,20,4155, 3099 4144x2784*/
+	{IMX_8BIT, 0x0345, 0x20 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0xA8 },
+	{IMX_8BIT, 0x0348, 0x10 },
+	{IMX_8BIT, 0x0349, 0x4F },
+	{IMX_8BIT, 0x034A, 0x0B },
+	{IMX_8BIT, 0x034B, 0x88 },
+	{IMX_8BIT, 0x034C, 0x04 }, /* 1036 * 696 */
+	{IMX_8BIT, 0x034D, 0x0C },
+	{IMX_8BIT, 0x034E, 0x02 },
+	{IMX_8BIT, 0x034F, 0xB8 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x04 }, /* 1036x696 */
+	{IMX_8BIT, 0x0355, 0x0C },
+	{IMX_8BIT, 0x0356, 0x02 },
+	{IMX_8BIT, 0x0357, 0xB8 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x04 },
+	{IMX_8BIT, 0x3311, 0x0C },
+	{IMX_8BIT, 0x3312, 0x02 },
+	{IMX_8BIT, 0x3313, 0xB8 },
+	{IMX_8BIT, 0x331C, 0x02 },
+	{IMX_8BIT, 0x331D, 0x21 },
+	{IMX_8BIT, 0x4084, 0x04 },
+	{IMX_8BIT, 0x4085, 0x0C },
+	{IMX_8BIT, 0x4086, 0x02 },
+	{IMX_8BIT, 0x4087, 0xB8 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* 480P DVS 936 x 602 */
+static const struct imx_reg imx135_480p_dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* mode setting */
+	{ IMX_8BIT, 0x0108, 0x03 },
+	{ IMX_8BIT, 0x0112, 0x0A },
+	{ IMX_8BIT, 0x0113, 0x0A },
+	{ IMX_8BIT, 0x0381, 0x01 },
+	{ IMX_8BIT, 0x0383, 0x01 },
+	{ IMX_8BIT, 0x0385, 0x01 },
+	{ IMX_8BIT, 0x0387, 0x01 },
+	{ IMX_8BIT, 0x0390, 0x01 },
+	{ IMX_8BIT, 0x0391, 0x22 },
+	{ IMX_8BIT, 0x0392, 0x00 },
+	{ IMX_8BIT, 0x0401, 0x02 },
+	{ IMX_8BIT, 0x0404, 0x00 },
+	{ IMX_8BIT, 0x0405, 0x23 },
+	{ IMX_8BIT, 0x4082, 0x00 },
+	{ IMX_8BIT, 0x4083, 0x00 },
+	{ IMX_8BIT, 0x7006, 0x04 },
+	/* size setting */
+	{ IMX_8BIT, 0x0344, 0x00 }, /* 56,244,4151,2877: 4096x2634 */
+	{ IMX_8BIT, 0x0345, 0x38 },
+	{ IMX_8BIT, 0x0346, 0x00 },
+	{ IMX_8BIT, 0x0347, 0xf4 },
+	{ IMX_8BIT, 0x0348, 0x10 },
+	{ IMX_8BIT, 0x0349, 0x37 },
+	{ IMX_8BIT, 0x034A, 0x0b },
+	{ IMX_8BIT, 0x034B, 0x3d },
+	{ IMX_8BIT, 0x034C, 0x03 }, /* 936 x 602 */
+	{ IMX_8BIT, 0x034D, 0xa8 },
+	{ IMX_8BIT, 0x034E, 0x02 },
+	{ IMX_8BIT, 0x034F, 0x5a },
+	{ IMX_8BIT, 0x0350, 0x00 },
+	{ IMX_8BIT, 0x0351, 0x00 },
+	{ IMX_8BIT, 0x0352, 0x00 },
+	{ IMX_8BIT, 0x0353, 0x00 },
+	{ IMX_8BIT, 0x0354, 0x08 }, /* 2058x1156 */
+	{ IMX_8BIT, 0x0355, 0x00 },
+	{ IMX_8BIT, 0x0356, 0x05 },
+	{ IMX_8BIT, 0x0357, 0x25 },
+	{ IMX_8BIT, 0x301D, 0x30 }, /* TODO! */
+	{ IMX_8BIT, 0x3310, 0x03 },
+	{ IMX_8BIT, 0x3311, 0xa8 },
+	{ IMX_8BIT, 0x3312, 0x02 },
+	{ IMX_8BIT, 0x3313, 0x5a },
+	{ IMX_8BIT, 0x331C, 0x01 }, /* TODO! */
+	{ IMX_8BIT, 0x331D, 0xd6 },
+	{ IMX_8BIT, 0x4084, 0x03 },
+	{ IMX_8BIT, 0x4085, 0xa8 },
+	{ IMX_8BIT, 0x4086, 0x02 },
+	{ IMX_8BIT, 0x4087, 0x5a },
+	{ IMX_8BIT, 0x4400, 0x00 },
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+/* VGA: H : 1036 V : 780 */
+static const struct imx_reg imx135_vga_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x00 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x10 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 52,20,4155, 3099 4144x3120*/
+	{IMX_8BIT, 0x0345, 0x20 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x00 },
+	{IMX_8BIT, 0x0348, 0x10 },
+	{IMX_8BIT, 0x0349, 0x4F },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x2F },
+	{IMX_8BIT, 0x034C, 0x04 }, /* 1036x780 */
+	{IMX_8BIT, 0x034D, 0x0C },
+	{IMX_8BIT, 0x034E, 0x03 },
+	{IMX_8BIT, 0x034F, 0x0C },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x04 }, /* 1036x780 */
+	{IMX_8BIT, 0x0355, 0x0C },
+	{IMX_8BIT, 0x0356, 0x03 },
+	{IMX_8BIT, 0x0357, 0x0C },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x04 },
+	{IMX_8BIT, 0x3311, 0x0C },
+	{IMX_8BIT, 0x3312, 0x03 },
+	{IMX_8BIT, 0x3313, 0x0C },
+	{IMX_8BIT, 0x331C, 0x02 },
+	{IMX_8BIT, 0x331D, 0x21 },
+	{IMX_8BIT, 0x4084, 0x04 },
+	{IMX_8BIT, 0x4085, 0x0C },
+	{IMX_8BIT, 0x4086, 0x03 },
+	{IMX_8BIT, 0x4087, 0x0C },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* VGA: H : 820 V : 616 */
+static const struct imx_reg imx135_vga_dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x14 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 52,20,4155, 3099 4104x3080*/
+	{IMX_8BIT, 0x0345, 0x34 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x14 },
+	{IMX_8BIT, 0x0348, 0x10 },
+	{IMX_8BIT, 0x0349, 0x3B },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x1B },
+	{IMX_8BIT, 0x034C, 0x03 }, /* 820x616 */
+	{IMX_8BIT, 0x034D, 0x34 },
+	{IMX_8BIT, 0x034E, 0x02 },
+	{IMX_8BIT, 0x034F, 0x68 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x04 }, /* 1026x770 */
+	{IMX_8BIT, 0x0355, 0x02 },
+	{IMX_8BIT, 0x0356, 0x03 },
+	{IMX_8BIT, 0x0357, 0x02 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x03 },
+	{IMX_8BIT, 0x3311, 0x34 },
+	{IMX_8BIT, 0x3312, 0x02 },
+	{IMX_8BIT, 0x3313, 0x68 },
+	{IMX_8BIT, 0x331C, 0x02 },
+	{IMX_8BIT, 0x331D, 0x21 },
+	{IMX_8BIT, 0x4084, 0x03 },
+	{IMX_8BIT, 0x4085, 0x34 },
+	{IMX_8BIT, 0x4086, 0x02 },
+	{IMX_8BIT, 0x4087, 0x68 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* VGA: H : 436 V : 360 */
+static const struct imx_reg imx135_436x360_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x22 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 212,0,3995,3119 3784x3120 */
+	{IMX_8BIT, 0x0345, 0xD4 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x00 },
+	{IMX_8BIT, 0x0348, 0x0F },
+	{IMX_8BIT, 0x0349, 0x9B },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x2F },
+
+	{IMX_8BIT, 0x034C, 0x01 }, /* 436x360 */
+	{IMX_8BIT, 0x034D, 0xB4 },
+	{IMX_8BIT, 0x034E, 0x01 },
+	{IMX_8BIT, 0x034F, 0x68 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x12 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x0C },
+
+	{IMX_8BIT, 0x0354, 0x03 }, /* 928x768 crop from 946x780*/
+	{IMX_8BIT, 0x0355, 0xA0 },
+	{IMX_8BIT, 0x0356, 0x03 },
+	{IMX_8BIT, 0x0357, 0x00 },
+
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x01 },
+	{IMX_8BIT, 0x3311, 0xB4 },
+	{IMX_8BIT, 0x3312, 0x01 },
+	{IMX_8BIT, 0x3313, 0x68 },
+	{IMX_8BIT, 0x331C, 0x02 },
+	{IMX_8BIT, 0x331D, 0x21 },
+	{IMX_8BIT, 0x4084, 0x01 },
+	{IMX_8BIT, 0x4085, 0xB4 },
+	{IMX_8BIT, 0x4086, 0x01 },
+	{IMX_8BIT, 0x4087, 0x68 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* QVGA: H : 408 V : 308 */
+static const struct imx_reg imx135_qvga__dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x28 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 64,20,4143,3099 4080x3080 */
+	{IMX_8BIT, 0x0345, 0x40 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x14 },
+	{IMX_8BIT, 0x0348, 0x10 },
+	{IMX_8BIT, 0x0349, 0x2F },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x1B },
+	{IMX_8BIT, 0x034C, 0x01 }, /* 408x308 */
+	{IMX_8BIT, 0x034D, 0x98 },
+	{IMX_8BIT, 0x034E, 0x01 },
+	{IMX_8BIT, 0x034F, 0x34 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x03 }, /* 1020x770 */
+	{IMX_8BIT, 0x0355, 0xFC },
+	{IMX_8BIT, 0x0356, 0x03 },
+	{IMX_8BIT, 0x0357, 0x02 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x01 },
+	{IMX_8BIT, 0x3311, 0x98 },
+	{IMX_8BIT, 0x3312, 0x01 },
+	{IMX_8BIT, 0x3313, 0x34 },
+	{IMX_8BIT, 0x331C, 0x01 },
+	{IMX_8BIT, 0x331D, 0x68 },
+	{IMX_8BIT, 0x4084, 0x01 },
+	{IMX_8BIT, 0x4085, 0x98 },
+	{IMX_8BIT, 0x4086, 0x01 },
+	{IMX_8BIT, 0x4087, 0x34 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* CIF H : 368 V : 304 */
+static const struct imx_reg imx135_cif_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x28 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x01 }, /* 264,42,3943,3081 3680x3040 */
+	{IMX_8BIT, 0x0345, 0x08 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x2a },
+	{IMX_8BIT, 0x0348, 0x0F },
+	{IMX_8BIT, 0x0349, 0x67 },
+	{IMX_8BIT, 0x034A, 0x0c },
+	{IMX_8BIT, 0x034B, 0x09 },
+	{IMX_8BIT, 0x034C, 0x01 }, /* 368x304 */
+	{IMX_8BIT, 0x034D, 0x70 },
+	{IMX_8BIT, 0x034E, 0x01 },
+	{IMX_8BIT, 0x034F, 0x30 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x03 }, /* 920x760 */
+	{IMX_8BIT, 0x0355, 0x98 },
+	{IMX_8BIT, 0x0356, 0x02 },
+	{IMX_8BIT, 0x0357, 0xf8 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x01 },
+	{IMX_8BIT, 0x3311, 0x70 },
+	{IMX_8BIT, 0x3312, 0x01 },
+	{IMX_8BIT, 0x3313, 0x30 },
+	{IMX_8BIT, 0x331C, 0x02 }, /* TODO! binning 4x4 must be 021c? */
+	{IMX_8BIT, 0x331D, 0x1C },
+	{IMX_8BIT, 0x4084, 0x01 },
+	{IMX_8BIT, 0x4085, 0x70 },
+	{IMX_8BIT, 0x4086, 0x01 },
+	{IMX_8BIT, 0x4087, 0x30 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* CIF H : 1888 V : 1548 */
+static const struct imx_reg imx135_cif_binning_1888x1548[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x22 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x00 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x10 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 264,42, 3776x3096 */
+	{IMX_8BIT, 0x0345, 0xD8 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x0C },
+	{IMX_8BIT, 0x0348, 0x0F },
+	{IMX_8BIT, 0x0349, 0x97 },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x23 },
+	{IMX_8BIT, 0x034C, 0x07 }, /* 1888x1548 */
+	{IMX_8BIT, 0x034D, 0x60 },
+	{IMX_8BIT, 0x034E, 0x06 },
+	{IMX_8BIT, 0x034F, 0x0C },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x07 }, /* 1888x1548 */
+	{IMX_8BIT, 0x0355, 0x60 },
+	{IMX_8BIT, 0x0356, 0x06 },
+	{IMX_8BIT, 0x0357, 0x0C },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x07 },
+	{IMX_8BIT, 0x3311, 0x60 },
+	{IMX_8BIT, 0x3312, 0x06 },
+	{IMX_8BIT, 0x3313, 0x0C },
+	{IMX_8BIT, 0x331C, 0x02 }, /* TODO! binning 4x4 must be 021c? */
+	{IMX_8BIT, 0x331D, 0x1C },
+	{IMX_8BIT, 0x4084, 0x07 },
+	{IMX_8BIT, 0x4085, 0x60 },
+	{IMX_8BIT, 0x4086, 0x06 },
+	{IMX_8BIT, 0x4087, 0x0C },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* QCIF H : 216 V : 176 */
+static const struct imx_reg imx135_qcif_dvs_binning[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	PLL_SETTINGS_FOR_MIPI_209_6MHZ_SALTBAY,
+	/* Mode setting */
+	{IMX_8BIT, 0x0108, 0x03 },
+	{IMX_8BIT, 0x0112, 0x0A },
+	{IMX_8BIT, 0x0113, 0x0A },
+	{IMX_8BIT, 0x0381, 0x01 },
+	{IMX_8BIT, 0x0383, 0x01 },
+	{IMX_8BIT, 0x0385, 0x01 },
+	{IMX_8BIT, 0x0387, 0x01 },
+	{IMX_8BIT, 0x0390, 0x01 },
+	{IMX_8BIT, 0x0391, 0x44 },
+	{IMX_8BIT, 0x0392, 0x00 },
+	{IMX_8BIT, 0x0401, 0x02 },
+	{IMX_8BIT, 0x0404, 0x00 },
+	{IMX_8BIT, 0x0405, 0x46 },
+	{IMX_8BIT, 0x4082, 0x00 },
+	{IMX_8BIT, 0x4083, 0x00 },
+	{IMX_8BIT, 0x7006, 0x04 },
+	/* Size setting */
+	{IMX_8BIT, 0x0344, 0x00 }, /* 212,20,3995,3099 3784x3080 */
+	{IMX_8BIT, 0x0345, 0xD4 },
+	{IMX_8BIT, 0x0346, 0x00 },
+	{IMX_8BIT, 0x0347, 0x14 },
+	{IMX_8BIT, 0x0348, 0x0F },
+	{IMX_8BIT, 0x0349, 0x9B },
+	{IMX_8BIT, 0x034A, 0x0C },
+	{IMX_8BIT, 0x034B, 0x1B },
+	{IMX_8BIT, 0x034C, 0x00 }, /* 216x176 */
+	{IMX_8BIT, 0x034D, 0xD8 },
+	{IMX_8BIT, 0x034E, 0x00 },
+	{IMX_8BIT, 0x034F, 0xB0 },
+	{IMX_8BIT, 0x0350, 0x00 },
+	{IMX_8BIT, 0x0351, 0x00 },
+	{IMX_8BIT, 0x0352, 0x00 },
+	{IMX_8BIT, 0x0353, 0x00 },
+	{IMX_8BIT, 0x0354, 0x03 }, /* 946x770 */
+	{IMX_8BIT, 0x0355, 0xB2 },
+	{IMX_8BIT, 0x0356, 0x03 },
+	{IMX_8BIT, 0x0357, 0x02 },
+	{IMX_8BIT, 0x301D, 0x30 },
+	{IMX_8BIT, 0x3310, 0x00 },
+	{IMX_8BIT, 0x3311, 0xD8 },
+	{IMX_8BIT, 0x3312, 0x00 },
+	{IMX_8BIT, 0x3313, 0xB0 },
+	{IMX_8BIT, 0x331C, 0x02 }, /* TODO! binning 4x4 must be 021c */
+	{IMX_8BIT, 0x331D, 0x1C },
+	{IMX_8BIT, 0x4084, 0x00 },
+	{IMX_8BIT, 0x4085, 0xD8 },
+	{IMX_8BIT, 0x4086, 0x00 },
+	{IMX_8BIT, 0x4087, 0xB0 },
+	{IMX_8BIT, 0x4400, 0x00 },
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/*
+ * ISP Scaling is now supported in offine capture use cases. Because of that
+ * we need only few modes to cover the different aspect ratios from the
+ * sensor and the ISP will scale it based on the requested resolution from HAL.
+ *
+ * There is a performance impact when continuous view finder option is chose
+ * for resolutions above 8MP. So 8MP and 6MP resolution are kept, so that lower
+ * than these take 8MP or 6MP espectively for down scaling based on the
+ * aspect ratio.
+ */
+struct imx_resolution imx135_res_preview_mofd[] = {
+	{
+		.desc = "imx135_cif_binning_preview",
+		.regs = imx135_cif_binning,
+		.width = 368,
+		.height = 304,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 9114,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_vga_binning_preview",
+		.regs = imx135_vga_binning,
+		.width = 1036,
+		.height = 780,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		 .desc = "imx135_480p_preview",
+		 .regs = imx135_480p_binning,
+		 .width = 1036,
+		 .height = 696,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		 .bin_factor_x = 2,
+		 .bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_1080p_binning_preview",
+		.regs = imx135_1080p_binning,
+		.width = 1936,
+		.height = 1104,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_3m__cont_cap",
+		.regs = imx135_3m_binning,
+		.width = 2064,
+		.height = 1552,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_6m_cont_cap",
+		.regs = imx135_6m,
+		.width = 3280,
+		.height = 1852,
+		.fps_options = {
+			{ /* Binning Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_8m_scaled_from_12m__cont_cap",
+		.regs = imx135_8m_scaled_from_12m,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 24,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 3280,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_10m__cont_cap",
+		.regs = imx135_10m,
+		.width = 4208,
+		.height = 2368,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2632,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_13m__cont_cap",
+		.regs = imx135_13m,
+		.width = 4208,
+		.height = 3120,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 24,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 3290,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+};
+
+struct imx_resolution imx135_res_preview[] = {
+	{
+		.desc = "imx135_xga_cropped_video",
+		.regs = imx135_xga_cropped,
+		.width = 832,
+		.height = 628,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_2m_cropped_video",
+		.regs = imx135_2m_cropped,
+		.width = 1648,
+		.height = 1240,
+		.fps_options = {
+			{ /* Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_1936x1096_cropped",
+		.regs = imx135_1936x1096_cropped,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{ /* Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_8m_cropped_video",
+		.regs = imx135_8m_cropped,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+};
+
+/*
+ * ISP Scaling is now supported in online capture use cases. Because of that
+ * we need only few modes to cover the different aspect ratios from the
+ * sensor and the ISP will scale it based on the requested resolution from HAL.
+ *
+ * There is a performance impact when continuous view finder option is chose
+ * for resolutions above 8MP. So 8MP and 6MP resolution are kept, so that lower
+ * than these take 8MP or 6MP espectively for down scaling based on the
+ * aspect ratio.
+ */
+struct imx_resolution imx135_res_still_mofd[] = {
+	{
+		.desc = "imx135_cif_binning_still",
+		.regs = imx135_cif_binning_1888x1548,
+		.width = 1888,
+		.height = 1548,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_vga_binning_preview",
+		.regs = imx135_vga_binning,
+		.width = 1036,
+		.height = 780,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		 .desc = "imx135_480p_preview",
+		 .regs = imx135_480p_binning,
+		 .width = 1036,
+		 .height = 696,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		 .bin_factor_x = 2,
+		 .bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_1080p_binning_still",
+		.regs = imx135_1080p_binning,
+		.width = 1936,
+		.height = 1104,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 15,
+				 .pixels_per_line = 9114,
+				 .lines_per_frame = 2453,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_3m__still",
+		.regs = imx135_3m_binning,
+		.width = 2064,
+		.height = 1552,
+		.fps_options = {
+			{ /* Binning Pixel clock: 335.36MHz */
+				 .fps = 15,
+				 .pixels_per_line = 9114,
+				 .lines_per_frame = 2453,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_6m_for_mipi_342_still",
+		.regs = imx135_6m_for_mipi_342,
+		.width = 3280,
+		.height = 1852,
+		.fps_options = {
+			{ /* Pixel clock: 273.6MHz */
+				 .fps = 11,
+				 .pixels_per_line = 9114,
+				 .lines_per_frame = 2664,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 342000,
+	},
+	{
+		.desc = "imx135_8m_scaled_from_12m_for_mipi342_still",
+		.regs = imx135_8m_scaled_from_12m_for_mipi342,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{ /* Pixel clock: 273.6MHz */
+				 .fps = 8,
+				 .pixels_per_line = 7672,
+				 .lines_per_frame = 4458,
+			},
+			{ /* Pixel clock: 273.6MHz */
+				 .fps = 15,
+				 .pixels_per_line = 5500,
+				 .lines_per_frame = 3314,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 342000,
+	},
+	{
+		.desc = "imx135_10m_for_mipi_342_still",
+		.regs = imx135_10m_for_mipi_342,
+		.width = 4208,
+		.height = 2368,
+		.fps_options = {
+			{ /* Pixel clock: 273.6MHz */
+				 .fps = 11,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 2664,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 342000,
+	},
+	{
+		.desc = "imx135_13m_still",
+		.regs = imx135_13m_for_mipi_342,
+		.width = 4208,
+		.height = 3120,
+		.fps_options = {
+			{ /* Pixel clock: 273.6MHz */
+				 .fps = 5,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 5990,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 342000,
+	},
+};
+
+struct imx_resolution imx135_res_still[] = {
+	{
+		.desc = "imx135_qvga",
+		.regs = imx135_336x256,
+		.width = 336,
+		.height = 256,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_cif",
+		.regs = imx135_368x304_cropped,
+		.width = 368,
+		.height = 304,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_xga_cropped_video",
+		.regs = imx135_xga_cropped,
+		.width = 832,
+		.height = 628,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_2M_for_11:9",
+		.regs = imx135_1424x1168_cropped,
+		.width = 1424,
+		.height = 1168,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_2m_cropped_video",
+		.regs = imx135_2m_cropped,
+		.width = 1648,
+		.height = 1240,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 15,
+				 .pixels_per_line = 6466,
+				 .lines_per_frame = 3710,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_6m_cropped_video",
+		.regs = imx135_6m_cropped,
+		.width = 3280,
+		.height = 1852,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 8,
+				 .pixels_per_line = 8850,
+				 .lines_per_frame = 5080,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_8m_cropped_video",
+		.regs = imx135_8m_cropped,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 8,
+				 .pixels_per_line = 8850,
+				 .lines_per_frame = 5080,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+};
+
+/*
+ * ISP scaling is not supported in case of video modes. So we need to have
+ * separate sensor mode for video use cases
+ */
+struct imx_resolution imx135_res_video[] = {
+	/* For binning modes pix clock is 335.36 MHz. */
+	{
+		.desc = "imx135_qcif_dvs_binning_video",
+		.regs = imx135_qcif_dvs_binning,
+		.width = 216,
+		.height = 176,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_cif_binning_video",
+		.regs = imx135_cif_binning,
+		.width = 368,
+		.height = 304,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_qvga__dvs_binning_video",
+		.regs = imx135_qvga__dvs_binning,
+		.width = 408,
+		.height = 308,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_436x360_binning_video",
+		.regs = imx135_436x360_binning,
+		.width = 436,
+		.height = 360,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_vga_dvs_binning_video",
+		.regs = imx135_vga_dvs_binning,
+		.width = 820,
+		.height = 616,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 9144,
+				 .lines_per_frame = 1226,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_480p_dvs_binning_video",
+		.regs = imx135_480p_dvs_binning,
+		.width = 936,
+		.height = 602,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_720P_dvs_video",
+		.regs = imx135_720pdvs_max_clock,
+		.width = 1568,
+		.height = 880,
+		.fps_options = {
+			{/* Pixel Clock : 360.96 MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5850,
+				 .lines_per_frame = 2000,
+			},
+			{/* Pixel Clock : 360.96 MHz */
+				 .fps = 60,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 1310,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_wvga_dvs_binning_video",
+		.regs = imx135_wvga_dvs_binning,
+		.width = 1640,
+		.height = 1024,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 5464,
+				 .lines_per_frame = 2046,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 209600,
+	},
+	{
+		.desc = "imx135_1936_1096_fullfov_max_clock",
+		.regs = imx135_1080p_nodvs_max_clock,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{/* Pixel Clock : 360.96 MHz */
+				 .fps = 30,
+				 .pixels_per_line = 5850,
+				 .lines_per_frame = 2000,
+			},
+			{/* Pixel Clock : 360.96 MHz */
+				 .fps = 60,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 1310,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_1080P_dvs_video",
+		.regs = imx135_2336x1320_max_clock,
+		.width = 2336,
+		.height = 1320,
+		.fps_options = {
+			{/* Pixel Clock : 360.96 MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2632,
+				 .regs = imx135_2336x1320_max_clock,
+				.mipi_freq = 451200,
+			},
+			{/* Pixel Clock : 399.36MHz */
+				 .fps = 60,
+				 .pixels_per_line = 4754,
+				 .lines_per_frame = 1400,
+				 .regs = imx135_2336x1320_cropped_mipi499,
+				.mipi_freq = 499200,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_6m_cont_cap",
+		.regs = imx135_6m,
+		.width = 3280,
+		.height = 1852,
+		.fps_options = {
+			{ /* Binning Pixel clock: 360.96MHz */
+				.fps = 30,
+				.pixels_per_line = 4572,
+				.lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+	{
+		.desc = "imx135_8m_cropped_video",
+		.regs = imx135_8m_cropped,
+		.width = 3280,
+		.height = 2464,
+		.fps_options = {
+			{ /* Pixel clock: 360.96MHz */
+				 .fps = 30,
+				 .pixels_per_line = 4572,
+				 .lines_per_frame = 2624,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.mipi_freq = 451200,
+	},
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx175.h b/drivers/staging/media/atomisp/i2c/imx/imx175.h
new file mode 100644
index 000000000000..5f409ccedc85
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx175.h
@@ -0,0 +1,1959 @@
+#ifndef __IMX175_H__
+#define __IMX175_H__
+#include "common.h"
+
+/************************** settings for imx *************************/
+static struct imx_reg const imx_STILL_8M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xC4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x66},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0C},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0xD0},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x09},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xA0},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_8M_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0B},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xB8},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x16},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x44},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0C},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0xD0},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x09},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xA0},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_3M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xC4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x66},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x08},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x06},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x10},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x19}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_3M_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0B},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xB8},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x16},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x44},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x08},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x06},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x10},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x19}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+
+static struct imx_reg const imx_STILL_5M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xC4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x66},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0A},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x07},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x90},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x14}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_5M_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0B},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xB8},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x16},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x44},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0A},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x07},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x90},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x14}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_6M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xC4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x66},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x32},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x6D},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0C},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0xD0},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x07},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x3C},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_6M_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEF},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0B},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xB8},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x16},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x44},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x32},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x6D},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x0C},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0xD0},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x07},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x3C},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_2M_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x8C},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xC4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x66},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x68},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x04},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xD0},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_2M_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x0A},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x8C},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x2c},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0B},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xB8},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x16},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x44},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x68},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x04},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xD0},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x77},
+	{IMX_8BIT, 0x3371, 0x2F},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x2F},
+	{IMX_8BIT, 0x3375, 0x37},
+	{IMX_8BIT, 0x3376, 0x9F},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x09},
+	{IMX_8BIT, 0x33D7, 0xA0},
+
+	{IMX_8BIT, 0x030e, 0x01},
+	{IMX_8BIT, 0x41c0, 0x01},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_PREVIEW_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x06},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x05},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x48},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x70},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x03},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x34},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x02},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x68},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x03},
+	{IMX_8BIT, 0x33D5, 0x34},
+	{IMX_8BIT, 0x33D6, 0x02},
+	{IMX_8BIT, 0x33D7, 0x68},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_WIDE_PREVIEW_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x06},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x0D},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x70},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x10},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x00},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x14},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x8C},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x68},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x03},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xBC},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x68},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0xBC},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/*****************************video************************/
+static struct imx_reg const imx_1080p_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x06},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x4C},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xA4},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x11},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0xC6},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x01},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0xDB},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x02},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x42},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0A},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xEA},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x07},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x61},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x09},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x05},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x20},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_1080p_no_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x08},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xD5},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x07},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xD0},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0F},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x3C},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x34},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x6B},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x07},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x94},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x04},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x44},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x1B}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_1080p_no_dvs_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x08},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xD5},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x09},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xA6},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x18},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x9C},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x34},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x6B},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x07},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x94},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x04},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x44},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x1B}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+/*****************************video************************/
+static struct imx_reg const imx_720p_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x00},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x13},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x9C},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x01},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0xD7},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x02},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x3E},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0A},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xEE},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x07},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x65},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x10},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x03},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x70},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x00}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x18}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_480p_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x00},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x13},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x9C},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x01},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0xD4},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0xC8},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0A},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xF1},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x07},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0xDB},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x03},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x70},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x02},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x50},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x02}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x15}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_720p_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x04},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x05},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x48},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x14},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x28},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x48},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x64},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0x87},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x3B},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x20},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x03},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x6C},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x20},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0x6C},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_STILL_720p_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x04},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x08},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0xCA},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x18},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x38},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x48},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x01},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x64},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0x87},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x3B},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x20},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x03},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x6C},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x20},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0x6C},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_WVGA_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xEC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x09},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x01},
+	{IMX_8BIT, 0x030D, 0x12},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x00},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x13},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x9C},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0xD0},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x08},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0xCF},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x06},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x68},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x04},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x00},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x01}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x57},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x6F},
+	{IMX_8BIT, 0x3371, 0x27},
+	{IMX_8BIT, 0x3372, 0x4F},
+	{IMX_8BIT, 0x3373, 0x2F},
+	{IMX_8BIT, 0x3374, 0x27},
+	{IMX_8BIT, 0x3375, 0x2F},
+	{IMX_8BIT, 0x3376, 0x97},
+	{IMX_8BIT, 0x3377, 0x37},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x0C},
+	{IMX_8BIT, 0x33D5, 0xD0},
+	{IMX_8BIT, 0x33D6, 0x07},
+	{IMX_8BIT, 0x33D7, 0x38},
+	{IMX_TOK_TERM, 0, 0}
+};
+static struct imx_reg const imx_CIF_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x04},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x00},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x11},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0xDB},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x01},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x70},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x01},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x30},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x20},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0x6C},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_VGA_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x04},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x06},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x00},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x11},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x94},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x03},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x34},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x02},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x68},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x20},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0x6C},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_VGA_strong_dvs_15fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0xFC},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x04},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x07},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x9E},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x1C},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0xB6},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x00},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x00},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x00},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x00},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x0C},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0xCF},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x09},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x9F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x03},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x34},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x02},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x68},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x06},
+	{IMX_8BIT, 0x33D5, 0x20},
+	{IMX_8BIT, 0x33D6, 0x03},
+	{IMX_8BIT, 0x33D7, 0x6C},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_QVGA_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x06},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x05},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x48},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x70},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x03},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0x38},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x02},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x68},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x09},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0x97},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x07},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x37},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x01},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0x98},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x01},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0x34},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x01},
+	{IMX_8BIT, 0x33D5, 0x98},
+	{IMX_8BIT, 0x33D6, 0x01},
+	{IMX_8BIT, 0x33D7, 0x34},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_QCIF_strong_dvs_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0100, 0x00},  /*	mode_select	*/
+	/* shutter */
+	{IMX_8BIT, 0x0202, 0x05},  /* coarse _integration_time[15:8] */
+	{IMX_8BIT, 0x0203, 0x44},  /* coarse _integration_time[7:0] */
+	/* pll */
+	{IMX_8BIT, 0x0301, 0x05},  /*	vt_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0303, 0x01},  /*	vt_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0305, 0x06},  /*	pre_pll_clk_div[7:0]	*/
+	{IMX_8BIT, 0x0309, 0x05},  /*	op_pix_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030B, 0x01},  /*	op_sys_clk_div[7:0]	*/
+	{IMX_8BIT, 0x030C, 0x00},
+	{IMX_8BIT, 0x030D, 0x6D},
+	/* image sizing */
+	{IMX_8BIT, 0x0340, 0x05},  /* frame_length_lines[15:8] */
+	{IMX_8BIT, 0x0341, 0x48},  /*	frame_length_lines[7:0]	*/
+	{IMX_8BIT, 0x0342, 0x0D},  /*	line_length_pck[15:8]	*/
+	{IMX_8BIT, 0x0343, 0x70},  /*	line_length_pck[7:0]	*/
+	{IMX_8BIT, 0x0344, 0x04},  /*	x_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0345, 0xB8},  /*	x_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0346, 0x03},  /*	y_addr_start[15:8]	*/
+	{IMX_8BIT, 0x0347, 0x70},  /*	y_addr_start[7:0]	*/
+	{IMX_8BIT, 0x0348, 0x08},  /*	x_addr_end[15:8]	*/
+	{IMX_8BIT, 0x0349, 0x17},  /*	x_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034A, 0x06},  /*	y_addr_end[15:8]	*/
+	{IMX_8BIT, 0x034B, 0x2F},  /*	y_addr_end[7:0]	*/
+	{IMX_8BIT, 0x034C, 0x00},  /*	x_output_size[15:8]	*/
+	{IMX_8BIT, 0x034D, 0xD8},  /*	x_output_size[7:0]	*/
+	{IMX_8BIT, 0x034E, 0x00},  /*	y_output_size[15:8]	*/
+	{IMX_8BIT, 0x034F, 0xB0},  /*	y_output_size[7:0]	*/
+	/* binning & scaling */
+	{IMX_8BIT, 0x0390, 0x02}, /* binning mode */
+	{IMX_8BIT, 0x0401, 0x00}, /* scaling mode*/
+	{IMX_8BIT, 0x0405, 0x10}, /* scale_m[7:0] */
+	/* timer */
+	{IMX_8BIT, 0x3344, 0x37},
+	{IMX_8BIT, 0x3345, 0x1F},
+	/* timing */
+	{IMX_8BIT, 0x3370, 0x5F},
+	{IMX_8BIT, 0x3371, 0x17},
+	{IMX_8BIT, 0x3372, 0x37},
+	{IMX_8BIT, 0x3373, 0x17},
+	{IMX_8BIT, 0x3374, 0x17},
+	{IMX_8BIT, 0x3375, 0x0F},
+	{IMX_8BIT, 0x3376, 0x57},
+	{IMX_8BIT, 0x3377, 0x27},
+	{IMX_8BIT, 0x33C8, 0x01},
+	{IMX_8BIT, 0x33D4, 0x00},
+	{IMX_8BIT, 0x33D5, 0xD8},
+	{IMX_8BIT, 0x33D6, 0x00},
+	{IMX_8BIT, 0x33D7, 0xB0},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx175_init_settings[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0103, 0x01},
+	/* misc control */
+	{IMX_8BIT, 0x3020, 0x10},
+	{IMX_8BIT, 0x302D, 0x02},
+	{IMX_8BIT, 0x302F, 0x80},
+	{IMX_8BIT, 0x3032, 0xA3},
+	{IMX_8BIT, 0x3033, 0x20},
+	{IMX_8BIT, 0x3034, 0x24},
+	{IMX_8BIT, 0x3041, 0x15},
+	{IMX_8BIT, 0x3042, 0x87},
+	{IMX_8BIT, 0x3050, 0x35},
+	{IMX_8BIT, 0x3056, 0x57},
+	{IMX_8BIT, 0x305D, 0x41},
+	{IMX_8BIT, 0x3097, 0x69},
+	{IMX_8BIT, 0x3109, 0x41},
+	{IMX_8BIT, 0x3148, 0x3F},
+	{IMX_8BIT, 0x330F, 0x07},
+	/* csi & inck */
+	{IMX_8BIT, 0x3364, 0x00},
+	{IMX_8BIT, 0x3368, 0x13},
+	{IMX_8BIT, 0x3369, 0x33},
+	/* znr */
+	{IMX_8BIT, 0x4100, 0x0E},
+	{IMX_8BIT, 0x4104, 0x32},
+	{IMX_8BIT, 0x4105, 0x32},
+	{IMX_8BIT, 0x4108, 0x01},
+	{IMX_8BIT, 0x4109, 0x7C},
+	{IMX_8BIT, 0x410A, 0x00},
+	{IMX_8BIT, 0x410B, 0x00},
+	GROUPED_PARAMETER_HOLD_DISABLE,
+	{IMX_TOK_TERM, 0, 0}
+};
+/* TODO settings of preview/still/video will be updated with new use case */
+struct imx_resolution imx175_res_preview[] = {
+	{
+		.desc = "CIF_strong_dvs_30fps",
+		.regs = imx_CIF_strong_dvs_30fps,
+		.width = 368,
+		.height = 304,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x11DB,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+
+	},
+	{
+		.desc = "VGA_strong_dvs_30fps",
+		.regs = imx_VGA_strong_dvs_30fps,
+		.width = 820,
+		.height = 616,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x11DB,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+	},
+	{
+		.desc = "WIDE_PREVIEW_30fps",
+		.regs = imx_WIDE_PREVIEW_30fps,
+		.width = 1640,
+		.height = 956,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x1000,
+				 .lines_per_frame = 0x0D70,
+			},
+			{
+			}
+		},
+		.mipi_freq = 174500,
+	},
+	{
+		.desc = "STILL_720p_30fps",
+		.regs = imx_STILL_720p_30fps,
+		.width = 1568,
+		.height = 876,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x1428,
+				 .lines_per_frame = 0x0548,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+	},
+	{
+		.desc = "STILL_2M_30fps",
+		.regs = imx_STILL_2M_30fps,
+		.width = 1640,
+		.height = 1232,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D66,
+				 .lines_per_frame = 0x09C4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "1080p_strong_dvs_30fps",
+		.regs = imx_1080p_no_dvs_30fps,
+		.width = 1940,
+		.height = 1092,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0F3C,
+				 .lines_per_frame = 0x07D0,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+	{
+		.desc = "STILL_3M_30fps",
+		.regs = imx_STILL_3M_30fps,
+		.width = 2064,
+		.height = 1552,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D66,
+				 .lines_per_frame = 0x09C4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_5M_30fps",
+		.regs = imx_STILL_5M_30fps,
+		.width = 2576,
+		.height = 1936,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D66,
+				 .lines_per_frame = 0x09C4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_6M_30fps",
+		.regs = imx_STILL_6M_30fps,
+		.width = 3280,
+		.height = 1852,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D66,
+				 .lines_per_frame = 0x09C4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_8M_30fps",
+		.regs = imx_STILL_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D66,
+				 .lines_per_frame = 0x09C4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+};
+
+struct imx_resolution imx175_res_still[] = {
+	{
+		.desc = "CIF_strong_dvs_30fps",
+		.regs = imx_CIF_strong_dvs_30fps,
+		.width = 368,
+		.height = 304,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x11DB,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261000,
+	},
+	{
+		.desc = "VGA_strong_dvs_15fps",
+		.regs = imx_VGA_strong_dvs_15fps,
+		.width = 820,
+		.height = 616,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1C86,
+				 .lines_per_frame = 0x079E,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+	},
+	{
+		.desc = "imx_STILL_720p_15fps",
+		.regs = imx_STILL_720p_15fps,
+		.width = 1568,
+		.height = 876,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1838,
+				 .lines_per_frame = 0x08CA,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+	},
+	{
+		.desc = "STILL_2M_15fps",
+		.regs = imx_STILL_2M_15fps,
+		.width = 1640,
+		.height = 1232,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1646,
+				 .lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "1080p_strong_dvs_15fps",
+		.regs = imx_1080p_no_dvs_15fps,
+		.width = 1940,
+		.height = 1092,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x189C,
+				 .lines_per_frame = 0x09A6,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+	{
+		.desc = "STILL_3M_15fps",
+		.regs = imx_STILL_3M_15fps,
+		.width = 2064,
+		.height = 1552,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1646,
+				 .lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_5M_15fps",
+		.regs = imx_STILL_5M_15fps,
+		.width = 2576,
+		.height = 1936,
+		.fps = 15,
+		.pixels_per_line = 0x1646, /* consistent with regs arrays */
+		.lines_per_frame = 0x0BB8, /* consistent with regs arrays */
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1646,
+				 .lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_6M_15fps",
+		.regs = imx_STILL_6M_15fps,
+		.width = 3280,
+		.height = 1852,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1646,
+				 .lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+	{
+		.desc = "STILL_8M_15fps",
+		.regs = imx_STILL_8M_15fps,
+		.width = 3280,
+		.height = 2464,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 15,
+				 .pixels_per_line = 0x1646,
+				 .lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+		.mipi_freq = 320000,
+	},
+};
+
+struct imx_resolution imx175_res_video[] = {
+	{
+		.desc = "QCIF_strong_dvs_30fps",
+		.regs = imx_QCIF_strong_dvs_30fps,
+		.width = 216,
+		.height = 176,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D70,
+				 .lines_per_frame = 0x0548,
+			},
+			{
+			}
+		},
+		.mipi_freq = 174500,
+	},
+	{
+		.desc =	"QVGA_strong_dvs_30fps",
+		.regs = imx_QVGA_strong_dvs_30fps,
+		.width = 408,
+		.height = 308,
+		.bin_factor_x =	4,
+		.bin_factor_y =	4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D70,
+				 .lines_per_frame = 0x0548,
+			},
+			{
+			}
+		},
+		.mipi_freq = 174500,
+	},
+	{
+		.desc = "VGA_strong_dvs_30fps",
+		.regs = imx_VGA_strong_dvs_30fps,
+		.width = 820,
+		.height = 616,
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x1194,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 261500,
+	},
+	{
+		.desc = "720p_strong_dvs_30fps",
+		.regs = imx_720p_strong_dvs_30fps,
+		.width = 1552,
+		.height = 880,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x139C,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+				 .fps = 60,
+				 .pixels_per_line = 0xD70,
+				 .lines_per_frame = 0x444,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+	{
+		.desc = "480p_strong_dvs_30fps",
+		.regs = imx_480p_strong_dvs_30fps,
+		.width = 880,
+		.height = 592,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x139C,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+	{
+		.desc = "WVGA_strong_dvs_30fps",
+		.regs = imx_WVGA_strong_dvs_30fps,
+		.width = 1640,
+		.height = 1024,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x139C,
+				 .lines_per_frame = 0x0600,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+	{
+		.desc = "1080p_strong_dvs_30fps",
+		.regs = imx_1080p_strong_dvs_30fps,
+		.width = 2320,
+		.height = 1312,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x11C6,
+				 .lines_per_frame = 0x06A4,
+			},
+			{
+			}
+		},
+		.mipi_freq = 292500,
+	},
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx208.h b/drivers/staging/media/atomisp/i2c/imx/imx208.h
new file mode 100644
index 000000000000..fed387f42f99
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx208.h
@@ -0,0 +1,550 @@
+/*
+ * Support for Sony IMX 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __IMX208_H__
+#define __IMX208_H__
+#include "common.h"
+
+/********************** settings for imx from vendor*********************/
+static struct imx_reg imx208_1080p_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0305, 0x02},    /* PREPLLCK DIV */
+	{IMX_8BIT, 0x0307, 0x54},    /* PLL MPY */
+	{IMX_8BIT, 0x303C, 0x3C},    /* PLL oscillation stable wait time */
+	{IMX_8BIT, 0x30A4, 0x02},    /* Default */
+	{IMX_8BIT, 0x0112, 0x0A},    /* CCP_data_format : RAW 10bit */
+	{IMX_8BIT, 0x0113, 0x0A},    /* CCP_data_format :  RAW 10bit */
+	{IMX_8BIT, 0x0340, 0x04},    /* frame length line [15:8] */
+	{IMX_8BIT, 0x0341, 0xAA},    /* frame length line [7:0] */
+	{IMX_8BIT, 0x0342, 0x08},    /* line length pck [15:8] */
+	{IMX_8BIT, 0x0343, 0xC8},    /* line length pck [7:0] */
+	{IMX_8BIT, 0x0344, 0x00},    /* x_addr_start[12:8] */
+	{IMX_8BIT, 0x0345, 0x00},    /* x_addr_start[7:0] */
+	{IMX_8BIT, 0x0346, 0x00},    /* y_addr_start[12:8] */
+	{IMX_8BIT, 0x0347, 0x00},    /* y_addr_start[7:0] */
+	{IMX_8BIT, 0x0348, 0x07},    /* x_addr_end [12:8] */
+	{IMX_8BIT, 0x0349, 0x8F},    /* x_addr_end [7:0] */
+	{IMX_8BIT, 0x034A, 0x04},    /* y_addr_end [12:8] */
+	{IMX_8BIT, 0x034B, 0x47},    /* y_addr_end [7:0] */
+	{IMX_8BIT, 0x034C, 0x07},    /* x_output_size [ 12:8] */
+	{IMX_8BIT, 0x034D, 0x90},    /* x_output_size [7:0] */
+	{IMX_8BIT, 0x034E, 0x04},    /* y_output_size [11:8] */
+	{IMX_8BIT, 0x034F, 0x48},    /* y_output_size [7:0] */
+	{IMX_8BIT, 0x0381, 0x01},    /* x_even_inc */
+	{IMX_8BIT, 0x0383, 0x01},    /* x_odd_inc */
+	{IMX_8BIT, 0x0385, 0x01},    /* y_even_inc */
+	{IMX_8BIT, 0x0387, 0x01},    /* y_odd_inc */
+	{IMX_8BIT, 0x3048, 0x00},    /* VMODEFDS  binning operation */
+	{IMX_8BIT, 0x304E, 0x0A},    /* VTPXCK_DIV */
+	{IMX_8BIT, 0x3050, 0x02},    /* OPSYCK_DIV */
+	{IMX_8BIT, 0x309B, 0x00},    /* RGDAFDSUMEN */
+	{IMX_8BIT, 0x30D5, 0x00},    /* HADDEN ( binning ) */
+	{IMX_8BIT, 0x3301, 0x01},    /* RGLANESEL */
+	{IMX_8BIT, 0x3318, 0x61},    /* MIPI Global Timing */
+	{IMX_8BIT, 0x0202, 0x01},    /* coarse integration time */
+	{IMX_8BIT, 0x0203, 0x90},    /* coarse integration time */
+	{IMX_8BIT, 0x0205, 0x00},    /* ana global gain */
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx208_1296x736_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0305, 0x02},    /* PREPLLCK DIV */
+	{IMX_8BIT, 0x0307, 0x54},    /* PLL MPY */
+	{IMX_8BIT, 0x303C, 0x3C},    /* PLL oscillation stable wait time */
+	{IMX_8BIT, 0x30A4, 0x02},    /* Default */
+	{IMX_8BIT, 0x0112, 0x0A},    /* CCP_data_format : RAW 10bit */
+	{IMX_8BIT, 0x0113, 0x0A},    /* CCP_data_format :  RAW 10bit */
+	{IMX_8BIT, 0x0340, 0x04},    /* frame length line [15:8] */
+	{IMX_8BIT, 0x0341, 0xAA},    /* frame length line [7:0] */
+	{IMX_8BIT, 0x0342, 0x08},    /* line length pck [15:8] */
+	{IMX_8BIT, 0x0343, 0xC8},    /* line length pck [7:0] */
+	{IMX_8BIT, 0x0344, 0x01},    /* x_addr_start[12:8] */
+	{IMX_8BIT, 0x0345, 0x40},    /* x_addr_start[7:0] */
+	{IMX_8BIT, 0x0346, 0x00},    /* y_addr_start[12:8] */
+	{IMX_8BIT, 0x0347, 0xB4},    /* y_addr_start[7:0] */
+	{IMX_8BIT, 0x0348, 0x06},    /* x_addr_end [12:8] */
+	{IMX_8BIT, 0x0349, 0x4F},    /* x_addr_end [7:0] */
+	{IMX_8BIT, 0x034A, 0x03},    /* y_addr_end [12:8] */
+	{IMX_8BIT, 0x034B, 0x93},    /* y_addr_end [7:0] */
+	{IMX_8BIT, 0x034C, 0x05},    /* x_output_size [ 12:8] */
+	{IMX_8BIT, 0x034D, 0x10},    /* x_output_size [7:0] */
+	{IMX_8BIT, 0x034E, 0x02},    /* y_output_size [11:8] */
+	{IMX_8BIT, 0x034F, 0xE0},    /* y_output_size [7:0] */
+	{IMX_8BIT, 0x0381, 0x01},    /* x_even_inc */
+	{IMX_8BIT, 0x0383, 0x01},    /* x_odd_inc */
+	{IMX_8BIT, 0x0385, 0x01},    /* y_even_inc */
+	{IMX_8BIT, 0x0387, 0x01},    /* y_odd_inc */
+	{IMX_8BIT, 0x3048, 0x00},    /* VMODEFDS  binning operation */
+	{IMX_8BIT, 0x304E, 0x0A},    /* VTPXCK_DIV */
+	{IMX_8BIT, 0x3050, 0x02},    /* OPSYCK_DIV */
+	{IMX_8BIT, 0x309B, 0x00},    /* RGDAFDSUMEN */
+	{IMX_8BIT, 0x30D5, 0x00},    /* HADDEN ( binning ) */
+	{IMX_8BIT, 0x3301, 0x01},    /* RGLANESEL */
+	{IMX_8BIT, 0x3318, 0x61},    /* MIPI Global Timing */
+	{IMX_8BIT, 0x0202, 0x01},    /* coarse integration time */
+	{IMX_8BIT, 0x0203, 0x90},    /* coarse integration time */
+	{IMX_8BIT, 0x0205, 0x00},    /* ana global gain */
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx208_1296x976_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0305, 0x02},    /* PREPLLCK DIV */
+	{IMX_8BIT, 0x0307, 0x54},    /* PLL MPY */
+	{IMX_8BIT, 0x303C, 0x3C},    /* PLL oscillation stable wait time */
+	{IMX_8BIT, 0x30A4, 0x02},    /* Default */
+	{IMX_8BIT, 0x0112, 0x0A},    /* CCP_data_format : RAW 10bit */
+	{IMX_8BIT, 0x0113, 0x0A},    /* CCP_data_format :  RAW 10bit */
+	{IMX_8BIT, 0x0340, 0x04},    /* frame length line [15:8] */
+	{IMX_8BIT, 0x0341, 0xAA},    /* frame length line [7:0] */
+	{IMX_8BIT, 0x0342, 0x08},    /* line length pck [15:8] */
+	{IMX_8BIT, 0x0343, 0xC8},    /* line length pck [7:0] */
+	{IMX_8BIT, 0x0344, 0x01},    /* x_addr_start[12:8] */
+	{IMX_8BIT, 0x0345, 0x40},    /* x_addr_start[7:0] */
+	{IMX_8BIT, 0x0346, 0x00},    /* y_addr_start[12:8] */
+	{IMX_8BIT, 0x0347, 0x3C},    /* y_addr_start[7:0] */
+	{IMX_8BIT, 0x0348, 0x06},    /* x_addr_end [12:8] */
+	{IMX_8BIT, 0x0349, 0x4F},    /* x_addr_end [7:0] */
+	{IMX_8BIT, 0x034A, 0x04},    /* y_addr_end [12:8] */
+	{IMX_8BIT, 0x034B, 0x0B},    /* y_addr_end [7:0] */
+	{IMX_8BIT, 0x034C, 0x05},    /* x_output_size [ 12:8] */
+	{IMX_8BIT, 0x034D, 0x10},    /* x_output_size [7:0] */
+	{IMX_8BIT, 0x034E, 0x03},    /* y_output_size [11:8] */
+	{IMX_8BIT, 0x034F, 0xD0},    /* y_output_size [7:0] */
+	{IMX_8BIT, 0x0381, 0x01},    /* x_even_inc */
+	{IMX_8BIT, 0x0383, 0x01},    /* x_odd_inc */
+	{IMX_8BIT, 0x0385, 0x01},    /* y_even_inc */
+	{IMX_8BIT, 0x0387, 0x01},    /* y_odd_inc */
+	{IMX_8BIT, 0x3048, 0x00},    /* VMODEFDS  binning operation */
+	{IMX_8BIT, 0x304E, 0x0A},    /* VTPXCK_DIV */
+	{IMX_8BIT, 0x3050, 0x02},    /* OPSYCK_DIV */
+	{IMX_8BIT, 0x309B, 0x00},    /* RGDAFDSUMEN */
+	{IMX_8BIT, 0x30D5, 0x00},    /* HADDEN ( binning ) */
+	{IMX_8BIT, 0x3301, 0x01},    /* RGLANESEL */
+	{IMX_8BIT, 0x3318, 0x61},    /* MIPI Global Timing */
+	{IMX_8BIT, 0x0202, 0x01},    /* coarse integration time */
+	{IMX_8BIT, 0x0203, 0x90},    /* coarse integration time */
+	{IMX_8BIT, 0x0205, 0x00},    /* ana global gain */
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx208_336x256_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0305, 0x02},    /* PREPLLCK DIV */
+	{IMX_8BIT, 0x0307, 0x54},    /* PLL MPY */
+	{IMX_8BIT, 0x303C, 0x3C},    /* PLL oscillation stable wait time */
+	{IMX_8BIT, 0x30A4, 0x02},    /* Default */
+	{IMX_8BIT, 0x0112, 0x0A},    /* CCP_data_format : RAW 10bit */
+	{IMX_8BIT, 0x0113, 0x0A},    /* CCP_data_format :  RAW 10bit */
+	{IMX_8BIT, 0x0340, 0x04},    /* frame length line [15:8] */
+	{IMX_8BIT, 0x0341, 0xAA},    /* frame length line [7:0] */
+	{IMX_8BIT, 0x0342, 0x08},    /* line length pck [15:8] */
+	{IMX_8BIT, 0x0343, 0xC8},    /* line length pck [7:0] */
+	{IMX_8BIT, 0x0344, 0x02},    /* x_addr_start[12:8] */
+	{IMX_8BIT, 0x0345, 0x78},    /* x_addr_start[7:0] */
+	{IMX_8BIT, 0x0346, 0x01},    /* y_addr_start[12:8] */
+	{IMX_8BIT, 0x0347, 0x24},    /* y_addr_start[7:0] */
+	{IMX_8BIT, 0x0348, 0x05},    /* x_addr_end [12:8] */
+	{IMX_8BIT, 0x0349, 0x17},    /* x_addr_end [7:0] */
+	{IMX_8BIT, 0x034A, 0x03},    /* y_addr_end [12:8] */
+	{IMX_8BIT, 0x034B, 0x23},    /* y_addr_end [7:0] */
+	{IMX_8BIT, 0x034C, 0x01},    /* x_output_size [ 12:8] */
+	{IMX_8BIT, 0x034D, 0x50},    /* x_output_size [7:0] */
+	{IMX_8BIT, 0x034E, 0x01},    /* y_output_size [11:8] */
+	{IMX_8BIT, 0x034F, 0x00},    /* y_output_size [7:0] */
+	{IMX_8BIT, 0x0381, 0x01},    /* x_even_inc */
+	{IMX_8BIT, 0x0383, 0x03},    /* x_odd_inc */
+	{IMX_8BIT, 0x0385, 0x01},    /* y_even_inc */
+	{IMX_8BIT, 0x0387, 0x03},    /* y_odd_inc */
+	{IMX_8BIT, 0x3048, 0x01},    /* VMODEFDS  binning operation */
+	{IMX_8BIT, 0x304E, 0x0A},    /* VTPXCK_DIV */
+	{IMX_8BIT, 0x3050, 0x02},    /* OPSYCK_DIV */
+	{IMX_8BIT, 0x309B, 0x00},    /* RGDAFDSUMEN */
+	{IMX_8BIT, 0x30D5, 0x03},    /* HADDEN ( binning ) */
+	{IMX_8BIT, 0x3301, 0x01},    /* RGLANESEL */
+	{IMX_8BIT, 0x3318, 0x66},    /* MIPI Global Timing */
+	{IMX_8BIT, 0x0202, 0x01},    /* coarse integration time */
+	{IMX_8BIT, 0x0203, 0x90},    /* coarse integration time */
+	{IMX_8BIT, 0x0205, 0x00},    /* ana global gain */
+
+	{IMX_TOK_TERM, 0, 0},
+};
+
+static struct imx_reg imx208_192x160_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0305, 0x02},    /* PREPLLCK DIV */
+	{IMX_8BIT, 0x0307, 0x54},    /* PLL MPY */
+	{IMX_8BIT, 0x303C, 0x3C},    /* PLL oscillation stable wait time */
+	{IMX_8BIT, 0x30A4, 0x02},    /* Default */
+	{IMX_8BIT, 0x0112, 0x0A},    /* CCP_data_format : RAW 10bit */
+	{IMX_8BIT, 0x0113, 0x0A},    /* CCP_data_format :  RAW 10bit */
+	{IMX_8BIT, 0x0340, 0x04},    /* frame length line [15:8] */
+	{IMX_8BIT, 0x0341, 0xAA},    /* frame length line [7:0] */
+	{IMX_8BIT, 0x0342, 0x08},    /* line length pck [15:8] */
+	{IMX_8BIT, 0x0343, 0xC8},    /* line length pck [7:0] */
+	{IMX_8BIT, 0x0344, 0x02},    /* x_addr_start[12:8] */
+	{IMX_8BIT, 0x0345, 0x48},    /* x_addr_start[7:0] */
+	{IMX_8BIT, 0x0346, 0x00},    /* y_addr_start[12:8] */
+	{IMX_8BIT, 0x0347, 0xE4},    /* y_addr_start[7:0] */
+	{IMX_8BIT, 0x0348, 0x05},    /* x_addr_end [12:8] */
+	{IMX_8BIT, 0x0349, 0x47},    /* x_addr_end [7:0] */
+	{IMX_8BIT, 0x034A, 0x03},    /* y_addr_end [12:8] */
+	{IMX_8BIT, 0x034B, 0x63},    /* y_addr_end [7:0] */
+	{IMX_8BIT, 0x034C, 0x00},    /* x_output_size [ 12:8] */
+	{IMX_8BIT, 0x034D, 0xC0},    /* x_output_size [7:0] */
+	{IMX_8BIT, 0x034E, 0x00},    /* y_output_size [11:8] */
+	{IMX_8BIT, 0x034F, 0xA0},    /* y_output_size [7:0] */
+	{IMX_8BIT, 0x0381, 0x03},    /* x_even_inc */
+	{IMX_8BIT, 0x0383, 0x05},    /* x_odd_inc */
+	{IMX_8BIT, 0x0385, 0x03},    /* y_even_inc */
+	{IMX_8BIT, 0x0387, 0x05},    /* y_odd_inc */
+	{IMX_8BIT, 0x3048, 0x01},    /* VMODEFDS  binning operation */
+	{IMX_8BIT, 0x304E, 0x0A},    /* VTPXCK_DIV */
+	{IMX_8BIT, 0x3050, 0x02},    /* OPSYCK_DIV */
+	{IMX_8BIT, 0x309B, 0x00},    /* RGDAFDSUMEN */
+	{IMX_8BIT, 0x30D5, 0x03},    /* HADDEN ( binning ) */
+	{IMX_8BIT, 0x3301, 0x11},    /* RGLANESEL */
+	{IMX_8BIT, 0x3318, 0x74},    /* MIPI Global Timing */
+	{IMX_8BIT, 0x0202, 0x01},    /* coarse integration time */
+	{IMX_8BIT, 0x0203, 0x90},    /* coarse integration time */
+	{IMX_8BIT, 0x0205, 0x00},    /* ana global gain */
+
+	{IMX_TOK_TERM, 0, 0},
+};
+/********************** settings for imx - reference *********************/
+static struct imx_reg const imx208_init_settings[] = {
+	{ IMX_TOK_TERM, 0, 0}
+};
+
+struct imx_resolution imx208_res_preview[] = {
+	{
+		.desc = "imx208_1080p_30fps",
+		.regs = imx208_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x976_30fps",
+		.regs = imx208_1296x976_30fps,
+		.width = 1296,
+		.height = 976,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x736_30fps",
+		.regs = imx208_1296x736_30fps,
+		.width = 1296,
+		.height = 736,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_336x256_30fps",
+		.regs = imx208_336x256_30fps,
+		.width = 336,
+		.height = 256,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 201600,
+	},
+	{
+		.desc = "imx208_192x160_30fps",
+		.regs = imx208_192x160_30fps,
+		.width = 192,
+		.height = 160,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 100800,
+	},
+};
+
+struct imx_resolution imx208_res_still[] = {
+	{
+		.desc = "imx208_1080p_30fps",
+		.regs = imx208_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x976_30fps",
+		.regs = imx208_1296x976_30fps,
+		.width = 1296,
+		.height = 976,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x736_30fps",
+		.regs = imx208_1296x736_30fps,
+		.width = 1296,
+		.height = 736,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_336x256_30fps",
+		.regs = imx208_336x256_30fps,
+		.width = 336,
+		.height = 256,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 201600,
+	},
+	{
+		.desc = "imx208_192x160_30fps",
+		.regs = imx208_192x160_30fps,
+		.width = 192,
+		.height = 160,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 100800,
+	},
+};
+
+struct imx_resolution imx208_res_video[] = {
+	{
+		.desc = "imx208_1080p_30fps",
+		.regs = imx208_1080p_30fps,
+		.width = 1936,
+		.height = 1096,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x976_30fps",
+		.regs = imx208_1296x976_30fps,
+		.width = 1296,
+		.height = 976,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_1296x736_30fps",
+		.regs = imx208_1296x736_30fps,
+		.width = 1296,
+		.height = 736,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 403200,
+	},
+	{
+		.desc = "imx208_336x256_30fps",
+		.regs = imx208_336x256_30fps,
+		.width = 336,
+		.height = 256,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 201600,
+	},
+	{
+		.desc = "imx208_192x160_30fps",
+		.regs = imx208_192x160_30fps,
+		.width = 192,
+		.height = 160,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x08C8,
+				.lines_per_frame = 0x04AA,
+			},
+			{
+			}
+		},
+		.bin_factor_x = 4,
+		.bin_factor_y = 4,
+		.used = 0,
+		.skip_frames = 2,
+		.mipi_freq = 100800,
+	},
+};
+#endif
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx219.h b/drivers/staging/media/atomisp/i2c/imx/imx219.h
new file mode 100644
index 000000000000..52df582c56d8
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx219.h
@@ -0,0 +1,227 @@
+#ifndef __IMX219_H__
+#define __IMX219_H__
+#include "common.h"
+
+#define IMX219_FRAME_LENGTH_LINES		0x0160
+#define IMX219_LINE_LENGTH_PIXELS		0x0162
+#define IMX219_HORIZONTAL_START_H		0x0164
+#define IMX219_VERTICAL_START_H			0x0168
+#define IMX219_HORIZONTAL_END_H			0x0166
+#define IMX219_VERTICAL_END_H			0x016A
+#define IMX219_HORIZONTAL_OUTPUT_SIZE_H	0x016c
+#define IMX219_VERTICAL_OUTPUT_SIZE_H	0x016E
+#define IMX219_COARSE_INTEGRATION_TIME	0x015A
+#define IMX219_IMG_ORIENTATION			0x0172
+#define IMX219_GLOBAL_GAIN				0x0157
+#define IMX219_DGC_ADJ					0x0158
+
+#define IMX219_DGC_LEN					4
+
+/************************** settings for imx *************************/
+static struct imx_reg const imx219_STILL_8M_30fps[] = {
+	{IMX_8BIT, 0x30EB, 0x05}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x0C}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x300A, 0xFF}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x300B, 0xFF}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x05}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x09}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x0114, 0x03}, /*CSI_LANE_MODE[1:0}*/
+	{IMX_8BIT, 0x0128, 0x00}, /*DPHY_CNTRL*/
+	{IMX_8BIT, 0x012A, 0x13}, /*EXCK_FREQ[15:8]*/
+	{IMX_8BIT, 0x012B, 0x34}, /*EXCK_FREQ[7:0]*/
+	{IMX_8BIT, 0x0160, 0x0A}, /*FRM_LENGTH_A[15:8]*/
+	{IMX_8BIT, 0x0161, 0x94}, /*FRM_LENGTH_A[7:0]*/
+	{IMX_8BIT, 0x0162, 0x0D}, /*LINE_LENGTH_A[15:8]*/
+	{IMX_8BIT, 0x0163, 0x78}, /*LINE_LENGTH_A[7:0]*/
+	{IMX_8BIT, 0x0164, 0x00}, /*X_ADD_STA_A[11:8]*/
+	{IMX_8BIT, 0x0165, 0x00}, /*X_ADD_STA_A[7:0]*/
+	{IMX_8BIT, 0x0166, 0x0C}, /*X_ADD_END_A[11:8]*/
+	{IMX_8BIT, 0x0167, 0xCF}, /*X_ADD_END_A[7:0]*/
+	{IMX_8BIT, 0x0168, 0x00}, /*Y_ADD_STA_A[11:8]*/
+	{IMX_8BIT, 0x0169, 0x00}, /*Y_ADD_STA_A[7:0]*/
+	{IMX_8BIT, 0x016A, 0x09}, /*Y_ADD_END_A[11:8]*/
+	{IMX_8BIT, 0x016B, 0x9F}, /*Y_ADD_END_A[7:0]*/
+	{IMX_8BIT, 0x016C, 0x0C}, /*X_OUTPUT_SIZE_A[11:8]*/
+	{IMX_8BIT, 0x016D, 0xD0}, /*X_OUTPUT_SIZE_A[7:0]*/
+	{IMX_8BIT, 0x016E, 0x09}, /*Y_OUTPUT_SIZE_A[11:8]*/
+	{IMX_8BIT, 0x016F, 0xA0}, /*Y_OUTPUT_SIZE_A[7:0]*/
+	{IMX_8BIT, 0x0170, 0x01}, /*X_ODD_INC_A[2:0]*/
+	{IMX_8BIT, 0x0171, 0x01}, /*Y_ODD_INC_A[2:0]*/
+	{IMX_8BIT, 0x0174, 0x00}, /*BINNING_MODE_H_A*/
+	{IMX_8BIT, 0x0175, 0x00}, /*BINNING_MODE_V_A*/
+	{IMX_8BIT, 0x018C, 0x0A}, /*CSI_DATA_FORMAT_A[15:8]*/
+	{IMX_8BIT, 0x018D, 0x0A}, /*CSI_DATA_FORMAT_A[7:0]*/
+	{IMX_8BIT, 0x0301, 0x05}, /*VTPXCK_DIV*/
+	{IMX_8BIT, 0x0303, 0x01}, /*VTSYCK_DIV*/
+	{IMX_8BIT, 0x0304, 0x02}, /*PREPLLCK_VT_DIV[3:0]*/
+	{IMX_8BIT, 0x0305, 0x02}, /*PREPLLCK_OP_DIV[3:0]*/
+	{IMX_8BIT, 0x0306, 0x00}, /*PLL_VT_MPY[10:8]*/
+	{IMX_8BIT, 0x0307, 0x49}, /*PLL_VT_MPY[7:0]*/
+	{IMX_8BIT, 0x0309, 0x0A}, /*OPPXCK_DIV[4:0]*/
+	{IMX_8BIT, 0x030B, 0x01}, /*OPSYCK_DIV*/
+	{IMX_8BIT, 0x030C, 0x00}, /*PLL_OP_MPY[10:8]*/
+	{IMX_8BIT, 0x030D, 0x4C}, /*PLL_OP_MPY[7:0]*/
+	{IMX_8BIT, 0x4767, 0x0F}, /*CIS Tuning*/
+	{IMX_8BIT, 0x4750, 0x14}, /*CIS Tuning*/
+	{IMX_8BIT, 0x47B4, 0x14}, /*CIS Tuning*/
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx219_STILL_6M_30fps[] = {
+	{IMX_8BIT, 0x30EB, 0x05}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x0C}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x300A, 0xFF}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x300B, 0xFF}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x05}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x30EB, 0x09}, /*Access Code for address over 0x3000*/
+	{IMX_8BIT, 0x0114, 0x03}, /*CSI_LANE_MODE[1:0}*/
+	{IMX_8BIT, 0x0128, 0x00}, /*DPHY_CNTRL*/
+	{IMX_8BIT, 0x012A, 0x13}, /*EXCK_FREQ[15:8]*/
+	{IMX_8BIT, 0x012B, 0x34}, /*EXCK_FREQ[7:0]*/
+	{IMX_8BIT, 0x0160, 0x07}, /*FRM_LENGTH_A[15:8]*/
+	{IMX_8BIT, 0x0161, 0x64}, /*FRM_LENGTH_A[7:0]*/
+	{IMX_8BIT, 0x0162, 0x0D}, /*LINE_LENGTH_A[15:8]*/
+	{IMX_8BIT, 0x0163, 0x78}, /*LINE_LENGTH_A[7:0]*/
+	{IMX_8BIT, 0x0164, 0x00}, /*X_ADD_STA_A[11:8]*/
+	{IMX_8BIT, 0x0165, 0x00}, /*X_ADD_STA_A[7:0]*/
+	{IMX_8BIT, 0x0166, 0x0C}, /*X_ADD_END_A[11:8]*/
+	{IMX_8BIT, 0x0167, 0xCF}, /*X_ADD_END_A[7:0]*/
+	{IMX_8BIT, 0x0168, 0x01}, /*Y_ADD_STA_A[11:8]*/
+	{IMX_8BIT, 0x0169, 0x32}, /*Y_ADD_STA_A[7:0]*/
+	{IMX_8BIT, 0x016A, 0x08}, /*Y_ADD_END_A[11:8]*/
+	{IMX_8BIT, 0x016B, 0x6D}, /*Y_ADD_END_A[7:0]*/
+	{IMX_8BIT, 0x016C, 0x0C}, /*X_OUTPUT_SIZE_A[11:8]*/
+	{IMX_8BIT, 0x016D, 0xD0}, /*X_OUTPUT_SIZE_A[7:0]*/
+	{IMX_8BIT, 0x016E, 0x07}, /*Y_OUTPUT_SIZE_A[11:8]*/
+	{IMX_8BIT, 0x016F, 0x3C}, /*Y_OUTPUT_SIZE_A[7:0]*/
+	{IMX_8BIT, 0x0170, 0x01}, /*X_ODD_INC_A[2:0]*/
+	{IMX_8BIT, 0x0171, 0x01}, /*Y_ODD_INC_A[2:0]*/
+	{IMX_8BIT, 0x0174, 0x00}, /*BINNING_MODE_H_A*/
+	{IMX_8BIT, 0x0175, 0x00}, /*BINNING_MODE_V_A*/
+	{IMX_8BIT, 0x018C, 0x0A}, /*CSI_DATA_FORMAT_A[15:8]*/
+	{IMX_8BIT, 0x018D, 0x0A}, /*CSI_DATA_FORMAT_A[7:0]*/
+	{IMX_8BIT, 0x0301, 0x05}, /*VTPXCK_DIV*/
+	{IMX_8BIT, 0x0303, 0x01}, /*VTSYCK_DIV*/
+	{IMX_8BIT, 0x0304, 0x02}, /*PREPLLCK_VT_DIV[3:0]*/
+	{IMX_8BIT, 0x0305, 0x02}, /*PREPLLCK_OP_DIV[3:0]*/
+	{IMX_8BIT, 0x0306, 0x00}, /*PLL_VT_MPY[10:8]*/
+	{IMX_8BIT, 0x0307, 0x33}, /*PLL_VT_MPY[7:0]*/
+	{IMX_8BIT, 0x0309, 0x0A}, /*OPPXCK_DIV[4:0]*/
+	{IMX_8BIT, 0x030B, 0x01}, /*OPSYCK_DIV*/
+	{IMX_8BIT, 0x030C, 0x00}, /*PLL_OP_MPY[10:8]*/
+	{IMX_8BIT, 0x030D, 0x36}, /*PLL_OP_MPY[7:0]*/
+	{IMX_8BIT, 0x4767, 0x0F}, /*CIS Tuning*/
+	{IMX_8BIT, 0x4750, 0x14}, /*CIS Tuning*/
+	{IMX_8BIT, 0x47B4, 0x14}, /*CIS Tuning*/
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx219_init_settings[] = {
+	{IMX_TOK_TERM, 0, 0}
+};
+
+struct imx_resolution imx219_res_preview[] = {
+	{
+		.desc = "STILL_6M_30fps",
+		.regs = imx219_STILL_6M_30fps,
+		.width = 3280,
+		.height = 1852,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D78,
+				 .lines_per_frame = 0x0764,
+			},
+			{
+			}
+		},
+		.mipi_freq = 259000,
+	},
+	{
+		.desc = "STILL_8M_30fps",
+		.regs = imx219_STILL_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D78,
+				 .lines_per_frame = 0x0A94,
+			},
+			{
+			}
+		},
+		.mipi_freq = 365000,
+	},
+};
+
+struct imx_resolution imx219_res_still[] = {
+	{
+		.desc = "STILL_6M_30fps",
+		.regs = imx219_STILL_6M_30fps,
+		.width = 3280,
+		.height = 1852,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D78,
+				 .lines_per_frame = 0x0764,
+			},
+			{
+			}
+		},
+		.mipi_freq = 259000,
+	},
+	{
+		.desc = "STILL_8M_30fps",
+		.regs = imx219_STILL_8M_30fps,
+		.width = 3280,
+		.height = 2464,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D78,
+				 .lines_per_frame = 0x0A94,
+			},
+			{
+			}
+		},
+		.mipi_freq = 365000,
+	},
+};
+
+struct imx_resolution imx219_res_video[] = {
+	{
+		.desc = "STILL_6M_30fps",
+		.regs = imx219_STILL_6M_30fps,
+		.width = 3280,
+		.height = 1852,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.used = 0,
+		.fps_options = {
+			{
+				 .fps = 30,
+				 .pixels_per_line = 0x0D78,
+				 .lines_per_frame = 0x0764,
+			},
+			{
+			}
+		},
+		.mipi_freq = 259000,
+	},
+};
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/imx/imx227.h b/drivers/staging/media/atomisp/i2c/imx/imx227.h
new file mode 100644
index 000000000000..10e5b86f6687
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/imx227.h
@@ -0,0 +1,726 @@
+#ifndef __IMX227_H__
+#define __IMX227_H__
+
+#include "common.h"
+
+#define IMX227_EMBEDDED_DATA_LINE_NUM 2
+#define IMX227_OUTPUT_DATA_FORMAT_REG  0x0112
+#define IMX227_OUTPUT_FORMAT_RAW10  0x0a0a
+
+/* AE Bracketing Registers */
+#define IMX227_BRACKETING_LUT_MODE_BIT_CONTINUE_STREAMING	0x1
+#define IMX227_BRACKETING_LUT_MODE_BIT_LOOP_MODE	0x2
+
+#define IMX227_BRACKETING_LUT_CONTROL		0x0E00
+#define IMX227_BRACKETING_LUT_MODE		0x0E01
+#define IMX227_BRACKETING_LUT_ENTRY_CONTROL	0x0E02
+
+/*
+ * The imx135 embedded data info:
+ * embedded data line num: 2
+ * line 0 effective data size(byte): 76
+ * line 1 effective data size(byte): 113
+ */
+static const uint32_t
+imx227_embedded_effective_size[IMX227_EMBEDDED_DATA_LINE_NUM] = {160, 62};
+
+/************************** settings for imx *************************/
+/* Full Output Mode */
+static struct imx_reg const imx_STILL_6_5M_25fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x6259, 0x06},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xd0},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* 4:3 Output Mode */
+static struct imx_reg const imx_STILL_5_5M_3X4_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0xb0},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x00},
+	{IMX_8BIT, 0x0348, 0x08},
+	{IMX_8BIT, 0x0349, 0xaf},
+	{IMX_8BIT, 0x034a, 0x0a},
+	{IMX_8BIT, 0x034b, 0x9f},
+	{IMX_8BIT, 0x034c, 0x08},
+	{IMX_8BIT, 0x034d, 0x00},
+	{IMX_8BIT, 0x034e, 0x0a},
+	{IMX_8BIT, 0x034f, 0xa0},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xd8},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* Square Output Mode */
+static struct imx_reg const imx_STILL_5_7M_1X1_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0344, 0x00},
+	{IMX_8BIT, 0x0345, 0x00},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0xa0},
+	{IMX_8BIT, 0x0348, 0x09},
+	{IMX_8BIT, 0x0349, 0x5f},
+	{IMX_8BIT, 0x034a, 0x09},
+	{IMX_8BIT, 0x034b, 0xff},
+	{IMX_8BIT, 0x034c, 0x09},
+	{IMX_8BIT, 0x034d, 0x60},
+	{IMX_8BIT, 0x034e, 0x09},
+	{IMX_8BIT, 0x034f, 0x60},
+
+	{IMX_8BIT, 0x6259, 0x06},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xd4},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* Full Frame 1080P Mode (use ISP scaler)*/
+static struct imx_reg const imx_VIDEO_4M_9X16_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xdc},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* Cropped 1080P Mode */
+static struct imx_reg const imx_VIDEO_2M_9X16_45fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0112, 0x0a},
+	{IMX_8BIT, 0x0113, 0x0a},
+	{IMX_8BIT, 0x0344, 0x02},
+	{IMX_8BIT, 0x0345, 0x8a},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0x88},
+	{IMX_8BIT, 0x0348, 0x06},
+	{IMX_8BIT, 0x0349, 0xd1},
+	{IMX_8BIT, 0x034a, 0x09},
+	{IMX_8BIT, 0x034b, 0x17},
+	{IMX_8BIT, 0x034c, 0x04},
+	{IMX_8BIT, 0x034d, 0x48},
+	{IMX_8BIT, 0x034e, 0x07},
+	{IMX_8BIT, 0x034f, 0x90},
+
+	{IMX_8BIT, 0x0380, 0x00},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0382, 0x00},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0384, 0x00},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0386, 0x00},
+	{IMX_8BIT, 0x0387, 0x01},
+
+	{IMX_8BIT, 0x0408, 0x00},
+	{IMX_8BIT, 0x0409, 0x00},
+	{IMX_8BIT, 0x040a, 0x00},
+	{IMX_8BIT, 0x040b, 0x00},
+	{IMX_8BIT, 0x040c, 0x04},
+	{IMX_8BIT, 0x040d, 0x48},
+	{IMX_8BIT, 0x040e, 0x07},
+	{IMX_8BIT, 0x040f, 0x90},
+
+	{IMX_8BIT, 0x0900, 0x00},
+	{IMX_8BIT, 0x0901, 0x00},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xdc},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3c},  /*	preset_en	*/
+
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* Moment mode */
+static struct imx_reg const imx_VIDEO_1_3M_3X4_60fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xd9},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* High Speed 3:4 mode */
+static struct imx_reg const imx_VIDEO_VGA_3X4_120fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x9004, 0xca},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3f},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+
+/* Binned 720P mode */
+static struct imx_reg const imx_VIDEO_1M_9X16_60fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0112, 0x0a},
+	{IMX_8BIT, 0x0113, 0x0a},
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0xd0},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x40},
+	{IMX_8BIT, 0x0348, 0x07},
+	{IMX_8BIT, 0x0349, 0x8f},
+	{IMX_8BIT, 0x034a, 0x0a},
+	{IMX_8BIT, 0x034b, 0x5f},
+	{IMX_8BIT, 0x034c, 0x02},
+	{IMX_8BIT, 0x034d, 0xe0},
+	{IMX_8BIT, 0x034e, 0x05},
+	{IMX_8BIT, 0x034f, 0x10},
+
+	{IMX_8BIT, 0x0380, 0x00},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0382, 0x00},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0384, 0x00},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0386, 0x00},
+	{IMX_8BIT, 0x0387, 0x01},
+
+	{IMX_8BIT, 0x0408, 0x00},
+	{IMX_8BIT, 0x0409, 0x00},
+	{IMX_8BIT, 0x040a, 0x00},
+	{IMX_8BIT, 0x040b, 0x00},
+	{IMX_8BIT, 0x040c, 0x02},
+	{IMX_8BIT, 0x040d, 0xe0},
+	{IMX_8BIT, 0x040e, 0x05},
+	{IMX_8BIT, 0x040f, 0x10},
+
+	{IMX_8BIT, 0x0900, 0x01},
+	{IMX_8BIT, 0x0901, 0x22},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xdd},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3c},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* Binned 496x868 mode */
+static struct imx_reg const imx_VIDEO_496x868_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0112, 0x0a},
+	{IMX_8BIT, 0x0113, 0x0a},
+	{IMX_8BIT, 0x0344, 0x02},
+	{IMX_8BIT, 0x0345, 0xc0},
+	{IMX_8BIT, 0x0346, 0x01},
+	{IMX_8BIT, 0x0347, 0xec},
+	{IMX_8BIT, 0x0348, 0x06},
+	{IMX_8BIT, 0x0349, 0x9f},
+	{IMX_8BIT, 0x034a, 0x08},
+	{IMX_8BIT, 0x034b, 0xb3},
+	{IMX_8BIT, 0x034c, 0x01},
+	{IMX_8BIT, 0x034d, 0xf0},
+	{IMX_8BIT, 0x034e, 0x03},
+	{IMX_8BIT, 0x034f, 0x64},
+
+	{IMX_8BIT, 0x0380, 0x00},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0382, 0x00},
+	{IMX_8BIT, 0x0383, 0x01},
+	{IMX_8BIT, 0x0384, 0x00},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0386, 0x00},
+	{IMX_8BIT, 0x0387, 0x01},
+
+	{IMX_8BIT, 0x0408, 0x00},
+	{IMX_8BIT, 0x0409, 0x00},
+	{IMX_8BIT, 0x040a, 0x00},
+	{IMX_8BIT, 0x040b, 0x00},
+	{IMX_8BIT, 0x040c, 0x01},
+	{IMX_8BIT, 0x040d, 0xf0},
+	{IMX_8BIT, 0x040e, 0x03},
+	{IMX_8BIT, 0x040f, 0x64},
+
+	{IMX_8BIT, 0x0900, 0x01},
+	{IMX_8BIT, 0x0901, 0x22},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xdd},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3c},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+
+/* Hangout mode */
+static struct imx_reg const imx_PREVIEW_374X652_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0112, 0x0a},
+	{IMX_8BIT, 0x0113, 0x0a},
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0xc0},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x30},
+	{IMX_8BIT, 0x0348, 0x07},
+	{IMX_8BIT, 0x0349, 0x9f},
+	{IMX_8BIT, 0x034a, 0x0a},
+	{IMX_8BIT, 0x034b, 0x6f},
+	{IMX_8BIT, 0x034c, 0x01},
+	{IMX_8BIT, 0x034d, 0x78},
+	{IMX_8BIT, 0x034e, 0x02},
+	{IMX_8BIT, 0x034f, 0x90},
+
+	{IMX_8BIT, 0x0380, 0x00},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0382, 0x00},
+	{IMX_8BIT, 0x0383, 0x03},
+	{IMX_8BIT, 0x0384, 0x00},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0386, 0x00},
+	{IMX_8BIT, 0x0387, 0x03},
+
+	{IMX_8BIT, 0x0408, 0x00},
+	{IMX_8BIT, 0x0409, 0x00},
+	{IMX_8BIT, 0x040a, 0x00},
+	{IMX_8BIT, 0x040b, 0x02},
+	{IMX_8BIT, 0x040c, 0x01},
+	{IMX_8BIT, 0x040d, 0x76},
+	{IMX_8BIT, 0x040e, 0x02},
+	{IMX_8BIT, 0x040f, 0x8c},
+
+	{IMX_8BIT, 0x0900, 0x01},
+	{IMX_8BIT, 0x0901, 0x22},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xde},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3c},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+static struct imx_reg const imx_VIDEO_NHD_9X16_30fps[] = {
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0112, 0x0a},
+	{IMX_8BIT, 0x0113, 0x0a},
+	{IMX_8BIT, 0x0344, 0x01},
+	{IMX_8BIT, 0x0345, 0xc0},
+	{IMX_8BIT, 0x0346, 0x00},
+	{IMX_8BIT, 0x0347, 0x30},
+	{IMX_8BIT, 0x0348, 0x07},
+	{IMX_8BIT, 0x0349, 0x9f},
+	{IMX_8BIT, 0x034a, 0x0a},
+	{IMX_8BIT, 0x034b, 0x6f},
+	{IMX_8BIT, 0x034c, 0x01},
+	{IMX_8BIT, 0x034d, 0x78},
+	{IMX_8BIT, 0x034e, 0x02},
+	{IMX_8BIT, 0x034f, 0x90},
+
+	{IMX_8BIT, 0x0380, 0x00},
+	{IMX_8BIT, 0x0381, 0x01},
+	{IMX_8BIT, 0x0382, 0x00},
+	{IMX_8BIT, 0x0383, 0x03},
+	{IMX_8BIT, 0x0384, 0x00},
+	{IMX_8BIT, 0x0385, 0x01},
+	{IMX_8BIT, 0x0386, 0x00},
+	{IMX_8BIT, 0x0387, 0x03},
+
+	{IMX_8BIT, 0x0408, 0x00},
+	{IMX_8BIT, 0x0409, 0x00},
+	{IMX_8BIT, 0x040a, 0x00},
+	{IMX_8BIT, 0x040b, 0x00},
+	{IMX_8BIT, 0x040c, 0x01},
+	{IMX_8BIT, 0x040d, 0x78},
+	{IMX_8BIT, 0x040e, 0x02},
+	{IMX_8BIT, 0x040f, 0x90},
+
+	{IMX_8BIT, 0x0900, 0x01},
+	{IMX_8BIT, 0x0901, 0x22},
+
+	{IMX_8BIT, 0x6259, 0x05},  /*	latency ctrl	*/
+	{IMX_8BIT, 0x9004, 0xde},  /*	preset_sel	*/
+	{IMX_8BIT, 0x9005, 0x3c},  /*	preset_en	*/
+	{IMX_8BIT, 0x0136, 0x13},
+	{IMX_8BIT, 0x0137, 0x33},
+	{IMX_TOK_TERM, 0, 0}
+};
+
+
+static struct imx_reg const imx227_init_settings[] = {
+	{IMX_8BIT, 0x0100, 0x00},  /*   mode_select     */
+	GROUPED_PARAMETER_HOLD_ENABLE,
+	{IMX_8BIT, 0x0306, 0x00},
+	{IMX_8BIT, 0x0307, 0xBB},
+	{IMX_8BIT, 0x030E, 0x03},
+	{IMX_8BIT, 0x030F, 0x0D},
+	{IMX_8BIT, 0x463b, 0x30},
+	{IMX_8BIT, 0x463e, 0x05},
+	{IMX_8BIT, 0x4612, 0x66},
+	{IMX_8BIT, 0x4815, 0x65},
+	{IMX_8BIT, 0x4991, 0x00},
+	{IMX_8BIT, 0x4992, 0x01},
+	{IMX_8BIT, 0x4993, 0xff},
+	{IMX_8BIT, 0x458b, 0x00},
+	{IMX_8BIT, 0x452a, 0x02},
+	{IMX_8BIT, 0x4a7c, 0x00},
+	{IMX_8BIT, 0x4a7d, 0x1c},
+	{IMX_8BIT, 0x4a7e, 0x00},
+	{IMX_8BIT, 0x4a7f, 0x17},
+	{IMX_8BIT, 0x462C, 0x2E},
+	{IMX_8BIT, 0x461B, 0x28},
+	{IMX_8BIT, 0x4663, 0x29},
+	{IMX_8BIT, 0x461A, 0x7C},
+	{IMX_8BIT, 0x4619, 0x28},
+	{IMX_8BIT, 0x4667, 0x22},
+	{IMX_8BIT, 0x466B, 0x23},
+	{IMX_8BIT, 0x40AD, 0xFF},
+	{IMX_8BIT, 0x40BE, 0x00},
+	{IMX_8BIT, 0x40BF, 0x6E},
+	{IMX_8BIT, 0x40CE, 0x00},
+	{IMX_8BIT, 0x40CF, 0x0A},
+	{IMX_8BIT, 0x40CA, 0x00},
+	{IMX_8BIT, 0x40CB, 0x1F},
+	{IMX_8BIT, 0x4D16, 0x00},
+	{IMX_8BIT, 0x6204, 0x01},
+	{IMX_8BIT, 0x6209, 0x00},
+	{IMX_8BIT, 0x621F, 0x01},
+	{IMX_8BIT, 0x621E, 0x10},
+	GROUPED_PARAMETER_HOLD_DISABLE,
+	{IMX_TOK_TERM, 0, 0}
+};
+
+/* TODO settings of preview/still/video will be updated with new use case */
+struct imx_resolution imx227_res_preview[] = {
+	{
+		.desc = "imx_PREVIEW_374X652_30fps",
+		.regs = imx_PREVIEW_374X652_30fps,
+		.width = 374,
+		.height = 652,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C0A,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_VIDEO_496x868_30fps",
+		.regs = imx_VIDEO_496x868_30fps,
+		.width = 496,
+		.height = 868,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C08,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_STILL_5_5M_3X4_30fps",
+		.regs = imx_STILL_5_5M_3X4_30fps,
+		.width = 2048,
+		.height = 2720,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0ED8,
+				.lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_STILL_5_7M_1X1_30fps",
+		.regs = imx_STILL_5_7M_1X1_30fps,
+		.width = 2400,
+		.height = 2400,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x1130,
+				.lines_per_frame = 0x0A1E,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_STILL_6_5M_25fps",
+		.regs = imx_STILL_6_5M_25fps,
+		.width = 2400,
+		.height = 2720,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 25,
+				.pixels_per_line = 0x1130,
+				.lines_per_frame = 0x0C24,
+			},
+			{
+			}
+		},
+	}
+};
+
+struct imx_resolution imx227_res_still[] = {
+	{
+		.desc = "imx_STILL_5_5M_3X4_30fps",
+		.regs = imx_STILL_5_5M_3X4_30fps,
+		.width = 2048,
+		.height = 2720,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 6,
+				.pixels_per_line = 0x2130,
+				.lines_per_frame = 0x1A22,
+			},
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0ED8,
+				.lines_per_frame = 0x0BB8,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_STILL_5_7M_1X1_30fps",
+		.regs = imx_STILL_5_7M_1X1_30fps,
+		.width = 2400,
+		.height = 2400,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 6,
+				.pixels_per_line = 0x266E,
+				.lines_per_frame = 0x1704,
+			},
+			{
+				.fps = 30,
+				.pixels_per_line = 0x1130,
+				.lines_per_frame = 0x0A1E,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_STILL_6_5M_25fps",
+		.regs = imx_STILL_6_5M_25fps,
+		.width = 2400,
+		.height = 2720,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 25,
+				.pixels_per_line = 0x1130,
+				.lines_per_frame = 0x0C24,
+			},
+			{
+			}
+		},
+	},
+};
+
+struct imx_resolution imx227_res_video[] = {
+	{
+		.desc = "imx_VIDEO_4M_9X16_30fps",
+		.regs = imx_VIDEO_4M_9X16_30fps,
+		.width = 1536,
+		.height = 2720,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C08,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_VIDEO_2M_9X16_45fps",
+		.regs = imx_VIDEO_2M_9X16_45fps,
+		.width = 1096,
+		.height = 1936,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C08,
+			},
+			{
+				.fps = 45,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0800,
+			},
+			{
+			}
+		},
+
+	},
+	{
+		.desc = "imx_VIDEO_1_3M_3X4_60fps",
+		.regs = imx_VIDEO_1_3M_3X4_60fps,
+		.width = 1024,
+		.height = 1360,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 60,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0604,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_VIDEO_496x868_30fps",
+		.regs = imx_VIDEO_496x868_30fps,
+		.width = 496,
+		.height = 868,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C08,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_VIDEO_1M_9X16_60fps",
+		.regs = imx_VIDEO_1M_9X16_60fps,
+		.width = 736,
+		.height = 1296,
+		.bin_factor_x = 1,
+		.bin_factor_y = 1,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 60,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0604,
+			},
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C10,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_VIDEO_VGA_3X4_120fps",
+		.regs = imx_VIDEO_VGA_3X4_120fps,
+		.width = 512,
+		.height = 680,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 120,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0302,
+			},
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C08,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "imx_VIDEO_NHD_9X16_30fps",
+		.regs = imx_VIDEO_NHD_9X16_30fps,
+		.width = 376,
+		.height = 656,
+		.bin_factor_x = 2,
+		.bin_factor_y = 2,
+		.mipi_freq = 499000,
+		.used = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 0x0E70,
+				.lines_per_frame = 0x0C0A,
+			},
+			{
+			}
+		},
+	},
+};
+
+#endif /* __IMX227_H__ */
diff --git a/drivers/staging/media/atomisp/i2c/imx/otp.c b/drivers/staging/media/atomisp/i2c/imx/otp.c
new file mode 100644
index 000000000000..462275038046
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/otp.c
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+void *dummy_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 *buf;
+
+	buf = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	return buf;
+}
diff --git a/drivers/staging/media/atomisp/i2c/imx/otp_brcc064_e2prom.c b/drivers/staging/media/atomisp/i2c/imx/otp_brcc064_e2prom.c
new file mode 100644
index 000000000000..b11f90c5960c
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/otp_brcc064_e2prom.c
@@ -0,0 +1,80 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include "common.h"
+
+/*
+ * Read EEPROM data from brcc064 and store
+ * it into a kmalloced buffer. On error return NULL.
+ * @size: set to the size of the returned EEPROM data.
+ */
+void *brcc064_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int e2prom_i2c_addr = dev_addr >> 1;
+	static const unsigned int max_read_size = 30;
+	int addr;
+	u32 s_addr = start_addr & E2PROM_ADDR_MASK;
+	unsigned char *buffer;
+
+	buffer = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+	if (!buffer)
+		return NULL;
+
+	for (addr = s_addr; addr < size; addr += max_read_size) {
+		struct i2c_msg msg[2];
+		unsigned int i2c_addr = e2prom_i2c_addr;
+		u16 addr_buf;
+		int r;
+
+		msg[0].flags = 0;
+		msg[0].addr = i2c_addr;
+		addr_buf = cpu_to_be16(addr & 0xFFFF);
+		msg[0].len = 2;
+		msg[0].buf = (u8 *)&addr_buf;
+
+		msg[1].addr = i2c_addr;
+		msg[1].flags = I2C_M_RD;
+		msg[1].len = min(max_read_size, size - addr);
+		msg[1].buf = &buffer[addr];
+
+		r = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+		if (r != ARRAY_SIZE(msg)) {
+			dev_err(&client->dev, "read failed at 0x%03x\n", addr);
+			return NULL;
+		}
+	}
+	return buffer;
+
+}
+
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/otp_e2prom.c b/drivers/staging/media/atomisp/i2c/imx/otp_e2prom.c
new file mode 100644
index 000000000000..73d041f97811
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/otp_e2prom.c
@@ -0,0 +1,89 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include "common.h"
+
+/*
+ * Read EEPROM data from the gerneral e2prom chip(eg.
+ * CAT24C08, CAT24C128, le24l042cs, and store
+ * it into a kmalloced buffer. On error return NULL.
+ * @size: set to the size of the returned EEPROM data.
+ */
+void *e2prom_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int e2prom_i2c_addr = dev_addr >> 1;
+	static const unsigned int max_read_size = 30;
+	int addr;
+	u32 s_addr = start_addr & E2PROM_ADDR_MASK;
+	bool two_addr = (start_addr & E2PROM_2ADDR) >> 31;
+	char *buffer;
+
+	buffer = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+	if (!buffer)
+		return NULL;
+
+	for (addr = s_addr; addr < size; addr += max_read_size) {
+		struct i2c_msg msg[2];
+		unsigned int i2c_addr = e2prom_i2c_addr;
+		u16 addr_buf;
+		int r;
+
+		msg[0].flags = 0;
+		if (two_addr) {
+			msg[0].addr = i2c_addr;
+			addr_buf = cpu_to_be16(addr & 0xFFFF);
+			msg[0].len = 2;
+			msg[0].buf = (u8 *)&addr_buf;
+		} else {
+			i2c_addr |= (addr >> 8) & 0x7;
+			msg[0].addr = i2c_addr;
+			addr_buf = addr & 0xFF;
+			msg[0].len = 1;
+			msg[0].buf = (u8 *)&addr_buf;
+		}
+
+		msg[1].addr = i2c_addr;
+		msg[1].flags = I2C_M_RD;
+		msg[1].len = min(max_read_size, size - addr);
+		msg[1].buf = &buffer[addr];
+
+		r = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+		if (r != ARRAY_SIZE(msg)) {
+			dev_err(&client->dev, "read failed at 0x%03x\n", addr);
+			return NULL;
+		}
+	}
+	return buffer;
+}
+
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/otp_imx.c b/drivers/staging/media/atomisp/i2c/imx/otp_imx.c
new file mode 100644
index 000000000000..1ca27c26ef75
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/otp_imx.c
@@ -0,0 +1,191 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include <asm/intel-mid.h>
+#include "common.h"
+
+/* Defines for OTP Data Registers */
+#define IMX_OTP_START_ADDR		0x3B04
+#define IMX_OTP_PAGE_SIZE		64
+#define IMX_OTP_READY_REG		0x3B01
+#define IMX_OTP_PAGE_REG		0x3B02
+#define IMX_OTP_MODE_REG		0x3B00
+#define IMX_OTP_PAGE_MAX		20
+#define IMX_OTP_READY_REG_DONE		1
+#define IMX_OTP_READ_ONETIME		32
+#define IMX_OTP_MODE_READ		1
+#define IMX227_OTP_START_ADDR           0x0A04
+#define IMX227_OTP_ENABLE_REG           0x0A00
+#define IMX227_OTP_READY_REG            0x0A01
+#define IMX227_OTP_PAGE_REG             0x0A02
+#define IMX227_OTP_READY_REG_DONE       1
+#define IMX227_OTP_MODE_READ            1
+
+static int
+imx_read_otp_data(struct i2c_client *client, u16 len, u16 reg, void *val)
+{
+	struct i2c_msg msg[2];
+	u16 data[IMX_SHORT_MAX] = { 0 };
+	int err;
+
+	if (len > IMX_BYTE_MAX) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(msg, 0 , sizeof(msg));
+	memset(data, 0 , sizeof(data));
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = I2C_MSG_LENGTH;
+	msg[0].buf = (u8 *)data;
+	/* high byte goes first */
+	data[0] = cpu_to_be16(reg);
+
+	msg[1].addr = client->addr;
+	msg[1].len = len;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = (u8 *)data;
+
+	err = i2c_transfer(client->adapter, msg, 2);
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		goto error;
+	}
+
+	memcpy(val, data, len);
+	return 0;
+
+error:
+	dev_err(&client->dev, "read from offset 0x%x error %d", reg, err);
+	return err;
+}
+
+static int imx_read_otp_reg_array(struct i2c_client *client, u16 size, u16 addr,
+				  u8 *buf)
+{
+	u16 index;
+	int ret;
+
+	for (index = 0; index + IMX_OTP_READ_ONETIME <= size;
+					index += IMX_OTP_READ_ONETIME) {
+		ret = imx_read_otp_data(client, IMX_OTP_READ_ONETIME,
+					addr + index, &buf[index]);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+void *imx_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 *buf;
+	int ret;
+	int i;
+
+	buf = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < IMX_OTP_PAGE_MAX; i++) {
+
+		/*set page NO.*/
+		ret = imx_write_reg(client, IMX_8BIT,
+			       IMX_OTP_PAGE_REG, i & 0xff);
+		if (ret)
+			goto fail;
+
+		/*set read mode*/
+		ret = imx_write_reg(client, IMX_8BIT,
+			       IMX_OTP_MODE_REG, IMX_OTP_MODE_READ);
+		if (ret)
+			goto fail;
+
+		/* Reading the OTP data array */
+		ret = imx_read_otp_reg_array(client, IMX_OTP_PAGE_SIZE,
+			IMX_OTP_START_ADDR, buf + i * IMX_OTP_PAGE_SIZE);
+		if (ret)
+			goto fail;
+	}
+
+	return buf;
+fail:
+	/* Driver has failed to find valid data */
+	dev_err(&client->dev, "sensor found no valid OTP data\n");
+	return ERR_PTR(ret);
+}
+
+void *imx227_otp_read(struct v4l2_subdev *sd, u8 dev_addr,
+	u32 start_addr, u32 size)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u8 *buf;
+	int ret;
+	int i;
+
+	buf = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+	if (!buf)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < IMX_OTP_PAGE_MAX; i++) {
+
+		/*set page NO.*/
+		ret = imx_write_reg(client, IMX_8BIT,
+			       IMX227_OTP_PAGE_REG, i & 0xff);
+		if (ret)
+			goto fail;
+
+		/*set read mode*/
+		ret = imx_write_reg(client, IMX_8BIT,
+			       IMX227_OTP_ENABLE_REG, IMX227_OTP_MODE_READ);
+		if (ret)
+			goto fail;
+
+		/* Reading the OTP data array */
+		ret = imx_read_otp_reg_array(client, IMX_OTP_PAGE_SIZE,
+			IMX227_OTP_START_ADDR, buf + i * IMX_OTP_PAGE_SIZE);
+		if (ret)
+			goto fail;
+	}
+
+	return buf;
+fail:
+	/* Driver has failed to find valid data */
+	dev_err(&client->dev, "sensor found no valid OTP data\n");
+	return ERR_PTR(ret);
+}
+
diff --git a/drivers/staging/media/atomisp/i2c/imx/vcm.c b/drivers/staging/media/atomisp/i2c/imx/vcm.c
new file mode 100644
index 000000000000..2d2df04c800a
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/imx/vcm.c
@@ -0,0 +1,45 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include "../../include/linux/atomisp_platform.h"
+
+int vcm_power_up(struct v4l2_subdev *sd)
+{
+	const struct camera_af_platform_data *vcm_platform_data;
+
+	vcm_platform_data = camera_get_af_platform_data();
+	if (NULL == vcm_platform_data)
+		return -ENODEV;
+	/* Enable power */
+	return vcm_platform_data->power_ctrl(sd, 1);
+}
+
+int vcm_power_down(struct v4l2_subdev *sd)
+{
+	const struct camera_af_platform_data *vcm_platform_data;
+
+	vcm_platform_data = camera_get_af_platform_data();
+	if (NULL == vcm_platform_data)
+		return -ENODEV;
+	return vcm_platform_data->power_ctrl(sd, 0);
+}
+
diff --git a/drivers/staging/media/atomisp/i2c/libmsrlisthelper.c b/drivers/staging/media/atomisp/i2c/libmsrlisthelper.c
new file mode 100644
index 000000000000..decb65cfd7c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/libmsrlisthelper.c
@@ -0,0 +1,209 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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 {
+	uint32_t tag;          /*!< Tag identifier, also checks endianness */
+	uint32_t size;         /*!< Container size including this header */
+	uint32_t version;      /*!< Version, format 0xYYMMDDVV */
+	uint32_t revision;     /*!< Revision, format 0xYYMMDDVV */
+	uint32_t config_bits;  /*!< Configuration flag bits set */
+	uint32_t checksum;     /*!< Global checksum, header included */
+} __packed;
+
+struct tbd_record_header {
+	uint32_t size;        /*!< Size of record including header */
+	uint8_t format_id;    /*!< tbd_format_t enumeration values used */
+	uint8_t packing_key;  /*!< Packing method; 0 = no packing */
+	uint16_t class_id;    /*!< tbd_class_t enumeration values used */
+} __packed;
+
+struct tbd_data_record_header {
+	uint16_t next_offset;
+	uint16_t flags;
+	uint16_t data_offset;
+	uint16_t 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. */
+
+	uint8_t *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)
+{
+	uint8_t *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/lm3554.c b/drivers/staging/media/atomisp/i2c/lm3554.c
new file mode 100644
index 000000000000..dd9c9c3ffff7
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/lm3554.c
@@ -0,0 +1,1009 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/gpio.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 <linux/gpio/consumer.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		(1 << 0)
+#define LM3554_FLAG_THERMAL_SHUTDOWN	(1 << 1)
+#define LM3554_FLAG_LED_FAULT		(1 << 2)
+#define LM3554_FLAG_TX1_INTERRUPT	(1 << 3)
+#define LM3554_FLAG_TX2_INTERRUPT	(1 << 4)
+#define LM3554_FLAG_LED_THERMAL_FAULT	(1 << 5)
+#define LM3554_FLAG_UNUSED		(1 << 6)
+#define LM3554_FLAG_INPUT_VOLTAGE_LOW	(1 << 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(long unsigned int arg)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata((struct i2c_client *)arg);
+	struct lm3554 *flash = to_lm3554(sd);
+	struct lm3554_platform_data *pdata = flash->pdata;
+
+	gpio_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)
+		gpio_set_value(pdata->gpio_strobe, 0);
+
+	/* Restore flash current settings */
+	ret = lm3554_set_flash(flash);
+	if (ret < 0)
+		goto err;
+
+	/* Strobe on Flash */
+	gpio_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;
+}
+
+#ifndef CSS15
+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;
+}
+#endif
+
+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;
+#ifndef CSS15
+	case V4L2_CID_FLASH_STATUS_REGISTER:
+		ret = lm3554_g_flash_status_register(&dev->sd, &ctrl->val);
+		break;
+#endif
+	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
+};
+
+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_BOOLEAN,
+	 .name = "Flash Status",
+	 .min = 0,
+	 .max = 100,
+	 .step = 1,
+	 .def = ATOMISP_FLASH_STATUS_OK,
+	 .flags = 0,
+	 },
+#ifndef CSS15
+	{
+	 .ops = &ctrl_ops,
+	 .id = V4L2_CID_FLASH_STATUS_REGISTER,
+	 .type = V4L2_CTRL_TYPE_BOOLEAN,
+	 .name = "Flash Status Register",
+	 .min = 0,
+	 .max = 100,
+	 .step = 1,
+	 .def = 0,
+	 .flags = 0,
+	 },
+#endif
+};
+
+/* -----------------------------------------------------------------------------
+ * 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*/
+	gpio_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 (!gpio_is_valid(pdata->gpio_reset))
+		return -EINVAL;
+
+	ret = gpio_direction_output(pdata->gpio_reset, 0);
+	if (ret < 0)
+		goto err_gpio_reset;
+	dev_info(&client->dev, "flash led reset successfully\n");
+
+	if (!gpio_is_valid(pdata->gpio_strobe)) {
+		ret = -EINVAL;
+		goto err_gpio_dir_reset;
+	}
+
+	ret = gpio_direction_output(pdata->gpio_strobe, 0);
+	if (ret < 0)
+		goto err_gpio_strobe;
+
+	return 0;
+
+err_gpio_strobe:
+	gpio_free(pdata->gpio_strobe);
+err_gpio_dir_reset:
+	gpio_direction_output(pdata->gpio_reset, 0);
+err_gpio_reset:
+	gpio_free(pdata->gpio_reset);
+
+	return ret;
+}
+
+static int 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 = gpio_direction_output(pdata->gpio_strobe, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = gpio_direction_output(pdata->gpio_reset, 0);
+	if (ret < 0)
+		return ret;
+
+	gpio_free(pdata->gpio_strobe);
+	gpio_free(pdata->gpio_reset);
+	return 0;
+}
+
+void *lm3554_platform_data_func(struct i2c_client *client)
+{
+	static struct lm3554_platform_data platform_data;
+
+	if (ACPI_COMPANION(&client->dev)) {
+		platform_data.gpio_reset =
+		    desc_to_gpio(gpiod_get_index(&(client->dev),
+						 NULL, 2, GPIOD_OUT_LOW));
+		platform_data.gpio_strobe =
+		    desc_to_gpio(gpiod_get_index(&(client->dev),
+						 NULL, 0, GPIOD_OUT_LOW));
+		platform_data.gpio_torch =
+		    desc_to_gpio(gpiod_get_index(&(client->dev),
+						 NULL, 1, GPIOD_OUT_LOW));
+	} else {
+		platform_data.gpio_reset = -1;
+		platform_data.gpio_strobe = -1;
+		platform_data.gpio_torch = -1;
+	}
+
+	dev_info(&client->dev, "camera pdata: lm3554: reset: %d strobe %d torch %d\n",
+		platform_data.gpio_reset, platform_data.gpio_strobe,
+		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,
+				  const struct i2c_device_id *id)
+{
+	int err = 0;
+	struct lm3554 *flash;
+	unsigned int i;
+	int ret;
+
+	flash = kzalloc(sizeof(*flash), GFP_KERNEL);
+	if (!flash) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	flash->pdata = client->dev.platform_data;
+
+	if (!flash->pdata || ACPI_COMPANION(&client->dev))
+		flash->pdata = lm3554_platform_data_func(client);
+
+	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;
+	ret =
+	    v4l2_ctrl_handler_init(&flash->ctrl_handler,
+				   ARRAY_SIZE(lm3554_controls));
+	if (ret) {
+		dev_err(&client->dev, "error initialize a ctrl_handler.\n");
+		goto fail2;
+	}
+
+	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");
+		goto fail2;
+	}
+
+	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 fail1;
+	}
+
+	flash->sd.entity.function = MEDIA_ENT_F_FLASH;
+
+	mutex_init(&flash->power_lock);
+
+	setup_timer(&flash->flash_off_delay, lm3554_flash_off_delay,
+		    (unsigned long)client);
+
+	err = lm3554_gpio_init(client);
+	if (err) {
+		dev_err(&client->dev, "gpio request/direction_output fail");
+		goto fail2;
+	}
+	if (ACPI_HANDLE(&client->dev))
+		err = atomisp_register_i2c_module(&flash->sd, NULL, LED_FLASH);
+	return 0;
+fail2:
+	media_entity_cleanup(&flash->sd.entity);
+	v4l2_ctrl_handler_free(&flash->ctrl_handler);
+fail1:
+	v4l2_device_unregister_subdev(&flash->sd);
+	kfree(flash);
+
+	return err;
+}
+
+static int lm3554_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lm3554 *flash = to_lm3554(sd);
+	int ret;
+
+	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);
+
+	ret = lm3554_gpio_uninit(client);
+	if (ret < 0)
+		goto fail;
+
+	kfree(flash);
+
+	return 0;
+fail:
+	dev_err(&client->dev, "gpio request/direction_output fail");
+	return ret;
+}
+
+static const struct i2c_device_id lm3554_id[] = {
+	{LM3554_NAME, 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, lm3554_id);
+
+static const struct dev_pm_ops lm3554_pm_ops = {
+	.suspend = lm3554_suspend,
+	.resume = lm3554_resume,
+};
+
+static struct acpi_device_id lm3554_acpi_match[] = {
+	{ "INTCF1C" },
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, lm3554_acpi_match);
+
+static struct i2c_driver lm3554_driver = {
+	.driver = {
+		.name = LM3554_NAME,
+		.pm   = &lm3554_pm_ops,
+		.acpi_match_table = ACPI_PTR(lm3554_acpi_match),
+	},
+	.probe = lm3554_probe,
+	.remove = lm3554_remove,
+	.id_table = lm3554_id,
+};
+
+static __init int init_lm3554(void)
+{
+	return i2c_add_driver(&lm3554_driver);
+}
+
+static __exit void exit_lm3554(void)
+{
+	i2c_del_driver(&lm3554_driver);
+}
+
+module_init(init_lm3554);
+module_exit(exit_lm3554);
+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/mt9m114.c b/drivers/staging/media/atomisp/i2c/mt9m114.c
new file mode 100644
index 000000000000..ced175c268d1
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/mt9m114.c
@@ -0,0 +1,1963 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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/gpio.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};
+	u16 *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 = (u16 *)data;
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == MISENSOR_8BIT) {
+		data[2] = (u8)(val);
+	} else if (data_length == MISENSOR_16BIT) {
+		u16 *wdata = (u16 *)&data[2];
+		*wdata = be16_to_cpu((u16)val);
+	} else {
+		/* MISENSOR_32BIT */
+		u32 *wdata = (u32 *)&data[2];
+		*wdata = be32_to_cpu(val);
+	}
+
+	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, "misensor_rmw_reg error exit, read failed\n");
+		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, "misensor_rmw_reg error exit, write failed\n");
+		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;
+
+	if (ctrl->index == 0)
+		return 0;
+
+again:
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = 2 + ctrl->index;
+	ctrl->buffer.addr = 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)
+{
+	u16 *data16;
+	u32 *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 = (u16 *)&ctrl->buffer.data[ctrl->index];
+		*data16 = cpu_to_be16((u16)next->val);
+		break;
+	case MISENSOR_32BIT:
+		data32 = (u32 *)&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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	/* 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 (NULL == 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 (NULL == 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);
+	else {
+		if (power_up(sd))
+			return -EINVAL;
+
+		return mt9m114_init_common(sd);
+	}
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 600
+static int distance(struct mt9m114_res_struct const *res, u32 w, u32 h)
+{
+	unsigned int w_ratio;
+	unsigned int h_ratio;
+	int match;
+
+	if (w == 0)
+		return -1;
+	w_ratio = (res->width << 13) / w;
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - 8192);
+
+	if ((w_ratio < 8192) || (h_ratio < 8192) ||
+	    (match > LARGEST_ALLOWED_RATIO_MISMATCH))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	const struct mt9m114_res_struct *tmp_res = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(mt9m114_res); i++) {
+		tmp_res = &mt9m114_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static int mt9m114_try_res(u32 *w, u32 *h)
+{
+	int idx = 0;
+
+	if ((*w > MT9M114_RES_960P_SIZE_H)
+		|| (*h > MT9M114_RES_960P_SIZE_V)) {
+		*w = MT9M114_RES_960P_SIZE_H;
+		*h = MT9M114_RES_960P_SIZE_V;
+	} else {
+		idx = nearest_resolution_index(*w, *h);
+
+		/*
+		 * nearest_resolution_index() doesn't return smaller
+		 *  resolutions. If it fails, it means the requested
+		 *  resolution is higher than wecan support. Fallback
+		 *  to highest possible resolution in this case.
+		 */
+		if (idx == -1)
+			idx = ARRAY_SIZE(mt9m114_res) - 1;
+
+		*w = mt9m114_res[idx].width;
+		*h = mt9m114_res[idx].height;
+	}
+
+	return 0;
+}
+
+static struct mt9m114_res_struct *mt9m114_to_res(u32 w, u32 h)
+{
+	int  index;
+
+	for (index = 0; index < N_RES; index++) {
+		if ((mt9m114_res[index].width == w) &&
+		    (mt9m114_res[index].height == h))
+			break;
+	}
+
+	/* No mode found */
+	if (index >= N_RES)
+		return NULL;
+
+	return &mt9m114_res[index];
+}
+
+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 != NULL)
+		*h_size = hsize;
+	if (v_size != NULL)
+		*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 = &info->data;
+	u32 reg_val;
+	int ret;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	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_pad_config *cfg,
+				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_pad_config *cfg,
+			   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_index;
+	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 == NULL)
+		return -EINVAL;
+
+	mt9m114_try_res(&width, &height);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *fmt;
+		return 0;
+	}
+	res_index = mt9m114_to_res(width, height);
+
+	/* Sanity check */
+	if (unlikely(!res_index)) {
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	switch (res_index->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_index->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_index->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_index->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_index->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 fine_integration = 0;
+	unsigned int FLines = 0;
+	unsigned int FrameLengthLines = 0; /* ExposureTime.FrameLengthLines; */
+	unsigned int AnalogGain, DigitalGain;
+	u32 AnalogGainToWrite = 0;
+	u16 exposure_local[3];
+
+	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; */
+	/* FrameLengthLines = ExposureTime.FrameLengthLines; */
+	FLines = mt9m114_res[dev->res].lines_per_frame;
+	AnalogGain = exposure->gain[0];
+	DigitalGain = exposure->gain[1];
+	if (!dev->streamon) {
+		/*Save the first exposure values while stream is off*/
+		dev->first_exp = coarse_integration;
+		dev->first_gain = AnalogGain;
+		dev->first_diggain = DigitalGain;
+	}
+	/* DigitalGain = 0x400 * (((u16) DigitalGain) >> 8) +
+	((unsigned int)(0x400 * (((u16) DigitalGain) & 0xFF)) >>8); */
+
+	/* set frame length */
+	if (FLines < coarse_integration + 6)
+		FLines = coarse_integration + 6;
+	if (FLines < FrameLengthLines)
+		FLines = FrameLengthLines;
+	ret = mt9m114_write_reg(client, MISENSOR_16BIT, 0x300A, FLines);
+	if (ret) {
+		v4l2_err(client, "%s: fail to set FLines\n", __func__);
+		return -EINVAL;
+	}
+
+	/* set coarse/fine integration */
+	exposure_local[0] = REG_EXPO_COARSE;
+	exposure_local[1] = (u16)coarse_integration;
+	exposure_local[2] = (u16)fine_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(AnalogGain)
+	{
+	case 0:
+	  AnalogGainToWrite = 0x0;
+	  break;
+	case 1:
+	  AnalogGainToWrite = 0x20;
+	  break;
+	case 2:
+	  AnalogGainToWrite = 0x60;
+	  break;
+	case 4:
+	  AnalogGainToWrite = 0xA0;
+	  break;
+	case 8:
+	  AnalogGainToWrite = 0xE0;
+	  break;
+	default:
+	  AnalogGainToWrite = 0x20;
+	  break;
+	}
+	*/
+	if (DigitalGain >= 16 || DigitalGain <= 1)
+		DigitalGain = 1;
+	/* AnalogGainToWrite =
+		(u16)((DigitalGain << 12) | AnalogGainToWrite); */
+	AnalogGainToWrite = (u16)((DigitalGain << 12) | (u16)AnalogGain);
+	ret = mt9m114_write_reg(client, MISENSOR_16BIT,
+					REG_GAIN, AnalogGainToWrite);
+	if (ret) {
+		v4l2_err(client, "%s: fail to set AnalogGainToWrite\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;
+}
+#ifndef CSS15
+/*
+ * 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_pad_config *cfg,
+					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;
+	}
+
+	clamp_t(int, win_left, 0, 4);
+	clamp_t(int, win_top, 0, 4);
+	clamp_t(int, win_right, 0, 4);
+	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;
+}
+#endif
+
+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;
+#ifndef CSS15
+	case V4L2_CID_EXPOSURE_METERING:
+		ret = mt9m114_s_exposure_metering(&dev->sd, ctrl->val);
+		break;
+#endif
+	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;
+#ifndef CSS15
+	case V4L2_CID_EXPOSURE_ZONE_NUM:
+		ret = mt9m114_g_exposure_zone_num(&dev->sd, &ctrl->val);
+		break;
+#endif
+	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,
+	 },
+#ifndef CSS15
+	{
+	 .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 = 1,
+	 .def = 1,
+	 .flags = 0,
+	 },
+#endif
+	{
+	 .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 retvalue;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "%s: i2c error", __func__);
+		return -ENODEV;
+	}
+	mt9m114_read_reg(client, MISENSOR_16BIT, (u32)MT9M114_PID, &retvalue);
+	dev->real_model_id = retvalue;
+
+	if (retvalue != 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 (NULL == platform_data)
+		return -ENODEV;
+
+	dev->platform_data =
+	    (struct camera_sensor_platform_data *)platform_data;
+
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			v4l2_err(client, "mt9m114 platform init err\n");
+			return ret;
+		}
+	}
+	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_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	return 0;
+}
+
+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_pad_config *cfg,
+				  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_pad_config *cfg,
+				   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 == NULL)
+		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_parm = mt9m114_s_parm,
+	.s_stream = mt9m114_s_stream,
+	.g_frame_interval = mt9m114_g_frame_interval,
+};
+
+static 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,
+#ifndef CSS15
+	.set_selection = mt9m114_s_exposure_selection,
+#endif
+};
+
+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 const struct media_entity_operations mt9m114_entity_ops = {
+	.link_setup = NULL,
+};
+
+static int 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);
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+	v4l2_device_unregister_subdev(sd);
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	kfree(dev);
+	return 0;
+}
+
+static int mt9m114_probe(struct i2c_client *client,
+		       const struct i2c_device_id *id)
+{
+	struct mt9m114_device *dev;
+	int ret = 0;
+	unsigned int i;
+	void *pdata;
+
+	/* Setup sensor configuration structure */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	v4l2_i2c_subdev_init(&dev->sd, client, &mt9m114_ops);
+	pdata = client->dev.platform_data;
+	if (ACPI_COMPANION(&client->dev))
+		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;
+}
+
+MODULE_DEVICE_TABLE(i2c, mt9m114_id);
+
+static 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 = ACPI_PTR(mt9m114_acpi_match),
+	},
+	.probe = mt9m114_probe,
+	.remove = mt9m114_remove,
+	.id_table = mt9m114_id,
+};
+
+static __init int init_mt9m114(void)
+{
+	return i2c_add_driver(&mt9m114_driver);
+}
+
+static __exit void exit_mt9m114(void)
+{
+	i2c_del_driver(&mt9m114_driver);
+}
+
+module_init(init_mt9m114);
+module_exit(exit_mt9m114);
+
+MODULE_AUTHOR("Shuguang Gong <Shuguang.gong@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/mt9m114.h b/drivers/staging/media/atomisp/i2c/mt9m114.h
new file mode 100644
index 000000000000..5e7d79d2e01b
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/mt9m114.h
@@ -0,0 +1,1786 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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	(1<<0)	/* awb steady */
+#define MISENSOR_AE_READY	(1<<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	(1 << 0)
+#define MT9M114_BPAT_GRGRBGBG	(1 << 1)
+#define MT9M114_BPAT_GBGBRGRG	(1 << 2)
+#define MT9M114_BPAT_BGBGGRGR	(1 << 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;
+	/* extention SENSOR version 2 */
+	unsigned int cie_profile;
+
+	/* extention 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))
+
+static const struct i2c_device_id mt9m114_id[] = {
+	{"mt9m114", 0},
+	{}
+};
+
+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}
+};
+
+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}
+};
+
+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}
+};
+
+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}
+};
+
+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}
+};
+
+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 */
+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},
+};
+
+/* [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] */
+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}
+};
+
+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},
+
+};
+
+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
diff --git a/drivers/staging/media/atomisp/i2c/ov2680.c b/drivers/staging/media/atomisp/i2c/ov2680.c
new file mode 100644
index 000000000000..566091035c64
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2680.c
@@ -0,0 +1,1559 @@
+/*
+ * 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 <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/gpio.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 = 0;
+static int v_flag = 0;
+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,
+			   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 != OV2680_8BIT && data_length != OV2680_16BIT
+					&& data_length != OV2680_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 == OV2680_8BIT)
+		*val = (u8)data[0];
+	else if (data_length == OV2680_16BIT)
+		*val = be16_to_cpu(*(u16 *)&data[0]);
+	else
+		*val = be32_to_cpu(*(u32 *)&data[0]);
+	//dev_dbg(&client->dev,  "++++i2c read adr%x = %x\n", reg,*val);
+	return 0;
+}
+
+static int ov2680_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);
+	//dev_dbg(&client->dev,  "+++i2c write reg=%x->%x\n", data[0]*256 +data[1],data[2]);
+	return ret == num_msg ? 0 : -EIO;
+}
+
+static int ov2680_write_reg(struct i2c_client *client, u16 data_length,
+							u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	u16 *wreg = (u16 *)data;
+	const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
+
+	if (data_length != OV2680_8BIT && data_length != OV2680_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 == OV2680_8BIT) {
+		data[2] = (u8)(val);
+	} else {
+		/* OV2680_16BIT */
+		u16 *wdata = (u16 *)&data[2];
+		*wdata = cpu_to_be16(val);
+	}
+
+	ret = ov2680_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;
+}
+
+/*
+ * ov2680_write_reg_array - Initializes a list of OV2680 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().
+ *
+ * __ov2680_flush_reg_array, __ov2680_buf_reg_array() and
+ * __ov2680_write_reg_is_consecutive() are internal functions to
+ * ov2680_write_reg_array_fast() and should be not used anywhere else.
+ *
+ */
+
+static int __ov2680_flush_reg_array(struct i2c_client *client,
+				    struct ov2680_write_ctrl *ctrl)
+{
+	u16 size;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	ctrl->buffer.addr = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return ov2680_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __ov2680_buf_reg_array(struct i2c_client *client,
+				  struct ov2680_write_ctrl *ctrl,
+				  const struct ov2680_reg *next)
+{
+	int size;
+	u16 *data16;
+
+	switch (next->type) {
+	case OV2680_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV2680_16BIT:
+		size = 2;
+		data16 = (u16 *)&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) >= OV2680_MAX_WRITE_BUF_SIZE)
+		return __ov2680_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int __ov2680_write_reg_is_consecutive(struct i2c_client *client,
+					     struct ov2680_write_ctrl *ctrl,
+					     const struct ov2680_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->reg;
+}
+
+static int ov2680_write_reg_array(struct i2c_client *client,
+				  const struct ov2680_reg *reglist)
+{
+	const struct ov2680_reg *next = reglist;
+	struct ov2680_write_ctrl ctrl;
+	int err;
+	dev_dbg(&client->dev,  "++++write reg array\n");
+	ctrl.index = 0;
+	for (; next->type != OV2680_TOK_TERM; next++) {
+		switch (next->type & OV2680_TOK_MASK) {
+		case OV2680_TOK_DELAY:
+			err = __ov2680_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.
+			 */
+			 dev_dbg(&client->dev,  "+++ov2680_write_reg_array reg=%x->%x\n", next->reg,next->val);
+			if (!__ov2680_write_reg_is_consecutive(client, &ctrl,
+								next)) {
+				err = __ov2680_flush_reg_array(client, &ctrl);
+			if (err)
+				return err;
+			}
+			err = __ov2680_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev, "%s: write error, aborted\n",
+					 __func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __ov2680_flush_reg_array(client, &ctrl);
+}
+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 = ov2680_res[dev->fmt_idx].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 = ov2680_res[dev->fmt_idx].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 v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct atomisp_sensor_mode_data *buf = &info->data;
+	unsigned int pix_clk_freq_hz;
+	u16 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;
+
+	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 = 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, OV2680_16BIT,
+					OV2680_HORIZONTAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_start = reg_val;
+
+	ret =  ov2680_read_reg(client, OV2680_16BIT,
+					OV2680_VERTICAL_START_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_start = reg_val;
+
+	ret = ov2680_read_reg(client, OV2680_16BIT,
+					OV2680_HORIZONTAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_horizontal_end = reg_val;
+
+	ret = ov2680_read_reg(client, OV2680_16BIT,
+					OV2680_VERTICAL_END_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->crop_vertical_end = reg_val;
+
+	ret = ov2680_read_reg(client, OV2680_16BIT,
+					OV2680_HORIZONTAL_OUTPUT_SIZE_H, &reg_val);
+	if (ret)
+		return ret;
+	buf->output_width = reg_val;
+
+	ret = ov2680_read_reg(client, OV2680_16BIT,
+					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,hts;
+	int ret,exp_val;
+	
+       dev_dbg(&client->dev, "+++++++__ov2680_set_exposure coarse_itg %d, gain %d, digitgain %d++\n",coarse_itg, gain, digitgain);
+
+	hts = ov2680_res[dev->fmt_idx].pixels_per_line;
+	vts = ov2680_res[dev->fmt_idx].lines_per_frame;
+
+	/* group hold */
+	ret = ov2680_write_reg(client, OV2680_8BIT,
+                                       OV2680_GROUP_ACCESS, 0x00);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x 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, OV2680_16BIT, OV2680_TIMING_VTS_H, vts);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x 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, OV2680_8BIT,
+			       OV2680_EXPOSURE_L, exp_val & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV2680_EXPOSURE_L);
+		return ret;
+	}
+
+	ret = ov2680_write_reg(client, OV2680_8BIT,
+			       OV2680_EXPOSURE_M, (exp_val >> 8) & 0xFF);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV2680_EXPOSURE_M);
+		return ret;
+	}
+
+	ret = ov2680_write_reg(client, OV2680_8BIT,
+			       OV2680_EXPOSURE_H, (exp_val >> 16) & 0x0F);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV2680_EXPOSURE_H);
+		return ret;
+	}
+
+	/* Analog gain */
+	ret = ov2680_write_reg(client, OV2680_16BIT, OV2680_AGC_H, gain);
+	if (ret) {
+		dev_err(&client->dev, "%s: write %x error, aborted\n",
+			__func__, OV2680_AGC_H);
+		return ret;
+	}
+	/* Digital gain */
+	if (digitgain) {
+		ret = ov2680_write_reg(client, OV2680_16BIT,
+				OV2680_MWB_RED_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov2680_write_reg(client, OV2680_16BIT,
+				OV2680_MWB_GREEN_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+
+		ret = ov2680_write_reg(client, OV2680_16BIT,
+				OV2680_MWB_BLUE_GAIN_H, digitgain);
+		if (ret) {
+			dev_err(&client->dev, "%s: write %x error, aborted\n",
+				__func__, OV2680_MWB_RED_GAIN_H);
+			return ret;
+		}
+	}
+
+	/* End group */
+	ret = ov2680_write_reg(client, OV2680_8BIT,
+			       OV2680_GROUP_ACCESS, 0x10);
+	if (ret)
+		return ret;
+
+	/* Delay launch group */
+	ret = ov2680_write_reg(client, OV2680_8BIT,
+					   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;
+
+	mutex_lock(&dev->input_lock);
+	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;
+	}
+
+	// EXPOSURE CONTROL DISABLED FOR INITIAL CHECKIN, TUNING DOESN'T WORK
+	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);
+	u16 reg_v, reg_v2;
+	int ret;
+
+	/* get exposure */
+	ret = ov2680_read_reg(client, OV2680_8BIT,
+					OV2680_EXPOSURE_L,
+					&reg_v);
+	if (ret)
+		goto err;
+
+	ret = ov2680_read_reg(client, OV2680_8BIT,
+					OV2680_EXPOSURE_M,
+					&reg_v2);
+	if (ret)
+		goto err;
+
+	reg_v += reg_v2 << 8;
+	ret = ov2680_read_reg(client, OV2680_8BIT,
+					OV2680_EXPOSURE_H,
+					&reg_v2);
+	if (ret)
+		goto err;
+
+	*value = reg_v + (((u32)reg_v2 << 16));
+err:
+	return ret;
+}
+
+static u32 ov2680_translate_bayer_order(enum atomisp_bayer_order code)
+{
+	switch (code) {
+	case atomisp_bayer_order_rggb:
+		return MEDIA_BUS_FMT_SRGGB10_1X10;
+	case atomisp_bayer_order_grbg:
+		return MEDIA_BUS_FMT_SGRBG10_1X10;
+	case atomisp_bayer_order_bggr:
+		return MEDIA_BUS_FMT_SBGGR10_1X10;
+	case atomisp_bayer_order_gbrg:
+		return MEDIA_BUS_FMT_SGBRG10_1X10;
+	}
+	return 0;
+}
+
+static int ov2680_v_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct camera_mipi_info *ov2680_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u16 val;
+	u8 index;
+	dev_dbg(&client->dev, "@%s: value:%d\n", __func__, value);
+	ret = ov2680_read_reg(client, OV2680_8BIT, 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, OV2680_8BIT,
+			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];
+		dev->format.code = ov2680_translate_bayer_order(
+			ov2680_info->raw_bayer_order);
+	}
+	return ret;
+}
+
+static int ov2680_h_flip(struct v4l2_subdev *sd, s32 value)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct camera_mipi_info *ov2680_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u16 val;
+	u8 index;
+	dev_dbg(&client->dev, "@%s: value:%d\n", __func__, value);
+
+	ret = ov2680_read_reg(client, OV2680_8BIT, 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, OV2680_8BIT,
+			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];
+		dev->format.code = ov2680_translate_bayer_order(
+			ov2680_info->raw_bayer_order);
+	}
+	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
+};
+
+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, OV2680_8BIT, OV2680_SW_RESET, 0x01);
+	ret |= ov2680_write_reg_array(client, ov2680_global_setting);
+
+	return ret;
+}
+
+static int ov2680_init(struct v4l2_subdev *sd)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	/* restore settings */
+	ov2680_res = ov2680_res_preview;
+	N_RES = N_RES_PREVIEW;
+
+	ret = ov2680_init_registers(sd);
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = 0;
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	/* 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);
+		ret |= dev->platform_data->gpio1_ctrl(sd, 1);
+		usleep_range(10000, 15000);
+	} else {
+		ret = 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;
+	}
+
+	/* 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);
+
+	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 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;
+	}
+
+	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 ov2680_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+
+	if (on == 0){
+		ret = power_down(sd);
+	} else {
+		ret = power_up(sd);	
+		if (!ret)
+			return ov2680_init(sd);
+	}
+	return ret;
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 600
+static int distance(struct ov2680_resolution *res, u32 w, u32 h)
+{
+	unsigned int w_ratio = (res->width << 13) / w;
+	unsigned int h_ratio;
+	int match;
+
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - ((int)8192));
+
+
+	if ((w_ratio < (int)8192) || (h_ratio < (int)8192)  ||
+		(match > LARGEST_ALLOWED_RATIO_MISMATCH))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	struct ov2680_resolution *tmp_res = NULL;
+
+	for (i = 0; i < N_RES; i++) {
+		tmp_res = &ov2680_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static int get_resolution_index(int w, int h)
+{
+	int i;
+
+	for (i = 0; i < N_RES; i++) {
+		if (w != ov2680_res[i].width)
+			continue;
+		if (h != ov2680_res[i].height)
+			continue;
+
+		return i;
+	}
+
+	return -1;
+}
+
+static int ov2680_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  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;
+	int ret = 0;
+	int idx = 0;
+	dev_dbg(&client->dev, "+++++ov2680_s_mbus_fmt+++++l\n");
+	if (format->pad)
+		return -EINVAL;
+
+	if (!fmt)
+		return -EINVAL;
+
+	ov2680_info = v4l2_get_subdev_hostdata(sd);
+	if (!ov2680_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 = ov2680_res[N_RES - 1].width;
+		fmt->height = ov2680_res[N_RES - 1].height;
+	} else {
+		fmt->width = ov2680_res[idx].width;
+		fmt->height = ov2680_res[idx].height;
+	}
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+		}
+	dev->fmt_idx = get_resolution_index(fmt->width, fmt->height);
+	dev_dbg(&client->dev, "+++++get_resolution_index=%d+++++l\n",
+		     dev->fmt_idx);
+	if (dev->fmt_idx == -1) {
+		dev_err(&client->dev, "get resolution fail\n");
+		mutex_unlock(&dev->input_lock);
+		return -EINVAL;
+	}
+	v4l2_info(client, "__s_mbus_fmt i=%d, w=%d, h=%d\n", dev->fmt_idx,
+		  fmt->width, fmt->height);
+	dev_dbg(&client->dev, "__s_mbus_fmt i=%d, w=%d, h=%d\n",
+		     dev->fmt_idx, fmt->width, fmt->height);
+
+	ret = ov2680_write_reg_array(client, ov2680_res[dev->fmt_idx].regs);
+	if (ret)
+		dev_err(&client->dev, "ov2680 write resolution register err\n");
+
+	ret = ov2680_get_intg_factor(client, ov2680_info,
+				     &ov2680_res[dev->fmt_idx]);
+	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);
+
+	v4l2_info(client, "\n%s idx %d \n", __func__, dev->fmt_idx);
+
+	/*ret = startup(sd);
+	 * if (ret)
+	 * dev_err(&client->dev, "ov2680 startup err\n");
+	 */
+err:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2680_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  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 = ov2680_res[dev->fmt_idx].width;
+	fmt->height = ov2680_res[dev->fmt_idx].height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int ov2680_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 = ov2680_read_reg(client, OV2680_8BIT,
+					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, OV2680_8BIT,
+					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, OV2680_8BIT,
+					OV2680_SC_CMMN_SUB_ID, &high);
+	revision = (u8) high & 0x0f;
+
+	dev_err(&client->dev, "sensor_revision id  = 0x%x\n", id);
+	dev_err(&client->dev, "detect ov2680 success\n");
+	dev_err(&client->dev, "################5##########\n");
+	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, OV2680_8BIT, OV2680_SW_STREAM,
+				enable ? OV2680_START_STREAMING :
+				OV2680_STOP_STREAMING);
+#if 0
+	/* restore settings */
+	ov2680_res = ov2680_res_preview;
+	N_RES = N_RES_PREVIEW;
+#endif
+
+	//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);
+	/* 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, "ov2680 power-off err.\n");
+		goto fail_power_off;
+	}
+
+	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");
+fail_power_off:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2680_g_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!param)
+		return -EINVAL;
+
+	if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(&client->dev,  "unsupported buffer type.\n");
+		return -EINVAL;
+	}
+
+	memset(param, 0, sizeof(*param));
+	param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (dev->fmt_idx >= 0 && dev->fmt_idx < N_RES) {
+		param->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+		param->parm.capture.timeperframe.numerator = 1;
+		param->parm.capture.capturemode = dev->run_mode;
+		param->parm.capture.timeperframe.denominator =
+			ov2680_res[dev->fmt_idx].fps;
+	}
+	return 0;
+}
+
+static int ov2680_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov2680_device *dev = to_ov2680_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	v4l2_info(client, "\n%s:run_mode :%x\n", __func__, dev->run_mode);
+
+	mutex_lock(&dev->input_lock);
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		ov2680_res = ov2680_res_video;
+		N_RES = N_RES_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		ov2680_res = ov2680_res_still;
+		N_RES = N_RES_STILL;
+		break;
+	default:
+		ov2680_res = ov2680_res_preview;
+		N_RES = N_RES_PREVIEW;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+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 = ov2680_res[dev->fmt_idx].fps;
+
+	return 0;
+}
+
+static int ov2680_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 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_pad_config *cfg,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+
+	if (index >= N_RES)
+		return -EINVAL;
+
+	fse->min_width = ov2680_res[index].width;
+	fse->min_height = ov2680_res[index].height;
+	fse->max_width = ov2680_res[index].width;
+	fse->max_height = ov2680_res[index].height;
+
+	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 = ov2680_res[dev->fmt_idx].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_parm = ov2680_g_parm,
+	.s_parm = ov2680_s_parm,
+	.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,
+	.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 int 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);
+
+	return 0;
+}
+
+static int ov2680_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ov2680_device *dev;
+	int ret;
+	void *pdata;
+	unsigned int i;
+
+	printk("++++ov2680_probe++++\n");
+	dev_info(&client->dev, "++++ov2680_probe++++\n");
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&(dev->sd), client, &ov2680_ops);
+
+	if (ACPI_COMPANION(&client->dev))
+		pdata = gmin_camera_platform_data(&dev->sd,
+						  ATOMISP_INPUT_FORMAT_RAW_10,
+						  atomisp_bayer_order_bggr);
+	else
+		pdata = client->dev.platform_data;
+
+	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->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(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 struct acpi_device_id ov2680_acpi_match[] = {
+	{"XXOV2680"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov2680_acpi_match);
+
+
+MODULE_DEVICE_TABLE(i2c, ov2680_id);
+static struct i2c_driver ov2680_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = OV2680_NAME,
+		.acpi_match_table = ACPI_PTR(ov2680_acpi_match),
+
+	},
+	.probe = ov2680_probe,
+	.remove = ov2680_remove,
+	.id_table = ov2680_id,
+};
+
+static int init_ov2680(void)
+{
+	return i2c_add_driver(&ov2680_driver);
+}
+
+static void exit_ov2680(void)
+{
+
+	i2c_del_driver(&ov2680_driver);
+}
+
+module_init(init_ov2680);
+module_exit(exit_ov2680);
+
+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/ov2680.h b/drivers/staging/media/atomisp/i2c/ov2680.h
new file mode 100644
index 000000000000..944fe8e3bcbf
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2680.h
@@ -0,0 +1,940 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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"
+
+#define OV2680_NAME		"ov2680"
+#define OV2680B_NAME	"ov2680b"
+#define OV2680F_NAME	"ov2680f"
+
+/* 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 {
+	u8 *desc;
+	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;
+	bool used;
+};
+
+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 v4l2_mbus_framefmt format;
+		struct mutex input_lock;
+	struct v4l2_ctrl_handler ctrl_handler;
+		struct camera_sensor_platform_data *platform_data;
+		struct timespec timestamp_t_focus_abs;
+		int vt_pix_clk_freq_mhz;
+		int fmt_idx;
+		int run_mode;
+		u8 res;
+		u8 type;
+	};
+
+	enum ov2680_tok_type {
+		OV2680_8BIT  = 0x0001,
+		OV2680_16BIT = 0x0002,
+		OV2680_32BIT = 0x0004,
+		OV2680_TOK_TERM   = 0xf000,	/* terminating token for reg list */
+		OV2680_TOK_DELAY  = 0xfe00,	/* delay token for reg list */
+		OV2680_TOK_MASK = 0xfff0
+	};
+
+	/**
+	 * 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 {
+		enum ov2680_tok_type type;
+		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 const struct i2c_device_id ov2680_id[] = {
+		{OV2680B_NAME, 0},
+		{OV2680F_NAME, 0},
+		{}
+	};
+
+	static struct ov2680_reg const ov2680_global_setting[] = {
+	    {OV2680_8BIT, 0x0103, 0x01},
+	    {OV2680_8BIT, 0x3002, 0x00},
+	    {OV2680_8BIT, 0x3016, 0x1c},
+	    {OV2680_8BIT, 0x3018, 0x44},
+	    {OV2680_8BIT, 0x3020, 0x00},
+	    {OV2680_8BIT, 0x3080, 0x02},
+	    {OV2680_8BIT, 0x3082, 0x45},
+	    {OV2680_8BIT, 0x3084, 0x09},
+	    {OV2680_8BIT, 0x3085, 0x04},
+	    {OV2680_8BIT, 0x3503, 0x03},
+	    {OV2680_8BIT, 0x350b, 0x36},
+	    {OV2680_8BIT, 0x3600, 0xb4},
+	    {OV2680_8BIT, 0x3603, 0x39},
+	    {OV2680_8BIT, 0x3604, 0x24},
+	    {OV2680_8BIT, 0x3605, 0x00},
+	    {OV2680_8BIT, 0x3620, 0x26},
+	    {OV2680_8BIT, 0x3621, 0x37},
+	    {OV2680_8BIT, 0x3622, 0x04},
+	    {OV2680_8BIT, 0x3628, 0x00},
+	    {OV2680_8BIT, 0x3705, 0x3c},
+	    {OV2680_8BIT, 0x370c, 0x50},
+	    {OV2680_8BIT, 0x370d, 0xc0},
+	    {OV2680_8BIT, 0x3718, 0x88},
+	    {OV2680_8BIT, 0x3720, 0x00},
+	    {OV2680_8BIT, 0x3721, 0x00},
+	    {OV2680_8BIT, 0x3722, 0x00},
+	    {OV2680_8BIT, 0x3723, 0x00},
+	    {OV2680_8BIT, 0x3738, 0x00},
+	    {OV2680_8BIT, 0x3717, 0x58},
+	    {OV2680_8BIT, 0x3781, 0x80},
+	    {OV2680_8BIT, 0x3789, 0x60},
+	    {OV2680_8BIT, 0x3800, 0x00},
+	    {OV2680_8BIT, 0x3819, 0x04},
+	    {OV2680_8BIT, 0x4000, 0x81},
+	    {OV2680_8BIT, 0x4001, 0x40},
+	    {OV2680_8BIT, 0x4602, 0x02},
+	    {OV2680_8BIT, 0x481f, 0x36},
+	    {OV2680_8BIT, 0x4825, 0x36},
+	    {OV2680_8BIT, 0x4837, 0x18},
+	    {OV2680_8BIT, 0x5002, 0x30},
+	    {OV2680_8BIT, 0x5004, 0x04},//manual awb 1x
+	    {OV2680_8BIT, 0x5005, 0x00},
+	    {OV2680_8BIT, 0x5006, 0x04},
+	    {OV2680_8BIT, 0x5007, 0x00},
+	    {OV2680_8BIT, 0x5008, 0x04},
+	    {OV2680_8BIT, 0x5009, 0x00},
+	    {OV2680_8BIT, 0x5080, 0x00},
+	    {OV2680_8BIT, 0x3701, 0x64},  //add on 14/05/13
+	    {OV2680_8BIT, 0x3784, 0x0c},  //based OV2680_R1A_AM10.ovt add on 14/06/13
+	    {OV2680_8BIT, 0x5780, 0x3e},  //based OV2680_R1A_AM10.ovt,Adjust DPC setting (57xx) on 14/06/13
+	    {OV2680_8BIT, 0x5781, 0x0f},
+	    {OV2680_8BIT, 0x5782, 0x04},
+	    {OV2680_8BIT, 0x5783, 0x02},
+	    {OV2680_8BIT, 0x5784, 0x01},
+	    {OV2680_8BIT, 0x5785, 0x01},
+	    {OV2680_8BIT, 0x5786, 0x00},
+	    {OV2680_8BIT, 0x5787, 0x04},
+	    {OV2680_8BIT, 0x5788, 0x02},
+	    {OV2680_8BIT, 0x5789, 0x00},
+	    {OV2680_8BIT, 0x578a, 0x01},
+	    {OV2680_8BIT, 0x578b, 0x02},
+	    {OV2680_8BIT, 0x578c, 0x03},
+	    {OV2680_8BIT, 0x578d, 0x03},
+	    {OV2680_8BIT, 0x578e, 0x08},
+	    {OV2680_8BIT, 0x578f, 0x0c},
+	    {OV2680_8BIT, 0x5790, 0x08},
+	    {OV2680_8BIT, 0x5791, 0x04},
+	    {OV2680_8BIT, 0x5792, 0x00},
+	    {OV2680_8BIT, 0x5793, 0x00},
+	    {OV2680_8BIT, 0x5794, 0x03}, //based OV2680_R1A_AM10.ovt,Adjust DPC setting (57xx) on 14/06/13
+		{OV2680_8BIT, 0x0100, 0x00},	//stream off
+
+		{OV2680_TOK_TERM, 0, 0}
+	};
+
+
+	/*
+	 * 176x144 30fps  VBlanking 1lane 10Bit (binning)
+	 */
+	static struct ov2680_reg const ov2680_QCIF_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x24},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x05},
+	{OV2680_8BIT, 0x3805, 0xaf},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0x47},
+	{OV2680_8BIT, 0x3808, 0x00},
+	{OV2680_8BIT, 0x3809, 0xC0},
+	{OV2680_8BIT, 0x380a, 0x00},
+	{OV2680_8BIT, 0x380b, 0xa0},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xb0},
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x04},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x04},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x4000, 0x81},
+	{OV2680_8BIT, 0x4001, 0x40},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc2},
+	{OV2680_8BIT, 0x3821, 0x01},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 * 352x288 30fps  VBlanking 1lane 10Bit (binning)
+	 */
+	static struct ov2680_reg const ov2680_CIF_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x24},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x03},
+	{OV2680_8BIT, 0x3805, 0x8f},
+	{OV2680_8BIT, 0x3806, 0x02},
+	{OV2680_8BIT, 0x3807, 0xe7},
+	{OV2680_8BIT, 0x3808, 0x01},
+	{OV2680_8BIT, 0x3809, 0x70},
+	{OV2680_8BIT, 0x380a, 0x01},
+	{OV2680_8BIT, 0x380b, 0x30},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xb0},
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x04},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x04},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc2},
+	{OV2680_8BIT, 0x3821, 0x01},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 * 336x256 30fps  VBlanking 1lane 10Bit (binning)
+	 */
+	static struct ov2680_reg const ov2680_QVGA_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x24},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x03},
+	{OV2680_8BIT, 0x3805, 0x4f},
+	{OV2680_8BIT, 0x3806, 0x02},
+	{OV2680_8BIT, 0x3807, 0x87},
+	{OV2680_8BIT, 0x3808, 0x01},
+	{OV2680_8BIT, 0x3809, 0x50},
+	{OV2680_8BIT, 0x380a, 0x01},
+	{OV2680_8BIT, 0x380b, 0x00},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xb0},
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x04},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x04},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc2},
+	{OV2680_8BIT, 0x3821, 0x01},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+
+	/*
+	 * 656x496 30fps  VBlanking 1lane 10Bit (binning)
+	 */
+	static struct ov2680_reg const ov2680_656x496_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x24},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x05},
+	{OV2680_8BIT, 0x3805, 0xcf},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0x67},
+	{OV2680_8BIT, 0x3808, 0x02},
+	{OV2680_8BIT, 0x3809, 0x90},
+	{OV2680_8BIT, 0x380a, 0x01},
+	{OV2680_8BIT, 0x380b, 0xf0},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xb0},
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x04},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x04},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc2},
+	{OV2680_8BIT, 0x3821, 0x01},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+	/*
+	* 800x600 30fps  VBlanking 1lane 10Bit (binning)
+	*/
+	static struct ov2680_reg const ov2680_720x592_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x26},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0x00}, // X_ADDR_START;
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x00}, // Y_ADDR_START;
+	{OV2680_8BIT, 0x3804, 0x05},
+	{OV2680_8BIT, 0x3805, 0xaf}, // X_ADDR_END;
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0xaf}, // Y_ADDR_END;
+	{OV2680_8BIT, 0x3808, 0x02},
+	{OV2680_8BIT, 0x3809, 0xd0}, // X_OUTPUT_SIZE;
+	{OV2680_8BIT, 0x380a, 0x02},
+	{OV2680_8BIT, 0x380b, 0x50}, // Y_OUTPUT_SIZE;
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xac}, // HTS;
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84}, // VTS;
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x00},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x00},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5708, 0x00},
+	{OV2680_8BIT, 0x5704, 0x02},
+	{OV2680_8BIT, 0x5705, 0xd0}, // X_WIN;
+	{OV2680_8BIT, 0x5706, 0x02},
+	{OV2680_8BIT, 0x5707, 0x50}, // Y_WIN;
+	{OV2680_8BIT, 0x3820, 0xc2}, // FLIP_FORMAT;
+	{OV2680_8BIT, 0x3821, 0x01}, // MIRROR_FORMAT;
+    {OV2680_8BIT, 0x5090, 0x00}, // PRE ISP CTRL16, default value is 0x0C;
+                                 // BIT[3]: Mirror order, BG or GB;
+                                 // BIT[2]: Flip order, BR or RB;
+	{OV2680_8BIT, 0x5081, 0x41},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	* 800x600 30fps  VBlanking 1lane 10Bit (binning)
+	*/
+	static struct ov2680_reg const ov2680_800x600_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x01},
+	{OV2680_8BIT, 0x3501, 0x26},
+	{OV2680_8BIT, 0x3502, 0x40},
+	{OV2680_8BIT, 0x370a, 0x23},
+	{OV2680_8BIT, 0x3801, 0x00},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x00},
+	{OV2680_8BIT, 0x3804, 0x06},
+	{OV2680_8BIT, 0x3805, 0x4f},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0xbf},
+	{OV2680_8BIT, 0x3808, 0x03},
+	{OV2680_8BIT, 0x3809, 0x20},
+	{OV2680_8BIT, 0x380a, 0x02},
+	{OV2680_8BIT, 0x380b, 0x58},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xac},
+	{OV2680_8BIT, 0x380e, 0x02},
+	{OV2680_8BIT, 0x380f, 0x84},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x00},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x00},
+	{OV2680_8BIT, 0x3814, 0x31},
+	{OV2680_8BIT, 0x3815, 0x31},
+	{OV2680_8BIT, 0x5708, 0x00},
+	{OV2680_8BIT, 0x5704, 0x03},
+	{OV2680_8BIT, 0x5705, 0x20},
+	{OV2680_8BIT, 0x5706, 0x02},
+	{OV2680_8BIT, 0x5707, 0x58},
+	{OV2680_8BIT, 0x3820, 0xc2},
+	{OV2680_8BIT, 0x3821, 0x01},
+    {OV2680_8BIT, 0x5090, 0x00},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x03},
+	{OV2680_8BIT, 0x5081, 0x41},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 * 720p=1280*720 30fps  VBlanking 1lane 10Bit (no-Scaling)
+	 */
+	static struct ov2680_reg const ov2680_720p_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x00},
+	{OV2680_8BIT, 0x3501, 0x48},
+	{OV2680_8BIT, 0x3502, 0xe0},
+	{OV2680_8BIT, 0x370a, 0x21},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0xf2},
+	{OV2680_8BIT, 0x3804, 0x05},
+	{OV2680_8BIT, 0x3805, 0xbf},
+	{OV2680_8BIT, 0x3806, 0x03},
+	{OV2680_8BIT, 0x3807, 0xdd},
+	{OV2680_8BIT, 0x3808, 0x05},
+	{OV2680_8BIT, 0x3809, 0x10},
+	{OV2680_8BIT, 0x380a, 0x02},
+	{OV2680_8BIT, 0x380b, 0xe0},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xa8},
+	{OV2680_8BIT, 0x380e, 0x05},
+	{OV2680_8BIT, 0x380f, 0x0e},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x08},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x06},
+	{OV2680_8BIT, 0x3814, 0x11},
+	{OV2680_8BIT, 0x3815, 0x11},
+	{OV2680_8BIT, 0x4008, 0x02},
+	{OV2680_8BIT, 0x4009, 0x09},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc0},
+	{OV2680_8BIT, 0x3821, 0x00},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 * 1296x976 30fps  VBlanking 1lane 10Bit(no-scaling)
+	 */
+	static struct ov2680_reg const ov2680_1296x976_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x00},
+	{OV2680_8BIT, 0x3501, 0x48},
+	{OV2680_8BIT, 0x3502, 0xe0},
+	{OV2680_8BIT, 0x370a, 0x21},
+	{OV2680_8BIT, 0x3801, 0xa0},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x05},
+	{OV2680_8BIT, 0x3805, 0xbf},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0x57},
+	{OV2680_8BIT, 0x3808, 0x05},
+	{OV2680_8BIT, 0x3809, 0x10},
+	{OV2680_8BIT, 0x380a, 0x03},
+	{OV2680_8BIT, 0x380b, 0xd0},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xa8},
+	{OV2680_8BIT, 0x380e, 0x05},
+	{OV2680_8BIT, 0x380f, 0x0e},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x08},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x08},
+	{OV2680_8BIT, 0x3814, 0x11},
+	{OV2680_8BIT, 0x3815, 0x11},
+	{OV2680_8BIT, 0x4008, 0x02},
+	{OV2680_8BIT, 0x4009, 0x09},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc0},
+	{OV2680_8BIT, 0x3821, 0x00}, //miror/flip
+    // {OV2680_8BIT, 0x5090, 0x0c},
+    {OV2680_TOK_TERM, 0, 0}
+ 	};
+
+	/*
+	 *   1456*1096 30fps  VBlanking 1lane 10bit(no-scaling)
+	*/
+	static struct ov2680_reg const ov2680_1456x1096_30fps[]= {
+	{OV2680_8BIT, 0x3086, 0x00},
+	{OV2680_8BIT, 0x3501, 0x48},
+	{OV2680_8BIT, 0x3502, 0xe0},
+	{OV2680_8BIT, 0x370a, 0x21},
+	{OV2680_8BIT, 0x3801, 0x90},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x78},
+	{OV2680_8BIT, 0x3804, 0x06},
+	{OV2680_8BIT, 0x3805, 0x4f},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0xC0},
+	{OV2680_8BIT, 0x3808, 0x05},
+	{OV2680_8BIT, 0x3809, 0xb0},
+	{OV2680_8BIT, 0x380a, 0x04},
+	{OV2680_8BIT, 0x380b, 0x48},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xa8},
+	{OV2680_8BIT, 0x380e, 0x05},
+	{OV2680_8BIT, 0x380f, 0x0e},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x08},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x00},
+	{OV2680_8BIT, 0x3814, 0x11},
+	{OV2680_8BIT, 0x3815, 0x11},
+	{OV2680_8BIT, 0x4008, 0x02},
+	{OV2680_8BIT, 0x4009, 0x09},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x00}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x10},
+	{OV2680_8BIT, 0x5705, 0xa0},
+	{OV2680_8BIT, 0x5706, 0x0c},
+	{OV2680_8BIT, 0x5707, 0x78},
+	{OV2680_8BIT, 0x3820, 0xc0},
+	{OV2680_8BIT, 0x3821, 0x00},
+    // {OV2680_8BIT, 0x5090, 0x0c},
+	{OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 *1616x916  30fps  VBlanking 1lane 10bit
+	 */
+
+	static struct ov2680_reg const ov2680_1616x916_30fps[] = {
+
+	{OV2680_8BIT, 0x3086, 0x00},
+	{OV2680_8BIT, 0x3501, 0x48},
+	{OV2680_8BIT, 0x3502, 0xe0},
+	{OV2680_8BIT, 0x370a, 0x21},
+	{OV2680_8BIT, 0x3801, 0x00},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x96},
+	{OV2680_8BIT, 0x3804, 0x06},
+	{OV2680_8BIT, 0x3805, 0x4f},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0x39},
+	{OV2680_8BIT, 0x3808, 0x06},
+	{OV2680_8BIT, 0x3809, 0x50},
+	{OV2680_8BIT, 0x380a, 0x03},
+	{OV2680_8BIT, 0x380b, 0x94},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xa8},
+	{OV2680_8BIT, 0x380e, 0x05},
+	{OV2680_8BIT, 0x380f, 0x0e},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x00},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x08},
+	{OV2680_8BIT, 0x3814, 0x11},
+	{OV2680_8BIT, 0x3815, 0x11},
+	{OV2680_8BIT, 0x4008, 0x02},
+	{OV2680_8BIT, 0x4009, 0x09},
+	{OV2680_8BIT, 0x5081, 0x41},
+	{OV2680_8BIT, 0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x06},
+	{OV2680_8BIT, 0x5705, 0x50},
+	{OV2680_8BIT, 0x5706, 0x03},
+	{OV2680_8BIT, 0x5707, 0x94},
+	{OV2680_8BIT, 0x3820, 0xc0},
+	{OV2680_8BIT, 0x3821, 0x00},
+    // {OV2680_8BIT, 0x5090, 0x0C},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	/*
+	 * 1612x1212 30fps VBlanking 1lane 10Bit
+	 */
+	static struct ov2680_reg const ov2680_1616x1082_30fps[] = {
+       {OV2680_8BIT, 0x3086, 0x00},
+       {OV2680_8BIT, 0x3501, 0x48},
+       {OV2680_8BIT, 0x3502, 0xe0},
+       {OV2680_8BIT, 0x370a, 0x21},
+       {OV2680_8BIT, 0x3801, 0x00},
+       {OV2680_8BIT, 0x3802, 0x00},
+       {OV2680_8BIT, 0x3803, 0x86},
+       {OV2680_8BIT, 0x3804, 0x06},
+       {OV2680_8BIT, 0x3805, 0x4f},
+       {OV2680_8BIT, 0x3806, 0x04},
+       {OV2680_8BIT, 0x3807, 0xbf},
+       {OV2680_8BIT, 0x3808, 0x06},
+       {OV2680_8BIT, 0x3809, 0x50},
+       {OV2680_8BIT, 0x380a, 0x04},
+       {OV2680_8BIT, 0x380b, 0x3a},
+       {OV2680_8BIT, 0x380c, 0x06},
+       {OV2680_8BIT, 0x380d, 0xa8},
+       {OV2680_8BIT, 0x380e, 0x05},
+       {OV2680_8BIT, 0x380f, 0x0e},
+       {OV2680_8BIT, 0x3810, 0x00},
+       {OV2680_8BIT, 0x3811, 0x00},
+       {OV2680_8BIT, 0x3812, 0x00},
+       {OV2680_8BIT, 0x3813, 0x00},
+       {OV2680_8BIT, 0x3814, 0x11},
+       {OV2680_8BIT, 0x3815, 0x11},
+       {OV2680_8BIT, 0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+       {OV2680_8BIT, 0x5704, 0x06},
+       {OV2680_8BIT, 0x5705, 0x50},
+       {OV2680_8BIT, 0x5706, 0x04},
+       {OV2680_8BIT, 0x5707, 0x3a},
+       {OV2680_8BIT, 0x3820, 0xc0},
+       {OV2680_8BIT, 0x3821, 0x00},
+       // {OV2680_8BIT, 0x5090, 0x0C},
+       {OV2680_8BIT, 0x4008, 0x02},
+       {OV2680_8BIT, 0x4009, 0x09},
+       {OV2680_8BIT, 0x5081, 0x41},
+		{OV2680_TOK_TERM, 0, 0}
+        };
+	/*
+	 * 1616x1216 30fps VBlanking 1lane 10Bit
+	 */
+	static struct ov2680_reg const ov2680_1616x1216_30fps[] = {
+	{OV2680_8BIT, 0x3086, 0x00},
+	{OV2680_8BIT, 0x3501, 0x48},
+	{OV2680_8BIT, 0x3502, 0xe0},
+	{OV2680_8BIT, 0x370a, 0x21},
+	{OV2680_8BIT, 0x3801, 0x00},
+	{OV2680_8BIT, 0x3802, 0x00},
+	{OV2680_8BIT, 0x3803, 0x00},
+	{OV2680_8BIT, 0x3804, 0x06},
+	{OV2680_8BIT, 0x3805, 0x4f},
+	{OV2680_8BIT, 0x3806, 0x04},
+	{OV2680_8BIT, 0x3807, 0xbf},
+	{OV2680_8BIT, 0x3808, 0x06},
+	{OV2680_8BIT, 0x3809, 0x50},//50},//4line for mirror and flip
+	{OV2680_8BIT, 0x380a, 0x04},
+	{OV2680_8BIT, 0x380b, 0xc0},//c0},
+	{OV2680_8BIT, 0x380c, 0x06},
+	{OV2680_8BIT, 0x380d, 0xa8},
+	{OV2680_8BIT, 0x380e, 0x05},
+	{OV2680_8BIT, 0x380f, 0x0e},
+	{OV2680_8BIT, 0x3810, 0x00},
+	{OV2680_8BIT, 0x3811, 0x00},
+	{OV2680_8BIT, 0x3812, 0x00},
+	{OV2680_8BIT, 0x3813, 0x00},
+	{OV2680_8BIT, 0x3814, 0x11},
+	{OV2680_8BIT, 0x3815, 0x11},
+	{OV2680_8BIT, 0x4008, 0x00},
+	{OV2680_8BIT, 0x4009, 0x0b},
+	{OV2680_8BIT, 0x5081, 0x01},
+	{OV2680_8BIT, 0x5708, 0x01}, //add for full size flip off and mirror off 2014/09/11
+	{OV2680_8BIT, 0x5704, 0x06},
+	{OV2680_8BIT, 0x5705, 0x50},
+	{OV2680_8BIT, 0x5706, 0x04},
+	{OV2680_8BIT, 0x5707, 0xcc},
+	{OV2680_8BIT, 0x3820, 0xc0},
+	{OV2680_8BIT, 0x3821, 0x00},
+    // {OV2680_8BIT, 0x5090, 0x0C},
+    {OV2680_TOK_TERM, 0, 0}
+	};
+
+	static struct ov2680_resolution ov2680_res_preview[] = {
+	{
+		.desc = "ov2680_1616x1216_30fps",
+ 	  	.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 66,
+		.fps = 30,
+		.used = 0,
+		.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,
+	},
+   	{
+		.desc = "ov2680_1616x916_30fps",
+		.width = 1616,
+		.height = 916,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.used = 0,
+		.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,
+	},
+};
+#define N_RES_PREVIEW (ARRAY_SIZE(ov2680_res_preview))
+
+static struct ov2680_resolution ov2680_res_still[] = {
+	{
+		.desc = "ov2680_1616x1216_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 66,
+		.fps = 30,
+		.used = 0,
+		.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,
+	},
+   	{
+		.desc = "ov2680_1616x916_30fps",
+		.width = 1616,
+		.height = 916,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.used = 0,
+		.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,
+	},
+};
+#define N_RES_STILL (ARRAY_SIZE(ov2680_res_still))
+
+static struct ov2680_resolution ov2680_res_video[] = {
+	{
+		.desc = "ov2680_1616x1216_30fps",
+		.width = 1616,
+		.height = 1216,
+		.pix_clk_freq = 66,
+		.fps = 30,
+		.used = 0,
+		.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,
+	},
+	{
+		.desc = "ov2680_720p_30fps",
+		.width = 1616,
+		.height = 916,
+		.fps = 30,
+		.pix_clk_freq = 66,
+		.used = 0,
+		.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,
+	},
+};
+#define N_RES_VIDEO (ARRAY_SIZE(ov2680_res_video))
+
+static struct ov2680_resolution *ov2680_res = ov2680_res_preview;
+static int N_RES = N_RES_PREVIEW;
+
+
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov2722.c b/drivers/staging/media/atomisp/i2c/ov2722.c
new file mode 100644
index 000000000000..b7afadebdf89
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2722.c
@@ -0,0 +1,1373 @@
+/*
+ * 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/gpio.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(*(u16 *)&data[0]);
+	else
+		*val = be32_to_cpu(*(u32 *)&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};
+	u16 *wreg = (u16 *)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 */
+		u16 *wdata = (u16 *)&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;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	ctrl->buffer.addr = 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;
+	u16 *data16;
+
+	switch (next->type) {
+	case OV2722_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV2722_16BIT:
+		size = 2;
+		data16 = (u16 *)&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 (0 == 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 = ov2722_res[dev->fmt_idx].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
+};
+
+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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	/* 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);
+	else {
+		ret = power_up(sd);
+		if (!ret)
+			return ov2722_init(sd);
+	}
+	return ret;
+}
+
+/*
+ * distance - calculate the distance
+ * @res: resolution
+ * @w: width
+ * @h: height
+ *
+ * Get the gap between resolution and w/h.
+ * res->width/height smaller than w/h wouldn't be considered.
+ * Returns the value of gap or -1 if fail.
+ */
+#define LARGEST_ALLOWED_RATIO_MISMATCH 800
+static int distance(struct ov2722_resolution *res, u32 w, u32 h)
+{
+	unsigned int w_ratio = (res->width << 13) / w;
+	unsigned int h_ratio;
+	int match;
+
+	if (h == 0)
+		return -1;
+	h_ratio = (res->height << 13) / h;
+	if (h_ratio == 0)
+		return -1;
+	match   = abs(((w_ratio << 13) / h_ratio) - 8192);
+
+	if ((w_ratio < 8192) || (h_ratio < 8192) ||
+	    (match > LARGEST_ALLOWED_RATIO_MISMATCH))
+		return -1;
+
+	return w_ratio + h_ratio;
+}
+
+/* Return the nearest higher resolution index */
+static int nearest_resolution_index(int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int min_dist = INT_MAX;
+	struct ov2722_resolution *tmp_res = NULL;
+
+	for (i = 0; i < N_RES; i++) {
+		tmp_res = &ov2722_res[i];
+		dist = distance(tmp_res, w, h);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static int get_resolution_index(int w, int h)
+{
+	int i;
+
+	for (i = 0; i < N_RES; i++) {
+		if (w != ov2722_res[i].width)
+			continue;
+		if (h != ov2722_res[i].height)
+			continue;
+
+		return i;
+	}
+
+	return -1;
+}
+
+/* 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, ov2722_res[dev->fmt_idx].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_pad_config *cfg,
+			  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 camera_mipi_info *ov2722_info = NULL;
+	int ret = 0;
+	int idx;
+	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);
+	idx = nearest_resolution_index(fmt->width, fmt->height);
+	if (idx == -1) {
+		/* return the largest resolution */
+		fmt->width = ov2722_res[N_RES - 1].width;
+		fmt->height = ov2722_res[N_RES - 1].height;
+	} else {
+		fmt->width = ov2722_res[idx].width;
+		fmt->height = ov2722_res[idx].height;
+	}
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->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;
+	}
+
+	dev->pixels_per_line = ov2722_res[dev->fmt_idx].pixels_per_line;
+	dev->lines_per_frame = ov2722_res[dev->fmt_idx].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,
+					&ov2722_res[dev->fmt_idx]);
+	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_pad_config *cfg,
+			  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 = ov2722_res[dev->fmt_idx].width;
+	fmt->height = ov2722_res[dev->fmt_idx].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, low;
+	int ret;
+	u16 id;
+	u8 revision;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	ret = ov2722_read_reg(client, OV2722_8BIT,
+					OV2722_SC_CMMN_CHIP_ID_H, &high);
+	if (ret) {
+		dev_err(&client->dev, "sensor_id_high = 0x%x\n", high);
+		return -ENODEV;
+	}
+	ret = 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;
+	}
+
+	ret = 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);
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			dev_err(&client->dev, "platform init err\n");
+			goto platform_init_failed;
+		}
+	}
+
+	/* 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:
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+platform_init_failed:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int ov2722_g_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!param)
+		return -EINVAL;
+
+	if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(&client->dev,  "unsupported buffer type.\n");
+		return -EINVAL;
+	}
+
+	memset(param, 0, sizeof(*param));
+	param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (dev->fmt_idx >= 0 && dev->fmt_idx < N_RES) {
+		param->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+		param->parm.capture.timeperframe.numerator = 1;
+		param->parm.capture.capturemode = dev->run_mode;
+		param->parm.capture.timeperframe.denominator =
+			ov2722_res[dev->fmt_idx].fps;
+	}
+	return 0;
+}
+
+static int ov2722_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	mutex_lock(&dev->input_lock);
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		ov2722_res = ov2722_res_video;
+		N_RES = N_RES_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		ov2722_res = ov2722_res_still;
+		N_RES = N_RES_STILL;
+		break;
+	default:
+		ov2722_res = ov2722_res_preview;
+		N_RES = N_RES_PREVIEW;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+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 = ov2722_res[dev->fmt_idx].fps;
+
+	return 0;
+}
+
+static int ov2722_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 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_pad_config *cfg,
+				  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 = ov2722_res[dev->fmt_idx].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_parm = ov2722_g_parm,
+	.s_parm = ov2722_s_parm,
+	.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 int ov2722_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov2722_device *dev = to_ov2722_sensor(sd);
+	dev_dbg(&client->dev, "ov2722_remove...\n");
+
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	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);
+
+	return 0;
+}
+
+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,
+			const struct i2c_device_id *id)
+{
+	struct ov2722_device *dev;
+	void *ovpdev;
+	int ret;
+	struct acpi_device *adev;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&(dev->sd), client, &ov2722_ops);
+
+	ovpdev = client->dev.platform_data;
+	adev = ACPI_COMPANION(&client->dev);
+	if (adev) {
+		adev->power.flags.power_resources = 0;
+		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);
+
+	if (ACPI_HANDLE(&client->dev))
+		ret = atomisp_register_i2c_module(&dev->sd, ovpdev, RAW_CAMERA);
+
+	return ret;
+
+out_ctrl_handler_free:
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+MODULE_DEVICE_TABLE(i2c, ov2722_id);
+
+static struct acpi_device_id ov2722_acpi_match[] = {
+	{ "INT33FB" },
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, ov2722_acpi_match);
+
+static struct i2c_driver ov2722_driver = {
+	.driver = {
+		.name = OV2722_NAME,
+		.acpi_match_table = ACPI_PTR(ov2722_acpi_match),
+	},
+	.probe = ov2722_probe,
+	.remove = ov2722_remove,
+	.id_table = ov2722_id,
+};
+
+static int init_ov2722(void)
+{
+	return i2c_add_driver(&ov2722_driver);
+}
+
+static void exit_ov2722(void)
+{
+
+	i2c_del_driver(&ov2722_driver);
+}
+
+module_init(init_ov2722);
+module_exit(exit_ov2722);
+
+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/ov2722.h b/drivers/staging/media/atomisp/i2c/ov2722.h
new file mode 100644
index 000000000000..b0d40965d89e
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov2722.h
@@ -0,0 +1,1267 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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_NAME		"ov2722"
+
+#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 camera_sensor_platform_data *platform_data;
+	int vt_pix_clk_freq_mhz;
+	int fmt_idx;
+	int run_mode;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 res;
+	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;
+};
+
+static const struct i2c_device_id ov2722_id[] = {
+	{OV2722_NAME, 0},
+	{}
+};
+
+/*
+ * Register settings for various resolution
+ */
+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},
+};
+
+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}
+};
+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},
+};
+
+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}
+};
+
+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},
+};
+
+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))
+
+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))
+
+static struct ov2722_resolution *ov2722_res = ov2722_res_preview;
+static int N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/Kconfig b/drivers/staging/media/atomisp/i2c/ov5693/Kconfig
new file mode 100644
index 000000000000..9fb1bffbe9b3
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/Kconfig
@@ -0,0 +1,11 @@
+config VIDEO_OV5693
+       tristate "Omnivision ov5693 sensor support"
+       depends on I2C && VIDEO_V4L2
+       ---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.
+
diff --git a/drivers/staging/media/atomisp/i2c/ov5693/Makefile b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
new file mode 100644
index 000000000000..fceb9e9b881b
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_VIDEO_OV5693) += ov5693.o
+
+ccflags-y += -Werror
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 000000000000..2dd894989cd9
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ad5823.h
@@ -0,0 +1,67 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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/ov5693.c b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c
new file mode 100644
index 000000000000..5e9dafe7cc32
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.c
@@ -0,0 +1,2066 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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/gpio.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 uint32_t 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(*(u16 *)&data[0]);
+	else
+		*val = be32_to_cpu(*(u32 *)&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;
+	u16 val;
+
+	val = cpu_to_be16(data);
+	msg.addr = VCM_ADDR;
+	msg.flags = 0;
+	msg.len = OV5693_16BIT;
+	msg.buf = (u8 *)&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};
+	u16 *wreg = (u16 *)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 */
+		u16 *wdata = (u16 *)&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;
+
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	ctrl->buffer.addr = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return ov5693_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __ov5693_buf_reg_array(struct i2c_client *client,
+				  struct ov5693_write_ctrl *ctrl,
+				  const struct ov5693_reg *next)
+{
+	int size;
+	u16 *data16;
+
+	switch (next->type) {
+	case OV5693_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV5693_16BIT:
+		size = 2;
+		data16 = (u16 *)&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 == NULL)
+		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 = 0;
+
+	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 (21 == i) {
+			if ((*b) == 0) {
+				dev->otp_size = 320;
+				break;
+			} else {
+				b = buf;
+				continue;
+			}
+		} else if (24 == i) {		//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 (27 == i) {		//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;
+}
+
+int ad5823_t_focus_vcm(struct v4l2_subdev *sd, u16 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret = -EINVAL;
+	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;
+}
+
+int ad5823_t_focus_abs(struct v4l2_subdev *sd, s32 value)
+{
+	int ret;
+
+	value = min(value, AD5823_MAX_FOCUS_POS);
+	ret = ad5823_t_focus_vcm(sd, value);
+
+	return ret;
+}
+
+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;
+		getnstimeofday(&(dev->timestamp_t_focus_abs));
+	} 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);
+	struct timespec temptime;
+	const struct timespec timedelay = {
+		0,
+		min((u32)abs(dev->number_of_steps) * DELAY_PER_STEP_NS,
+		(u32)DELAY_MAX_PER_STEP_NS),
+	};
+
+	getnstimeofday(&temptime);
+	temptime = timespec_sub(temptime, (dev->timestamp_t_focus_abs));
+	if (timespec_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_TIMEING:
+		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
+};
+
+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_TIMEING,
+	 .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;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	/* 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)
+{
+	int ret;
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	ret = dev->platform_data->gpio0_ctrl(sd, flag);
+
+	return ret;
+}
+
+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 (NULL == 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 (NULL == 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_pad_config *cfg,
+			  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 == NULL)
+		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) {
+		cfg->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_pad_config *cfg,
+			  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);
+	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 == NULL)
+		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_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!param)
+		return -EINVAL;
+
+	if (param->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		dev_err(&client->dev,  "unsupported buffer type.\n");
+		return -EINVAL;
+	}
+
+	memset(param, 0, sizeof(*param));
+	param->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (dev->fmt_idx >= 0 && dev->fmt_idx < N_RES) {
+		param->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+		param->parm.capture.timeperframe.numerator = 1;
+		param->parm.capture.capturemode = dev->run_mode;
+		param->parm.capture.timeperframe.denominator =
+			ov5693_res[dev->fmt_idx].fps;
+	}
+	return 0;
+}
+
+static int ov5693_s_parm(struct v4l2_subdev *sd,
+			struct v4l2_streamparm *param)
+{
+	struct ov5693_device *dev = to_ov5693_sensor(sd);
+	dev->run_mode = param->parm.capture.capturemode;
+
+	mutex_lock(&dev->input_lock);
+	switch (dev->run_mode) {
+	case CI_MODE_VIDEO:
+		ov5693_res = ov5693_res_video;
+		N_RES = N_RES_VIDEO;
+		break;
+	case CI_MODE_STILL_CAPTURE:
+		ov5693_res = ov5693_res_still;
+		N_RES = N_RES_STILL;
+		break;
+	default:
+		ov5693_res = ov5693_res_preview;
+		N_RES = N_RES_PREVIEW;
+	}
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+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_pad_config *cfg,
+				 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_pad_config *cfg,
+				  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_parm = ov5693_g_parm,
+	.s_parm = ov5693_s_parm,
+	.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 int 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);
+
+	return 0;
+}
+
+static int ov5693_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ov5693_device *dev;
+	int i2c;
+	int ret = 0;
+	void *pdata = client->dev.platform_data;
+	struct acpi_device *adev;
+	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, "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) {
+		dev_err(&client->dev, "out of memory\n");
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	dev->fmt_idx = 0;
+	v4l2_i2c_subdev_init(&(dev->sd), client, &ov5693_ops);
+
+	adev = ACPI_COMPANION(&client->dev);
+	if (adev) {
+		adev->power.flags.power_resources = 0;
+		pdata = gmin_camera_platform_data(&dev->sd,
+						  ATOMISP_INPUT_FORMAT_RAW_10,
+						  atomisp_bayer_order_bggr);
+	}
+
+	if (!pdata)
+		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;
+}
+
+MODULE_DEVICE_TABLE(i2c, ov5693_id);
+
+static struct acpi_device_id ov5693_acpi_match[] = {
+	{"INT33BE"},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, ov5693_acpi_match);
+
+static struct i2c_driver ov5693_driver = {
+	.driver = {
+		.name = OV5693_NAME,
+		.acpi_match_table = ACPI_PTR(ov5693_acpi_match),
+	},
+	.probe = ov5693_probe,
+	.remove = ov5693_remove,
+	.id_table = ov5693_id,
+};
+
+static int init_ov5693(void)
+{
+	return i2c_add_driver(&ov5693_driver);
+}
+
+static void exit_ov5693(void)
+{
+
+	i2c_del_driver(&ov5693_driver);
+}
+
+module_init(init_ov5693);
+module_exit(exit_ov5693);
+
+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 000000000000..d88ac1777d86
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov5693/ov5693.h
@@ -0,0 +1,1381 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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"
+
+#define OV5693_NAME		"ov5693"
+
+#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;
+	struct timespec 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 const struct i2c_device_id ov5693_id[] = {
+	{OV5693_NAME, 0},
+	{}
+};
+
+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}
+};
+
+/*
+ * 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}
+};
+
+/*
+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)
+ */
+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}
+};
+
+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/Binnning 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)
+ */
+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}
+};
+
+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
+ */
+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}
+};
+
+/*
+ * 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
+ */
+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}
+};
+
+/*
+ * 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}
+};
+
+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))
+
+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))
+
+static struct ov5693_resolution *ov5693_res = ov5693_res_preview;
+static int N_RES = N_RES_PREVIEW;
+#endif
diff --git a/drivers/staging/media/atomisp/i2c/ov8858.c b/drivers/staging/media/atomisp/i2c/ov8858.c
new file mode 100644
index 000000000000..9574bc49113c
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov8858.c
@@ -0,0 +1,2221 @@
+/*
+ * Support for OmniVision ov8858 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <media/v4l2-device.h>
+#include <linux/acpi.h>
+#include "../include/linux/atomisp_gmin_platform.h"
+#ifdef CONFIG_PLATFORM_BTNS
+#include "ov8858_btns.h"
+#else
+#include "ov8858.h"
+#endif
+static int ov8858_i2c_read(struct i2c_client *client, u16 len, u16 addr,
+			   u8 *buf)
+{
+	struct i2c_msg msg[2];
+	u8 address[2];
+	int err;
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no adapter\n", __func__);
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "%s: len = %d, addr = 0x%04x\n",
+		__func__, len, addr);
+
+	memset(msg, 0, sizeof(msg));
+
+	address[0] = (addr >> 8) & 0xff;
+	address[1] = addr & 0xff;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = I2C_MSG_LENGTH;
+	msg[0].buf = address;
+
+	msg[1].addr = client->addr;
+	msg[1].len = len;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = buf;
+
+	err = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (err != 2) {
+		if (err >= 0)
+			err = -EIO;
+		goto error;
+	}
+
+	return 0;
+error:
+	dev_err(&client->dev, "reading from address 0x%x error %d", addr, err);
+	return err;
+}
+
+static int ov8858_read_reg(struct i2c_client *client, u16 type, u16 reg,
+			   u16 *val)
+{
+	u8 data[OV8858_SHORT_MAX];
+	int err;
+
+	dev_dbg(&client->dev, "%s: type = %d, reg = 0x%04x\n",
+		__func__, type, reg);
+
+	/* read only 8 and 16 bit values */
+	if (type != OV8858_8BIT && type != OV8858_16BIT) {
+		dev_err(&client->dev, "%s error, invalid data length\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	memset(data, 0, sizeof(data));
+
+	err = ov8858_i2c_read(client, type, reg, data);
+	if (err)
+		goto error;
+
+	/* high byte comes first */
+	if (type == OV8858_8BIT)
+		*val = (u8)data[0];
+	else
+		*val = data[0] << 8 | data[1];
+
+	dev_dbg(&client->dev, "%s: val = 0x%04x\n", __func__, *val);
+
+	return 0;
+
+error:
+	dev_err(&client->dev, "read from offset 0x%x error %d", reg, err);
+	return err;
+}
+
+static int ov8858_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
+ov8858_write_reg(struct i2c_client *client, u16 data_length, u16 reg, u16 val)
+{
+	int ret;
+	unsigned char data[4] = {0};
+	u16 *wreg;
+	const u16 len = data_length + sizeof(u16); /* 16-bit address + data */
+
+	dev_dbg(&client->dev,
+		"%s: data_length = %d, reg = 0x%04x, val = 0x%04x\n",
+		__func__, data_length, reg, val);
+
+	if (!client->adapter) {
+		dev_err(&client->dev, "%s error, no adapter\n", __func__);
+		return -ENODEV;
+	}
+
+	if (data_length != OV8858_8BIT && data_length != OV8858_16BIT) {
+		dev_err(&client->dev, "%s error, invalid length\n", __func__);
+		return -EINVAL;
+	}
+
+	/* high byte goes out first */
+	wreg = (u16 *)data;
+	*wreg = cpu_to_be16(reg);
+
+	if (data_length == OV8858_8BIT) {
+		data[2] = (u8)(val);
+	} else {
+		/* OV8858_16BIT */
+		u16 *wdata = (u16 *)&data[2];
+		*wdata = be16_to_cpu(val);
+	}
+
+	ret = ov8858_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;
+}
+
+/*
+ * ov8858_write_reg_array - Initializes a list of 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().
+ *
+ * __ov8858_flush_reg_array(), __ov8858_buf_reg_array() and
+ * __ov8858_write_reg_is_consecutive() are internal functions to
+ * ov8858_write_reg_array() and should be not used anywhere else.
+ *
+ */
+static int __ov8858_flush_reg_array(struct i2c_client *client,
+				    struct ov8858_write_ctrl *ctrl)
+{
+	u16 size;
+	if (ctrl->index == 0)
+		return 0;
+
+	size = sizeof(u16) + ctrl->index; /* 16-bit address + data */
+	ctrl->buffer.addr = cpu_to_be16(ctrl->buffer.addr);
+	ctrl->index = 0;
+
+	return ov8858_i2c_write(client, size, (u8 *)&ctrl->buffer);
+}
+
+static int __ov8858_buf_reg_array(struct i2c_client *client,
+				  struct ov8858_write_ctrl *ctrl,
+				  const struct ov8858_reg *next)
+{
+	int size;
+	u16 *data16;
+
+	switch (next->type) {
+	case OV8858_8BIT:
+		size = 1;
+		ctrl->buffer.data[ctrl->index] = (u8)next->val;
+		break;
+	case OV8858_16BIT:
+		size = 2;
+		data16 = (u16 *)&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->sreg;
+
+	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) >= OV8858_MAX_WRITE_BUF_SIZE)
+		__ov8858_flush_reg_array(client, ctrl);
+
+	return 0;
+}
+
+static int
+__ov8858_write_reg_is_consecutive(struct i2c_client *client,
+				  struct ov8858_write_ctrl *ctrl,
+				  const struct ov8858_reg *next)
+{
+	if (ctrl->index == 0)
+		return 1;
+
+	return ctrl->buffer.addr + ctrl->index == next->sreg;
+}
+
+static int ov8858_write_reg_array(struct i2c_client *client,
+				  const struct ov8858_reg *reglist)
+{
+	const struct ov8858_reg *next = reglist;
+	struct ov8858_write_ctrl ctrl;
+	int err;
+
+	ctrl.index = 0;
+	for (; next->type != OV8858_TOK_TERM; next++) {
+		switch (next->type & OV8858_TOK_MASK) {
+		case OV8858_TOK_DELAY:
+			err = __ov8858_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 proceeding
+			 */
+			if (!__ov8858_write_reg_is_consecutive(client,
+							       &ctrl, next)) {
+				err = __ov8858_flush_reg_array(client, &ctrl);
+				if (err)
+					return err;
+			}
+			err = __ov8858_buf_reg_array(client, &ctrl, next);
+			if (err) {
+				dev_err(&client->dev, "%s: write error\n",
+					__func__);
+				return err;
+			}
+			break;
+		}
+	}
+
+	return __ov8858_flush_reg_array(client, &ctrl);
+}
+
+static int __ov8858_min_fps_diff(int fps,
+				 const struct ov8858_fps_setting *fps_list)
+{
+	int diff = INT_MAX;
+	int i;
+
+	if (fps == 0)
+		return 0;
+
+	for (i = 0; i < MAX_FPS_OPTIONS_SUPPORTED; i++) {
+		if (!fps_list[i].fps)
+			break;
+		if (abs(fps_list[i].fps - fps) < diff)
+			diff = abs(fps_list[i].fps - fps);
+	}
+
+	return diff;
+}
+
+static int __ov8858_nearest_fps_index(int fps,
+				      const struct ov8858_fps_setting *fps_list)
+{
+	int fps_index = 0;
+	int i;
+
+	for (i = 0; i < MAX_FPS_OPTIONS_SUPPORTED; i++) {
+		if (!fps_list[i].fps)
+			break;
+		if (abs(fps_list[i].fps - fps)
+		    < abs(fps_list[fps_index].fps - fps))
+			fps_index = i;
+	}
+	return fps_index;
+}
+
+static int __ov8858_update_frame_timing(struct v4l2_subdev *sd,
+					u16 *hts, u16 *vts)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+
+
+	dev_dbg(&client->dev, "%s OV8858_TIMING_HTS=0x%04x\n",
+		__func__, *hts);
+
+	/* HTS = pixel_per_line / 2 */
+	ret = ov8858_write_reg(client, OV8858_16BIT,
+				OV8858_TIMING_HTS, *hts >> 1);
+	if (ret)
+		return ret;
+	dev_dbg(&client->dev, "%s OV8858_TIMING_VTS=0x%04x\n",
+		__func__, *vts);
+
+	return ov8858_write_reg(client, OV8858_16BIT, OV8858_TIMING_VTS, *vts);
+}
+
+static int __ov8858_set_exposure(struct v4l2_subdev *sd, int exposure, int gain,
+				 int dig_gain, u16 *hts, u16 *vts)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int exp_val, ret;
+	dev_dbg(&client->dev, "%s, exposure = %d, gain=%d, dig_gain=%d\n",
+		__func__, exposure, gain, dig_gain);
+
+	if (dev->limit_exposure_flag) {
+		if (exposure > *vts - OV8858_INTEGRATION_TIME_MARGIN)
+			exposure = *vts - OV8858_INTEGRATION_TIME_MARGIN;
+	} else {
+		if (*vts < exposure + OV8858_INTEGRATION_TIME_MARGIN)
+			*vts = (u16) exposure + OV8858_INTEGRATION_TIME_MARGIN;
+	}
+
+	ret = __ov8858_update_frame_timing(sd, hts, vts);
+	if (ret)
+		return ret;
+
+	/* For ov8858, the low 4 bits are fraction bits and must be kept 0 */
+	exp_val = exposure << 4;
+	ret = ov8858_write_reg(client, OV8858_8BIT,
+			       OV8858_LONG_EXPO+2, exp_val & 0xFF);
+	if (ret)
+		return ret;
+
+	ret = ov8858_write_reg(client, OV8858_8BIT,
+			       OV8858_LONG_EXPO+1, (exp_val >> 8) & 0xFF);
+	if (ret)
+		return ret;
+
+	ret = ov8858_write_reg(client, OV8858_8BIT,
+			       OV8858_LONG_EXPO, (exp_val >> 16) & 0x0F);
+	if (ret)
+		return ret;
+
+	/* Digital gain : to all MWB channel gains */
+	if (dig_gain) {
+		ret = ov8858_write_reg(client, OV8858_16BIT,
+				OV8858_MWB_RED_GAIN_H, dig_gain);
+		if (ret)
+			return ret;
+
+		ret = ov8858_write_reg(client, OV8858_16BIT,
+				OV8858_MWB_GREEN_GAIN_H, dig_gain);
+		if (ret)
+			return ret;
+
+		ret = ov8858_write_reg(client, OV8858_16BIT,
+				OV8858_MWB_BLUE_GAIN_H, dig_gain);
+		if (ret)
+			return ret;
+	}
+
+	ret = ov8858_write_reg(client, OV8858_16BIT, OV8858_LONG_GAIN,
+				gain & 0x07ff);
+	if (ret)
+		return ret;
+
+	dev->gain = gain;
+	dev->exposure = exposure;
+	dev->digital_gain = dig_gain;
+
+	return 0;
+}
+
+static int ov8858_set_exposure(struct v4l2_subdev *sd, int exposure, int gain,
+				int dig_gain)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	const struct ov8858_resolution *res;
+	u16 hts, vts;
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+
+	/* Validate exposure:  cannot exceed 16bit value */
+	exposure = clamp_t(int, exposure, 0, OV8858_MAX_EXPOSURE_VALUE);
+
+	/* Validate gain: must not exceed maximum 8bit value */
+	gain = clamp_t(int, gain, 0, OV8858_MAX_GAIN_VALUE);
+
+	/* Validate digital gain: must not exceed 12 bit value*/
+	dig_gain = clamp_t(int, dig_gain, 0, OV8858_MWB_GAIN_MAX);
+
+	res = &dev->curr_res_table[dev->fmt_idx];
+	/*
+	 * Vendor: HTS reg value is half the total pixel line
+	 */
+	hts = res->fps_options[dev->fps_index].pixels_per_line;
+	vts = res->fps_options[dev->fps_index].lines_per_frame;
+
+	ret = __ov8858_set_exposure(sd, exposure, gain, dig_gain, &hts, &vts);
+
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+/*
+   When exposure gain value set to sensor, the sensor changed value.
+   So we need the function to get real value
+ */
+static int ov8858_g_update_exposure(struct v4l2_subdev *sd,
+				struct atomisp_update_exposure *exposure)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int gain = exposure->gain;
+
+	dev_dbg(&client->dev, "%s: gain: %d, digi_gain: %d\n", __func__,
+			exposure->gain, exposure->digi_gain);
+	exposure->update_digi_gain = dev->digital_gain;
+	/* This real gain value fetching function is provided by vendor */
+	exposure->update_gain = (((gain & 0x700) >> 8) + 1) * (gain & 0xFF);
+
+	return 0;
+}
+
+static int ov8858_s_exposure(struct v4l2_subdev *sd,
+			     struct atomisp_exposure *exposure)
+{
+	return ov8858_set_exposure(sd, exposure->integration_time[0],
+				exposure->gain[0], exposure->gain[1]);
+}
+
+static int ov8858_priv_int_data_init(struct v4l2_subdev *sd)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 size = OV8858_OTP_END_ADDR - OV8858_OTP_START_ADDR + 1;
+	int r;
+	u16 isp_ctrl2 = 0;
+
+	if (!dev->otp_data) {
+		dev->otp_data = devm_kzalloc(&client->dev, size, GFP_KERNEL);
+		if (!dev->otp_data) {
+			dev_err(&client->dev, "%s: can't allocate memory",
+				__func__);
+			r = -ENOMEM;
+			goto error3;
+		}
+
+		/* Streaming has to be on */
+		r = ov8858_write_reg(client, OV8858_8BIT, OV8858_STREAM_MODE,
+				     0x01);
+		if (r)
+			goto error2;
+
+		/* Turn off Dead Pixel Correction */
+		r = ov8858_read_reg(client, OV8858_8BIT,
+				    OV8858_OTP_ISP_CTRL2, &isp_ctrl2);
+		if (r)
+			goto error1;
+
+		r = ov8858_write_reg(client, OV8858_8BIT, OV8858_OTP_ISP_CTRL2,
+				     isp_ctrl2 & ~OV8858_OTP_DPC_ENABLE);
+		if (r)
+			goto error1;
+
+		/* Enable partial OTP read mode */
+		r = ov8858_write_reg(client, OV8858_8BIT, OV8858_OTP_MODE_CTRL,
+				     OV8858_OTP_MODE_PROGRAM_DISABLE |
+				     OV8858_OTP_MODE_MANUAL);
+		if (r)
+			goto error1;
+
+		/* Set address range of OTP memory to read */
+		r = ov8858_write_reg(client, OV8858_16BIT,
+				     OV8858_OTP_START_ADDR_REG,
+				     OV8858_OTP_START_ADDR);
+		if (r)
+			goto error1;
+
+		r = ov8858_write_reg(client, OV8858_16BIT,
+				     OV8858_OTP_END_ADDR_REG,
+				     OV8858_OTP_END_ADDR);
+		if (r)
+			goto error1;
+
+		/* Load the OTP data into the OTP buffer */
+		r = ov8858_write_reg(client, OV8858_8BIT, OV8858_OTP_LOAD_CTRL,
+				     OV8858_OTP_LOAD_ENABLE);
+		if (r)
+			goto error1;
+
+		/* Wait for the data to load into the buffer */
+		usleep_range(5000, 5500);
+
+		/* Read the OTP data from the buffer */
+		r = ov8858_i2c_read(client, size, OV8858_OTP_START_ADDR,
+				    dev->otp_data);
+		if (r)
+			goto error1;
+
+		/* Turn on Dead Pixel Correction */
+		r = ov8858_write_reg(client, OV8858_8BIT, OV8858_OTP_ISP_CTRL2,
+				     isp_ctrl2 | OV8858_OTP_DPC_ENABLE);
+		if (r)
+			goto error1;
+
+		/* Stop streaming */
+		r = ov8858_write_reg(client, 1, OV8858_STREAM_MODE, 0x00);
+		if (r) {
+			dev_err(&client->dev, "%s: cannot turn off streaming\n",
+				__func__);
+			goto error1;
+		}
+	}
+
+
+	return 0;
+
+error1:
+	/* Turn on Dead Pixel Correction and set streaming off */
+	ov8858_write_reg(client, OV8858_8BIT, OV8858_OTP_ISP_CTRL2,
+			     isp_ctrl2 | OV8858_OTP_DPC_ENABLE);
+	ov8858_write_reg(client, 1, OV8858_STREAM_MODE, 0x00);
+error2:
+	devm_kfree(&client->dev, dev->otp_data);
+	dev->otp_data = NULL;
+error3:
+	dev_err(&client->dev, "%s: OTP reading failed\n", __func__);
+	return r;
+}
+
+static int ov8858_g_priv_int_data(struct v4l2_subdev *sd,
+				  struct v4l2_private_int_data *priv)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u32 size = OV8858_OTP_END_ADDR - OV8858_OTP_START_ADDR + 1;
+	int r;
+
+	mutex_lock(&dev->input_lock);
+
+	if (!dev->otp_data) {
+		dev_err(&client->dev, "%s: otp data is NULL\n", __func__);
+		mutex_unlock(&dev->input_lock);
+		return -EFAULT;
+	}
+
+	if (copy_to_user(priv->data, dev->otp_data,
+			 min_t(__u32, priv->size, size))) {
+		r = -EFAULT;
+		dev_err(&client->dev, "%s: OTP reading failed\n", __func__);
+		mutex_unlock(&dev->input_lock);
+		return r;
+	}
+
+	priv->size = size;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int __ov8858_init(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret;
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	/* Sets the default FPS */
+	dev->fps_index = 0;
+
+	/* Set default exposure values (initially start values) */
+	dev->exposure = 256;
+	dev->gain = 16;
+	dev->digital_gain = 1024;
+	dev->limit_exposure_flag = false;
+
+	dev_dbg(&client->dev, "%s: Writing basic settings to ov8858\n",
+		__func__);
+	ret = ov8858_write_reg_array(client, ov8858_BasicSettings);
+	if (ret)
+		return ret;
+
+	return ov8858_priv_int_data_init(sd);
+}
+
+static int ov8858_init(struct v4l2_subdev *sd, u32 val)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __ov8858_init(sd);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static void ov8858_uninit(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct v4l2_ctrl *ctrl;
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	dev->exposure = 0;
+	dev->gain     = 0;
+	dev->digital_gain = 0;
+	dev->limit_exposure_flag = false;
+	mutex_unlock(&dev->input_lock);
+	ctrl = v4l2_ctrl_find(sd->ctrl_handler,
+				V4L2_CID_EXPOSURE_AUTO_PRIORITY);
+	if (ctrl)
+		v4l2_ctrl_s_ctrl(ctrl, V4L2_EXPOSURE_AUTO);
+	mutex_lock(&dev->input_lock);
+}
+
+static int ov8858_g_comp_delay(struct v4l2_subdev *sd, unsigned int *usec)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret = 0, exposure;
+	u16 vts, data;
+
+	if (dev->exposure == 0) {
+		ret = ov8858_read_reg(client, OV8858_16BIT,
+				       OV8858_LONG_EXPO + 1, &data);
+		if (ret)
+			return ret;
+		exposure = data;
+		exposure >>= 4;
+	} else {
+		exposure = dev->exposure;
+	}
+
+	ret = ov8858_read_reg(client, OV8858_16BIT, OV8858_TIMING_VTS, &vts);
+	if (ret || vts == 0)
+		vts = OV8858_DEPTH_VTS_CONST;
+
+	*usec = (exposure * 33333 / vts);
+	if (*usec >  OV8858_DEPTH_COMP_CONST)
+		*usec = *usec  - OV8858_DEPTH_COMP_CONST;
+	else
+		*usec = OV8858_DEPTH_COMP_CONST;
+
+	return 0;
+}
+
+static long ov8858_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	switch (cmd) {
+	case ATOMISP_IOC_S_EXPOSURE:
+		return ov8858_s_exposure(sd, (struct atomisp_exposure *)arg);
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+		return ov8858_g_priv_int_data(sd, arg);
+	case ATOMISP_IOC_G_DEPTH_SYNC_COMP:
+		return ov8858_g_comp_delay(sd, (unsigned int *)arg);
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+		return ov8858_g_update_exposure(sd,
+				(struct atomisp_update_exposure *)arg);
+	default:
+		dev_dbg(&client->dev, "Unhandled command 0x%X\n", cmd);
+		return -EINVAL;
+	}
+}
+
+static int __power_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	int ret = 0;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	if (!dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->power_ctrl)
+		return dev->platform_data->power_ctrl(sd, flag);
+
+	if (dev->platform_data->v1p2_ctrl) {
+		ret = dev->platform_data->v1p2_ctrl(sd, flag);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to power %s 1.2v power rail\n",
+				flag ? "up" : "down");
+			return ret;
+		}
+	}
+
+	if (dev->platform_data->v2p8_ctrl) {
+		ret = dev->platform_data->v2p8_ctrl(sd, flag);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to power %s 2.8v power rail\n",
+				flag ? "up" : "down");
+			return ret;
+		}
+	}
+
+	if (dev->platform_data->v1p8_ctrl) {
+		ret = dev->platform_data->v1p8_ctrl(sd, flag);
+		if (ret) {
+			dev_err(&client->dev,
+				"failed to power %s 1.8v power rail\n",
+				flag ? "up" : "down");
+			if (dev->platform_data->v2p8_ctrl)
+				dev->platform_data->v2p8_ctrl(sd, 0);
+			return ret;
+		}
+	}
+
+	if (flag)
+		msleep(20); /* Wait for power lines to stabilize */
+	return ret;
+}
+
+static int __gpio_ctrl(struct v4l2_subdev *sd, bool flag)
+{
+	struct i2c_client *client;
+	struct ov8858_device *dev;
+
+	if (!sd)
+		return -EINVAL;
+
+	client = v4l2_get_subdevdata(sd);
+	dev = to_ov8858_sensor(sd);
+
+	if (!client || !dev || !dev->platform_data)
+		return -ENODEV;
+
+	/* Non-gmin platforms use the legacy callback */
+	if (dev->platform_data->gpio_ctrl)
+		return dev->platform_data->gpio_ctrl(sd, flag);
+
+	if (dev->platform_data->gpio0_ctrl)
+		return dev->platform_data->gpio0_ctrl(sd, flag);
+
+	dev_err(&client->dev, "failed to find platform gpio callback\n");
+
+	return -EINVAL;
+}
+
+static int power_up(struct v4l2_subdev *sd)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret;
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	/* Enable power */
+	ret = __power_ctrl(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "power rail on failed %d.\n", ret);
+		goto fail_power;
+	}
+
+	/* Enable clock */
+	ret = dev->platform_data->flisclk_ctrl(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "flisclk on failed %d\n", ret);
+		goto fail_clk;
+	}
+
+	/* Release reset */
+	ret = __gpio_ctrl(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "gpio on failed %d\n", ret);
+		goto fail_gpio;
+	}
+
+	/* Minumum delay is 8192 clock cycles before first i2c transaction,
+	 * which is 1.37 ms at the lowest allowed clock rate 6 MHz */
+	usleep_range(2000, 2500);
+	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 ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	ret = dev->platform_data->flisclk_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "flisclk off failed\n");
+
+	ret = __gpio_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "gpio off failed\n");
+
+	ret = __power_ctrl(sd, 0);
+	if (ret)
+		dev_err(&client->dev, "power rail off failed.\n");
+
+	return ret;
+}
+
+static int __ov8858_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret, r = 0;
+
+	if (on == 0) {
+		ov8858_uninit(sd);
+		if (dev->vcm_driver && dev->vcm_driver->power_down)
+			r = dev->vcm_driver->power_down(sd);
+		ret = power_down(sd);
+		if (r != 0 && ret == 0)
+			ret = r;
+	} else {
+		ret = power_up(sd);
+		if (ret)
+			power_down(sd);
+		if (dev->vcm_driver && dev->vcm_driver->power_up) {
+			ret = dev->vcm_driver->power_up(sd);
+			if (ret) {
+				power_down(sd);
+				return ret;
+			}
+		}
+		return __ov8858_init(sd);
+	}
+
+	return ret;
+}
+
+static int ov8858_s_power(struct v4l2_subdev *sd, int on)
+{
+	int ret;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	ret = __ov8858_s_power(sd, on);
+	mutex_unlock(&dev->input_lock);
+
+	/*
+	 * FIXME: Compatibility with old behaviour: return to preview
+	 * when the device is power cycled.
+	 */
+	if (!ret && on)
+		v4l2_ctrl_s_ctrl(dev->run_mode, ATOMISP_RUN_MODE_PREVIEW);
+
+	return ret;
+}
+
+/*
+ * Return value of the specified register, first try getting it from
+ * the register list and if not found, get from the sensor via i2c.
+ */
+static int ov8858_get_register(struct v4l2_subdev *sd, int reg, int type,
+			       const struct ov8858_reg *reglist)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct ov8858_reg *next;
+	u16 val;
+
+	/* Try if the values are in the register list */
+	for (next = reglist; next->type != OV8858_TOK_TERM; next++) {
+		if (next->sreg == reg) {
+			if (type == OV8858_8BIT)
+				return next->val;
+
+			if (type == OV8858_16BIT &&
+			    next[1].type != OV8858_TOK_TERM)
+				return next[0].val << 8 | next[1].val;
+		}
+	}
+
+	/* If not, read from sensor */
+	if (ov8858_read_reg(client, type, reg, &val)) {
+		dev_err(&client->dev, "failed to read register 0x%08x\n", reg);
+		return -EIO;
+	}
+
+	return val;
+}
+
+static inline int ov8858_get_register_16bit(struct v4l2_subdev *sd, int reg,
+					    const struct ov8858_reg *reglist)
+{
+	return ov8858_get_register(sd, reg, OV8858_16BIT, reglist);
+}
+
+static inline int ov8858_get_register_8bit(struct v4l2_subdev *sd, int reg,
+					   const struct ov8858_reg *reglist)
+{
+	return ov8858_get_register(sd, reg, OV8858_8BIT, reglist);
+}
+
+static int __ov8858_get_pll1_values(struct v4l2_subdev *sd,
+				    int *value,
+				    const struct ov8858_reg *reglist)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int prediv_idx;
+	unsigned int multiplier;
+	unsigned int sys_prediv;
+	unsigned int prediv_coef[] = {2, 3, 4, 5, 6, 8, 12, 16};
+	int ret;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL1_PREDIV0, reglist);
+	if (ret < 0)
+		return ret;
+
+	if (ret & OV8858_PLL1_PREDIV0_MASK)
+		*value /= 2;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL1_PREDIV, reglist);
+
+	if (ret < 0)
+		return ret;
+
+	prediv_idx = ret & OV8858_PLL1_PREDIV_MASK;
+	*value = *value * 2 / prediv_coef[prediv_idx];
+
+	ret = ov8858_get_register_16bit(sd, OV8858_PLL1_MULTIPLIER, reglist);
+	if (ret < 0)
+		return ret;
+
+	multiplier = ret;
+	*value *= multiplier & OV8858_PLL1_MULTIPLIER_MASK;
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL1_SYS_PRE_DIV, reglist);
+
+	if (ret < 0)
+		return ret;
+
+	sys_prediv = ret & OV8858_PLL1_SYS_PRE_DIV_MASK;
+	*value /= (sys_prediv + 3);
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL1_SYS_DIVIDER, reglist);
+
+	if (ret < 0)
+		return ret;
+
+	if (ret & OV8858_PLL1_SYS_DIVIDER_MASK)
+		*value /= 2;
+
+	dev_dbg(&client->dev, "%s: *value: %d\n", __func__, *value);
+
+	return 0;
+}
+
+static int __ov8858_get_pll2a_values(struct v4l2_subdev *sd, int *value,
+				     const struct ov8858_reg *reglist)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int prediv_idx;
+	unsigned int multiplier;
+	unsigned int prediv_coef[] = {2, 3, 4, 5, 6, 8, 12, 16};
+	int ret;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL2_PREDIV0, reglist);
+	if (ret < 0)
+		return ret;
+
+	if (ret & OV8858_PLL2_PREDIV0_MASK)
+		*value /= 2;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL2_PREDIV, reglist);
+	if (ret < 0)
+		return ret;
+
+	prediv_idx = (ret & OV8858_PLL2_PREDIV_MASK);
+	*value = *value * 2 / prediv_coef[prediv_idx];
+
+	ret = ov8858_get_register_16bit(sd, OV8858_PLL2_MULTIPLIER, reglist);
+	if (ret < 0)
+		return ret;
+
+	multiplier = ret;
+	*value *= multiplier & OV8858_PLL2_MULTIPLIER_MASK;
+	dev_dbg(&client->dev, "%s: *value: %d\n", __func__, *value);
+
+	return 0;
+}
+static int __ov8858_get_pll2b_values(struct v4l2_subdev *sd, int *value,
+				     const struct ov8858_reg *reglist)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int dac_divider;
+	int ret;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL2_DAC_DIVIDER, reglist);
+	if (ret < 0)
+		return ret;
+
+	dac_divider = (ret & OV8858_PLL2_DAC_DIVIDER_MASK) + 1;
+	*value /= dac_divider;
+
+	dev_dbg(&client->dev, "%s: *value: %d\n", __func__, *value);
+
+	return 0;
+}
+static int __ov8858_get_pll2c_values(struct v4l2_subdev *sd, int *value,
+				     const struct ov8858_reg *reglist)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	unsigned int sys_pre_div;
+	unsigned int sys_divider_idx;
+	unsigned int sys_divider_coef[] = {2, 3, 4, 5, 6, 7, 8, 10};
+	int ret;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL2_SYS_PRE_DIV, reglist);
+	if (ret < 0)
+		return ret;
+
+	sys_pre_div = (ret & OV8858_PLL2_SYS_PRE_DIV_MASK) + 1;
+	*value /= sys_pre_div;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL2_SYS_DIVIDER, reglist);
+	if (ret < 0)
+		return ret;
+
+	sys_divider_idx = ret & OV8858_PLL2_SYS_DIVIDER_MASK;
+	*value *= 2 /  sys_divider_coef[sys_divider_idx];
+
+	dev_dbg(&client->dev, "%s: *value: %d\n", __func__, *value);
+
+	return 0;
+}
+
+static int ov8858_get_intg_factor(struct v4l2_subdev *sd,
+				  struct camera_mipi_info *info,
+				  const struct ov8858_reg *reglist)
+{
+	const unsigned int ext_clk = 19200000; /* Hz */
+	struct atomisp_sensor_mode_data *m = &info->data;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *d = &client->dev;
+	const struct ov8858_resolution *res =
+				&dev->curr_res_table[dev->fmt_idx];
+	unsigned int pll_sclksel1;
+	unsigned int pll_sclksel2;
+	unsigned int sys_pre_div;
+	unsigned int sclk_pdiv;
+	unsigned int sclk = ext_clk;
+	u16 hts;
+	int ret;
+
+	memset(&info->data, 0, sizeof(info->data));
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL_SCLKSEL1, reglist);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(d, "%s: OV8858_PLL_SCLKSEL1: 0x%02x\n", __func__, ret);
+	pll_sclksel1 = ret & OV8858_PLL_SCLKSEL1_MASK;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_PLL_SCLKSEL2, reglist);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(d, "%s: OV8858_PLL_SCLKSEL2: 0x%02x\n", __func__, ret);
+	pll_sclksel2 = ret & OV8858_PLL_SCLKSEL2_MASK;
+
+	if (pll_sclksel2) {
+		ret = __ov8858_get_pll2a_values(sd, &sclk, reglist);
+		if (ret < 0)
+			return ret;
+		ret = __ov8858_get_pll2b_values(sd, &sclk, reglist);
+		if (ret < 0)
+			return ret;
+	} else if (pll_sclksel1) {
+		ret = __ov8858_get_pll2a_values(sd, &sclk, reglist);
+		if (ret < 0)
+			return ret;
+		ret = __ov8858_get_pll2c_values(sd, &sclk, reglist);
+		if (ret < 0)
+			return ret;
+	} else {
+		ret = __ov8858_get_pll1_values(sd, &sclk, reglist);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ov8858_get_register_8bit(sd, OV8858_SRB_HOST_INPUT_DIS, reglist);
+	if (ret < 0)
+		return ret;
+
+	dev_dbg(d, "%s: OV8858_SRB_HOST_INPUT_DIS: 0x%02x\n", __func__, ret);
+
+	sys_pre_div = ret & OV8858_SYS_PRE_DIV_MASK;
+	sys_pre_div >>= OV8858_SYS_PRE_DIV_OFFSET;
+
+	if (sys_pre_div == 1)
+		sclk /= 2;
+	else if (sys_pre_div == 2)
+		sclk /= 4;
+
+	sclk_pdiv = ret & OV8858_SCLK_PDIV_MASK;
+	sclk_pdiv >>= OV8858_SCLK_PDIV_OFFSET;
+
+	if (sclk_pdiv > 1)
+		sclk /= sclk_pdiv;
+
+	dev_dbg(d, "%s: sclk: %d\n", __func__, sclk);
+
+	dev->vt_pix_clk_freq_mhz = sclk;
+	m->vt_pix_clk_freq_mhz = sclk;
+
+	/* HTS and VTS */
+	m->frame_length_lines =
+			res->fps_options[dev->fps_index].lines_per_frame;
+	m->line_length_pck = res->fps_options[dev->fps_index].pixels_per_line;
+
+	m->coarse_integration_time_min = 0;
+	m->coarse_integration_time_max_margin = OV8858_INTEGRATION_TIME_MARGIN;
+	ret = ov8858_read_reg(client, OV8858_16BIT, OV8858_TIMING_HTS, &hts);
+	if (ret < 0)
+		return ret;
+	m->hts = hts;
+	dev_dbg(&client->dev, "%s: get HTS %d\n", __func__, hts);
+
+	/* OV Sensor do not use fine integration time. */
+	m->fine_integration_time_min = 0;
+	m->fine_integration_time_max_margin = 0;
+
+	/*
+	 * read_mode indicate whether binning is used for calculating
+	 * the correct exposure value from the user side. So adapt the
+	 * read mode values accordingly.
+	 */
+	m->read_mode = res->bin_factor_x ?
+		OV8858_READ_MODE_BINNING_ON : OV8858_READ_MODE_BINNING_OFF;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_H_INC_ODD, res->regs);
+	if (ret < 0)
+		return ret;
+	m->binning_factor_x = (ret + 1) / 2;
+
+	ret = ov8858_get_register_8bit(sd, OV8858_V_INC_ODD, res->regs);
+	if (ret < 0)
+		return ret;
+	m->binning_factor_y = (ret + 1) / 2;
+
+	/* Get the cropping and output resolution to ISP for this mode. */
+	ret =  ov8858_get_register_16bit(sd, OV8858_HORIZONTAL_START_H,
+					 res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->crop_horizontal_start = ret;
+
+	ret = ov8858_get_register_16bit(sd, OV8858_VERTICAL_START_H, res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->crop_vertical_start = ret;
+
+	ret = ov8858_get_register_16bit(sd, OV8858_HORIZONTAL_END_H, res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->crop_horizontal_end = ret;
+
+	ret = ov8858_get_register_16bit(sd, OV8858_VERTICAL_END_H, res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->crop_vertical_end = ret;
+
+	ret = ov8858_get_register_16bit(sd, OV8858_HORIZONTAL_OUTPUT_SIZE_H,
+					res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->output_width = ret;
+
+	ret = ov8858_get_register_16bit(sd, OV8858_VERTICAL_OUTPUT_SIZE_H,
+					res->regs);
+	if (ret < 0)
+		return ret;
+
+	m->output_height = ret;
+
+	return 0;
+}
+
+/*
+ * 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 ov8858_resolution const *res, const u32 w,
+		    const 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;
+}
+
+/*
+ * Returns the nearest higher resolution index.
+ * @w: width
+ * @h: height
+ * matching is done based on enveloping resolution and
+ * aspect ratio. If the aspect ratio cannot be matched
+ * to any index, -1 is returned.
+ */
+static int nearest_resolution_index(struct v4l2_subdev *sd, int w, int h)
+{
+	int i;
+	int idx = -1;
+	int dist;
+	int fps_diff;
+	int min_fps_diff = INT_MAX;
+	int min_dist = INT_MAX;
+	int min_res_w = INT_MAX;
+	const struct ov8858_resolution *tmp_res = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	dev_dbg(&client->dev, "%s: w=%d, h=%d\n", __func__, w, h);
+
+	for (i = 0; i < dev->entries_curr_table; i++) {
+		tmp_res = &dev->curr_res_table[i];
+		dist = distance(tmp_res, w, h);
+		dev_dbg(&client->dev,
+			"%s[%d]: %dx%d distance=%d\n", tmp_res->desc,
+			i, tmp_res->width, tmp_res->height, dist);
+		if (dist == -1)
+			continue;
+		if (dist < min_dist) {
+			min_dist = dist;
+			min_res_w = tmp_res->width;
+			min_fps_diff = __ov8858_min_fps_diff(dev->fps,
+						tmp_res->fps_options);
+			idx = i;
+		}
+		if (dist == min_dist) {
+			fps_diff = __ov8858_min_fps_diff(dev->fps,
+						tmp_res->fps_options);
+			if (fps_diff < min_fps_diff) {
+				min_fps_diff = fps_diff;
+				idx = i;
+			}
+			if (tmp_res->width < min_res_w) {
+				min_res_w = tmp_res->width;
+				idx = i;
+			}
+		}
+	}
+
+	return idx;
+}
+
+static int ov8858_set_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct camera_mipi_info *ov8858_info = NULL;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct ov8858_resolution *res;
+	int ret;
+	int idx;
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	ov8858_info = v4l2_get_subdev_hostdata(sd);
+	if (ov8858_info == NULL)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+
+	if ((fmt->width > OV8858_RES_WIDTH_MAX) ||
+	    (fmt->height > OV8858_RES_HEIGHT_MAX)) {
+		fmt->width = OV8858_RES_WIDTH_MAX;
+		fmt->height = OV8858_RES_HEIGHT_MAX;
+	} else {
+		idx = nearest_resolution_index(sd, fmt->width, fmt->height);
+
+		/*
+		 * nearest_resolution_index() doesn't return smaller
+		 * resolutions. If it fails, it means the requested resolution
+		 * is higher than we can support. Fallback to highest possible
+		 * resolution in this case.
+		 */
+		if (idx == -1)
+			idx = dev->entries_curr_table - 1;
+
+		fmt->width = dev->curr_res_table[idx].width;
+		fmt->height = dev->curr_res_table[idx].height;
+	}
+
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+		cfg->try_fmt = *fmt;
+		mutex_unlock(&dev->input_lock);
+		return 0;
+	}
+
+	dev->fmt_idx = nearest_resolution_index(sd, fmt->width, fmt->height);
+	if (dev->fmt_idx == -1) {
+		ret = -EINVAL;
+		goto out;
+	}
+	res = &dev->curr_res_table[dev->fmt_idx];
+	dev_dbg(&client->dev, "%s: selected width = %d, height = %d\n",
+		__func__, res->width, res->height);
+
+	/* Adjust the FPS selection based on the resolution selected */
+	dev->fps_index = __ov8858_nearest_fps_index(dev->fps, res->fps_options);
+	dev->fps = res->fps_options[dev->fps_index].fps;
+	dev->regs = res->fps_options[dev->fps_index].regs;
+	if (!dev->regs)
+		dev->regs = res->regs;
+
+	ret = ov8858_write_reg_array(client, dev->regs);
+	if (ret)
+		goto out;
+
+	dev->pixels_per_line = res->fps_options[dev->fps_index].pixels_per_line;
+	dev->lines_per_frame = res->fps_options[dev->fps_index].lines_per_frame;
+
+	/* ov8858 only support RGB RAW10 output */
+	ov8858_info->metadata_width = res->width * 10 / 8;
+	ov8858_info->metadata_height = 2;
+	ov8858_info->metadata_format = ATOMISP_INPUT_FORMAT_EMBEDDED;
+
+	/* Set the initial exposure */
+	ret = __ov8858_set_exposure(sd, dev->exposure, dev->gain,
+				    dev->digital_gain, &dev->pixels_per_line,
+				    &dev->lines_per_frame);
+	if (ret)
+		goto out;
+
+	ret = ov8858_get_intg_factor(sd, ov8858_info, dev->regs);
+
+out:
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ov8858_get_fmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+
+	if (format->pad)
+		return -EINVAL;
+	if (!fmt)
+		return -EINVAL;
+
+	mutex_lock(&dev->input_lock);
+	fmt->width = dev->curr_res_table[dev->fmt_idx].width;
+	fmt->height = dev->curr_res_table[dev->fmt_idx].height;
+	fmt->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int ov8858_detect(struct i2c_client *client, u16 *id)
+{
+	struct i2c_adapter *adapter = client->adapter;
+	u16 id_hi = 0;
+	u16 id_low = 0;
+	int ret;
+
+	/* i2c check */
+	if (!i2c_check_functionality(adapter, I2C_FUNC_I2C))
+		return -ENODEV;
+
+	dev_dbg(&client->dev, "%s: I2C functionality ok\n", __func__);
+	ret = ov8858_read_reg(client, OV8858_8BIT, OV8858_CHIP_ID_HIGH, &id_hi);
+	if (ret)
+		return ret;
+	dev_dbg(&client->dev, "%s: id_high = 0x%04x\n", __func__, id_hi);
+	ret = ov8858_read_reg(client, OV8858_8BIT, OV8858_CHIP_ID_LOW, &id_low);
+	if (ret)
+		return ret;
+	dev_dbg(&client->dev, "%s: id_low = 0x%04x\n", __func__, id_low);
+	*id = (id_hi << 8) | id_low;
+
+	dev_dbg(&client->dev, "%s: chip_id = 0x%04x\n", __func__, *id);
+
+	dev_info(&client->dev, "%s: chip_id = 0x%04x\n", __func__, *id);
+	if (*id != OV8858_CHIP_ID)
+		return -ENODEV;
+
+	/* Stream off now. */
+	return ov8858_write_reg(client, OV8858_8BIT, OV8858_STREAM_MODE, 0);
+}
+
+static void __ov8858_print_timing(struct v4l2_subdev *sd)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 width = dev->curr_res_table[dev->fmt_idx].width;
+	u16 height = dev->curr_res_table[dev->fmt_idx].height;
+
+	dev_dbg(&client->dev, "Dump ov8858 timing in stream on:\n");
+	dev_dbg(&client->dev, "width: %d:\n", width);
+	dev_dbg(&client->dev, "height: %d:\n", height);
+	dev_dbg(&client->dev, "pixels_per_line: %d:\n", dev->pixels_per_line);
+	dev_dbg(&client->dev, "line per frame: %d:\n", dev->lines_per_frame);
+	dev_dbg(&client->dev, "pix freq: %d:\n", dev->vt_pix_clk_freq_mhz);
+	/* updated formula: pixels_per_line = 2 * HTS */
+	/* updated formula: fps = SCLK / (VTS * HTS) */
+	dev_dbg(&client->dev, "init fps: %d:\n", dev->vt_pix_clk_freq_mhz /
+		(dev->pixels_per_line / 2) / dev->lines_per_frame);
+	dev_dbg(&client->dev, "HBlank: %d nS:\n",
+		1000 * (dev->pixels_per_line - width) /
+		(dev->vt_pix_clk_freq_mhz / 1000000));
+	dev_dbg(&client->dev, "VBlank: %d uS:\n",
+		(dev->lines_per_frame - height) * dev->pixels_per_line /
+		(dev->vt_pix_clk_freq_mhz / 1000000));
+}
+
+/*
+ * ov8858 stream on/off
+ */
+static int ov8858_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	int ret;
+	u16 val;
+	dev_dbg(&client->dev, "%s: enable = %d\n", __func__, enable);
+
+	/* Set orientation */
+	ret = ov8858_read_reg(client, OV8858_8BIT, OV8858_FORMAT2, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8858_write_reg(client, OV8858_8BIT, OV8858_FORMAT2,
+			       dev->hflip ? val | OV8858_FLIP_ENABLE :
+			       val & ~OV8858_FLIP_ENABLE);
+	if (ret)
+		return ret;
+
+	ret = ov8858_read_reg(client, OV8858_8BIT, OV8858_FORMAT1, &val);
+	if (ret)
+		return ret;
+
+	ret = ov8858_write_reg(client, OV8858_8BIT, OV8858_FORMAT1,
+			       dev->vflip ? val | OV8858_FLIP_ENABLE :
+			       val & ~OV8858_FLIP_ENABLE);
+	if (ret)
+		return ret;
+
+	mutex_lock(&dev->input_lock);
+	if (enable) {
+		__ov8858_print_timing(sd);
+		ret = ov8858_write_reg_array(client, ov8858_streaming);
+		if (ret != 0) {
+			dev_err(&client->dev, "write_reg_array err\n");
+			goto out;
+		}
+		dev->streaming = 1;
+	} else {
+		ret = ov8858_write_reg_array(client, ov8858_soft_standby);
+		if (ret != 0) {
+			dev_err(&client->dev, "write_reg_array err\n");
+			goto out;
+		}
+		dev->streaming = 0;
+		dev->fps_index = 0;
+		dev->fps = 0;
+	}
+out:
+	mutex_unlock(&dev->input_lock);
+	return ret;
+}
+
+static int __update_ov8858_device_settings(struct ov8858_device *dev,
+					   u16 sensor_id)
+{
+	if (sensor_id == OV8858_CHIP_ID)
+#ifdef CONFIG_PLATFORM_BTNS
+		dev->vcm_driver = &ov8858_vcms[OV8858_ID_DEFAULT];
+#else
+		dev->vcm_driver = &ov8858_vcms[OV8858_SUNNY];
+#endif
+	else
+		return -ENODEV;
+
+	if (dev->vcm_driver && dev->vcm_driver->init)
+		return dev->vcm_driver->init(&dev->sd);
+
+	return 0;
+}
+
+static int ov8858_s_config(struct v4l2_subdev *sd,
+			   int irq, void *pdata)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	u16 sensor_id;
+	int ret;
+
+	if (pdata == NULL)
+		return -ENODEV;
+
+	dev->platform_data = pdata;
+
+	mutex_lock(&dev->input_lock);
+
+	if (dev->platform_data->platform_init) {
+		ret = dev->platform_data->platform_init(client);
+		if (ret) {
+			mutex_unlock(&dev->input_lock);
+			dev_err(&client->dev, "platform init error %d!\n", ret);
+			return ret;
+		}
+	}
+
+	ret = __ov8858_s_power(sd, 1);
+	if (ret) {
+		dev_err(&client->dev, "power-up error %d!\n", ret);
+		mutex_unlock(&dev->input_lock);
+		return ret;
+	}
+
+	ret = dev->platform_data->csi_cfg(sd, 1);
+	if (ret)
+		goto fail_csi_cfg;
+
+	/* config & detect sensor */
+	ret = ov8858_detect(client, &sensor_id);
+	if (ret) {
+		dev_err(&client->dev, "detect error %d!\n", ret);
+		goto fail_detect;
+	}
+
+	dev->sensor_id = sensor_id;
+
+	/* power off sensor */
+	ret = __ov8858_s_power(sd, 0);
+	if (ret) {
+		dev->platform_data->csi_cfg(sd, 0);
+		dev_err(&client->dev, "__ov8858_s_power-down error %d!\n", ret);
+		goto fail_update;
+	}
+
+	/* Resolution settings depend on sensor type and platform */
+	ret = __update_ov8858_device_settings(dev, dev->sensor_id);
+	if (ret) {
+		dev->platform_data->csi_cfg(sd, 0);
+		dev_err(&client->dev, "__update_ov8858_device_settings error %d!\n", ret);
+		goto fail_update;
+	}
+
+	mutex_unlock(&dev->input_lock);
+	return ret;
+
+fail_detect:
+	dev->platform_data->csi_cfg(sd, 0);
+fail_csi_cfg:
+	__ov8858_s_power(sd, 0);
+fail_update:
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+	mutex_unlock(&dev->input_lock);
+	dev_err(&client->dev, "sensor power-gating failed\n");
+	return ret;
+}
+
+static int
+ov8858_enum_mbus_code(struct v4l2_subdev *sd,
+		      struct v4l2_subdev_pad_config *cfg,
+		      struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index)
+		return -EINVAL;
+	code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
+
+	return 0;
+}
+
+static int
+ov8858_enum_frame_size(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_frame_size_enum *fse)
+{
+	int index = fse->index;
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	if (index >= dev->entries_curr_table) {
+		mutex_unlock(&dev->input_lock);
+		return -EINVAL;
+	}
+
+	fse->min_width = dev->curr_res_table[index].width;
+	fse->min_height = dev->curr_res_table[index].height;
+	fse->max_width = dev->curr_res_table[index].width;
+	fse->max_height = dev->curr_res_table[index].height;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static int ov8858_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov8858_device *dev = container_of(
+		ctrl->handler, struct ov8858_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+
+	/* input_lock is taken by the control framework, so it
+	 * doesn't need to be taken here.
+	 */
+
+	switch (ctrl->id) {
+	case V4L2_CID_RUN_MODE:
+		switch (ctrl->val) {
+		case ATOMISP_RUN_MODE_VIDEO:
+			dev->curr_res_table = ov8858_res_video;
+			dev->entries_curr_table = ARRAY_SIZE(ov8858_res_video);
+			break;
+		case ATOMISP_RUN_MODE_STILL_CAPTURE:
+			dev->curr_res_table = ov8858_res_still;
+			dev->entries_curr_table = ARRAY_SIZE(ov8858_res_still);
+			break;
+		default:
+			dev->curr_res_table = ov8858_res_preview;
+			dev->entries_curr_table =
+					ARRAY_SIZE(ov8858_res_preview);
+		}
+
+		dev->fmt_idx = 0;
+		dev->fps_index = 0;
+
+		return 0;
+	case V4L2_CID_FOCUS_ABSOLUTE:
+		if (dev->vcm_driver && dev->vcm_driver->t_focus_abs)
+			return dev->vcm_driver->t_focus_abs(&dev->sd,
+							    ctrl->val);
+		return 0;
+	case V4L2_CID_EXPOSURE_AUTO_PRIORITY:
+		if (ctrl->val == V4L2_EXPOSURE_AUTO)
+			dev->limit_exposure_flag = false;
+		else if (ctrl->val == V4L2_EXPOSURE_APERTURE_PRIORITY)
+			dev->limit_exposure_flag = true;
+		return 0;
+	case V4L2_CID_HFLIP:
+		dev->hflip = ctrl->val;
+		return 0;
+	case V4L2_CID_VFLIP:
+		dev->vflip = ctrl->val;
+		return 0;
+	default:
+		dev_err(&client->dev, "%s: Error: Invalid ctrl: 0x%X\n",
+			__func__, ctrl->id);
+		return -EINVAL;
+	}
+}
+
+static int ov8858_g_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov8858_device *dev = container_of(
+		ctrl->handler, struct ov8858_device, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
+	int r_odd, r_even;
+	int i = dev->fmt_idx;
+
+	switch (ctrl->id) {
+	case V4L2_CID_FOCUS_STATUS:
+		if (dev->vcm_driver && dev->vcm_driver->q_focus_status)
+			return dev->vcm_driver->q_focus_status(&dev->sd,
+							       &(ctrl->val));
+		return 0;
+	case V4L2_CID_BIN_FACTOR_HORZ:
+		r_odd = ov8858_get_register_8bit(&dev->sd, OV8858_H_INC_ODD,
+						 dev->curr_res_table[i].regs);
+		if (r_odd < 0)
+			return r_odd;
+		r_even = ov8858_get_register_8bit(&dev->sd, OV8858_H_INC_EVEN,
+						  dev->curr_res_table[i].regs);
+		if (r_even < 0)
+			return r_even;
+		ctrl->val = fls(r_odd + (r_even)) - 2;
+		return 0;
+
+	case V4L2_CID_BIN_FACTOR_VERT:
+		r_odd = ov8858_get_register_8bit(&dev->sd, OV8858_V_INC_ODD,
+						 dev->curr_res_table[i].regs);
+		if (r_odd < 0)
+			return r_odd;
+		r_even = ov8858_get_register_8bit(&dev->sd, OV8858_V_INC_EVEN,
+						  dev->curr_res_table[i].regs);
+		if (r_even < 0)
+			return r_even;
+		ctrl->val = fls(r_odd + (r_even)) - 2;
+		return 0;
+	case V4L2_CID_HFLIP:
+		ctrl->val = dev->hflip;
+		break;
+	case V4L2_CID_VFLIP:
+		ctrl->val = dev->vflip;
+		break;
+	case V4L2_CID_EXPOSURE_ABSOLUTE:
+		ctrl->val = dev->exposure;
+		break;
+	default:
+		dev_warn(&client->dev,
+			 "%s: Error: Invalid ctrl: 0x%X\n", __func__, ctrl->id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+ov8858_g_frame_interval(struct v4l2_subdev *sd,
+			struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	const struct ov8858_resolution *res =
+				&dev->curr_res_table[dev->fmt_idx];
+
+	mutex_lock(&dev->input_lock);
+	interval->interval.denominator = res->fps_options[dev->fps_index].fps;
+	interval->interval.numerator = 1;
+	mutex_unlock(&dev->input_lock);
+	return 0;
+}
+
+static int __ov8858_s_frame_interval(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	const struct ov8858_resolution *res =
+				&dev->curr_res_table[dev->fmt_idx];
+	struct camera_mipi_info *info = NULL;
+	unsigned int fps_index;
+	int ret = 0;
+	int fps;
+
+	info = v4l2_get_subdev_hostdata(sd);
+	if (info == NULL)
+		return -EINVAL;
+
+	if (!interval->interval.numerator)
+		interval->interval.numerator = 1;
+
+	fps = interval->interval.denominator / interval->interval.numerator;
+
+	/* No need to proceed further if we are not streaming */
+	if (!dev->streaming) {
+		/* Save the new FPS and use it while selecting setting */
+		dev->fps = fps;
+		return 0;
+	}
+
+	 /* Ignore if we are already using the required FPS. */
+	if (fps == res->fps_options[dev->fps_index].fps)
+		return 0;
+
+	fps_index = __ov8858_nearest_fps_index(fps, res->fps_options);
+
+	if (res->fps_options[fps_index].regs &&
+	    res->fps_options[fps_index].regs != dev->regs) {
+		dev_err(&client->dev,
+			"Sensor is streaming, can't apply new configuration\n");
+		return -EBUSY;
+	}
+
+	dev->fps_index = fps_index;
+	dev->fps = res->fps_options[dev->fps_index].fps;
+
+	/* Update the new frametimings based on FPS */
+	dev->pixels_per_line =
+		res->fps_options[dev->fps_index].pixels_per_line;
+	dev->lines_per_frame =
+		res->fps_options[dev->fps_index].lines_per_frame;
+
+	/* update frametiming. Conside the curren exposure/gain as well */
+	ret = __ov8858_update_frame_timing(sd,
+			&dev->pixels_per_line, &dev->lines_per_frame);
+	if (ret)
+		return ret;
+
+	/* Update the new values so that user side knows the current settings */
+	ret = ov8858_get_intg_factor(sd, info, dev->regs);
+	if (ret)
+		return ret;
+
+	interval->interval.denominator = res->fps_options[dev->fps_index].fps;
+	interval->interval.numerator = 1;
+	__ov8858_print_timing(sd);
+
+	return ret;
+}
+
+static int ov8858_s_frame_interval(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_frame_interval *interval)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	int ret;
+
+	mutex_lock(&dev->input_lock);
+	ret = __ov8858_s_frame_interval(sd, interval);
+	mutex_unlock(&dev->input_lock);
+
+	return ret;
+}
+
+static int ov8858_g_skip_frames(struct v4l2_subdev *sd, u32 *frames)
+{
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+
+	mutex_lock(&dev->input_lock);
+	*frames = dev->curr_res_table[dev->fmt_idx].skip_frames;
+	mutex_unlock(&dev->input_lock);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_sensor_ops ov8858_sensor_ops = {
+	.g_skip_frames	= ov8858_g_skip_frames,
+};
+
+static const struct v4l2_ctrl_ops ctrl_ops = {
+	.s_ctrl = ov8858_s_ctrl,
+	.g_volatile_ctrl = ov8858_g_ctrl,
+};
+
+static const struct v4l2_subdev_video_ops ov8858_video_ops = {
+	.s_stream = ov8858_s_stream,
+	.g_frame_interval = ov8858_g_frame_interval,
+	.s_frame_interval = ov8858_s_frame_interval,
+};
+
+static const struct v4l2_subdev_core_ops ov8858_core_ops = {
+	.s_power = ov8858_s_power,
+	.ioctl = ov8858_ioctl,
+	.init = ov8858_init,
+};
+
+static const struct v4l2_subdev_pad_ops ov8858_pad_ops = {
+	.enum_mbus_code = ov8858_enum_mbus_code,
+	.enum_frame_size = ov8858_enum_frame_size,
+	.get_fmt = ov8858_get_fmt,
+	.set_fmt = ov8858_set_fmt,
+};
+
+static const struct v4l2_subdev_ops ov8858_ops = {
+	.core = &ov8858_core_ops,
+	.video = &ov8858_video_ops,
+	.pad = &ov8858_pad_ops,
+	.sensor = &ov8858_sensor_ops,
+};
+
+static const struct media_entity_operations ov_entity_ops = {
+	.link_setup = NULL,
+};
+
+static int ov8858_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct ov8858_device *dev = to_ov8858_sensor(sd);
+	if (dev->platform_data->platform_deinit)
+		dev->platform_data->platform_deinit();
+
+	media_entity_cleanup(&dev->sd.entity);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
+	dev->platform_data->csi_cfg(sd, 0);
+	v4l2_device_unregister_subdev(sd);
+	kfree(dev);
+
+	return 0;
+}
+
+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 = "run mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.def = 4,
+	.max = 4,
+	.qmenu = ctrl_run_mode_menu,
+};
+
+static const struct v4l2_ctrl_config ctrls[] = {
+	{
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_VFLIP,
+		.name = "Vertical flip",
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.min = false,
+		.max = true,
+		.step = 1,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_HFLIP,
+		.name = "Horizontal flip",
+		.type = V4L2_CTRL_TYPE_BOOLEAN,
+		.min = false,
+		.max = true,
+		.step = 1,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
+		.name = "Absolute exposure",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.max = 0xffff,
+		.min = 0x0,
+		.step = 1,
+		.def = 0x00,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_ABSOLUTE,
+		.name = "Focus absolute",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.step = 1,
+		.max = OV8858_MAX_FOCUS_POS,
+	}, {
+		/* This one is junk: see the spec for proper use of this CID. */
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCUS_STATUS,
+		.name = "Focus status",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.step = 1,
+		.max = 100,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+	}, {
+		/* This is crap. For compatibility use only. */
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FOCAL_ABSOLUTE,
+		.name = "Focal lenght",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = (OV8858_FOCAL_LENGTH_NUM << 16) |
+		       OV8858_FOCAL_LENGTH_DEM,
+		.max = (OV8858_FOCAL_LENGTH_NUM << 16) |
+		       OV8858_FOCAL_LENGTH_DEM,
+		.step = 1,
+		.def = (OV8858_FOCAL_LENGTH_NUM << 16) |
+		       OV8858_FOCAL_LENGTH_DEM,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY,
+	}, {
+		/* This one is crap, too. For compatibility use only. */
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_ABSOLUTE,
+		.name = "F-number",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = (OV8858_F_NUMBER_DEFAULT_NUM << 16) |
+		       OV8858_F_NUMBER_DEM,
+		.max = (OV8858_F_NUMBER_DEFAULT_NUM << 16) |
+		       OV8858_F_NUMBER_DEM,
+		.step = 1,
+		.def = (OV8858_F_NUMBER_DEFAULT_NUM << 16) |
+		       OV8858_F_NUMBER_DEM,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY,
+	}, {
+		/*
+		 * The most utter crap. _Never_ use this, even for
+		 * compatibility reasons!
+		 */
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_FNUMBER_RANGE,
+		.name = "F-number range",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = (OV8858_F_NUMBER_DEFAULT_NUM << 24) |
+		       (OV8858_F_NUMBER_DEM << 16) |
+		       (OV8858_F_NUMBER_DEFAULT_NUM << 8) |
+		       OV8858_F_NUMBER_DEM,
+		.max = (OV8858_F_NUMBER_DEFAULT_NUM << 24) |
+		       (OV8858_F_NUMBER_DEM << 16) |
+		       (OV8858_F_NUMBER_DEFAULT_NUM << 8) |
+		       OV8858_F_NUMBER_DEM,
+		.step = 1,
+		.def = (OV8858_F_NUMBER_DEFAULT_NUM << 24) |
+		       (OV8858_F_NUMBER_DEM << 16) |
+		       (OV8858_F_NUMBER_DEFAULT_NUM << 8) |
+		       OV8858_F_NUMBER_DEM,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_HORZ,
+		.name = "Horizontal binning factor",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.max = OV8858_BIN_FACTOR_MAX,
+		.step = 1,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_BIN_FACTOR_VERT,
+		.name = "Vertical binning factor",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.max = OV8858_BIN_FACTOR_MAX,
+		.step = 1,
+		.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_VOLATILE,
+	}, {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_EXPOSURE_AUTO_PRIORITY,
+		.name = "Exposure auto priority",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.min = V4L2_EXPOSURE_AUTO,
+		.max = V4L2_EXPOSURE_APERTURE_PRIORITY,
+		.step = 1,
+	}
+};
+
+static int ov8858_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ov8858_device *dev;
+	unsigned int i;
+	int ret = 0;
+	struct camera_sensor_platform_data *pdata;
+
+	dev_dbg(&client->dev, "%s:\n", __func__);
+
+	/* allocate sensor device & init sub device */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&client->dev, "%s: out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	mutex_init(&dev->input_lock);
+
+	if (id)
+		dev->i2c_id = id->driver_data;
+	dev->fmt_idx = 0;
+	dev->sensor_id = OV_ID_DEFAULT;
+	dev->vcm_driver = &ov8858_vcms[OV8858_ID_DEFAULT];
+
+	v4l2_i2c_subdev_init(&(dev->sd), client, &ov8858_ops);
+
+	if (ACPI_COMPANION(&client->dev)) {
+		pdata = gmin_camera_platform_data(&dev->sd,
+						  ATOMISP_INPUT_FORMAT_RAW_10,
+						  atomisp_bayer_order_bggr);
+		if (!pdata) {
+			dev_err(&client->dev,
+				"%s: failed to get acpi platform data\n",
+				__func__);
+			goto out_free;
+		}
+		ret = ov8858_s_config(&dev->sd, client->irq, pdata);
+		if (ret) {
+			dev_err(&client->dev,
+				"%s: failed to set config\n", __func__);
+			goto out_free;
+		}
+		ret = atomisp_register_i2c_module(&dev->sd, pdata, RAW_CAMERA);
+		if (ret) {
+			dev_err(&client->dev,
+				"%s: failed to register subdev\n", __func__);
+			goto out_free;
+		}
+	}
+	/*
+	 * sd->name is updated with sensor driver name by the v4l2.
+	 * change it to sensor name in this case.
+	 */
+	snprintf(dev->sd.name, sizeof(dev->sd.name), "%s%x %d-%04x",
+		 OV_SUBDEV_PREFIX, dev->sensor_id,
+		 i2c_adapter_id(client->adapter), client->addr);
+
+	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.ops = &ov_entity_ops;
+	dev->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = v4l2_ctrl_handler_init(&dev->ctrl_handler, ARRAY_SIZE(ctrls) + 1);
+	if (ret) {
+		ov8858_remove(client);
+		return ret;
+	}
+
+	dev->run_mode = v4l2_ctrl_new_custom(&dev->ctrl_handler,
+					     &ctrl_run_mode, NULL);
+
+	for (i = 0; i < ARRAY_SIZE(ctrls); i++)
+		v4l2_ctrl_new_custom(&dev->ctrl_handler, &ctrls[i], NULL);
+
+	if (dev->ctrl_handler.error) {
+		ov8858_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;
+	v4l2_ctrl_handler_setup(&dev->ctrl_handler);
+
+	ret = media_entity_pads_init(&dev->sd.entity, 1, &dev->pad);
+	if (ret) {
+		ov8858_remove(client);
+		return ret;
+	}
+
+	return 0;
+
+out_free:
+	v4l2_device_unregister_subdev(&dev->sd);
+	kfree(dev);
+	return ret;
+}
+
+static const struct i2c_device_id ov8858_id[] = {
+	{OV8858_NAME, 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ov8858_id);
+
+static struct acpi_device_id ov8858_acpi_match[] = {
+	{"INT3477"},
+	{},
+};
+
+static struct i2c_driver ov8858_driver = {
+	.driver = {
+		.name = OV8858_NAME,
+		.acpi_match_table = ACPI_PTR(ov8858_acpi_match),
+	},
+	.probe = ov8858_probe,
+	.remove = ov8858_remove,
+	.id_table = ov8858_id,
+};
+
+static __init int ov8858_init_mod(void)
+{
+	return i2c_add_driver(&ov8858_driver);
+}
+
+static __exit void ov8858_exit_mod(void)
+{
+	i2c_del_driver(&ov8858_driver);
+}
+
+module_init(ov8858_init_mod);
+module_exit(ov8858_exit_mod);
+
+MODULE_DESCRIPTION("A low-level driver for Omnivision OV8858 sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/media/atomisp/i2c/ov8858.h b/drivers/staging/media/atomisp/i2c/ov8858.h
new file mode 100644
index 000000000000..9be6a0e63861
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov8858.h
@@ -0,0 +1,1482 @@
+/*
+ * Support for the Omnivision OV8858 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __OV8858_H__
+#define __OV8858_H__
+#include "../include/linux/atomisp_platform.h"
+#include <media/v4l2-ctrls.h>
+
+#define I2C_MSG_LENGTH		0x2
+
+/*
+ * This should be added into include/linux/videodev2.h
+ * NOTE: This is most likely not used anywhere.
+ */
+#define V4L2_IDENT_OV8858	V4L2_IDENT_UNKNOWN
+
+/*
+ * Indexes for VCM driver lists
+ */
+#define OV8858_ID_DEFAULT	0
+#define OV8858_SUNNY		1
+
+#define OV8858_OTP_START_ADDR	0x7010
+#define OV8858_OTP_END_ADDR	0x7186
+
+/*
+ * ov8858 System control registers
+ */
+
+#define OV8858_OTP_LOAD_CTRL		0x3D81
+#define OV8858_OTP_MODE_CTRL		0x3D84
+#define OV8858_OTP_START_ADDR_REG	0x3D88
+#define OV8858_OTP_END_ADDR_REG		0x3D8A
+#define OV8858_OTP_ISP_CTRL2		0x5002
+
+#define OV8858_OTP_MODE_MANUAL		BIT(6)
+#define OV8858_OTP_MODE_PROGRAM_DISABLE	BIT(7)
+#define OV8858_OTP_LOAD_ENABLE		BIT(0)
+#define OV8858_OTP_DPC_ENABLE		BIT(3)
+
+#define OV8858_PLL1_PREDIV0		0x030A
+#define OV8858_PLL1_PREDIV		0x0300
+#define OV8858_PLL1_MULTIPLIER		0x0301
+#define OV8858_PLL1_SYS_PRE_DIV		0x0305
+#define OV8858_PLL1_SYS_DIVIDER		0x0306
+
+#define OV8858_PLL1_PREDIV0_MASK	BIT(0)
+#define OV8858_PLL1_PREDIV_MASK		(BIT(0) | BIT(1) | BIT(2))
+#define OV8858_PLL1_MULTIPLIER_MASK	0x01FF
+#define OV8858_PLL1_SYS_PRE_DIV_MASK	(BIT(0) | BIT(1))
+#define OV8858_PLL1_SYS_DIVIDER_MASK	BIT(0)
+
+#define OV8858_PLL2_PREDIV0		0x0312
+#define OV8858_PLL2_PREDIV		0x030B
+#define OV8858_PLL2_MULTIPLIER		0x030C
+#define OV8858_PLL2_DAC_DIVIDER		0x0312
+#define OV8858_PLL2_SYS_PRE_DIV		0x030F
+#define OV8858_PLL2_SYS_DIVIDER		0x030E
+
+#define OV8858_PLL2_PREDIV0_MASK	BIT(4)
+#define OV8858_PLL2_PREDIV_MASK		(BIT(0) | BIT(1) | BIT(2))
+#define OV8858_PLL2_MULTIPLIER_MASK	0x01FF
+#define OV8858_PLL2_DAC_DIVIDER_MASK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define OV8858_PLL2_SYS_PRE_DIV_MASK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define OV8858_PLL2_SYS_DIVIDER_MASK	(BIT(0) | BIT(1) | BIT(2))
+
+#define OV8858_PLL_SCLKSEL1		0x3032
+#define OV8858_PLL_SCLKSEL2		0x3033
+#define OV8858_SRB_HOST_INPUT_DIS	0x3106
+
+#define OV8858_PLL_SCLKSEL1_MASK	BIT(7)
+#define OV8858_PLL_SCLKSEL2_MASK	BIT(1)
+
+#define OV8858_SYS_PRE_DIV_OFFSET	2
+#define OV8858_SYS_PRE_DIV_MASK		(BIT(2) | BIT(3))
+#define OV8858_SCLK_PDIV_OFFSET		4
+#define OV8858_SCLK_PDIV_MASK		(BIT(4) | BIT(5) | BIT(6) | BIT(7))
+
+#define OV8858_TIMING_HTS			0x380C
+#define OV8858_TIMING_VTS			0x380E
+
+#define OV8858_HORIZONTAL_START_H		0x3800
+#define OV8858_VERTICAL_START_H			0x3802
+#define OV8858_HORIZONTAL_END_H			0x3804
+#define OV8858_VERTICAL_END_H			0x3806
+#define OV8858_HORIZONTAL_OUTPUT_SIZE_H		0x3808
+#define OV8858_VERTICAL_OUTPUT_SIZE_H		0x380A
+
+#define OV8858_GROUP_ACCESS			0x3208
+#define OV8858_GROUP_ZERO			0x00
+#define OV8858_GROUP_ACCESS_HOLD_START		0x00
+#define OV8858_GROUP_ACCESS_HOLD_END		0x10
+#define OV8858_GROUP_ACCESS_DELAY_LAUNCH	0xA0
+#define OV8858_GROUP_ACCESS_QUICK_LAUNCH	0xE0
+
+#define OV_SUBDEV_PREFIX			"ov"
+#define OV_ID_DEFAULT				0x0000
+#define	OV8858_NAME				"ov8858"
+#define OV8858_CHIP_ID				0x8858
+
+#define OV8858_LONG_EXPO			0x3500
+#define OV8858_LONG_GAIN			0x3508
+#define OV8858_LONG_DIGI_GAIN			0x350A
+#define OV8858_SHORT_GAIN			0x350C
+#define OV8858_SHORT_DIGI_GAIN			0x350E
+
+#define OV8858_FORMAT1				0x3820
+#define OV8858_FORMAT2				0x3821
+
+#define OV8858_FLIP_ENABLE			0x06
+
+#define OV8858_MWB_RED_GAIN_H			0x5032
+#define OV8858_MWB_GREEN_GAIN_H			0x5034
+#define OV8858_MWB_BLUE_GAIN_H			0x5036
+#define OV8858_MWB_GAIN_MAX			0x0FFF
+
+#define OV8858_CHIP_ID_HIGH			0x300B
+#define OV8858_CHIP_ID_LOW			0x300C
+#define OV8858_STREAM_MODE			0x0100
+
+#define OV8858_FOCAL_LENGTH_NUM			294	/* 2.94mm */
+#define OV8858_FOCAL_LENGTH_DEM			100
+#define OV8858_F_NUMBER_DEFAULT_NUM		24	/* 2.4 */
+#define OV8858_F_NUMBER_DEM			10
+
+#define OV8858_H_INC_ODD			0x3814
+#define OV8858_H_INC_EVEN			0x3815
+#define OV8858_V_INC_ODD			0x382A
+#define OV8858_V_INC_EVEN			0x382B
+
+#define OV8858_READ_MODE_BINNING_ON		0x0400 /* ToDo: Check this */
+#define OV8858_READ_MODE_BINNING_OFF		0x00   /* ToDo: Check this */
+#define OV8858_BIN_FACTOR_MAX			2
+#define OV8858_INTEGRATION_TIME_MARGIN		14
+
+#define OV8858_MAX_VTS_VALUE			0xFFFF
+#define OV8858_MAX_EXPOSURE_VALUE \
+		(OV8858_MAX_VTS_VALUE - OV8858_INTEGRATION_TIME_MARGIN)
+#define OV8858_MAX_GAIN_VALUE			0x07FF
+
+#define OV8858_MAX_FOCUS_POS			1023
+
+#define OV8858_TEST_PATTERN_REG			0x5E00
+
+struct ov8858_vcm {
+	int (*power_up)(struct v4l2_subdev *sd);
+	int (*power_down)(struct v4l2_subdev *sd);
+	int (*init)(struct v4l2_subdev *sd);
+	int (*t_focus_vcm)(struct v4l2_subdev *sd, u16 val);
+	int (*t_focus_abs)(struct v4l2_subdev *sd, s32 value);
+	int (*t_focus_rel)(struct v4l2_subdev *sd, s32 value);
+	int (*q_focus_status)(struct v4l2_subdev *sd, s32 *value);
+	int (*q_focus_abs)(struct v4l2_subdev *sd, s32 *value);
+	int (*t_vcm_slew)(struct v4l2_subdev *sd, s32 value);
+	int (*t_vcm_timing)(struct v4l2_subdev *sd, s32 value);
+};
+
+/*
+ * Defines for register writes and register array processing
+ * */
+#define OV8858_BYTE_MAX				32
+#define OV8858_SHORT_MAX			16
+#define OV8858_TOK_MASK				0xFFF0
+
+#define MAX_FPS_OPTIONS_SUPPORTED		3
+
+#define OV8858_DEPTH_COMP_CONST			2200
+#define OV8858_DEPTH_VTS_CONST			2573
+
+enum ov8858_tok_type {
+	OV8858_8BIT  = 0x0001,
+	OV8858_16BIT = 0x0002,
+	OV8858_TOK_TERM   = 0xF000,	/* terminating token for reg list */
+	OV8858_TOK_DELAY  = 0xFE00	/* delay token for reg list */
+};
+
+/*
+ * If register address or register width is not 32 bit width,
+ * user needs to convert it manually
+ */
+struct s_register_setting {
+	u32 reg;
+	u32 val;
+};
+
+/**
+ * struct ov8858_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 ov8858_reg {
+	enum ov8858_tok_type type;
+	u16 sreg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+struct ov8858_fps_setting {
+	int fps;
+	unsigned short pixels_per_line;
+	unsigned short lines_per_frame;
+	const struct ov8858_reg *regs; /* regs that the fps setting needs */
+};
+
+struct ov8858_resolution {
+	u8 *desc;
+	const struct ov8858_reg *regs;
+	int res;
+	int width;
+	int height;
+	bool used;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	unsigned short skip_frames;
+	const struct ov8858_fps_setting fps_options[MAX_FPS_OPTIONS_SUPPORTED];
+};
+
+/*
+ * ov8858 device structure
+ * */
+struct ov8858_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 */
+	int fmt_idx;
+	int streaming;
+	int vt_pix_clk_freq_mhz;
+	int fps_index;
+	u16 sensor_id;			/* Sensor id from registers */
+	u16 i2c_id;			/* Sensor id from i2c_device_id */
+	int exposure;
+	int gain;
+	u16 digital_gain;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 fps;
+	u8 *otp_data;
+	/* Prevent the framerate from being lowered in low light scenes. */
+	int limit_exposure_flag;
+	bool hflip;
+	bool vflip;
+
+	const struct ov8858_reg *regs;
+	struct ov8858_vcm *vcm_driver;
+	const struct ov8858_resolution *curr_res_table;
+	int entries_curr_table;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *run_mode;
+};
+
+#define to_ov8858_sensor(x) container_of(x, struct ov8858_device, sd)
+
+#define OV8858_MAX_WRITE_BUF_SIZE	32
+struct ov8858_write_buffer {
+	u16 addr;
+	u8 data[OV8858_MAX_WRITE_BUF_SIZE];
+};
+
+struct ov8858_write_ctrl {
+	int index;
+	struct ov8858_write_buffer buffer;
+};
+
+static const struct ov8858_reg ov8858_soft_standby[] = {
+	{OV8858_8BIT, 0x0100, 0x00},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_streaming[] = {
+	{OV8858_8BIT, 0x0100, 0x01},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_param_hold[] = {
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_HOLD_START},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_param_update[] = {
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_HOLD_END},
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_DELAY_LAUNCH},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+extern int dw9718_vcm_power_up(struct v4l2_subdev *sd);
+extern int dw9718_vcm_power_down(struct v4l2_subdev *sd);
+extern int dw9718_vcm_init(struct v4l2_subdev *sd);
+extern int dw9718_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int dw9718_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int vcm_power_up(struct v4l2_subdev *sd);
+extern int vcm_power_down(struct v4l2_subdev *sd);
+
+static struct ov8858_vcm ov8858_vcms[] = {
+	[OV8858_SUNNY] = {
+		.power_up = dw9718_vcm_power_up,
+		.power_down = dw9718_vcm_power_down,
+		.init = dw9718_vcm_init,
+		.t_focus_vcm = dw9718_t_focus_vcm,
+		.t_focus_abs = dw9718_t_focus_abs,
+		.t_focus_rel = dw9718_t_focus_rel,
+		.q_focus_status = dw9718_q_focus_status,
+		.q_focus_abs = dw9718_q_focus_abs,
+		.t_vcm_slew = dw9718_t_vcm_slew,
+		.t_vcm_timing = dw9718_t_vcm_timing,
+	},
+	[OV8858_ID_DEFAULT] = {
+		.power_up = NULL,
+		.power_down = NULL,
+	},
+};
+
+
+#define OV8858_RES_WIDTH_MAX	3280
+#define OV8858_RES_HEIGHT_MAX	2464
+
+static struct ov8858_reg ov8858_BasicSettings[] = {
+	{OV8858_8BIT, 0x0103, 0x01}, /* software_reset */
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	/* PLL settings */
+	{OV8858_8BIT, 0x0300, 0x05}, /* pll1_pre_div = /4 */
+	{OV8858_8BIT, 0x0302, 0xAF}, /* pll1_multiplier = 175 */
+	{OV8858_8BIT, 0x0303, 0x00}, /* pll1_divm = /(1 + 0) */
+	{OV8858_8BIT, 0x0304, 0x03}, /* pll1_div_mipi = /8 */
+	{OV8858_8BIT, 0x030B, 0x02}, /* pll2_pre_div = /2 */
+	{OV8858_8BIT, 0x030D, 0x4E}, /* pll2_r_divp = 78 */
+	{OV8858_8BIT, 0x030E, 0x00}, /* pll2_r_divs = /1 */
+	{OV8858_8BIT, 0x030F, 0x04}, /* pll2_r_divsp = /(1 + 4) */
+	/* pll2_pre_div0 = /1, pll2_r_divdac = /(1 + 1) */
+	{OV8858_8BIT, 0x0312, 0x01},
+	{OV8858_8BIT, 0x031E, 0x0C}, /* pll1_no_lat = 1, mipi_bitsel_man = 0 */
+
+	/* PAD OEN2, VSYNC out enable=0x80, disable=0x00 */
+	{OV8858_8BIT, 0x3002, 0x80},
+	/* PAD OUT2, VSYNC pulse direction low-to-high = 1 */
+	{OV8858_8BIT, 0x3007, 0x01},
+	/* PAD SEL2, VSYNC out value = 0 */
+	{OV8858_8BIT, 0x300D, 0x00},
+	/* PAD OUT2, VSYNC out select = 0 */
+	{OV8858_8BIT, 0x3010, 0x00},
+
+	/* Npump clock div = /2, Ppump clock div = /4 */
+	{OV8858_8BIT, 0x3015, 0x01},
+	/*
+	 * mipi_lane_mode = 1+3, mipi_lvds_sel = 1 = MIPI enable,
+	 * r_phy_pd_mipi_man = 0, lane_dis_option = 0
+	 */
+	{OV8858_8BIT, 0x3018, 0x72},
+	/* Clock switch output = normal, pclk_div = /1 */
+	{OV8858_8BIT, 0x3020, 0x93},
+	/*
+	 * lvds_mode_o = 0, clock lane disable when pd_mipi = 0,
+	 * pd_mipi enable when rst_sync = 1
+	 */
+	{OV8858_8BIT, 0x3022, 0x01},
+	{OV8858_8BIT, 0x3031, 0x0A}, /* mipi_bit_sel = 10 */
+	{OV8858_8BIT, 0x3034, 0x00}, /* Unknown */
+	/* sclk_div = /1, sclk_pre_div = /1, chip debug = 1 */
+	{OV8858_8BIT, 0x3106, 0x01},
+
+	{OV8858_8BIT, 0x3305, 0xF1}, /* Unknown */
+	{OV8858_8BIT, 0x3307, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x3308, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3309, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x330A, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330B, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x330C, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330F, 0x40}, /* Unknown */
+
+	{OV8858_8BIT, 0x3500, 0x00}, /* long exposure = 0x9A20 */
+	{OV8858_8BIT, 0x3501, 0x9A}, /* long exposure = 0x9A20 */
+	{OV8858_8BIT, 0x3502, 0x20}, /* long exposure = 0x9A20 */
+	/*
+	 * Digital fraction gain delay option = Delay 1 frame,
+	 * Gain change delay option = Delay 1 frame,
+	 * Gain delay option = Delay 1 frame,
+	 * Gain manual as sensor gain = Input gain as real gain format,
+	 * Exposure delay option (must be 0 = Delay 1 frame,
+	 * Exposure change delay option (must be 0) = Delay 1 frame
+	 */
+	{OV8858_8BIT, 0x3503, 0x00},
+	{OV8858_8BIT, 0x3505, 0x80}, /* gain conversation option */
+	/*
+	 * [10:7] are integer gain, [6:0] are fraction gain. For example:
+	 * 0x80 is 1x gain, 0x100 is 2x gain, 0x1C0 is 3.5x gain
+	 */
+	{OV8858_8BIT, 0x3508, 0x02}, /* long gain = 0x0200 */
+	{OV8858_8BIT, 0x3509, 0x00}, /* long gain = 0x0200 */
+	{OV8858_8BIT, 0x350C, 0x00}, /* short gain = 0x0080 */
+	{OV8858_8BIT, 0x350D, 0x80}, /* short gain = 0x0080 */
+	{OV8858_8BIT, 0x3510, 0x00}, /* short exposure = 0x000200 */
+	{OV8858_8BIT, 0x3511, 0x02}, /* short exposure = 0x000200 */
+	{OV8858_8BIT, 0x3512, 0x00}, /* short exposure = 0x000200 */
+
+	{OV8858_8BIT, 0x3600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3601, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3602, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3603, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3604, 0x22}, /* Unknown */
+	{OV8858_8BIT, 0x3605, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x3606, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3607, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3608, 0x11}, /* Unknown */
+	{OV8858_8BIT, 0x3609, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x360A, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x360B, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x360C, 0xDC}, /* Unknown */
+	{OV8858_8BIT, 0x360D, 0x40}, /* Unknown */
+	{OV8858_8BIT, 0x360E, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x360F, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3610, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x3611, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3612, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x3613, 0x80}, /* Unknown */
+	{OV8858_8BIT, 0x3614, 0x58}, /* Unknown */
+	{OV8858_8BIT, 0x3615, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3616, 0x4A}, /* Unknown */
+	{OV8858_8BIT, 0x3617, 0x90}, /* Unknown */
+	{OV8858_8BIT, 0x3618, 0x56}, /* Unknown */
+	{OV8858_8BIT, 0x3619, 0x70}, /* Unknown */
+	{OV8858_8BIT, 0x361A, 0x99}, /* Unknown */
+	{OV8858_8BIT, 0x361B, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361C, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x361D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361F, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3633, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3634, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3635, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3636, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3638, 0xFF}, /* Unknown */
+	{OV8858_8BIT, 0x3645, 0x13}, /* Unknown */
+	{OV8858_8BIT, 0x3646, 0x83}, /* Unknown */
+	{OV8858_8BIT, 0x364A, 0x07}, /* Unknown */
+
+	{OV8858_8BIT, 0x3700, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x3701, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x3702, 0x50}, /* Unknown */
+	{OV8858_8BIT, 0x3703, 0x32}, /* Unknown */
+	{OV8858_8BIT, 0x3704, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3705, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3706, 0x6A}, /* Unknown */
+	{OV8858_8BIT, 0x3707, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3708, 0x48}, /* Unknown */
+	{OV8858_8BIT, 0x3709, 0x66}, /* Unknown */
+	{OV8858_8BIT, 0x370A, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x370B, 0x6A}, /* Unknown */
+	{OV8858_8BIT, 0x370C, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x3712, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x3714, 0x24}, /* Unknown */
+	{OV8858_8BIT, 0x3718, 0x14}, /* Unknown */
+	{OV8858_8BIT, 0x3719, 0x31}, /* Unknown */
+	{OV8858_8BIT, 0x371E, 0x31}, /* Unknown */
+	{OV8858_8BIT, 0x371F, 0x7F}, /* Unknown */
+	{OV8858_8BIT, 0x3720, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3721, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3724, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3725, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3726, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3728, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3729, 0x03}, /* Unknown */
+	{OV8858_8BIT, 0x372A, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x372B, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372C, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372D, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372E, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x372F, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3730, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3731, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3732, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3733, 0x10}, /* Unknown */
+	{OV8858_8BIT, 0x3734, 0x40}, /* Unknown */
+	{OV8858_8BIT, 0x3736, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x373A, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x373B, 0x0B}, /* Unknown */
+	{OV8858_8BIT, 0x373C, 0x14}, /* Unknown */
+	{OV8858_8BIT, 0x373E, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3755, 0x10}, /* Unknown */
+	{OV8858_8BIT, 0x3758, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3759, 0x4C}, /* Unknown */
+	{OV8858_8BIT, 0x375A, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x375B, 0x26}, /* Unknown */
+	{OV8858_8BIT, 0x375C, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x375D, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x375E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x375F, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3760, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3761, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3762, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3763, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3766, 0xFF}, /* Unknown */
+	{OV8858_8BIT, 0x3768, 0x22}, /* Unknown */
+	{OV8858_8BIT, 0x3769, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x376A, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x376B, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x376F, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3772, 0x46}, /* Unknown */
+	{OV8858_8BIT, 0x3773, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x3774, 0x2C}, /* Unknown */
+	{OV8858_8BIT, 0x3775, 0x13}, /* Unknown */
+	{OV8858_8BIT, 0x3776, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3777, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x37A0, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37A1, 0x7A}, /* Unknown */
+	{OV8858_8BIT, 0x37A2, 0x7A}, /* Unknown */
+	{OV8858_8BIT, 0x37A3, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A4, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A5, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A6, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A7, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37A8, 0x98}, /* Unknown */
+	{OV8858_8BIT, 0x37A9, 0x98}, /* Unknown */
+	{OV8858_8BIT, 0x37AA, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37AB, 0x5C}, /* Unknown */
+	{OV8858_8BIT, 0x37AC, 0x5C}, /* Unknown */
+	{OV8858_8BIT, 0x37AD, 0x55}, /* Unknown */
+	{OV8858_8BIT, 0x37AE, 0x19}, /* Unknown */
+	{OV8858_8BIT, 0x37AF, 0x19}, /* Unknown */
+	{OV8858_8BIT, 0x37B0, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B1, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B2, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B3, 0x84}, /* Unknown */
+	{OV8858_8BIT, 0x37B4, 0x84}, /* Unknown */
+	{OV8858_8BIT, 0x37B5, 0x66}, /* Unknown */
+	{OV8858_8BIT, 0x37B6, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B7, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B8, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B9, 0xFF}, /* Unknown */
+
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high */
+	{OV8858_8BIT, 0x3809, 0xC0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x09}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x90}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3810, 0x00}, /* h_win offset high */
+	{OV8858_8BIT, 0x3811, 0x04}, /* h_win offset low */
+	{OV8858_8BIT, 0x3812, 0x00}, /* v_win offset high */
+	{OV8858_8BIT, 0x3813, 0x02}, /* v_win offset low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3837, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x3841, 0xFF}, /* AUTO_SIZE_CTRL */
+	{OV8858_8BIT, 0x3846, 0x48}, /* Unknown */
+
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3D8C, 0x73}, /* OTP_SETTING_STT_ADDRESS */
+	{OV8858_8BIT, 0x3D8D, 0xDE}, /* OTP_SETTING_STT_ADDRESS */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x3F0A, 0x80}, /* PSRAM control register */
+
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x0B}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4023, 0xC3}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4024, 0x0C}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4025, 0x36}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4026, 0x0C}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4027, 0x37}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4300, 0xFF}, /* clip_max[11:4] = 0xFFF */
+	{OV8858_8BIT, 0x4301, 0x00}, /* clip_min[11:4] = 0 */
+	{OV8858_8BIT, 0x4302, 0x0F}, /* clip_min/max[3:0] */
+	{OV8858_8BIT, 0x4307, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4316, 0x00}, /* CTRL16 = default */
+	{OV8858_8BIT, 0x4503, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x4500, 0x38}, /* Unknown */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	/* wkup_dly = Mark1 wakeup delay/2^10 = 0x25 */
+	{OV8858_8BIT, 0x4808, 0x25},
+	{OV8858_8BIT, 0x4816, 0x52}, /* Embedded data type*/
+	{OV8858_8BIT, 0x481F, 0x32}, /* clk_prepare_min = 0x32 */
+	{OV8858_8BIT, 0x4825, 0x3A}, /* lpx_p_min = 0x3A */
+	{OV8858_8BIT, 0x4826, 0x40}, /* hs_prepare_min = 0x40 */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_8BIT, 0x4850, 0x10}, /* LANE SEL01 */
+	{OV8858_8BIT, 0x4851, 0x32}, /* LANE SEL02 */
+
+	{OV8858_8BIT, 0x4B00, 0x2A}, /* Unknown */
+	{OV8858_8BIT, 0x4B0D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4D00, 0x04}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D01, 0x18}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D02, 0xC3}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D03, 0xFF}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D04, 0xFF}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D05, 0xFF}, /* TPM_CTRL_REG */
+
+	/*
+	 * Lens correction (LENC) function enable = 0
+	 * Slave sensor AWB Gain function enable = 1
+	 * Slave sensor AWB Statistics function enable = 1
+	 * Master sensor AWB Gain function enable = 1
+	 * Master sensor AWB Statistics function enable = 1
+	 * Black DPC function enable = 1
+	 * White DPC function enable =1
+	 */
+	{OV8858_8BIT, 0x5000, 0x7E},
+	{OV8858_8BIT, 0x5001, 0x01}, /* BLC function enable = 1 */
+	/*
+	 * Horizontal scale function enable = 0
+	 * WBMATCH bypass mode = Select slave sensor's gain
+	 * WBMATCH function enable = 0
+	 * Master MWB gain support RGBC = 0
+	 * OTP_DPC function enable = 1
+	 * Manual mode of VarioPixel function enable = 0
+	 * Manual enable of VarioPixel function enable = 0
+	 * Use VSYNC to latch ISP modules's function enable signals = 0
+	 */
+	{OV8858_8BIT, 0x5002, 0x08},
+	/*
+	 * Bypass all ISP modules after BLC module = 0
+	 * DPC_DBC buffer control enable = 1
+	 * WBMATCH VSYNC selection = Select master sensor's VSYNC fall
+	 * Select master AWB gain to embed line = AWB gain before manual mode
+	 * Enable BLC's input flip_i signal = 0
+	 */
+	{OV8858_8BIT, 0x5003, 0x20},
+	{OV8858_8BIT, 0x5041, 0x1D}, /* ISP CTRL41 - embedded data=on */
+	{OV8858_8BIT, 0x5046, 0x12}, /* ISP CTRL46 = default */
+	/*
+	 * Tail enable = 1
+	 * Saturate cross cluster enable = 1
+	 * Remove cross cluster enable = 1
+	 * Enable to remove connected defect pixels in same channel = 1
+	 * Enable to remove connected defect pixels in different channel = 1
+	 * Smooth enable, use average G for recovery = 1
+	 * Black/white sensor mode enable = 0
+	 * Manual mode enable = 0
+	 */
+	{OV8858_8BIT, 0x5780, 0xFC},
+	{OV8858_8BIT, 0x5784, 0x0C}, /* DPC CTRL04 */
+	{OV8858_8BIT, 0x5787, 0x40}, /* DPC CTRL07 */
+	{OV8858_8BIT, 0x5788, 0x08}, /* DPC CTRL08 */
+	{OV8858_8BIT, 0x578A, 0x02}, /* DPC CTRL0A */
+	{OV8858_8BIT, 0x578B, 0x01}, /* DPC CTRL0B */
+	{OV8858_8BIT, 0x578C, 0x01}, /* DPC CTRL0C */
+	{OV8858_8BIT, 0x578E, 0x02}, /* DPC CTRL0E */
+	{OV8858_8BIT, 0x578F, 0x01}, /* DPC CTRL0F */
+	{OV8858_8BIT, 0x5790, 0x01}, /* DPC CTRL10 */
+	{OV8858_8BIT, 0x5901, 0x00}, /* VAP CTRL01 = default */
+	/* WINC CTRL08 = embedded data in 1st line*/
+	{OV8858_8BIT, 0x5A08, 0x00},
+	{OV8858_8BIT, 0x5B00, 0x02}, /* OTP CTRL00 */
+	{OV8858_8BIT, 0x5B01, 0x10}, /* OTP CTRL01 */
+	{OV8858_8BIT, 0x5B02, 0x03}, /* OTP CTRL02 */
+	{OV8858_8BIT, 0x5B03, 0xCF}, /* OTP CTRL03 */
+	{OV8858_8BIT, 0x5B05, 0x6C}, /* OTP CTRL05 = default */
+	{OV8858_8BIT, 0x5E00, 0x00}, /* PRE CTRL00 = default */
+	{OV8858_8BIT, 0x5E01, 0x41}, /* PRE_CTRL01 = default */
+
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+/*****************************STILL********************************/
+
+static const struct ov8858_reg ov8858_8M[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low 3283 */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3280 x 2464 */
+	{OV8858_8BIT, 0x3809, 0xD0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x09}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0xa0}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x0B}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4023, 0xC3}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4024, 0x0C}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4025, 0x36}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4026, 0x0C}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4027, 0x37}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_3276x1848[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x10}, /* h_crop_start low  0c->10*/
+	{OV8858_8BIT, 0x3802, 0x01}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x42}, /* v_crop_start low 3e->42*/
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x08}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x71}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3276 x 1848 */
+	{OV8858_8BIT, 0x3809, 0xCC}, /* h_output_size low d0->cc*/
+	{OV8858_8BIT, 0x380A, 0x07}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x38}, /* v_output_size low 3c->38*/
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x0B}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4023, 0xC3}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4024, 0x0C}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4025, 0x36}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4026, 0x0C}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4027, 0x37}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_6M[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x01}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x3E}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x08}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x71}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3280 x 1852 */
+	{OV8858_8BIT, 0x3809, 0xD0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x07}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x3C}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x0B}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4023, 0xC3}, /* Anchor left end = 0x0BC3 */
+	{OV8858_8BIT, 0x4024, 0x0C}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4025, 0x36}, /* Anchor right start = 0x0C36 */
+	{OV8858_8BIT, 0x4026, 0x0C}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4027, 0x37}, /* Anchor right end = 0x0C37 */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1080P_60[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x17}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x02}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x26}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x02}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x8C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0A}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0x9D}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x07}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x0A}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x07}, /* h_output_size high*/
+	{OV8858_8BIT, 0x3809, 0x90}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x04}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xEC}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x07}, /* Anchor left end = 0x072D */
+	{OV8858_8BIT, 0x4023, 0x2D}, /* Anchor left end = 0x072D */
+	{OV8858_8BIT, 0x4024, 0x07}, /* Anchor right start = 0x079E */
+	{OV8858_8BIT, 0x4025, 0x9E}, /* Anchor right start = 0x079E */
+	{OV8858_8BIT, 0x4026, 0x07}, /* Anchor right end = 0x079F */
+	{OV8858_8BIT, 0x4027, 0x9F}, /* Anchor right end = 0x079F */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xef}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x16}, /* pclk_period = 0x16 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1080P_30[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x17}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x02}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x26}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x02}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x8C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0A}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0x9D}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x07}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x0A}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x07}, /* h_output_size high*/
+	{OV8858_8BIT, 0x3809, 0x90}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x07}, /* Anchor left end = 0x072D */
+	{OV8858_8BIT, 0x4023, 0x2D}, /* Anchor left end = 0x072D */
+	{OV8858_8BIT, 0x4024, 0x07}, /* Anchor right start = 0x079E */
+	{OV8858_8BIT, 0x4025, 0x9E}, /* Anchor right start = 0x079E */
+	{OV8858_8BIT, 0x4026, 0x07}, /* Anchor right end = 0x079F */
+	{OV8858_8BIT, 0x4027, 0x9F}, /* Anchor right end = 0x079F */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x0C}, /* Bottom black line start = 12 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xef}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x16}, /* pclk_period = 0x16 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1640x1232[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high 3283 */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 1232 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0xD0}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x10}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x04}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4023, 0xB9}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4024, 0x05}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4025, 0x2A}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4026, 0x05}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4027, 0x2B}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x04}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x08}, /* Bottom black line start = 8 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1640x1096[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high 3283 */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 1096 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x10}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x04}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4023, 0xB9}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4024, 0x05}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4025, 0x2A}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4026, 0x05}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4027, 0x2B}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x04}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x08}, /* Bottom black line start = 8 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+
+static const struct ov8858_reg ov8858_1640x926[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x00},  /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 926 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x03}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x9E}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x10}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x04}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4023, 0xB9}, /* Anchor left end = 0x04B9 */
+	{OV8858_8BIT, 0x4024, 0x05}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4025, 0x2A}, /* Anchor right start = 0x052A */
+	{OV8858_8BIT, 0x4026, 0x05}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4027, 0x2B}, /* Anchor right end = 0x052B */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x02}, /* Top zero line number = 2 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x04}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x02}, /* Bottom zero start line = 2 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x08}, /* Bottom black line start = 8 */
+	{OV8858_8BIT, 0x402F, 0x02}, /* Bottom black line number = 2 */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static struct ov8858_resolution ov8858_res_preview[] = {
+	{
+		.desc = "ov8858_1640x926_PREVIEW",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1232_PREVIEW",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_3276x1848_PREVIEW",
+		.width = 3276,
+		.height = 1848,
+		.used = 0,
+		.regs = ov8858_3276x1848,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_8M_PREVIEW",
+		.width = 3280,
+		.height = 2464,
+		.used = 0,
+		.regs = ov8858_8M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+};
+
+static struct ov8858_resolution ov8858_res_still[] = {
+	{
+		.desc = "ov8858_1640x1232_STILL",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x926_STILL",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_3276X1848_STILL",
+		.width = 3276,
+		.height = 1848,
+		.used = 0,
+		.regs = ov8858_3276x1848,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options =  {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_8M_STILL",
+		.width = 3280,
+		.height = 2464,
+		.used = 0,
+		.regs = ov8858_8M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				/* Pixel clock: 149.76MHZ */
+				.fps = 10,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 3859,
+			},
+			{
+			}
+		},
+	},
+};
+
+static struct ov8858_resolution ov8858_res_video[] = {
+	{
+		.desc = "ov8858_1640x926_VIDEO",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1232_VIDEO",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1096_VIDEO",
+		.width = 1640,
+		.height = 1096,
+		.used = 0,
+		.regs = ov8858_1640x1096,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_6M_VIDEO",
+		.width = 3280,
+		.height = 1852,
+		.used = 0,
+		.regs = ov8858_6M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options =  {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_8M_VIDEO",
+		.width = 3280,
+		.height = 2464,
+		.used = 0,
+		.regs = ov8858_8M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+};
+
+#endif /* __OV8858_H__ */
diff --git a/drivers/staging/media/atomisp/i2c/ov8858_btns.h b/drivers/staging/media/atomisp/i2c/ov8858_btns.h
new file mode 100644
index 000000000000..09e3cdc1a394
--- /dev/null
+++ b/drivers/staging/media/atomisp/i2c/ov8858_btns.h
@@ -0,0 +1,1284 @@
+/*
+ * Support for the Omnivision OV8858 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; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __OV8858_H__
+#define __OV8858_H__
+#include "../include/linux/atomisp_platform.h"
+#include <media/v4l2-ctrls.h>
+
+#define I2C_MSG_LENGTH		0x2
+
+/*
+ * This should be added into include/linux/videodev2.h
+ * NOTE: This is most likely not used anywhere.
+ */
+#define V4L2_IDENT_OV8858	V4L2_IDENT_UNKNOWN
+
+/*
+ * Indexes for VCM driver lists
+ */
+#define OV8858_ID_DEFAULT	0
+#define OV8858_SUNNY		1
+
+#define OV8858_OTP_START_ADDR	0x7010
+#define OV8858_OTP_END_ADDR	0x7186
+
+/*
+ * ov8858 System control registers
+ */
+
+#define OV8858_OTP_LOAD_CTRL		0x3D81
+#define OV8858_OTP_MODE_CTRL		0x3D84
+#define OV8858_OTP_START_ADDR_REG	0x3D88
+#define OV8858_OTP_END_ADDR_REG		0x3D8A
+#define OV8858_OTP_ISP_CTRL2		0x5002
+
+#define OV8858_OTP_MODE_MANUAL		BIT(6)
+#define OV8858_OTP_MODE_PROGRAM_DISABLE	BIT(7)
+#define OV8858_OTP_LOAD_ENABLE		BIT(0)
+#define OV8858_OTP_DPC_ENABLE		BIT(3)
+
+#define OV8858_PLL1_PREDIV0		0x030A
+#define OV8858_PLL1_PREDIV		0x0300
+#define OV8858_PLL1_MULTIPLIER		0x0301
+#define OV8858_PLL1_SYS_PRE_DIV		0x0305
+#define OV8858_PLL1_SYS_DIVIDER		0x0306
+
+#define OV8858_PLL1_PREDIV0_MASK	BIT(0)
+#define OV8858_PLL1_PREDIV_MASK		(BIT(0) | BIT(1) | BIT(2))
+#define OV8858_PLL1_MULTIPLIER_MASK	0x01FF
+#define OV8858_PLL1_SYS_PRE_DIV_MASK	(BIT(0) | BIT(1))
+#define OV8858_PLL1_SYS_DIVIDER_MASK	BIT(0)
+
+#define OV8858_PLL2_PREDIV0		0x0312
+#define OV8858_PLL2_PREDIV		0x030B
+#define OV8858_PLL2_MULTIPLIER		0x030C
+#define OV8858_PLL2_DAC_DIVIDER		0x0312
+#define OV8858_PLL2_SYS_PRE_DIV		0x030F
+#define OV8858_PLL2_SYS_DIVIDER		0x030E
+
+#define OV8858_PLL2_PREDIV0_MASK	BIT(4)
+#define OV8858_PLL2_PREDIV_MASK		(BIT(0) | BIT(1) | BIT(2))
+#define OV8858_PLL2_MULTIPLIER_MASK	0x01FF
+#define OV8858_PLL2_DAC_DIVIDER_MASK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define OV8858_PLL2_SYS_PRE_DIV_MASK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
+#define OV8858_PLL2_SYS_DIVIDER_MASK	(BIT(0) | BIT(1) | BIT(2))
+
+#define OV8858_PLL_SCLKSEL1		0x3032
+#define OV8858_PLL_SCLKSEL2		0x3033
+#define OV8858_SRB_HOST_INPUT_DIS	0x3106
+
+#define OV8858_PLL_SCLKSEL1_MASK	BIT(7)
+#define OV8858_PLL_SCLKSEL2_MASK	BIT(1)
+
+#define OV8858_SYS_PRE_DIV_OFFSET	2
+#define OV8858_SYS_PRE_DIV_MASK		(BIT(2) | BIT(3))
+#define OV8858_SCLK_PDIV_OFFSET		4
+#define OV8858_SCLK_PDIV_MASK		(BIT(4) | BIT(5) | BIT(6) | BIT(7))
+
+#define OV8858_TIMING_HTS			0x380C
+#define OV8858_TIMING_VTS			0x380E
+
+#define OV8858_HORIZONTAL_START_H		0x3800
+#define OV8858_VERTICAL_START_H			0x3802
+#define OV8858_HORIZONTAL_END_H			0x3804
+#define OV8858_VERTICAL_END_H			0x3806
+#define OV8858_HORIZONTAL_OUTPUT_SIZE_H		0x3808
+#define OV8858_VERTICAL_OUTPUT_SIZE_H		0x380A
+
+#define OV8858_GROUP_ACCESS			0x3208
+#define OV8858_GROUP_ZERO			0x00
+#define OV8858_GROUP_ACCESS_HOLD_START		0x00
+#define OV8858_GROUP_ACCESS_HOLD_END		0x10
+#define OV8858_GROUP_ACCESS_DELAY_LAUNCH	0xA0
+#define OV8858_GROUP_ACCESS_QUICK_LAUNCH	0xE0
+
+#define OV_SUBDEV_PREFIX			"ov"
+#define OV_ID_DEFAULT				0x0000
+#define	OV8858_NAME				"ov8858"
+#define OV8858_CHIP_ID				0x8858
+
+#define OV8858_LONG_EXPO			0x3500
+#define OV8858_LONG_GAIN			0x3508
+#define OV8858_LONG_DIGI_GAIN			0x350A
+#define OV8858_SHORT_GAIN			0x350C
+#define OV8858_SHORT_DIGI_GAIN			0x350E
+
+#define OV8858_FORMAT1				0x3820
+#define OV8858_FORMAT2				0x3821
+
+#define OV8858_FLIP_ENABLE			0x06
+
+#define OV8858_MWB_RED_GAIN_H			0x5032
+#define OV8858_MWB_GREEN_GAIN_H			0x5034
+#define OV8858_MWB_BLUE_GAIN_H			0x5036
+#define OV8858_MWB_GAIN_MAX			0x0FFF
+
+#define OV8858_CHIP_ID_HIGH			0x300B
+#define OV8858_CHIP_ID_LOW			0x300C
+#define OV8858_STREAM_MODE			0x0100
+
+#define OV8858_FOCAL_LENGTH_NUM			294	/* 2.94mm */
+#define OV8858_FOCAL_LENGTH_DEM			100
+#define OV8858_F_NUMBER_DEFAULT_NUM		24	/* 2.4 */
+#define OV8858_F_NUMBER_DEM			10
+
+#define OV8858_H_INC_ODD			0x3814
+#define OV8858_H_INC_EVEN			0x3815
+#define OV8858_V_INC_ODD			0x382A
+#define OV8858_V_INC_EVEN			0x382B
+
+#define OV8858_READ_MODE_BINNING_ON		0x0400 /* ToDo: Check this */
+#define OV8858_READ_MODE_BINNING_OFF		0x00   /* ToDo: Check this */
+#define OV8858_BIN_FACTOR_MAX			2
+#define OV8858_INTEGRATION_TIME_MARGIN		14
+
+#define OV8858_MAX_VTS_VALUE			0xFFFF
+#define OV8858_MAX_EXPOSURE_VALUE \
+		(OV8858_MAX_VTS_VALUE - OV8858_INTEGRATION_TIME_MARGIN)
+#define OV8858_MAX_GAIN_VALUE			0x07FF
+
+#define OV8858_MAX_FOCUS_POS			1023
+
+#define OV8858_TEST_PATTERN_REG			0x5E00
+
+struct ov8858_vcm {
+	int (*power_up)(struct v4l2_subdev *sd);
+	int (*power_down)(struct v4l2_subdev *sd);
+	int (*init)(struct v4l2_subdev *sd);
+	int (*t_focus_vcm)(struct v4l2_subdev *sd, u16 val);
+	int (*t_focus_abs)(struct v4l2_subdev *sd, s32 value);
+	int (*t_focus_rel)(struct v4l2_subdev *sd, s32 value);
+	int (*q_focus_status)(struct v4l2_subdev *sd, s32 *value);
+	int (*q_focus_abs)(struct v4l2_subdev *sd, s32 *value);
+	int (*t_vcm_slew)(struct v4l2_subdev *sd, s32 value);
+	int (*t_vcm_timing)(struct v4l2_subdev *sd, s32 value);
+};
+
+/*
+ * Defines for register writes and register array processing
+ * */
+#define OV8858_BYTE_MAX				32
+#define OV8858_SHORT_MAX			16
+#define OV8858_TOK_MASK				0xFFF0
+
+#define MAX_FPS_OPTIONS_SUPPORTED		3
+
+#define OV8858_DEPTH_COMP_CONST			2200
+#define OV8858_DEPTH_VTS_CONST			2573
+
+enum ov8858_tok_type {
+	OV8858_8BIT  = 0x0001,
+	OV8858_16BIT = 0x0002,
+	OV8858_TOK_TERM   = 0xF000,	/* terminating token for reg list */
+	OV8858_TOK_DELAY  = 0xFE00	/* delay token for reg list */
+};
+
+/*
+ * If register address or register width is not 32 bit width,
+ * user needs to convert it manually
+ */
+struct s_register_setting {
+	u32 reg;
+	u32 val;
+};
+
+/**
+ * struct ov8858_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 ov8858_reg {
+	enum ov8858_tok_type type;
+	u16 sreg;
+	u32 val;	/* @set value for read/mod/write, @mask */
+};
+
+struct ov8858_fps_setting {
+	int fps;
+	unsigned short pixels_per_line;
+	unsigned short lines_per_frame;
+	const struct ov8858_reg *regs; /* regs that the fps setting needs */
+};
+
+struct ov8858_resolution {
+	u8 *desc;
+	const struct ov8858_reg *regs;
+	int res;
+	int width;
+	int height;
+	bool used;
+	u8 bin_factor_x;
+	u8 bin_factor_y;
+	unsigned short skip_frames;
+	const struct ov8858_fps_setting fps_options[MAX_FPS_OPTIONS_SUPPORTED];
+};
+
+/*
+ * ov8858 device structure
+ * */
+struct ov8858_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 */
+	int fmt_idx;
+	int streaming;
+	int vt_pix_clk_freq_mhz;
+	int fps_index;
+	u16 sensor_id;			/* Sensor id from registers */
+	u16 i2c_id;			/* Sensor id from i2c_device_id */
+	int exposure;
+	int gain;
+	u16 digital_gain;
+	u16 pixels_per_line;
+	u16 lines_per_frame;
+	u8 fps;
+	u8 *otp_data;
+	/* Prevent the framerate from being lowered in low light scenes. */
+	int limit_exposure_flag;
+	bool hflip;
+	bool vflip;
+
+	const struct ov8858_reg *regs;
+	struct ov8858_vcm *vcm_driver;
+	const struct ov8858_resolution *curr_res_table;
+	int entries_curr_table;
+
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *run_mode;
+};
+
+#define to_ov8858_sensor(x) container_of(x, struct ov8858_device, sd)
+
+#define OV8858_MAX_WRITE_BUF_SIZE	32
+struct ov8858_write_buffer {
+	u16 addr;
+	u8 data[OV8858_MAX_WRITE_BUF_SIZE];
+};
+
+struct ov8858_write_ctrl {
+	int index;
+	struct ov8858_write_buffer buffer;
+};
+
+static const struct ov8858_reg ov8858_soft_standby[] = {
+	{OV8858_8BIT, 0x0100, 0x00},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_streaming[] = {
+	{OV8858_8BIT, 0x0100, 0x01},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_param_hold[] = {
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_HOLD_START},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_param_update[] = {
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_HOLD_END},
+	{OV8858_8BIT, OV8858_GROUP_ACCESS,
+			OV8858_GROUP_ZERO | OV8858_GROUP_ACCESS_DELAY_LAUNCH},
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+extern int dw9718_vcm_power_up(struct v4l2_subdev *sd);
+extern int dw9718_vcm_power_down(struct v4l2_subdev *sd);
+extern int dw9718_vcm_init(struct v4l2_subdev *sd);
+extern int dw9718_t_focus_vcm(struct v4l2_subdev *sd, u16 val);
+extern int dw9718_t_focus_abs(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_focus_rel(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_q_focus_status(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_q_focus_abs(struct v4l2_subdev *sd, s32 *value);
+extern int dw9718_t_vcm_slew(struct v4l2_subdev *sd, s32 value);
+extern int dw9718_t_vcm_timing(struct v4l2_subdev *sd, s32 value);
+
+extern int vcm_power_up(struct v4l2_subdev *sd);
+extern int vcm_power_down(struct v4l2_subdev *sd);
+
+static struct ov8858_vcm ov8858_vcms[] = {
+	[OV8858_SUNNY] = {
+		.power_up = dw9718_vcm_power_up,
+		.power_down = dw9718_vcm_power_down,
+		.init = dw9718_vcm_init,
+		.t_focus_vcm = dw9718_t_focus_vcm,
+		.t_focus_abs = dw9718_t_focus_abs,
+		.t_focus_rel = dw9718_t_focus_rel,
+		.q_focus_status = dw9718_q_focus_status,
+		.q_focus_abs = dw9718_q_focus_abs,
+		.t_vcm_slew = dw9718_t_vcm_slew,
+		.t_vcm_timing = dw9718_t_vcm_timing,
+	},
+	[OV8858_ID_DEFAULT] = {
+		.power_up = NULL,
+		.power_down = NULL,
+	},
+};
+
+
+#define OV8858_RES_WIDTH_MAX	3280
+#define OV8858_RES_HEIGHT_MAX	2464
+
+static struct ov8858_reg ov8858_BasicSettings[] = {
+	{OV8858_8BIT, 0x0103, 0x01}, /* software_reset */
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	/* PLL settings */
+	{OV8858_8BIT, 0x0300, 0x05}, /* pll1_pre_div = /4 */
+	{OV8858_8BIT, 0x0302, 0xAF}, /* pll1_multiplier = 175 */
+	{OV8858_8BIT, 0x0303, 0x00}, /* pll1_divm = /(1 + 0) */
+	{OV8858_8BIT, 0x0304, 0x03}, /* pll1_div_mipi = /8 */
+	{OV8858_8BIT, 0x030B, 0x02}, /* pll2_pre_div = /2 */
+	{OV8858_8BIT, 0x030D, 0x4E}, /* pll2_r_divp = 78 */
+	{OV8858_8BIT, 0x030E, 0x00}, /* pll2_r_divs = /1 */
+	{OV8858_8BIT, 0x030F, 0x04}, /* pll2_r_divsp = /(1 + 4) */
+	/* pll2_pre_div0 = /1, pll2_r_divdac = /(1 + 1) */
+	{OV8858_8BIT, 0x0312, 0x01},
+	{OV8858_8BIT, 0x031E, 0x0C}, /* pll1_no_lat = 1, mipi_bitsel_man = 0 */
+
+	/* PAD OEN2, VSYNC out enable=0x80, disable=0x00 */
+	{OV8858_8BIT, 0x3002, 0x80},
+	/* PAD OUT2, VSYNC pulse direction low-to-high = 1 */
+	{OV8858_8BIT, 0x3007, 0x01},
+	/* PAD SEL2, VSYNC out value = 0 */
+	{OV8858_8BIT, 0x300D, 0x00},
+	/* PAD OUT2, VSYNC out select = 0 */
+	{OV8858_8BIT, 0x3010, 0x00},
+
+	/* Npump clock div = /2, Ppump clock div = /4 */
+	{OV8858_8BIT, 0x3015, 0x01},
+	/*
+	 * mipi_lane_mode = 1+3, mipi_lvds_sel = 1 = MIPI enable,
+	 * r_phy_pd_mipi_man = 0, lane_dis_option = 0
+	 */
+	{OV8858_8BIT, 0x3018, 0x72},
+	/* Clock switch output = normal, pclk_div = /1 */
+	{OV8858_8BIT, 0x3020, 0x93},
+	/*
+	 * lvds_mode_o = 0, clock lane disable when pd_mipi = 0,
+	 * pd_mipi enable when rst_sync = 1
+	 */
+	{OV8858_8BIT, 0x3022, 0x01},
+	{OV8858_8BIT, 0x3031, 0x0A}, /* mipi_bit_sel = 10 */
+	{OV8858_8BIT, 0x3034, 0x00}, /* Unknown */
+	/* sclk_div = /1, sclk_pre_div = /1, chip debug = 1 */
+	{OV8858_8BIT, 0x3106, 0x01},
+
+	{OV8858_8BIT, 0x3305, 0xF1}, /* Unknown */
+	{OV8858_8BIT, 0x3307, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x3308, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3309, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x330A, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330B, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x330C, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x330F, 0x40}, /* Unknown */
+
+	{OV8858_8BIT, 0x3500, 0x00}, /* long exposure = 0x9A20 */
+	{OV8858_8BIT, 0x3501, 0x9A}, /* long exposure = 0x9A20 */
+	{OV8858_8BIT, 0x3502, 0x20}, /* long exposure = 0x9A20 */
+	/*
+	 * Digital fraction gain delay option = Delay 1 frame,
+	 * Gain change delay option = Delay 1 frame,
+	 * Gain delay option = Delay 1 frame,
+	 * Gain manual as sensor gain = Input gain as real gain format,
+	 * Exposure delay option (must be 0 = Delay 1 frame,
+	 * Exposure change delay option (must be 0) = Delay 1 frame
+	 */
+	{OV8858_8BIT, 0x3503, 0x00},
+	{OV8858_8BIT, 0x3505, 0x80}, /* gain conversation option */
+	/*
+	 * [10:7] are integer gain, [6:0] are fraction gain. For example:
+	 * 0x80 is 1x gain, 0x100 is 2x gain, 0x1C0 is 3.5x gain
+	 */
+	{OV8858_8BIT, 0x3508, 0x02}, /* long gain = 0x0200 */
+	{OV8858_8BIT, 0x3509, 0x00}, /* long gain = 0x0200 */
+	{OV8858_8BIT, 0x350C, 0x00}, /* short gain = 0x0080 */
+	{OV8858_8BIT, 0x350D, 0x80}, /* short gain = 0x0080 */
+	{OV8858_8BIT, 0x3510, 0x00}, /* short exposure = 0x000200 */
+	{OV8858_8BIT, 0x3511, 0x02}, /* short exposure = 0x000200 */
+	{OV8858_8BIT, 0x3512, 0x00}, /* short exposure = 0x000200 */
+
+	{OV8858_8BIT, 0x3600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3601, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3602, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3603, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3604, 0x22}, /* Unknown */
+	{OV8858_8BIT, 0x3605, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x3606, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3607, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3608, 0x11}, /* Unknown */
+	{OV8858_8BIT, 0x3609, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x360A, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x360B, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x360C, 0xDC}, /* Unknown */
+	{OV8858_8BIT, 0x360D, 0x40}, /* Unknown */
+	{OV8858_8BIT, 0x360E, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x360F, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3610, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x3611, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3612, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x3613, 0x80}, /* Unknown */
+	{OV8858_8BIT, 0x3614, 0x58}, /* Unknown */
+	{OV8858_8BIT, 0x3615, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3616, 0x4A}, /* Unknown */
+	{OV8858_8BIT, 0x3617, 0x90}, /* Unknown */
+	{OV8858_8BIT, 0x3618, 0x56}, /* Unknown */
+	{OV8858_8BIT, 0x3619, 0x70}, /* Unknown */
+	{OV8858_8BIT, 0x361A, 0x99}, /* Unknown */
+	{OV8858_8BIT, 0x361B, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361C, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x361D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x361F, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3633, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3634, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3635, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3636, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3638, 0xFF}, /* Unknown */
+	{OV8858_8BIT, 0x3645, 0x13}, /* Unknown */
+	{OV8858_8BIT, 0x3646, 0x83}, /* Unknown */
+	{OV8858_8BIT, 0x364A, 0x07}, /* Unknown */
+
+	{OV8858_8BIT, 0x3700, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x3701, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x3702, 0x50}, /* Unknown */
+	{OV8858_8BIT, 0x3703, 0x32}, /* Unknown */
+	{OV8858_8BIT, 0x3704, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3705, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3706, 0x6A}, /* Unknown */
+	{OV8858_8BIT, 0x3707, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3708, 0x48}, /* Unknown */
+	{OV8858_8BIT, 0x3709, 0x66}, /* Unknown */
+	{OV8858_8BIT, 0x370A, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x370B, 0x6A}, /* Unknown */
+	{OV8858_8BIT, 0x370C, 0x07}, /* Unknown */
+	{OV8858_8BIT, 0x3712, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x3714, 0x24}, /* Unknown */
+	{OV8858_8BIT, 0x3718, 0x14}, /* Unknown */
+	{OV8858_8BIT, 0x3719, 0x31}, /* Unknown */
+	{OV8858_8BIT, 0x371E, 0x31}, /* Unknown */
+	{OV8858_8BIT, 0x371F, 0x7F}, /* Unknown */
+	{OV8858_8BIT, 0x3720, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3721, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3724, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3725, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3726, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3728, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x3729, 0x03}, /* Unknown */
+	{OV8858_8BIT, 0x372A, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x372B, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372C, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372D, 0xA6}, /* Unknown */
+	{OV8858_8BIT, 0x372E, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x372F, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x3730, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3731, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x3732, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3733, 0x10}, /* Unknown */
+	{OV8858_8BIT, 0x3734, 0x40}, /* Unknown */
+	{OV8858_8BIT, 0x3736, 0x30}, /* Unknown */
+	{OV8858_8BIT, 0x373A, 0x0A}, /* Unknown */
+	{OV8858_8BIT, 0x373B, 0x0B}, /* Unknown */
+	{OV8858_8BIT, 0x373C, 0x14}, /* Unknown */
+	{OV8858_8BIT, 0x373E, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3755, 0x10}, /* Unknown */
+	{OV8858_8BIT, 0x3758, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3759, 0x4C}, /* Unknown */
+	{OV8858_8BIT, 0x375A, 0x0C}, /* Unknown */
+	{OV8858_8BIT, 0x375B, 0x26}, /* Unknown */
+	{OV8858_8BIT, 0x375C, 0x20}, /* Unknown */
+	{OV8858_8BIT, 0x375D, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x375E, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x375F, 0x28}, /* Unknown */
+	{OV8858_8BIT, 0x3760, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3761, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3762, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3763, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3766, 0xFF}, /* Unknown */
+	{OV8858_8BIT, 0x3768, 0x22}, /* Unknown */
+	{OV8858_8BIT, 0x3769, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x376A, 0x44}, /* Unknown */
+	{OV8858_8BIT, 0x376B, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x376F, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3772, 0x46}, /* Unknown */
+	{OV8858_8BIT, 0x3773, 0x04}, /* Unknown */
+	{OV8858_8BIT, 0x3774, 0x2C}, /* Unknown */
+	{OV8858_8BIT, 0x3775, 0x13}, /* Unknown */
+	{OV8858_8BIT, 0x3776, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3777, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x37A0, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37A1, 0x7A}, /* Unknown */
+	{OV8858_8BIT, 0x37A2, 0x7A}, /* Unknown */
+	{OV8858_8BIT, 0x37A3, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A4, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A5, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A6, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37A7, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37A8, 0x98}, /* Unknown */
+	{OV8858_8BIT, 0x37A9, 0x98}, /* Unknown */
+	{OV8858_8BIT, 0x37AA, 0x88}, /* Unknown */
+	{OV8858_8BIT, 0x37AB, 0x5C}, /* Unknown */
+	{OV8858_8BIT, 0x37AC, 0x5C}, /* Unknown */
+	{OV8858_8BIT, 0x37AD, 0x55}, /* Unknown */
+	{OV8858_8BIT, 0x37AE, 0x19}, /* Unknown */
+	{OV8858_8BIT, 0x37AF, 0x19}, /* Unknown */
+	{OV8858_8BIT, 0x37B0, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B1, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B2, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B3, 0x84}, /* Unknown */
+	{OV8858_8BIT, 0x37B4, 0x84}, /* Unknown */
+	{OV8858_8BIT, 0x37B5, 0x66}, /* Unknown */
+	{OV8858_8BIT, 0x37B6, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B7, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B8, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x37B9, 0xFF}, /* Unknown */
+
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high */
+	{OV8858_8BIT, 0x3809, 0xC0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x09}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x90}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3810, 0x00}, /* h_win offset high */
+	{OV8858_8BIT, 0x3811, 0x04}, /* h_win offset low */
+	{OV8858_8BIT, 0x3812, 0x00}, /* v_win offset high */
+	{OV8858_8BIT, 0x3813, 0x02}, /* v_win offset low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3837, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x3841, 0xFF}, /* AUTO_SIZE_CTRL */
+	{OV8858_8BIT, 0x3846, 0x48}, /* Unknown */
+
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3D8C, 0x73}, /* OTP_SETTING_STT_ADDRESS */
+	{OV8858_8BIT, 0x3D8D, 0xDE}, /* OTP_SETTING_STT_ADDRESS */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x3F0A, 0x80}, /* PSRAM control register */
+
+	{OV8858_8BIT, 0x4000, 0xF1}, /* BLC CTRL00 = default */
+	{OV8858_8BIT, 0x4001, 0x00}, /* BLC CTRL01 */
+	{OV8858_8BIT, 0x4002, 0x27}, /* BLC offset = 0x27 */
+	{OV8858_8BIT, 0x4005, 0x10}, /* BLC target = 0x0010 */
+	{OV8858_8BIT, 0x4009, 0x81}, /* BLC CTRL09 */
+	{OV8858_8BIT, 0x400B, 0x0C}, /* BLC CTRL0B = default */
+	{OV8858_8BIT, 0x400A, 0x01},
+	{OV8858_8BIT, 0x4011, 0x20}, /* BLC CTRL11 = 0x20 */
+	{OV8858_8BIT, 0x401B, 0x00}, /* Zero line R coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401D, 0x00}, /* Zero line T coeff. = 0x0000 */
+	{OV8858_8BIT, 0x401F, 0x00}, /* BLC CTRL1F */
+	{OV8858_8BIT, 0x4020, 0x00}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4021, 0x04}, /* Anchor left start = 0x0004 */
+	{OV8858_8BIT, 0x4022, 0x0C}, /* Anchor left end = 0x0C60 */
+	{OV8858_8BIT, 0x4023, 0x60}, /* Anchor left end = 0x0C60 */
+	{OV8858_8BIT, 0x4024, 0x0F}, /* Anchor right start = 0x0F36 */
+	{OV8858_8BIT, 0x4025, 0x36}, /* Anchor right start = 0x0F36 */
+	{OV8858_8BIT, 0x4026, 0x0F}, /* Anchor right end = 0x0F37 */
+	{OV8858_8BIT, 0x4027, 0x37}, /* Anchor right end = 0x0F37 */
+	{OV8858_8BIT, 0x4028, 0x00}, /* Top zero line start = 0 */
+	{OV8858_8BIT, 0x4029, 0x04}, /* Top zero line number = 4 */
+	{OV8858_8BIT, 0x402A, 0x04}, /* Top black line start = 4 */
+	{OV8858_8BIT, 0x402B, 0x08}, /* Top black line number = 8 */
+	{OV8858_8BIT, 0x402C, 0x00}, /* Bottom zero start line = 0 */
+	{OV8858_8BIT, 0x402D, 0x02}, /* Bottom zero line number = 2 */
+	{OV8858_8BIT, 0x402E, 0x04}, /* Bottom black line start = 4 */
+	{OV8858_8BIT, 0x402F, 0x08}, /* Bottom black line number = 8 */
+
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4300, 0xFF}, /* clip_max[11:4] = 0xFFF */
+	{OV8858_8BIT, 0x4301, 0x00}, /* clip_min[11:4] = 0 */
+	{OV8858_8BIT, 0x4302, 0x0F}, /* clip_min/max[3:0] */
+	{OV8858_8BIT, 0x4307, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4316, 0x00}, /* CTRL16 = default */
+	{OV8858_8BIT, 0x4503, 0x18}, /* Unknown */
+	{OV8858_8BIT, 0x4500, 0x38}, /* Unknown */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	/* wkup_dly = Mark1 wakeup delay/2^10 = 0x25 */
+	{OV8858_8BIT, 0x4808, 0x25},
+	{OV8858_8BIT, 0x4816, 0x52}, /* Embedded data type*/
+	{OV8858_8BIT, 0x481F, 0x32}, /* clk_prepare_min = 0x32 */
+	{OV8858_8BIT, 0x4825, 0x3A}, /* lpx_p_min = 0x3A */
+	{OV8858_8BIT, 0x4826, 0x40}, /* hs_prepare_min = 0x40 */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_8BIT, 0x4850, 0x10}, /* LANE SEL01 */
+	{OV8858_8BIT, 0x4851, 0x32}, /* LANE SEL02 */
+
+	{OV8858_8BIT, 0x4B00, 0x2A}, /* Unknown */
+	{OV8858_8BIT, 0x4B0D, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4D00, 0x04}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D01, 0x18}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D02, 0xC3}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D03, 0xFF}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D04, 0xFF}, /* TPM_CTRL_REG */
+	{OV8858_8BIT, 0x4D05, 0xFF}, /* TPM_CTRL_REG */
+
+	/*
+	 * Lens correction (LENC) function enable = 0
+	 * Slave sensor AWB Gain function enable = 1
+	 * Slave sensor AWB Statistics function enable = 1
+	 * Master sensor AWB Gain function enable = 1
+	 * Master sensor AWB Statistics function enable = 1
+	 * Black DPC function enable = 1
+	 * White DPC function enable =1
+	 */
+	{OV8858_8BIT, 0x5000, 0x7E},
+	{OV8858_8BIT, 0x5001, 0x01}, /* BLC function enable = 1 */
+	/*
+	 * Horizontal scale function enable = 0
+	 * WBMATCH bypass mode = Select slave sensor's gain
+	 * WBMATCH function enable = 0
+	 * Master MWB gain support RGBC = 0
+	 * OTP_DPC function enable = 1
+	 * Manual mode of VarioPixel function enable = 0
+	 * Manual enable of VarioPixel function enable = 0
+	 * Use VSYNC to latch ISP modules's function enable signals = 0
+	 */
+	{OV8858_8BIT, 0x5002, 0x08},
+	/*
+	 * Bypass all ISP modules after BLC module = 0
+	 * DPC_DBC buffer control enable = 1
+	 * WBMATCH VSYNC selection = Select master sensor's VSYNC fall
+	 * Select master AWB gain to embed line = AWB gain before manual mode
+	 * Enable BLC's input flip_i signal = 0
+	 */
+	{OV8858_8BIT, 0x5003, 0x20},
+	{OV8858_8BIT, 0x5041, 0x1D}, /* ISP CTRL41 - embedded data=on */
+	{OV8858_8BIT, 0x5046, 0x12}, /* ISP CTRL46 = default */
+	/*
+	 * Tail enable = 1
+	 * Saturate cross cluster enable = 1
+	 * Remove cross cluster enable = 1
+	 * Enable to remove connected defect pixels in same channel = 1
+	 * Enable to remove connected defect pixels in different channel = 1
+	 * Smooth enable, use average G for recovery = 1
+	 * Black/white sensor mode enable = 0
+	 * Manual mode enable = 0
+	 */
+	{OV8858_8BIT, 0x5780, 0xFC},
+	{OV8858_8BIT, 0x5784, 0x0C}, /* DPC CTRL04 */
+	{OV8858_8BIT, 0x5787, 0x40}, /* DPC CTRL07 */
+	{OV8858_8BIT, 0x5788, 0x08}, /* DPC CTRL08 */
+	{OV8858_8BIT, 0x578A, 0x02}, /* DPC CTRL0A */
+	{OV8858_8BIT, 0x578B, 0x01}, /* DPC CTRL0B */
+	{OV8858_8BIT, 0x578C, 0x01}, /* DPC CTRL0C */
+	{OV8858_8BIT, 0x578E, 0x02}, /* DPC CTRL0E */
+	{OV8858_8BIT, 0x578F, 0x01}, /* DPC CTRL0F */
+	{OV8858_8BIT, 0x5790, 0x01}, /* DPC CTRL10 */
+	{OV8858_8BIT, 0x5901, 0x00}, /* VAP CTRL01 = default */
+	/* WINC CTRL08 = embedded data in 1st line*/
+	{OV8858_8BIT, 0x5A08, 0x00},
+	{OV8858_8BIT, 0x5B00, 0x02}, /* OTP CTRL00 */
+	{OV8858_8BIT, 0x5B01, 0x10}, /* OTP CTRL01 */
+	{OV8858_8BIT, 0x5B02, 0x03}, /* OTP CTRL02 */
+	{OV8858_8BIT, 0x5B03, 0xCF}, /* OTP CTRL03 */
+	{OV8858_8BIT, 0x5B05, 0x6C}, /* OTP CTRL05 = default */
+	{OV8858_8BIT, 0x5E00, 0x00}, /* PRE CTRL00 = default */
+	{OV8858_8BIT, 0x5E01, 0x41}, /* PRE_CTRL01 = default */
+
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+/*****************************STILL********************************/
+
+static const struct ov8858_reg ov8858_8M[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low 3283 */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3280 x 2464 */
+	{OV8858_8BIT, 0x3809, 0xD0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x09}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0xa0}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_3276x1848[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x10}, /* h_crop_start low  0c->10*/
+	{OV8858_8BIT, 0x3802, 0x01}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x42}, /* v_crop_start low 3e->42*/
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x08}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x71}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3276 x 1848 */
+	{OV8858_8BIT, 0x3809, 0xCC}, /* h_output_size low d0->cc*/
+	{OV8858_8BIT, 0x380A, 0x07}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x38}, /* v_output_size low 3c->38*/
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_6M[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x01}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x3E}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x08}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x71}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x0C}, /* h_output_size high 3280 x 1852 */
+	{OV8858_8BIT, 0x3809, 0xD0}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x07}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x3C}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0x97}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1080P_60[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x17}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x02}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x26}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x02}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x8C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0A}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0x9D}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x07}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x0A}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x07}, /* h_output_size high*/
+	{OV8858_8BIT, 0x3809, 0x90}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x04}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xEC}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xef}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x16}, /* pclk_period = 0x16 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1080P_30[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x17}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x02}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x26}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x02}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x8C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0A}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0x9D}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x07}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0x0A}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x07}, /* h_output_size high*/
+	{OV8858_8BIT, 0x3809, 0x90}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x0A}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0x0D}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x01}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x40}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x01}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x06}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x01}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x14}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x10}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xef}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x16}, /* pclk_period = 0x16 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1640x1232[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high 3283 */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 1232 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0xD0}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static const struct ov8858_reg ov8858_1640x1096[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low 12 */
+	{OV8858_8BIT, 0x3802, 0x00}, /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high 3283 */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 1096 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x04}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x48}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+
+static const struct ov8858_reg ov8858_1640x926[] = {
+	{OV8858_8BIT, 0x0100, 0x00}, /* software_standby */
+	{OV8858_8BIT, 0x3778, 0x16}, /* Unknown */
+	{OV8858_8BIT, 0x3800, 0x00}, /* h_crop_start high */
+	{OV8858_8BIT, 0x3801, 0x0C}, /* h_crop_start low */
+	{OV8858_8BIT, 0x3802, 0x00},  /* v_crop_start high */
+	{OV8858_8BIT, 0x3803, 0x0C}, /* v_crop_start low */
+	{OV8858_8BIT, 0x3804, 0x0C}, /* h_crop_end high */
+	{OV8858_8BIT, 0x3805, 0xD3}, /* h_crop_end low */
+	{OV8858_8BIT, 0x3806, 0x09}, /* v_crop_end high */
+	{OV8858_8BIT, 0x3807, 0xA3}, /* v_crop_end low */
+	{OV8858_8BIT, 0x3808, 0x06}, /* h_output_size high 1640 x 926 */
+	{OV8858_8BIT, 0x3809, 0x68}, /* h_output_size low */
+	{OV8858_8BIT, 0x380A, 0x03}, /* v_output_size high */
+	{OV8858_8BIT, 0x380B, 0x9E}, /* v_output_size low */
+	{OV8858_8BIT, 0x380C, 0x07}, /* horizontal timing size high */
+	{OV8858_8BIT, 0x380D, 0x94}, /* horizontal timing size low */
+	{OV8858_8BIT, 0x380E, 0x09}, /* vertical timing size high */
+	{OV8858_8BIT, 0x380F, 0xAA}, /* vertical timing size low */
+	{OV8858_8BIT, 0x3814, 0x03}, /* h_odd_inc */
+	{OV8858_8BIT, 0x3815, 0x01}, /* h_even_inc */
+	{OV8858_8BIT, 0x3820, 0x00}, /* format1 */
+	{OV8858_8BIT, 0x3821, 0x67}, /* format2 */
+	{OV8858_8BIT, 0x382A, 0x03}, /* v_odd_inc */
+	{OV8858_8BIT, 0x382B, 0x01}, /* v_even_inc */
+	{OV8858_8BIT, 0x3830, 0x08}, /* Unknown */
+	{OV8858_8BIT, 0x3836, 0x02}, /* Unknown */
+	{OV8858_8BIT, 0x3D85, 0x16}, /* OTP_REG85 */
+	{OV8858_8BIT, 0x3F08, 0x08}, /* PSRAM control register */
+	{OV8858_8BIT, 0x4034, 0x3F}, /* Unknown */
+	{OV8858_8BIT, 0x403D, 0x04}, /* BLC CTRL3D */
+	{OV8858_8BIT, 0x4600, 0x00}, /* Unknown */
+	{OV8858_8BIT, 0x4601, 0xCB}, /* Unknown */
+	{OV8858_8BIT, 0x4837, 0x14}, /* pclk_period = 0x14 */
+	{OV8858_TOK_TERM, 0, 0}
+};
+
+static struct ov8858_resolution ov8858_res_preview[] = {
+	{
+		.desc = "ov8858_1640x926_PREVIEW",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1232_PREVIEW",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1936x1096_PREVIEW",
+		.width = 1936,
+		.height = 1096,
+		.used = 0,
+		.regs = ov8858_1080P_30,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_3276x1848_PREVIEW",
+		.width = 3276,
+		.height = 1848,
+		.used = 0,
+		.regs = ov8858_3276x1848,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_8M_PREVIEW",
+		.width = 3280,
+		.height = 2464,
+		.used = 0,
+		.regs = ov8858_8M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+};
+
+static struct ov8858_resolution ov8858_res_still[] = {
+	{
+		.desc = "ov8858_1640x1232_STILL",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 0,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x926_STILL",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_3276X1848_STILL",
+		.width = 3276,
+		.height = 1848,
+		.used = 0,
+		.regs = ov8858_3276x1848,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options =  {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_8M_STILL",
+		.width = 3280,
+		.height = 2464,
+		.used = 0,
+		.regs = ov8858_8M,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				/* Pixel clock: 149.76MHZ */
+				.fps = 10,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 3859,
+			},
+			{
+			}
+		},
+	},
+};
+
+static struct ov8858_resolution ov8858_res_video[] = {
+	{
+		.desc = "ov8858_1640x926_VIDEO",
+		.width = 1640,
+		.height = 926,
+		.used = 0,
+		.regs = ov8858_1640x926,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1232_VIDEO",
+		.width = 1640,
+		.height = 1232,
+		.used = 0,
+		.regs = ov8858_1640x1232,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+	{
+		.desc = "ov8858_1640x1096_VIDEO",
+		.width = 1640,
+		.height = 1096,
+		.used = 0,
+		.regs = ov8858_1640x1096,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+    {
+		.desc = "ov8858_1080P_30_VIDEO",
+		.width = 1936,
+		.height = 1096,
+		.used = 0,
+		.regs = ov8858_1080P_30,
+		.bin_factor_x = 0,
+		.bin_factor_y = 0,
+		.skip_frames = 1,
+		.fps_options = {
+			{
+				.fps = 30,
+				.pixels_per_line = 3880,
+				.lines_per_frame = 2573,
+			},
+			{
+			}
+		},
+	},
+};
+
+#endif /* __OV8858_H__ */
diff --git a/drivers/staging/media/atomisp/include/asm/intel_mid_pcihelpers.h b/drivers/staging/media/atomisp/include/asm/intel_mid_pcihelpers.h
new file mode 100644
index 000000000000..c5e22bba455a
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/asm/intel_mid_pcihelpers.h
@@ -0,0 +1,37 @@
+/*
+ * Access to message bus through three registers
+ * in CUNIT(0:0:0) PCI configuration space.
+ * MSGBUS_CTRL_REG(0xD0):
+ *   31:24      = message bus opcode
+ *   23:16      = message bus port
+ *   15:8       = message bus address, low 8 bits.
+ *   7:4        = message bus byte enables
+ * MSGBUS_CTRL_EXT_REG(0xD8):
+ *   31:8       = message bus address, high 24 bits.
+ * MSGBUS_DATA_REG(0xD4):
+ *   hold the data for write or read
+ */
+#define PCI_ROOT_MSGBUS_CTRL_REG        0xD0
+#define PCI_ROOT_MSGBUS_DATA_REG        0xD4
+#define PCI_ROOT_MSGBUS_CTRL_EXT_REG    0xD8
+#define PCI_ROOT_MSGBUS_READ            0x10
+#define PCI_ROOT_MSGBUS_WRITE           0x11
+#define PCI_ROOT_MSGBUS_DWORD_ENABLE    0xf0
+
+/* In BYT platform for all internal PCI devices d3 delay
+ * of 3 ms is sufficient. Default value of 10 ms is overkill.
+ */
+#define INTERNAL_PCI_PM_D3_WAIT		3
+
+#define ISP_SUB_CLASS			0x80
+#define SUB_CLASS_MASK			0xFF00
+
+u32 intel_mid_msgbus_read32_raw(u32 cmd);
+u32 intel_mid_msgbus_read32(u8 port, u32 addr);
+void intel_mid_msgbus_write32_raw(u32 cmd, u32 data);
+void intel_mid_msgbus_write32(u8 port, u32 addr, u32 data);
+u32 intel_mid_msgbus_read32_raw_ext(u32 cmd, u32 cmd_ext);
+void intel_mid_msgbus_write32_raw_ext(u32 cmd, u32 cmd_ext, u32 data);
+u32 intel_mid_soc_stepping(void);
+int intel_mid_dw_i2c_acquire_ownership(void);
+int intel_mid_dw_i2c_release_ownership(void);
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 000000000000..35865462ccf9
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp.h
@@ -0,0 +1,1367 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifdef CSS15
+#include <linux/atomisp_css15.h>
+#else
+
+#ifndef _ATOM_ISP_H
+#define _ATOM_ISP_H
+
+#include <linux/types.h>
+#include <linux/version.h>
+
+/* struct media_device_info.driver_version */
+#define ATOMISP_CSS_VERSION_MASK	0x00ffffff
+#define ATOMISP_CSS_VERSION_15		KERNEL_VERSION(1, 5, 0)
+#define ATOMISP_CSS_VERSION_20		KERNEL_VERSION(2, 0, 0)
+#define ATOMISP_CSS_VERSION_21		KERNEL_VERSION(2, 1, 0)
+
+/* 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 */
+#define V4L2_MBUS_FMT_CUSTOM_M10MO_RAW	0x800b
+
+/* 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 {
+	uint32_t enable;
+	uint32_t width;
+	uint32_t aligned_width;
+	uint32_t height;
+	uint32_t aligned_height;
+	uint32_t bqs_per_grid_cell;
+	uint32_t num_hor_coefs;
+	uint32_t num_ver_coefs;
+};
+
+struct atomisp_dvs_envelop {
+	unsigned int width;
+	unsigned int height;
+};
+
+struct atomisp_grid_info {
+	uint32_t enable;
+	uint32_t use_dmem;
+	uint32_t has_histogram;
+	uint32_t s3a_width;
+	uint32_t s3a_height;
+	uint32_t aligned_width;
+	uint32_t aligned_height;
+	uint32_t s3a_bqs_per_grid_cell;
+	uint32_t deci_factor_log2;
+	uint32_t 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;
+	uint32_t exp_id;
+};
+
+struct atomisp_3a_rgby_output {
+	uint32_t r;
+	uint32_t g;
+	uint32_t b;
+	uint32_t 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;
+	uint32_t width;
+	uint32_t height;
+	uint32_t stride; /* in bytes */
+	uint32_t exp_id; /* exposure ID */
+	uint32_t *effective_width; /* mipi packets valid data size */
+};
+
+struct atomisp_metadata {
+	void __user *data;
+	uint32_t width;
+	uint32_t height;
+	uint32_t stride; /* in bytes */
+	uint32_t exp_id; /* exposure ID */
+	uint32_t *effective_width; /* mipi packets valid data size */
+};
+
+struct atomisp_ext_isp_ctrl {
+	uint32_t id;
+	uint32_t 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;
+	uint32_t exp_id; /* exposure ID */
+	uint32_t 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 {
+	uint32_t metadata_height;
+	uint32_t metadata_stride;
+};
+
+/*
+ * Generic resolution structure.
+ */
+struct atomisp_resolution {
+	uint32_t width;  /**< Width */
+	uint32_t height; /**< Height */
+};
+
+/*
+ * This specifies the coordinates (x,y)
+ */
+struct atomisp_zoom_point {
+	int32_t x; /**< x coordinate */
+	int32_t 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 {
+	uint32_t dx; /**< Horizontal zoom factor */
+	uint32_t 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 {
+	uint32_t exp_id;
+	uint32_t width_y;
+	uint32_t height_y;
+	uint32_t width_uv;
+	uint32_t height_uv;
+	uint32_t *xcoords_y;
+	uint32_t *ycoords_y;
+	uint32_t *xcoords_uv;
+	uint32_t *ycoords_uv;
+};
+
+struct atomisp_formats_config {
+	uint32_t 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_dvs2_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.
+	 */
+	uint32_t	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!!
+	 */
+	uint32_t	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	(1 << 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;
+	uint8_t binning_factor_x; /* horizontal binning factor used */
+	uint8_t binning_factor_y; /* vertical binning factor used */
+	uint16_t 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           (1U << 0)
+#define ATOMISP_FOCUS_STATUS_ACCEPTS_NEW_MOVE (1U << 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_acc_type {
+	ATOMISP_ACC_STANDALONE,	/* Stand-alone acceleration */
+	ATOMISP_ACC_OUTPUT,	/* Accelerator stage on output frame */
+	ATOMISP_ACC_VIEWFINDER	/* Accelerator stage on viewfinder frame */
+};
+
+enum atomisp_acc_arg_type {
+	ATOMISP_ACC_ARG_SCALAR_IN,    /* Scalar input argument */
+	ATOMISP_ACC_ARG_SCALAR_OUT,   /* Scalar output argument */
+	ATOMISP_ACC_ARG_SCALAR_IO,    /* Scalar in/output argument */
+	ATOMISP_ACC_ARG_PTR_IN,	     /* Pointer input argument */
+	ATOMISP_ACC_ARG_PTR_OUT,	     /* Pointer output argument */
+	ATOMISP_ACC_ARG_PTR_IO,	     /* Pointer in/output argument */
+	ATOMISP_ARG_PTR_NOFLUSH,  /* Pointer argument will not be flushed */
+	ATOMISP_ARG_PTR_STABLE,   /* Pointer input argument that is stable */
+	ATOMISP_ACC_ARG_FRAME	     /* Frame argument */
+};
+
+/** ISP memories, isp2400 */
+enum atomisp_acc_memory {
+	ATOMISP_ACC_MEMORY_PMEM0 = 0,
+	ATOMISP_ACC_MEMORY_DMEM0,
+	/* for backward compatibility */
+	ATOMISP_ACC_MEMORY_DMEM = ATOMISP_ACC_MEMORY_DMEM0,
+	ATOMISP_ACC_MEMORY_VMEM0,
+	ATOMISP_ACC_MEMORY_VAMEM0,
+	ATOMISP_ACC_MEMORY_VAMEM1,
+	ATOMISP_ACC_MEMORY_VAMEM2,
+	ATOMISP_ACC_MEMORY_HMEM0,
+	ATOMISP_ACC_NR_MEMORY
+};
+
+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
+
+struct atomisp_sp_arg {
+	enum atomisp_acc_arg_type type;	/* Type  of SP argument */
+	void                    *value;	/* Value of SP argument */
+	unsigned int             size;	/* Size  of SP argument */
+};
+
+/* Acceleration API */
+
+/* For CSS 1.0 only */
+struct atomisp_acc_fw_arg {
+	unsigned int fw_handle;
+	unsigned int index;
+	void __user *value;
+	size_t size;
+};
+
+/*
+ * Set arguments after first mapping with ATOMISP_IOC_ACC_S_MAPPED_ARG.
+ */
+struct atomisp_acc_s_mapped_arg {
+	unsigned int fw_handle;
+	__u32 memory;			/* one of enum atomisp_acc_memory */
+	size_t length;
+	unsigned long css_ptr;
+};
+
+struct atomisp_acc_fw_abort {
+	unsigned int fw_handle;
+	/* Timeout in us */
+	unsigned int timeout;
+};
+
+struct atomisp_acc_fw_load {
+	unsigned int size;
+	unsigned int fw_handle;
+	void __user *data;
+};
+
+/*
+ * Load firmware to specified pipeline.
+ */
+struct atomisp_acc_fw_load_to_pipe {
+	__u32 flags;			/* Flags, see below for valid values */
+	unsigned int fw_handle;		/* Handle, filled by kernel. */
+	__u32 size;			/* Firmware binary size */
+	void __user *data;		/* Pointer to firmware */
+	__u32 type;			/* Binary type */
+	__u32 reserved[3];		/* Set to zero */
+};
+/*
+ * Set Senor run mode
+ */
+struct atomisp_s_runmode {
+	__u32 mode;
+};
+
+#define ATOMISP_ACC_FW_LOAD_FL_PREVIEW		(1 << 0)
+#define ATOMISP_ACC_FW_LOAD_FL_COPY		(1 << 1)
+#define ATOMISP_ACC_FW_LOAD_FL_VIDEO		(1 << 2)
+#define ATOMISP_ACC_FW_LOAD_FL_CAPTURE		(1 << 3)
+#define ATOMISP_ACC_FW_LOAD_FL_ACC		(1 << 4)
+#define ATOMISP_ACC_FW_LOAD_FL_ENABLE		(1 << 16)
+
+#define ATOMISP_ACC_FW_LOAD_TYPE_NONE		0 /* Normal binary: don't use */
+#define ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT		1 /* Stage on output */
+#define ATOMISP_ACC_FW_LOAD_TYPE_VIEWFINDER	2 /* Stage on viewfinder */
+#define ATOMISP_ACC_FW_LOAD_TYPE_STANDALONE	3 /* Stand-alone acceleration */
+
+struct atomisp_acc_map {
+	__u32 flags;			/* Flags, see list below */
+	__u32 length;			/* Length of data in bytes */
+	void __user *user_ptr;		/* Pointer into user space */
+	unsigned long css_ptr;		/* Pointer into CSS address space */
+	__u32 reserved[4];		/* Set to zero */
+};
+
+#define ATOMISP_MAP_FLAG_NOFLUSH	0x0001	/* Do not flush cache */
+#define ATOMISP_MAP_FLAG_CACHED		0x0002	/* Enable cache */
+
+struct atomisp_acc_state {
+	__u32 flags;			/* Flags, see list below */
+#define ATOMISP_STATE_FLAG_ENABLE	ATOMISP_ACC_FW_LOAD_FL_ENABLE
+	unsigned int fw_handle;
+};
+
+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)
+
+/* Accelerate ioctls */
+#define ATOMISP_IOC_ACC_LOAD \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 24, struct atomisp_acc_fw_load)
+
+#define ATOMISP_IOC_ACC_UNLOAD \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 24, unsigned int)
+
+/* For CSS 1.0 only */
+#define ATOMISP_IOC_ACC_S_ARG \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 24, struct atomisp_acc_fw_arg)
+
+#define ATOMISP_IOC_ACC_START \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 24, unsigned int)
+
+#define ATOMISP_IOC_ACC_WAIT \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 25, unsigned int)
+
+#define ATOMISP_IOC_ACC_ABORT \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 25, struct atomisp_acc_fw_abort)
+
+#define ATOMISP_IOC_ACC_DESTAB \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 25, struct atomisp_acc_fw_arg)
+
+/* 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)
+
+/*
+ * Ioctls to map and unmap user buffers to CSS address space for acceleration.
+ * User fills fields length and user_ptr and sets other fields to zero,
+ * kernel may modify the flags and sets css_ptr.
+ */
+#define ATOMISP_IOC_ACC_MAP \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_map)
+
+/* User fills fields length, user_ptr, and css_ptr and zeroes other fields. */
+#define ATOMISP_IOC_ACC_UNMAP \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_map)
+
+#define ATOMISP_IOC_ACC_S_MAPPED_ARG \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_s_mapped_arg)
+
+#define ATOMISP_IOC_ACC_LOAD_TO_PIPE \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 31, struct atomisp_acc_fw_load_to_pipe)
+
+#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_S_ACC_STATE \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 41, struct atomisp_acc_state)
+
+#define ATOMISP_IOC_G_ACC_STATE \
+	_IOR('v', BASE_VIDIOC_PRIVATE + 41, struct atomisp_acc_state)
+
+#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_TIMEING               (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            (1 << 0)
+#define V4L2_2A_STATUS_AWB_READY           (1 << 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 */
+#endif /* CSS15*/
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 000000000000..5390b97ac6e7
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp_gmin_platform.h
@@ -0,0 +1,40 @@
+/*
+ * 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"
+
+const struct atomisp_camera_caps *atomisp_get_default_camera_caps(void);
+const struct atomisp_platform_data *atomisp_get_platform_data(void);
+const struct camera_af_platform_data *camera_get_af_platform_data(void);
+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_config_var(struct device *dev, const char *var, char *out, size_t *out_len);
+int gmin_get_var_int(struct device *dev, const char *var, int def);
+int camera_sensor_csi(struct v4l2_subdev *sd, u32 port,
+                      u32 lanes, u32 format, u32 bayer_order, int flag);
+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 000000000000..dbac2b777dad
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/atomisp_platform.h
@@ -0,0 +1,262 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef ATOMISP_PLATFORM_H_
+#define ATOMISP_PLATFORM_H_
+
+#include <linux/i2c.h>
+#include <linux/sfi.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 */
+};
+
+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;
+};
+
+/* Describe the capacities of one single sensor. */
+struct atomisp_sensor_caps {
+	/* The number of streams this sensor can output. */
+	int stream_num;
+	bool is_slave;
+};
+
+/* Describe the capacities of sensors connected to one camera port. */
+struct atomisp_camera_caps {
+	/* The number of sensors connected to this camera port. */
+	int sensor_num;
+	/* The capacities of each sensor. */
+	struct atomisp_sensor_caps sensor[MAX_SENSORS_PER_PORT];
+	/* Define whether stream control is required for multiple streams. */
+	bool multi_stream_ctrl;
+};
+
+/*
+ *  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 (*gpio_ctrl)(struct v4l2_subdev *subdev, int flag);
+	int (*flisclk_ctrl)(struct v4l2_subdev *subdev, int flag);
+	int (*power_ctrl)(struct v4l2_subdev *subdev, int flag);
+	int (*csi_cfg)(struct v4l2_subdev *subdev, int flag);
+	bool (*low_fps)(void);
+	int (*platform_init)(struct i2c_client *);
+	int (*platform_deinit)(void);
+	char *(*msr_file_name)(void);
+	struct atomisp_camera_caps *(*get_camera_caps)(void);
+	int (*gpio_intr_ctrl)(struct v4l2_subdev *subdev);
+
+	/* New G-Min power and GPIO interface, replaces
+	 * power/gpio_ctrl with methods 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_af_platform_data {
+	int (*power_ctrl)(struct v4l2_subdev *subdev, int flag);
+};
+
+const struct camera_af_platform_data *camera_get_af_platform_data(void);
+
+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;
+	uint32_t                        metadata_width;
+	uint32_t                        metadata_height;
+	const uint32_t                  *metadata_effective_width;
+};
+
+extern const struct atomisp_platform_data *atomisp_get_platform_data(void);
+extern const struct atomisp_camera_caps *atomisp_get_default_camera_caps(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_MFLD	__IS_SOC(0x27)
+#define IS_BYT	__IS_SOC(0x37)
+#define IS_CHT	__IS_SOC(0x4C)
+#define IS_MOFD	__IS_SOC(0x5A)
+
+#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 000000000000..589f4eae38ca
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/libmsrlisthelper.h
@@ -0,0 +1,32 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef __LIBMSRLISTHELPER_H__
+#define __LIBMSRLISTHELPER_H__
+
+struct i2c_client;
+struct firmware;
+
+extern int load_msr_list(struct i2c_client *client, char *path,
+		const struct firmware **fw);
+extern int apply_msr_data(struct i2c_client *client, const struct firmware *fw);
+extern void release_msr_list(struct i2c_client *client,
+		const struct firmware *fw);
+
+
+#endif /* ifndef __LIBMSRLISTHELPER_H__ */
diff --git a/drivers/staging/media/atomisp/include/linux/vlv2_plat_clock.h b/drivers/staging/media/atomisp/include/linux/vlv2_plat_clock.h
new file mode 100644
index 000000000000..ed709bdd6498
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/linux/vlv2_plat_clock.h
@@ -0,0 +1,30 @@
+/*
+ * vlv2_plat_clock.h
+ *
+ * Copyright (C) 2013 Intel Corp
+ * Author: Asutosh Pathak <asutosh.pathak@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * 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; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef __VLV2_PLAT_CLOCK_H
+#define __VLV2_PLAT_CLOCK_H
+
+int vlv2_plat_set_clock_freq(int clock_num, int freq_type);
+int vlv2_plat_get_clock_freq(int clock_num);
+
+int vlv2_plat_configure_clock(int clock_num, u32 conf);
+int vlv2_plat_get_clock_status(int clock_num);
+
+#endif /* __VLV2_PLAT_CLOCK_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 000000000000..7d6a8c05dd52
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/media/lm3554.h
@@ -0,0 +1,136 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef _LM3554_H_
+#define _LM3554_H_
+
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+
+#define LM3554_NAME    "lm3554"
+#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 {
+	int gpio_torch;
+	int gpio_strobe;
+	int 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/media/lm3642.h b/drivers/staging/media/atomisp/include/media/lm3642.h
new file mode 100644
index 000000000000..545d95763335
--- /dev/null
+++ b/drivers/staging/media/atomisp/include/media/lm3642.h
@@ -0,0 +1,153 @@
+/*
+ * include/media/lm3642.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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.
+ */
+
+#ifndef _LM3642_H_
+#define _LM3642_H_
+
+#include <linux/videodev2.h>
+#include <media/v4l2-subdev.h>
+
+#define LM3642_NAME    "lm3642"
+#define LM3642_ID      3642
+
+#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, \
+	}
+
+
+/* Default Values */
+#define LM3642_DEFAULT_TIMEOUT           300U
+#define LM3642_DEFAULT_RAMP_TIME	 0x10 /* 1.024ms */
+#define LM3642_DEFAULT_INDICATOR_CURRENT 0x01 /* 1.88A */
+#define LM3642_DEFAULT_FLASH_CURRENT	 0x0f /* 1500mA */
+
+/* Value settings for Flash Time-out Duration*/
+#define LM3642_MIN_TIMEOUT              100U
+#define LM3642_MAX_TIMEOUT              800U
+#define LM3642_TIMEOUT_STEPSIZE         100U
+
+/* Flash modes */
+#define LM3642_MODE_SHUTDOWN            0
+#define LM3642_MODE_INDICATOR           1
+#define LM3642_MODE_TORCH               2
+#define LM3642_MODE_FLASH               3
+
+/* timer delay time */
+#define LM3642_TIMER_DELAY		5
+
+/* Percentage <-> value macros */
+#define LM3642_MIN_PERCENT                   0U
+#define LM3642_MAX_PERCENT                   100U
+#define LM3642_CLAMP_PERCENTAGE(val) \
+	clamp(val, LM3642_MIN_PERCENT, LM3642_MAX_PERCENT)
+
+#define LM3642_VALUE_TO_PERCENT(v, step) \
+	(((((unsigned long)((v)+1))*(step))+50)/100)
+#define LM3642_PERCENT_TO_VALUE(p, step) \
+	(((((unsigned long)(p))*100)+((step)>>1))/(step)-1)
+
+/* Product specific limits
+ * TODO: get these from platform data */
+#define LM3642_FLASH_MAX_LVL   0x0F /* 1500mA */
+#define LM3642_TORCH_MAX_LVL   0x07 /* 187mA */
+#define LM3642_INDICATOR_MAX_LVL   0x01 /* 1.88A */
+
+/* Flash brightness, input is percentage, output is [0..15] */
+#define LM3642_FLASH_STEP	\
+	((100ul*(LM3642_MAX_PERCENT) \
+	+((LM3642_FLASH_MAX_LVL+1)>>1)) \
+	/((LM3642_FLASH_MAX_LVL+1)))
+#define LM3642_FLASH_DEFAULT_BRIGHTNESS \
+	LM3642_VALUE_TO_PERCENT(15, LM3642_FLASH_STEP)
+
+/* Torch brightness, input is percentage, output is [0..7] */
+#define LM3642_TORCH_STEP	\
+	((100ul*(LM3642_MAX_PERCENT) \
+	+((LM3642_TORCH_MAX_LVL+1)>>1)) \
+	/((LM3642_TORCH_MAX_LVL+1)))
+#define LM3642_TORCH_DEFAULT_BRIGHTNESS \
+	LM3642_VALUE_TO_PERCENT(0, LM3642_TORCH_STEP)
+
+/* Indicator brightness, input is percentage, output is [0..1] */
+#define LM3642_INDICATOR_STEP	\
+	((100ul*(LM3642_MAX_PERCENT) \
+	+((LM3642_INDICATOR_MAX_LVL+1)>>1)) \
+	/((LM3642_INDICATOR_MAX_LVL+1)))
+#define LM3642_INDICATOR_DEFAULT_BRIGHTNESS \
+	LM3642_VALUE_TO_PERCENT(1, LM3642_INDICATOR_STEP)
+
+/*
+ * lm3642_platform_data - Flash controller platform data
+ */
+struct lm3642_platform_data {
+	int gpio_torch;
+	int gpio_strobe;
+	int (*power_ctrl)(struct v4l2_subdev *subdev, int on);
+
+	unsigned int torch_en;
+	unsigned int flash_en;
+	unsigned int tx_en;
+	unsigned int ivfm_en;
+};
+
+#endif /* _LM3642_H_ */
+
diff --git a/drivers/staging/media/atomisp/pci/Kconfig b/drivers/staging/media/atomisp/pci/Kconfig
new file mode 100644
index 000000000000..a72421431c7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/Kconfig
@@ -0,0 +1,13 @@
+#
+# Kconfig for ISP driver
+#
+
+config VIDEO_ATOMISP
+       tristate "Intel Atom Image Signal Processor Driver"
+       depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
+       select VIDEOBUF_VMALLOC
+        ---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
diff --git a/drivers/staging/media/atomisp/pci/Makefile b/drivers/staging/media/atomisp/pci/Makefile
new file mode 100644
index 000000000000..61ad1fbb1ee6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for ISP driver
+#
+
+obj-$(CONFIG_VIDEO_ATOMISP) += atomisp2/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/Makefile b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
new file mode 100644
index 000000000000..3fa7c1c1479f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/Makefile
@@ -0,0 +1,355 @@
+atomisp-objs += \
+	atomisp_drvfs.o \
+	atomisp_file.o \
+	css2400/sh_css_mipi.o \
+	css2400/runtime/pipeline/src/pipeline.o \
+	css2400/runtime/spctrl/src/spctrl.o \
+	css2400/runtime/rmgr/src/rmgr.o \
+	css2400/runtime/rmgr/src/rmgr_vbuf.o \
+	css2400/runtime/isp_param/src/isp_param.o \
+	css2400/runtime/inputfifo/src/inputfifo.o \
+	css2400/runtime/queue/src/queue_access.o \
+	css2400/runtime/queue/src/queue.o \
+	css2400/runtime/frame/src/frame.o \
+	css2400/runtime/eventq/src/eventq.o \
+	css2400/runtime/binary/src/binary.o \
+	css2400/runtime/timer/src/timer.o \
+	css2400/runtime/isys/src/csi_rx_rmgr.o \
+	css2400/runtime/isys/src/isys_stream2mmio_rmgr.o \
+	css2400/runtime/isys/src/virtual_isys.o \
+	css2400/runtime/isys/src/rx.o \
+	css2400/runtime/isys/src/isys_dma_rmgr.o \
+	css2400/runtime/isys/src/ibuf_ctrl_rmgr.o \
+	css2400/runtime/isys/src/isys_init.o \
+	css2400/runtime/bufq/src/bufq.o \
+	css2400/runtime/ifmtr/src/ifmtr.o \
+	css2400/runtime/debug/src/ia_css_debug.o \
+	css2400/runtime/event/src/event.o \
+	css2400/sh_css_sp.o \
+	css2400/css_2400_system/spmem_dump.o \
+	css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.o \
+	css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.o \
+	css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.o \
+	css2400/sh_css_stream_format.o \
+	css2400/sh_css_hrt.o \
+	css2400/sh_css_properties.o \
+	css2400/memory_realloc.o \
+	css2400/hive_isp_css_shared/host/tag.o \
+	css2400/sh_css_params.o \
+	css2400/sh_css.o \
+	css2400/isp/kernels/hdr/ia_css_hdr.host.o \
+	css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.o \
+	css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.o \
+	css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.o \
+	css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.o \
+	css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.o \
+	css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.o \
+	css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.o \
+	css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.o \
+	css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.o \
+	css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.o \
+	css2400/isp/kernels/output/output_1.0/ia_css_output.host.o \
+	css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.o \
+	css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.o \
+	css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.o \
+	css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.o \
+	css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.o \
+	css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.o \
+	css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.o \
+	css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.o \
+	css2400/isp/kernels/dpc2/ia_css_dpc2.host.o \
+	css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.o \
+	css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.o \
+	css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.o \
+	css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.o \
+	css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.o \
+	css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.o \
+	css2400/isp/kernels/bh/bh_2/ia_css_bh.host.o \
+	css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.o \
+	css2400/isp/kernels/bnlm/ia_css_bnlm.host.o \
+	css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.o \
+	css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.o \
+	css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.o \
+	css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.o \
+	css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.o \
+	css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.o \
+	css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.o \
+	css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.o \
+	css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.o \
+	css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.o \
+	css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.o \
+	css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.o \
+	css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.o \
+	css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.o \
+	css2400/isp/kernels/de/de_1.0/ia_css_de.host.o \
+	css2400/isp/kernels/de/de_2/ia_css_de2.host.o \
+	css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.o \
+	css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.o \
+	css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.o \
+	css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.o \
+	css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.o \
+	css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.o \
+	css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.o \
+	css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.o \
+	css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.o \
+	css2400/isp/kernels/ob/ob2/ia_css_ob2.host.o \
+	css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.o \
+	css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.o \
+	css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.o \
+	css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.o \
+	css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.o \
+	css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.o \
+	css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.o \
+	css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.o \
+	css2400/sh_css_pipe.o \
+	css2400/ia_css_device_access.o \
+	css2400/sh_css_host_data.o \
+	css2400/sh_css_mmu.o \
+	css2400/sh_css_metadata.o \
+	css2400/base/refcount/src/refcount.o \
+	css2400/base/circbuf/src/circbuf.o \
+	css2400/sh_css_irq.o \
+	css2400/camera/pipe/src/pipe_binarydesc.o \
+	css2400/camera/pipe/src/pipe_util.o \
+	css2400/camera/pipe/src/pipe_stagedesc.o \
+	css2400/camera/util/src/util.o \
+	css2400/sh_css_metrics.o \
+	css2400/sh_css_version.o \
+	css2400/ia_css_memory_access.o \
+	css2400/sh_css_param_shading.o \
+	css2400/sh_css_morph.o \
+	css2400/sh_css_firmware.o \
+	css2400/hive_isp_css_common/host/isp.o \
+	css2400/hive_isp_css_common/host/gdc.o \
+	css2400/hive_isp_css_common/host/sp.o \
+	css2400/hive_isp_css_common/host/vmem.o \
+	css2400/hive_isp_css_common/host/dma.o \
+	css2400/hive_isp_css_common/host/input_formatter.o \
+	css2400/hive_isp_css_common/host/debug.o \
+	css2400/hive_isp_css_common/host/hmem.o \
+	css2400/hive_isp_css_common/host/gp_device.o \
+	css2400/hive_isp_css_common/host/fifo_monitor.o \
+	css2400/hive_isp_css_common/host/gp_timer.o \
+	css2400/hive_isp_css_common/host/irq.o \
+	css2400/hive_isp_css_common/host/input_system.o \
+	css2400/hive_isp_css_common/host/timed_ctrl.o \
+	css2400/hive_isp_css_common/host/mmu.o \
+	css2400/hive_isp_css_common/host/event_fifo.o \
+	css2400/sh_css_param_dvs.o \
+	css2400/sh_css_shading.o \
+	css2400/sh_css_stream.o \
+	mmu/sh_mmu_mrfld.o \
+	mmu/isp_mmu.o \
+	atomisp_acc.o \
+	atomisp_compat_css20.o \
+	atomisp_fops.o \
+	atomisp_subdev.o \
+	atomisp_ioctl.o \
+	atomisp_compat_ioctl32.o \
+	atomisp_csi2.o \
+	atomisp_cmd.o \
+	atomisp_tpg.o \
+	hmm/hmm_vm.o \
+	hmm/hmm.o \
+	hmm/hmm_bo.o \
+	hmm/hmm_reserved_pool.o \
+	hmm/hmm_dynamic_pool.o \
+	hrt/hive_isp_css_mm_hrt.o \
+	atomisp_v4l2.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= \
+	css2400/css_2401_system/spmem_dump.o \
+	css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.o \
+	css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.o \
+	css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.o \
+	css2400/css_2401_csi2p_system/spmem_dump.o \
+	css2400/css_2401_csi2p_system/host/isys_stream2mmio.o \
+	css2400/css_2401_csi2p_system/host/ibuf_ctrl.o \
+	css2400/css_2401_csi2p_system/host/isys_irq.o \
+	css2400/css_2401_csi2p_system/host/isys_dma.o \
+	css2400/css_2401_csi2p_system/host/csi_rx.o \
+	css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.o \
+	css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.o \
+	css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.o \
+
+#	-I$(atomisp)/css2400/css_2401_csi2p_system/ \
+#	-I$(atomisp)/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ \
+#	-I$(atomisp)/css2400/css_2401_csi2p_system/host/ \
+#	-I$(atomisp)/css2400/css_2401_csi2p_system/hrt/ \
+#	-I$(atomisp)/css2400/css_2401_system/hive_isp_css_2401_system_generated/ \
+#	-I$(atomisp)/css2400/css_2401_system/hrt/ \
+
+
+
+obj-$(CONFIG_VIDEO_ATOMISP) += atomisp.o
+
+atomisp = $(srctree)/drivers/staging/media/atomisp/pci/atomisp2
+
+INCLUDES += \
+	-I$(atomisp)/ \
+	-I$(atomisp)/css2400/ \
+	-I$(atomisp)/hrt/ \
+	-I$(atomisp)/include/ \
+	-I$(atomisp)/include/hmm/ \
+	-I$(atomisp)/include/mmu/ \
+	-I$(atomisp)/css2400/base/circbuf/interface/ \
+	-I$(atomisp)/css2400/base/refcount/interface/ \
+	-I$(atomisp)/css2400/camera/pipe/interface/ \
+	-I$(atomisp)/css2400/camera/util/interface/ \
+	-I$(atomisp)/css2400/css_2400_system/ \
+	-I$(atomisp)/css2400/css_2400_system/hive_isp_css_2400_system_generated/ \
+	-I$(atomisp)/css2400/css_2400_system/hrt/ \
+	-I$(atomisp)/css2400/hive_isp_css_common/ \
+	-I$(atomisp)/css2400/hive_isp_css_common/host/ \
+	-I$(atomisp)/css2400/hive_isp_css_include/ \
+	-I$(atomisp)/css2400/hive_isp_css_include/device_access/ \
+	-I$(atomisp)/css2400/hive_isp_css_include/host/ \
+	-I$(atomisp)/css2400/hive_isp_css_include/memory_access/ \
+	-I$(atomisp)/css2400/hive_isp_css_shared/ \
+	-I$(atomisp)/css2400/hive_isp_css_shared/host/ \
+	-I$(atomisp)/css2400/isp/kernels/ \
+	-I$(atomisp)/css2400/isp/kernels/aa/aa_2/ \
+	-I$(atomisp)/css2400/isp/kernels/anr/anr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/anr/anr_2/ \
+	-I$(atomisp)/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/bh/bh_2/ \
+	-I$(atomisp)/css2400/isp/kernels/bnlm/ \
+	-I$(atomisp)/css2400/isp/kernels/bnr/ \
+	-I$(atomisp)/css2400/isp/kernels/bnr/bnr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/bnr/bnr2_2/ \
+	-I$(atomisp)/css2400/isp/kernels/cnr/ \
+	-I$(atomisp)/css2400/isp/kernels/cnr/cnr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/cnr/cnr_2/ \
+	-I$(atomisp)/css2400/isp/kernels/conversion/ \
+	-I$(atomisp)/css2400/isp/kernels/conversion/conversion_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/copy_output/ \
+	-I$(atomisp)/css2400/isp/kernels/copy_output/copy_output_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/crop/ \
+	-I$(atomisp)/css2400/isp/kernels/crop/crop_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/csc/ \
+	-I$(atomisp)/css2400/isp/kernels/csc/csc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ctc/ \
+	-I$(atomisp)/css2400/isp/kernels/ctc/ctc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ctc/ctc1_5/ \
+	-I$(atomisp)/css2400/isp/kernels/ctc/ctc2/ \
+	-I$(atomisp)/css2400/isp/kernels/de/ \
+	-I$(atomisp)/css2400/isp/kernels/de/de_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/de/de_2/ \
+	-I$(atomisp)/css2400/isp/kernels/dpc2/ \
+	-I$(atomisp)/css2400/isp/kernels/dp/ \
+	-I$(atomisp)/css2400/isp/kernels/dp/dp_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/dvs/ \
+	-I$(atomisp)/css2400/isp/kernels/dvs/dvs_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/eed1_8/ \
+	-I$(atomisp)/css2400/isp/kernels/fc/ \
+	-I$(atomisp)/css2400/isp/kernels/fc/fc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/fixedbds/ \
+	-I$(atomisp)/css2400/isp/kernels/fixedbds/fixedbds_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/fpn/ \
+	-I$(atomisp)/css2400/isp/kernels/fpn/fpn_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/gc/ \
+	-I$(atomisp)/css2400/isp/kernels/gc/gc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/gc/gc_2/ \
+	-I$(atomisp)/css2400/isp/kernels/hdr/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/bayer_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/common/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/plane_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/yuv420_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/io_ls/yuv444_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/common/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/iterator/ \
+	-I$(atomisp)/css2400/isp/kernels/iterator/iterator_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/macc/ \
+	-I$(atomisp)/css2400/isp/kernels/macc/macc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/macc/macc1_5/ \
+	-I$(atomisp)/css2400/isp/kernels/norm/ \
+	-I$(atomisp)/css2400/isp/kernels/norm/norm_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ob/ \
+	-I$(atomisp)/css2400/isp/kernels/ob/ob_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ob/ob2/ \
+	-I$(atomisp)/css2400/isp/kernels/output/ \
+	-I$(atomisp)/css2400/isp/kernels/output/output_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/qplane/ \
+	-I$(atomisp)/css2400/isp/kernels/qplane/qplane_2/ \
+	-I$(atomisp)/css2400/isp/kernels/raw_aa_binning/ \
+	-I$(atomisp)/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/raw/ \
+	-I$(atomisp)/css2400/isp/kernels/raw/raw_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ref/ \
+	-I$(atomisp)/css2400/isp/kernels/ref/ref_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/s3a/ \
+	-I$(atomisp)/css2400/isp/kernels/s3a/s3a_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/s3a_stat_ls/ \
+	-I$(atomisp)/css2400/isp/kernels/scale/ \
+	-I$(atomisp)/css2400/isp/kernels/scale/scale_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/sc/ \
+	-I$(atomisp)/css2400/isp/kernels/sc/sc_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/sdis/ \
+	-I$(atomisp)/css2400/isp/kernels/sdis/common/ \
+	-I$(atomisp)/css2400/isp/kernels/sdis/sdis_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/sdis/sdis_2/ \
+	-I$(atomisp)/css2400/isp/kernels/tdf/ \
+	-I$(atomisp)/css2400/isp/kernels/tdf/tdf_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/tnr/ \
+	-I$(atomisp)/css2400/isp/kernels/tnr/tnr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/tnr/tnr3/ \
+	-I$(atomisp)/css2400/isp/kernels/uds/ \
+	-I$(atomisp)/css2400/isp/kernels/uds/uds_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/vf/ \
+	-I$(atomisp)/css2400/isp/kernels/vf/vf_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/wb/ \
+	-I$(atomisp)/css2400/isp/kernels/wb/wb_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/xnr/ \
+	-I$(atomisp)/css2400/isp/kernels/xnr/xnr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/xnr/xnr_3.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ynr/ \
+	-I$(atomisp)/css2400/isp/kernels/ynr/ynr_1.0/ \
+	-I$(atomisp)/css2400/isp/kernels/ynr/ynr_2/ \
+	-I$(atomisp)/css2400/isp/kernels/yuv_ls \
+	-I$(atomisp)/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ \
+	-I$(atomisp)/css2400/isp/modes/interface/ \
+	-I$(atomisp)/css2400/runtime/binary/interface/ \
+	-I$(atomisp)/css2400/runtime/bufq/interface/ \
+	-I$(atomisp)/css2400/runtime/debug/interface/ \
+	-I$(atomisp)/css2400/runtime/event/interface/ \
+	-I$(atomisp)/css2400/runtime/eventq/interface/ \
+	-I$(atomisp)/css2400/runtime/frame/interface/ \
+	-I$(atomisp)/css2400/runtime/ifmtr/interface/ \
+	-I$(atomisp)/css2400/runtime/inputfifo/interface/ \
+	-I$(atomisp)/css2400/runtime/isp_param/interface/ \
+	-I$(atomisp)/css2400/runtime/isys/interface/ \
+	-I$(atomisp)/css2400/runtime/isys/src/ \
+	-I$(atomisp)/css2400/runtime/pipeline/interface/ \
+	-I$(atomisp)/css2400/runtime/queue/interface/ \
+	-I$(atomisp)/css2400/runtime/queue/src/ \
+	-I$(atomisp)/css2400/runtime/rmgr/interface/ \
+	-I$(atomisp)/css2400/runtime/spctrl/interface/ \
+	-I$(atomisp)/css2400/runtime/tagger/interface/
+
+ifeq ($(CONFIG_ION),y)
+INCLUDES += -I$(srctree)/drivers/staging/android/ion
+endif
+
+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
+
+DEFINES += -DATOMISP_POSTFIX=\"css2400b0_v21\" -DISP2400B0
+DEFINES += -DSYSTEM_hive_isp_css_2400_system -DISP2400
+
+ccflags-y += $(INCLUDES) $(DEFINES) -fno-common -Werror
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp-regs.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp-regs.h
new file mode 100644
index 000000000000..513a430ee01a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp-regs.h
@@ -0,0 +1,209 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef ATOMISP_REGS_H
+#define ATOMISP_REGS_H
+
+/* common register definitions */
+#define PUNIT_PORT		0x04
+#define CCK_PORT		0x14
+
+#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
+
+#if defined(ISP2401)
+#define ISP_FREQ_MAX	ISP_FREQ_320MHZ
+#else
+#define ISP_FREQ_MAX	ISP_FREQ_400MHZ
+#endif
+
+/* 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/atomisp2/atomisp_acc.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_acc.c
new file mode 100644
index 000000000000..1eac329339b7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_acc.c
@@ -0,0 +1,608 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+/*
+ * This file implements loadable acceleration firmware API,
+ * including ioctls to map and unmap acceleration parameters and buffers.
+ */
+
+#include <linux/init.h>
+#include <media/v4l2-event.h>
+
+#include "atomisp_acc.h"
+#include "atomisp_internal.h"
+#include "atomisp_compat.h"
+#include "atomisp_cmd.h"
+
+#include "hrt/hive_isp_css_mm_hrt.h"
+#include "memory_access/memory_access.h"
+#include "ia_css.h"
+
+static const struct {
+	unsigned int flag;
+	enum atomisp_css_pipe_id pipe_id;
+} acc_flag_to_pipe[] = {
+	{ ATOMISP_ACC_FW_LOAD_FL_PREVIEW, CSS_PIPE_ID_PREVIEW },
+	{ ATOMISP_ACC_FW_LOAD_FL_COPY, CSS_PIPE_ID_COPY },
+	{ ATOMISP_ACC_FW_LOAD_FL_VIDEO, CSS_PIPE_ID_VIDEO },
+	{ ATOMISP_ACC_FW_LOAD_FL_CAPTURE, CSS_PIPE_ID_CAPTURE },
+	{ ATOMISP_ACC_FW_LOAD_FL_ACC, CSS_PIPE_ID_ACC }
+};
+
+/*
+ * Allocate struct atomisp_acc_fw along with space for firmware.
+ * The returned struct atomisp_acc_fw is cleared (firmware region is not).
+ */
+static struct atomisp_acc_fw *acc_alloc_fw(unsigned int fw_size)
+{
+	struct atomisp_acc_fw *acc_fw;
+
+	acc_fw = kzalloc(sizeof(*acc_fw), GFP_KERNEL);
+	if (!acc_fw)
+		return NULL;
+
+	acc_fw->fw = vmalloc(fw_size);
+	if (!acc_fw->fw) {
+		kfree(acc_fw);
+		return NULL;
+	}
+
+	return acc_fw;
+}
+
+static void acc_free_fw(struct atomisp_acc_fw *acc_fw)
+{
+	vfree(acc_fw->fw);
+	kfree(acc_fw);
+}
+
+static struct atomisp_acc_fw *
+acc_get_fw(struct atomisp_sub_device *asd, unsigned int handle)
+{
+	struct atomisp_acc_fw *acc_fw;
+
+	list_for_each_entry(acc_fw, &asd->acc.fw, list)
+		if (acc_fw->handle == handle)
+			return acc_fw;
+
+	return NULL;
+}
+
+static struct atomisp_map *acc_get_map(struct atomisp_sub_device *asd,
+				       unsigned long css_ptr, size_t length)
+{
+	struct atomisp_map *atomisp_map;
+
+	list_for_each_entry(atomisp_map, &asd->acc.memory_maps, list) {
+		if (atomisp_map->ptr == css_ptr &&
+		    atomisp_map->length == length)
+			return atomisp_map;
+	}
+	return NULL;
+}
+
+static int acc_stop_acceleration(struct atomisp_sub_device *asd)
+{
+	int ret;
+
+	ret = atomisp_css_stop_acc_pipe(asd);
+	atomisp_css_destroy_acc_pipe(asd);
+
+	return ret;
+}
+
+void atomisp_acc_cleanup(struct atomisp_device *isp)
+{
+	int i;
+
+	for (i = 0; i < isp->num_of_streams; i++)
+		ida_destroy(&isp->asd[i].acc.ida);
+}
+
+void atomisp_acc_release(struct atomisp_sub_device *asd)
+{
+	struct atomisp_acc_fw *acc_fw, *ta;
+	struct atomisp_map *atomisp_map, *tm;
+
+	/* Stop acceleration if already running */
+	if (asd->acc.pipeline)
+		acc_stop_acceleration(asd);
+
+	/* Unload all loaded acceleration binaries */
+	list_for_each_entry_safe(acc_fw, ta, &asd->acc.fw, list) {
+		list_del(&acc_fw->list);
+		ida_remove(&asd->acc.ida, acc_fw->handle);
+		acc_free_fw(acc_fw);
+	}
+
+	/* Free all mapped memory blocks */
+	list_for_each_entry_safe(atomisp_map, tm, &asd->acc.memory_maps, list) {
+		list_del(&atomisp_map->list);
+		hmm_free(atomisp_map->ptr);
+		kfree(atomisp_map);
+	}
+}
+
+int atomisp_acc_load_to_pipe(struct atomisp_sub_device *asd,
+			     struct atomisp_acc_fw_load_to_pipe *user_fw)
+{
+	static const unsigned int pipeline_flags =
+		ATOMISP_ACC_FW_LOAD_FL_PREVIEW | ATOMISP_ACC_FW_LOAD_FL_COPY |
+		ATOMISP_ACC_FW_LOAD_FL_VIDEO |
+		ATOMISP_ACC_FW_LOAD_FL_CAPTURE | ATOMISP_ACC_FW_LOAD_FL_ACC;
+
+	struct atomisp_acc_fw *acc_fw;
+	int handle;
+
+	if (!user_fw->data || user_fw->size < sizeof(*acc_fw->fw))
+		return -EINVAL;
+
+	/* Binary has to be enabled at least for one pipeline */
+	if (!(user_fw->flags & pipeline_flags))
+		return -EINVAL;
+
+	/* We do not support other flags yet */
+	if (user_fw->flags & ~pipeline_flags)
+		return -EINVAL;
+
+	if (user_fw->type < ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT ||
+	    user_fw->type > ATOMISP_ACC_FW_LOAD_TYPE_STANDALONE)
+		return -EINVAL;
+
+	if (asd->acc.pipeline || asd->acc.extension_mode)
+		return -EBUSY;
+
+	acc_fw = acc_alloc_fw(user_fw->size);
+	if (!acc_fw)
+		return -ENOMEM;
+
+	if (copy_from_user(acc_fw->fw, user_fw->data, user_fw->size)) {
+		acc_free_fw(acc_fw);
+		return -EFAULT;
+	}
+
+	if (!ida_pre_get(&asd->acc.ida, GFP_KERNEL) ||
+	    ida_get_new_above(&asd->acc.ida, 1, &handle)) {
+		acc_free_fw(acc_fw);
+		return -ENOSPC;
+	}
+
+	user_fw->fw_handle = handle;
+	acc_fw->handle = handle;
+	acc_fw->flags = user_fw->flags;
+	acc_fw->type = user_fw->type;
+	acc_fw->fw->handle = handle;
+
+	/*
+	 * correct isp firmware type in order ISP firmware can be appended
+	 * to correct pipe properly
+	 */
+	if (acc_fw->fw->type == ia_css_isp_firmware) {
+		static const int type_to_css[] = {
+			[ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT] =
+				IA_CSS_ACC_OUTPUT,
+			[ATOMISP_ACC_FW_LOAD_TYPE_VIEWFINDER] =
+				IA_CSS_ACC_VIEWFINDER,
+			[ATOMISP_ACC_FW_LOAD_TYPE_STANDALONE] =
+				IA_CSS_ACC_STANDALONE,
+		};
+		acc_fw->fw->info.isp.type = type_to_css[acc_fw->type];
+	}
+
+	list_add_tail(&acc_fw->list, &asd->acc.fw);
+	return 0;
+}
+
+int atomisp_acc_load(struct atomisp_sub_device *asd,
+		     struct atomisp_acc_fw_load *user_fw)
+{
+	struct atomisp_acc_fw_load_to_pipe ltp = {0};
+	int r;
+
+	ltp.flags = ATOMISP_ACC_FW_LOAD_FL_ACC;
+	ltp.type = ATOMISP_ACC_FW_LOAD_TYPE_STANDALONE;
+	ltp.size = user_fw->size;
+	ltp.data = user_fw->data;
+	r = atomisp_acc_load_to_pipe(asd, &ltp);
+	user_fw->fw_handle = ltp.fw_handle;
+	return r;
+}
+
+int atomisp_acc_unload(struct atomisp_sub_device *asd, unsigned int *handle)
+{
+	struct atomisp_acc_fw *acc_fw;
+
+	if (asd->acc.pipeline || asd->acc.extension_mode)
+		return -EBUSY;
+
+	acc_fw = acc_get_fw(asd, *handle);
+	if (!acc_fw)
+		return -EINVAL;
+
+	list_del(&acc_fw->list);
+	ida_remove(&asd->acc.ida, acc_fw->handle);
+	acc_free_fw(acc_fw);
+
+	return 0;
+}
+
+int atomisp_acc_start(struct atomisp_sub_device *asd, unsigned int *handle)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_acc_fw *acc_fw;
+	int ret;
+	unsigned int nbin;
+
+	if (asd->acc.pipeline || asd->acc.extension_mode)
+		return -EBUSY;
+
+	/* Invalidate caches. FIXME: should flush only necessary buffers */
+	wbinvd();
+
+	ret = atomisp_css_create_acc_pipe(asd);
+	if (ret)
+		return ret;
+
+	nbin = 0;
+	list_for_each_entry(acc_fw, &asd->acc.fw, list) {
+		if (*handle != 0 && *handle != acc_fw->handle)
+			continue;
+
+		if (acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_STANDALONE)
+			continue;
+
+		/* Add the binary into the pipeline */
+		ret = atomisp_css_load_acc_binary(asd, acc_fw->fw, nbin);
+		if (ret < 0) {
+			dev_err(isp->dev, "acc_load_binary failed\n");
+			goto err_stage;
+		}
+
+		ret = atomisp_css_set_acc_parameters(acc_fw);
+		if (ret < 0) {
+			dev_err(isp->dev, "acc_set_parameters failed\n");
+			goto err_stage;
+		}
+		nbin++;
+	}
+	if (nbin < 1) {
+		/* Refuse creating pipelines with no binaries */
+		dev_err(isp->dev, "%s: no acc binary available\n", __func__);
+		ret = -EINVAL;
+		goto err_stage;
+	}
+
+	ret = atomisp_css_start_acc_pipe(asd);
+	if (ret) {
+		dev_err(isp->dev, "%s: atomisp_acc_start_acc_pipe failed\n",
+			__func__);
+		goto err_stage;
+	}
+
+	return 0;
+
+err_stage:
+	atomisp_css_destroy_acc_pipe(asd);
+	return ret;
+}
+
+int atomisp_acc_wait(struct atomisp_sub_device *asd, unsigned int *handle)
+{
+	struct atomisp_device *isp = asd->isp;
+	int ret;
+
+	if (!asd->acc.pipeline)
+		return -ENOENT;
+
+	if (*handle && !acc_get_fw(asd, *handle))
+		return -EINVAL;
+
+	ret = atomisp_css_wait_acc_finish(asd);
+	if (acc_stop_acceleration(asd) == -EIO) {
+		atomisp_reset(isp);
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+void atomisp_acc_done(struct atomisp_sub_device *asd, unsigned int handle)
+{
+	struct v4l2_event event = { 0 };
+
+	event.type = V4L2_EVENT_ATOMISP_ACC_COMPLETE;
+	event.u.frame_sync.frame_sequence = atomic_read(&asd->sequence);
+	event.id = handle;
+
+	v4l2_event_queue(asd->subdev.devnode, &event);
+}
+
+int atomisp_acc_map(struct atomisp_sub_device *asd, struct atomisp_acc_map *map)
+{
+	struct atomisp_map *atomisp_map;
+	ia_css_ptr cssptr;
+	int pgnr;
+
+	if (map->css_ptr)
+		return -EINVAL;
+
+	if (asd->acc.pipeline)
+		return -EBUSY;
+
+	if (map->user_ptr) {
+		/* Buffer to map must be page-aligned */
+		if ((unsigned long)map->user_ptr & ~PAGE_MASK) {
+			dev_err(asd->isp->dev,
+				"%s: mapped buffer address %p is not page aligned\n",
+				__func__, map->user_ptr);
+			return -EINVAL;
+		}
+
+		pgnr = DIV_ROUND_UP(map->length, PAGE_SIZE);
+		cssptr = hrt_isp_css_mm_alloc_user_ptr(
+				map->length, map->user_ptr,
+				pgnr, HRT_USR_PTR,
+				(map->flags & ATOMISP_MAP_FLAG_CACHED));
+	} else {
+		/* Allocate private buffer. */
+		if (map->flags & ATOMISP_MAP_FLAG_CACHED)
+			cssptr = hrt_isp_css_mm_calloc_cached(map->length);
+		else
+			cssptr = hrt_isp_css_mm_calloc(map->length);
+	}
+
+	if (!cssptr)
+		return -ENOMEM;
+
+	atomisp_map = kmalloc(sizeof(*atomisp_map), GFP_KERNEL);
+	if (!atomisp_map) {
+		hmm_free(cssptr);
+		return -ENOMEM;
+	}
+	atomisp_map->ptr = cssptr;
+	atomisp_map->length = map->length;
+	list_add(&atomisp_map->list, &asd->acc.memory_maps);
+
+	dev_dbg(asd->isp->dev, "%s: userptr %p, css_address 0x%x, size %d\n",
+		__func__, map->user_ptr, cssptr, map->length);
+	map->css_ptr = cssptr;
+	return 0;
+}
+
+int atomisp_acc_unmap(struct atomisp_sub_device *asd, struct atomisp_acc_map *map)
+{
+	struct atomisp_map *atomisp_map;
+
+	if (asd->acc.pipeline)
+		return -EBUSY;
+
+	atomisp_map = acc_get_map(asd, map->css_ptr, map->length);
+	if (!atomisp_map)
+		return -EINVAL;
+
+	list_del(&atomisp_map->list);
+	hmm_free(atomisp_map->ptr);
+	kfree(atomisp_map);
+	return 0;
+}
+
+int atomisp_acc_s_mapped_arg(struct atomisp_sub_device *asd,
+			     struct atomisp_acc_s_mapped_arg *arg)
+{
+	struct atomisp_acc_fw *acc_fw;
+
+	if (arg->memory >= ATOMISP_ACC_NR_MEMORY)
+		return -EINVAL;
+
+	if (asd->acc.pipeline)
+		return -EBUSY;
+
+	acc_fw = acc_get_fw(asd, arg->fw_handle);
+	if (!acc_fw)
+		return -EINVAL;
+
+	if (arg->css_ptr != 0 || arg->length != 0) {
+		/* Unless the parameter is cleared, check that it exists */
+		if (!acc_get_map(asd, arg->css_ptr, arg->length))
+			return -EINVAL;
+	}
+
+	acc_fw->args[arg->memory].length = arg->length;
+	acc_fw->args[arg->memory].css_ptr = arg->css_ptr;
+
+	dev_dbg(asd->isp->dev, "%s: mem %d, address %p, size %ld\n",
+		__func__, arg->memory, (void *)arg->css_ptr,
+		(unsigned long)arg->length);
+	return 0;
+}
+
+/*
+ * Appends the loaded acceleration binary extensions to the
+ * current ISP mode. Must be called just before sh_css_start().
+ */
+int atomisp_acc_load_extensions(struct atomisp_sub_device *asd)
+{
+	struct atomisp_acc_fw *acc_fw;
+	bool ext_loaded = false;
+	bool continuous = asd->continuous_mode->val &&
+			  asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW;
+	int ret = 0, i = -1;
+	struct atomisp_device *isp = asd->isp;
+
+	if (asd->acc.pipeline || asd->acc.extension_mode)
+		return -EBUSY;
+
+	/* Invalidate caches. FIXME: should flush only necessary buffers */
+	wbinvd();
+
+	list_for_each_entry(acc_fw, &asd->acc.fw, list) {
+		if (acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT &&
+		    acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_VIEWFINDER)
+			continue;
+
+		for (i = 0; i < ARRAY_SIZE(acc_flag_to_pipe); i++) {
+			/* QoS (ACC pipe) acceleration stages are currently
+			 * allowed only in continuous mode. Skip them for
+			 * all other modes. */
+			if (!continuous &&
+			    acc_flag_to_pipe[i].flag ==
+			    ATOMISP_ACC_FW_LOAD_FL_ACC)
+				continue;
+
+			if (acc_fw->flags & acc_flag_to_pipe[i].flag) {
+				ret = atomisp_css_load_acc_extension(asd,
+					acc_fw->fw,
+					acc_flag_to_pipe[i].pipe_id,
+					acc_fw->type);
+				if (ret)
+					goto error;
+
+				ext_loaded = true;
+			}
+		}
+
+		ret = atomisp_css_set_acc_parameters(acc_fw);
+		if (ret < 0)
+			goto error;
+	}
+
+	if (!ext_loaded)
+		return ret;
+
+	ret = atomisp_css_update_stream(asd);
+	if (ret) {
+		dev_err(isp->dev, "%s: update stream failed.\n", __func__);
+		goto error;
+	}
+
+	asd->acc.extension_mode = true;
+	return 0;
+
+error:
+	while (--i >= 0) {
+		if (acc_fw->flags & acc_flag_to_pipe[i].flag) {
+			atomisp_css_unload_acc_extension(asd, acc_fw->fw,
+					acc_flag_to_pipe[i].pipe_id);
+		}
+	}
+
+	list_for_each_entry_continue_reverse(acc_fw, &asd->acc.fw, list) {
+		if (acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT &&
+		    acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_VIEWFINDER)
+			continue;
+
+		for (i = ARRAY_SIZE(acc_flag_to_pipe) - 1; i >= 0; i--) {
+			if (!continuous &&
+			    acc_flag_to_pipe[i].flag ==
+			    ATOMISP_ACC_FW_LOAD_FL_ACC)
+				continue;
+			if (acc_fw->flags & acc_flag_to_pipe[i].flag) {
+				atomisp_css_unload_acc_extension(asd,
+					acc_fw->fw,
+					acc_flag_to_pipe[i].pipe_id);
+			}
+		}
+	}
+	return ret;
+}
+
+void atomisp_acc_unload_extensions(struct atomisp_sub_device *asd)
+{
+	struct atomisp_acc_fw *acc_fw;
+	int i;
+
+	if (!asd->acc.extension_mode)
+		return;
+
+	list_for_each_entry_reverse(acc_fw, &asd->acc.fw, list) {
+		if (acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_OUTPUT &&
+		    acc_fw->type != ATOMISP_ACC_FW_LOAD_TYPE_VIEWFINDER)
+			continue;
+
+		for (i = ARRAY_SIZE(acc_flag_to_pipe) - 1; i >= 0; i--) {
+			if (acc_fw->flags & acc_flag_to_pipe[i].flag) {
+				atomisp_css_unload_acc_extension(asd,
+					acc_fw->fw,
+					acc_flag_to_pipe[i].pipe_id);
+			}
+		}
+	}
+
+	asd->acc.extension_mode = false;
+}
+
+int atomisp_acc_set_state(struct atomisp_sub_device *asd,
+			  struct atomisp_acc_state *arg)
+{
+	struct atomisp_acc_fw *acc_fw;
+	bool enable = (arg->flags & ATOMISP_STATE_FLAG_ENABLE) != 0;
+	struct ia_css_pipe *pipe;
+	enum ia_css_err r;
+	int i;
+
+	if (!asd->acc.extension_mode)
+		return -EBUSY;
+
+	if (arg->flags & ~ATOMISP_STATE_FLAG_ENABLE)
+		return -EINVAL;
+
+	acc_fw = acc_get_fw(asd, arg->fw_handle);
+	if (!acc_fw)
+		return -EINVAL;
+
+	if (enable)
+		wbinvd();
+
+	for (i = 0; i < ARRAY_SIZE(acc_flag_to_pipe); i++) {
+		if (acc_fw->flags & acc_flag_to_pipe[i].flag) {
+			pipe = asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+				pipes[acc_flag_to_pipe[i].pipe_id];
+			r = ia_css_pipe_set_qos_ext_state(pipe, acc_fw->handle,
+							  enable);
+			if (r != IA_CSS_SUCCESS)
+				return -EBADRQC;
+		}
+	}
+
+	if (enable)
+		acc_fw->flags |= ATOMISP_ACC_FW_LOAD_FL_ENABLE;
+	else
+		acc_fw->flags &= ~ATOMISP_ACC_FW_LOAD_FL_ENABLE;
+
+	return 0;
+}
+
+int atomisp_acc_get_state(struct atomisp_sub_device *asd,
+			  struct atomisp_acc_state *arg)
+{
+	struct atomisp_acc_fw *acc_fw;
+
+	if (!asd->acc.extension_mode)
+		return -EBUSY;
+
+	acc_fw = acc_get_fw(asd, arg->fw_handle);
+	if (!acc_fw)
+		return -EINVAL;
+
+	arg->flags = acc_fw->flags;
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_acc.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_acc.h
new file mode 100644
index 000000000000..5b58e7d9ca5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_acc.h
@@ -0,0 +1,124 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __ATOMISP_ACC_H__
+#define __ATOMISP_ACC_H__
+
+#include "../../include/linux/atomisp.h"
+#include "atomisp_internal.h"
+
+#include "ia_css_types.h"
+
+/*
+ * Interface functions for AtomISP driver acceleration API implementation.
+ */
+
+struct atomisp_sub_device;
+
+void atomisp_acc_cleanup(struct atomisp_device *isp);
+
+/*
+ * Free up any allocated resources.
+ * Must be called each time when the device is closed.
+ * Note that there isn't corresponding open() call;
+ * this function may be called sequentially multiple times.
+ * Must be called to free up resources before driver is unloaded.
+ */
+void atomisp_acc_release(struct atomisp_sub_device *asd);
+
+/* Load acceleration binary. DEPRECATED. */
+int atomisp_acc_load(struct atomisp_sub_device *asd,
+		     struct atomisp_acc_fw_load *fw);
+
+/* Load acceleration binary with specified properties */
+int atomisp_acc_load_to_pipe(struct atomisp_sub_device *asd,
+			     struct atomisp_acc_fw_load_to_pipe *fw);
+
+/* Unload specified acceleration binary */
+int atomisp_acc_unload(struct atomisp_sub_device *asd,
+		       unsigned int *handle);
+
+/*
+ * Map a memory region into ISP memory space.
+ */
+int atomisp_acc_map(struct atomisp_sub_device *asd,
+		    struct atomisp_acc_map *map);
+
+/*
+ * Unmap a mapped memory region.
+ */
+int atomisp_acc_unmap(struct atomisp_sub_device *asd,
+		      struct atomisp_acc_map *map);
+
+/*
+ * Set acceleration binary argument to a previously mapped memory region.
+ */
+int atomisp_acc_s_mapped_arg(struct atomisp_sub_device *asd,
+			     struct atomisp_acc_s_mapped_arg *arg);
+
+
+/*
+ * Start acceleration.
+ * Return immediately, acceleration is left running in background.
+ * Specify either acceleration binary or pipeline which to start.
+ */
+int atomisp_acc_start(struct atomisp_sub_device *asd,
+		      unsigned int *handle);
+
+/*
+ * Wait until acceleration finishes.
+ * This MUST be called after each acceleration has been started.
+ * Specify either acceleration binary or pipeline handle.
+ */
+int atomisp_acc_wait(struct atomisp_sub_device *asd,
+		     unsigned int *handle);
+
+/*
+ * Used by ISR to notify ACC stage finished.
+ * This is internally used and does not export as IOCTL.
+ */
+void atomisp_acc_done(struct atomisp_sub_device *asd, unsigned int handle);
+
+/*
+ * Appends the loaded acceleration binary extensions to the
+ * current ISP mode. Must be called just before atomisp_css_start().
+ */
+int atomisp_acc_load_extensions(struct atomisp_sub_device *asd);
+
+/*
+ * Must be called after streaming is stopped:
+ * unloads any loaded acceleration extensions.
+ */
+void atomisp_acc_unload_extensions(struct atomisp_sub_device *asd);
+
+/*
+ * Set acceleration firmware flags.
+ */
+int atomisp_acc_set_state(struct atomisp_sub_device *asd,
+			  struct atomisp_acc_state *arg);
+
+/*
+ * Get acceleration firmware flags.
+ */
+int atomisp_acc_get_state(struct atomisp_sub_device *asd,
+			  struct atomisp_acc_state *arg);
+
+#endif /* __ATOMISP_ACC_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.c
new file mode 100644
index 000000000000..97093baf28ac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.c
@@ -0,0 +1,6751 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include <linux/firmware.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/kfifo.h>
+#include <linux/pm_runtime.h>
+#include <linux/timer.h>
+#include <asm/intel-mid.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_acc.h"
+#include "atomisp_compat.h"
+#include "atomisp_subdev.h"
+#include "atomisp_dfs_tables.h"
+
+#include "hrt/hive_isp_css_mm_hrt.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 "error_support.h"
+#include "hrt/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;
+};
+
+/*
+ * atomisp_kernel_malloc: chooses whether kmalloc() or vmalloc() is preferable.
+ *
+ * It is also a wrap functions to pass into css framework.
+ */
+void *atomisp_kernel_malloc(size_t bytes)
+{
+	/* vmalloc() is preferable if allocating more than 1 page */
+	if (bytes > PAGE_SIZE)
+		return vmalloc(bytes);
+
+	return kmalloc(bytes, GFP_KERNEL);
+}
+
+/*
+ * atomisp_kernel_zalloc: chooses whether set 0 to the allocated memory.
+ *
+ * It is also a wrap functions to pass into css framework.
+ */
+void *atomisp_kernel_zalloc(size_t bytes, bool zero_mem)
+{
+	void *ptr = atomisp_kernel_malloc(bytes);
+
+	if (ptr && zero_mem)
+		memset(ptr, 0, bytes);
+
+	return ptr;
+}
+
+/*
+ * Free buffer allocated with atomisp_kernel_malloc()/atomisp_kernel_zalloc
+ * helper
+ */
+void atomisp_kernel_free(void *ptr)
+{
+	/* Verify if buffer was allocated by vmalloc() or kmalloc() */
+	if (is_vmalloc_addr(ptr))
+		vfree(ptr);
+	else
+		kfree(ptr);
+}
+
+/*
+ * 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);
+}
+
+/*
+ * get struct atomisp_acc_pipe from v4l2 video_device
+ */
+struct atomisp_acc_pipe *atomisp_to_acc_pipe(struct video_device *dev)
+{
+	return (struct atomisp_acc_pipe *)
+	    container_of(dev, struct atomisp_acc_pipe, vdev);
+}
+
+static unsigned short atomisp_get_sensor_fps(struct atomisp_sub_device *asd)
+{
+	struct v4l2_subdev_frame_interval fi;
+	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;
+	}
+
+	isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+	if (isp_sspm1 & ISP_FREQ_VALID_MASK) {
+		dev_dbg(isp->dev, "clearing ISPSSPM1 valid bit.\n");
+		intel_mid_msgbus_write32(PUNIT_PORT, 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;
+
+	isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+	isp_sspm1 &= ~(0x1F << ISP_REQ_FREQ_OFFSET);
+
+	for (i = 0; i < ISP_DFS_TRY_TIMES; i++) {
+		intel_mid_msgbus_write32(PUNIT_PORT, ISPSSPM1,
+				   isp_sspm1
+				   | ratio << ISP_REQ_FREQ_OFFSET
+				   | 1 << ISP_FREQ_VALID_OFFSET
+				   | guar_ratio << ISP_REQ_GUAR_FREQ_OFFSET);
+
+		isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+
+		timeout = 20;
+		while ((isp_sspm1 & ISP_FREQ_VALID_MASK) && timeout) {
+			isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+			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;
+	}
+
+	isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+	timeout = 10;
+	while (((isp_sspm1 >> ISP_FREQ_STAT_OFFSET) != ratio) && timeout) {
+		isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
+		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)
+{
+	/* 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 ((isp->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)
+		return -EINVAL;
+
+	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 *dev)
+{
+	u32 msg32;
+	u16 msg16;
+
+	pci_read_config_dword(dev, PCI_MSI_CAPID, &msg32);
+	msg32 |= 1 << MSI_ENABLE_BIT;
+	pci_write_config_dword(dev, PCI_MSI_CAPID, msg32);
+
+	msg32 = (1 << INTR_IER) | (1 << INTR_IIR);
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, msg32);
+
+	pci_read_config_word(dev, PCI_COMMAND, &msg16);
+	msg16 |= (PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
+		  PCI_COMMAND_INTX_DISABLE);
+	pci_write_config_word(dev, PCI_COMMAND, msg16);
+}
+
+void atomisp_msi_irq_uninit(struct atomisp_device *isp, struct pci_dev *dev)
+{
+	u32 msg32;
+	u16 msg16;
+
+	pci_read_config_dword(dev, PCI_MSI_CAPID, &msg32);
+	msg32 &=  ~(1 << MSI_ENABLE_BIT);
+	pci_write_config_dword(dev, PCI_MSI_CAPID, msg32);
+
+	msg32 = 0x0;
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, msg32);
+
+	pci_read_config_word(dev, PCI_COMMAND, &msg16);
+	msg16 &= ~(PCI_COMMAND_MASTER);
+	pci_write_config_word(dev, 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 ia_css_csi2_port 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 & CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
+		dev_err(isp->dev, "  buffer overrun");
+	if (infos & CSS_RX_IRQ_INFO_ERR_SOT)
+		dev_err(isp->dev, "  start-of-transmission error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
+		dev_err(isp->dev, "  start-of-transmission sync error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_CONTROL)
+		dev_err(isp->dev, "  control error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
+		dev_err(isp->dev, "  2 or more ECC errors");
+	if (infos & CSS_RX_IRQ_INFO_ERR_CRC)
+		dev_err(isp->dev, "  CRC mismatch");
+	if (infos & CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
+		dev_err(isp->dev, "  unknown error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
+		dev_err(isp->dev, "  frame sync error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
+		dev_err(isp->dev, "  frame data error");
+	if (infos & CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
+		dev_err(isp->dev, "  data timeout");
+	if (infos & CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
+		dev_err(isp->dev, "  unknown escape command entry");
+	if (infos & 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)
+{
+	u32 msg_ret;
+	pci_read_config_dword(isp->pdev, PCI_INTERRUPT_CTRL, &msg_ret);
+	msg_ret |= 1 << INTR_IIR;
+	pci_write_config_dword(isp->pdev, PCI_INTERRUPT_CTRL, msg_ret);
+}
+
+static struct atomisp_sub_device *
+__get_asd_from_port(struct atomisp_device *isp, mipi_port_ID_t 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 == false) {
+		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;
+	}
+
+	dev_dbg(isp->dev, "irq:0x%x\n", irq_infos);
+
+	clear_irq_reg(isp);
+
+	if (!atomisp_streaming_count(isp) && !atomisp_is_acc_enabled(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 & 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 & CSS_IRQ_INFO_EVENTS_READY)
+			atomic_set(&asd->sequence,
+				   atomic_read(&asd->sequence_temp));
+	}
+
+	if (irq_infos & CSS_IRQ_INFO_CSS_RECEIVER_SOF)
+		irq_infos &= ~CSS_IRQ_INFO_CSS_RECEIVER_SOF;
+
+	if ((irq_infos & CSS_IRQ_INFO_INPUT_SYSTEM_ERROR) ||
+	    (irq_infos & CSS_IRQ_INFO_IF_ERROR)) {
+		/* handle mipi receiver error */
+		u32 rx_infos;
+		enum ia_css_csi2_port port;
+
+		for (port = IA_CSS_CSI2_PORT0; port <= IA_CSS_CSI2_PORT2;
+		     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)) ==
+		       IA_CSS_SUCCESS) {
+			/* 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:no subdev.event:%d",  __func__,
+					eof_event.event.type);
+				continue;
+			}
+
+			atomisp_eof_event(asd, eof_event.event.exp_id);
+			dev_dbg(isp->dev, "%s 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);
+
+	return IRQ_WAKE_THREAD;
+
+out_nowake:
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	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;
+}
+
+#ifndef ISP2401
+bool atomisp_buffers_queued(struct atomisp_sub_device *asd)
+#else
+bool atomisp_buffers_queued_pipe(struct atomisp_video_pipe *pipe)
+#endif
+{
+#ifndef ISP2401
+	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 ?
+		    true : false;
+#else
+	return pipe->buffers_in_css ? true : false;
+#endif
+}
+
+/* 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_io_base:%p\n", atomisp_io_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;
+	do {
+		data = _hrt_master_port_uload_32(addr);
+
+		dev_dbg(isp->dev, "%s, \t [0x%x]:0x%x\n", __func__, addr, data);
+		addr += sizeof(unsigned int);
+		size32 -= 1;
+	} while (size32 > 0);
+}
+
+static struct videobuf_buffer *atomisp_css_frame_to_vbuf(
+	struct atomisp_video_pipe *pipe, struct atomisp_css_frame *frame)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct atomisp_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 get_buf_timestamp(struct timeval *tv)
+{
+	struct timespec ts;
+	ktime_get_ts(&ts);
+	tv->tv_sec = ts.tv_sec;
+	tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+}
+
+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) {
+			get_buf_timestamp(&pipe->capq.bufs[i]->ts);
+			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);
+		atomisp_kernel_free(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 atomisp_css_pipe_id css_pipe_id,
+		enum atomisp_css_buffer_type buf_type)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (css_pipe_id == CSS_PIPE_ID_COPY &&
+	    isp->inputs[asd->input_curr].camera_caps->
+		sensor[asd->sensor_curr].stream_num > 1) {
+		switch (stream_id) {
+		case ATOMISP_INPUT_STREAM_PREVIEW:
+			return &asd->video_out_preview;
+		case ATOMISP_INPUT_STREAM_POSTVIEW:
+			return &asd->video_out_vf;
+		case ATOMISP_INPUT_STREAM_VIDEO:
+			return &asd->video_out_video_capture;
+		case ATOMISP_INPUT_STREAM_CAPTURE:
+		default:
+			return &asd->video_out_capture;
+		}
+	}
+
+	/* 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 == 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 CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+					return &asd->video_out_video_capture;
+				case CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
+					return &asd->video_out_preview;
+				case 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 CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+					return &asd->video_out_preview;
+				case CSS_BUFFER_TYPE_OUTPUT_FRAME:
+					return &asd->video_out_capture;
+				default:
+					return &asd->video_out_vf;
+				}
+			}
+		} else if (buf_type == 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 == 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 == CSS_PIPE_ID_VIDEO ||
+		    css_pipe_id == CSS_PIPE_ID_COPY) {
+			if (buf_type == 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 == CSS_PIPE_ID_PREVIEW ||
+		    css_pipe_id == CSS_PIPE_ID_COPY)
+			return &asd->video_out_preview;
+	}
+	/* For capture pipe. */
+	if (buf_type == 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 atomisp_css_buffer_type buf_type,
+		      enum atomisp_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 atomisp_css_frame *frame = NULL;
+	struct atomisp_s3a_buf *s3a_buf = NULL, *_s3a_buf_tmp;
+	struct atomisp_dis_buf *dis_buf = NULL, *_dis_buf_tmp;
+	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf_tmp;
+	enum atomisp_metadata_type md_type;
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_control ctrl;
+#ifdef ISP2401
+	bool reset_wdt_timer = false;
+#endif
+
+	if (
+	    buf_type != CSS_BUFFER_TYPE_METADATA &&
+	    buf_type != CSS_BUFFER_TYPE_3A_STATISTICS &&
+	    buf_type != CSS_BUFFER_TYPE_DIS_STATISTICS &&
+	    buf_type != CSS_BUFFER_TYPE_OUTPUT_FRAME &&
+	    buf_type != CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
+	    buf_type != CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME &&
+	    buf_type != CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
+	    buf_type != 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 == NULL) {
+		dev_err(isp->dev, "error getting atomisp pipe\n");
+		return;
+	}
+
+	switch (buf_type) {
+	case CSS_BUFFER_TYPE_3A_STATISTICS:
+		list_for_each_entry_safe(s3a_buf, _s3a_buf_tmp,
+					 &asd->s3a_stats_in_css, list) {
+			if (s3a_buf->s3a_data ==
+				buffer.css_buffer.data.stats_3a) {
+				list_del_init(&s3a_buf->list);
+				list_add_tail(&s3a_buf->list,
+					      &asd->s3a_stats_ready);
+				break;
+			}
+		}
+
+		asd->s3a_bufs_in_css[css_pipe_id]--;
+		atomisp_3a_stats_ready_event(asd, buffer.css_buffer.exp_id);
+		dev_dbg(isp->dev, "%s: s3a stat with exp_id %d is ready\n",
+			__func__, s3a_buf->s3a_data->exp_id);
+		break;
+	case CSS_BUFFER_TYPE_METADATA:
+		if (error)
+			break;
+
+		md_type = atomisp_get_metadata_type(asd, css_pipe_id);
+		list_for_each_entry_safe(md_buf, _md_buf_tmp,
+					 &asd->metadata_in_css[md_type], list) {
+			if (md_buf->metadata ==
+				buffer.css_buffer.data.metadata) {
+				list_del_init(&md_buf->list);
+				list_add_tail(&md_buf->list,
+					      &asd->metadata_ready[md_type]);
+				break;
+			}
+		}
+		asd->metadata_bufs_in_css[stream_id][css_pipe_id]--;
+		atomisp_metadata_ready_event(asd, md_type);
+		dev_dbg(isp->dev, "%s: metadata with exp_id %d is ready\n",
+			__func__, md_buf->metadata->exp_id);
+		break;
+	case CSS_BUFFER_TYPE_DIS_STATISTICS:
+		list_for_each_entry_safe(dis_buf, _dis_buf_tmp,
+						&asd->dis_stats_in_css, list) {
+			if (dis_buf->dis_data ==
+				buffer.css_buffer.data.stats_dvs) {
+				spin_lock_irqsave(&asd->dis_stats_lock,
+						  irqflags);
+				list_del_init(&dis_buf->list);
+				list_add(&dis_buf->list, &asd->dis_stats);
+				asd->params.dis_proj_data_valid = true;
+				spin_unlock_irqrestore(&asd->dis_stats_lock,
+						       irqflags);
+				break;
+			}
+		}
+		asd->dis_bufs_in_css--;
+		dev_dbg(isp->dev, "%s: dis stat with exp_id %d is ready\n",
+			__func__, dis_buf->dis_data->exp_id);
+		break;
+	case CSS_BUFFER_TYPE_VF_OUTPUT_FRAME:
+	case CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
+#ifdef ISP2401
+		reset_wdt_timer = true;
+#endif
+		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 == 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
+			    == CSS_FRAME_FLASH_STATE_PARTIAL)
+				dev_dbg(isp->dev, "%s thumb partially flashed\n",
+					__func__);
+			else if (frame->flash_state
+				 == 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);
+#ifndef ISP2401
+			asd->pending_capture_request--;
+			dev_dbg(isp->dev, "Trigger capture again for new buffer. err=%d\n",
+				err);
+#else
+				asd->pending_capture_request--;
+				asd->re_trigger_capture = false;
+				dev_dbg(isp->dev, "Trigger capture again for new buffer. err=%d\n",
+						err);
+			} else {
+				asd->re_trigger_capture = true;
+			}
+#endif
+		}
+		break;
+	case CSS_BUFFER_TYPE_OUTPUT_FRAME:
+	case CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
+#ifdef ISP2401
+		reset_wdt_timer = true;
+#endif
+		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 == 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);
+			atomisp_kernel_free(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
+			    == 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
+				   == 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 == CSS_PIPE_ID_PREVIEW ||
+			    css_pipe_id == CSS_PIPE_ID_VIDEO) {
+				asd->latest_preview_exp_id = frame->exp_id;
+			} else if (css_pipe_id ==
+					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 == CSS_PIPE_ID_PREVIEW) ||
+		    (css_pipe_id == 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)
+				atomisp_css_set_dz_config(asd,
+					&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) {
+		get_buf_timestamp(&vb->ts);
+		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);
+	}
+#ifdef ISP2401
+	atomic_set(&pipe->wdt_count, 0);
+#endif
+	/*
+	 * 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);
+#ifdef ISP2401
+
+	/* If there are no buffers queued then
+	 * delete wdt timer. */
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+		return;
+	if (!atomisp_buffers_queued_pipe(pipe))
+		atomisp_wdt_stop_pipe(pipe, false);
+	else if (reset_wdt_timer)
+		/* SOF irq should not reset wdt timer. */
+		atomisp_wdt_refresh_pipe(pipe,
+					 ATOMISP_WDT_KEEP_CURRENT_DELAY);
+#endif
+}
+
+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};
+
+		atomisp_css_allocate_continuous_frames(false, asd);
+		atomisp_css_update_continuous_frames(asd);
+
+		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)
+{
+	enum atomisp_css_pipe_id css_pipe_id;
+	bool stream_restart[MAX_STREAM_NUM] = {0};
+	bool depth_mode = false;
+	int i, ret, depth_cnt = 0;
+
+	if (!isp->sw_contex.file_input)
+		atomisp_css_irq_enable(isp,
+				       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];
+		struct ia_css_pipeline *acc_pipeline;
+		struct ia_css_pipe *acc_pipe = NULL;
+
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED &&
+		    !asd->stream_prepared)
+			continue;
+
+		/*
+		* AtomISP::waitStageUpdate is blocked when WDT happens.
+		* By calling acc_done() for all loaded fw_handles,
+		* HAL will be unblocked.
+		*/
+		acc_pipe = asd->stream_env[i].pipes[CSS_PIPE_ID_ACC];
+		if (acc_pipe != NULL) {
+			acc_pipeline = ia_css_pipe_get_pipeline(acc_pipe);
+			if (acc_pipeline) {
+				struct ia_css_pipeline_stage *stage;
+				for (stage = acc_pipeline->stages; stage;
+					stage = stage->next) {
+					const struct ia_css_fw_info *fw;
+					fw = stage->firmware;
+					atomisp_acc_done(asd, fw->handle);
+				}
+			}
+		}
+
+		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;
+
+		asd->streaming = ATOMISP_DEVICE_STREAMING_STOPPING;
+
+		/* 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_acc_unload_extensions(asd);
+
+		atomisp_clear_css_buffer_counters(asd);
+
+		css_pipe_id = atomisp_get_css_pipe_id(asd);
+		atomisp_css_stop(asd, css_pipe_id, true);
+
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+
+		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(isp->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,
+					CSS_INPUT_MODE_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
+			asd->streaming = ATOMISP_DEVICE_STREAMING_ENABLED;
+
+		atomisp_csi2_configure(asd);
+	}
+
+	if (!isp->sw_contex.file_input) {
+		atomisp_css_irq_enable(isp, 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 failed!\n");
+	} else {
+		if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_MAX, true) < 0)
+			dev_dbg(isp->dev, "dfs failed!\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_wdt_work(struct work_struct *work)
+{
+	struct atomisp_device *isp = container_of(work, struct atomisp_device,
+						  wdt_work);
+	int i;
+#ifdef ISP2401
+	unsigned int pipe_wdt_cnt[MAX_STREAM_NUM][4] = { {0} };
+	bool css_recover = true;
+#endif
+
+	rt_mutex_lock(&isp->mutex);
+	if (!atomisp_streaming_count(isp)) {
+		atomic_set(&isp->wdt_work_queued, 0);
+		rt_mutex_unlock(&isp->mutex);
+		return;
+	}
+
+#ifndef ISP2401
+	dev_err(isp->dev, "timeout %d of %d\n",
+		atomic_read(&isp->wdt_count) + 1,
+		ATOMISP_ISP_MAX_TIMEOUT_COUNT);
+#else
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		pipe_wdt_cnt[i][0] +=
+			atomic_read(&asd->video_out_capture.wdt_count);
+		pipe_wdt_cnt[i][1] +=
+			atomic_read(&asd->video_out_vf.wdt_count);
+		pipe_wdt_cnt[i][2] +=
+			atomic_read(&asd->video_out_preview.wdt_count);
+		pipe_wdt_cnt[i][3] +=
+			atomic_read(&asd->video_out_video_capture.wdt_count);
+		css_recover =
+			(pipe_wdt_cnt[i][0] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
+			 pipe_wdt_cnt[i][1] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
+			 pipe_wdt_cnt[i][2] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
+			 pipe_wdt_cnt[i][3] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT)
+			 ? true : false;
+		dev_err(isp->dev, "pipe on asd%d timeout cnt: (%d, %d, %d, %d) of %d, recover = %d\n",
+			asd->index, pipe_wdt_cnt[i][0], pipe_wdt_cnt[i][1],
+			pipe_wdt_cnt[i][2], pipe_wdt_cnt[i][3],
+			ATOMISP_ISP_MAX_TIMEOUT_COUNT, css_recover);
+	}
+#endif
+
+#ifndef ISP2401
+	if (atomic_inc_return(&isp->wdt_count) <
+	    ATOMISP_ISP_MAX_TIMEOUT_COUNT) {
+#else
+	if (css_recover) {
+#endif
+		unsigned int old_dbglevel = dbg_level;
+		atomisp_css_debug_dump_sp_sw_debug_info();
+		atomisp_css_debug_dump_debug_info(__func__);
+		dbg_level = old_dbglevel;
+		for (i = 0; i < isp->num_of_streams; i++) {
+			struct atomisp_sub_device *asd = &isp->asd[i];
+
+			if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+				continue;
+			dev_err(isp->dev, "%s, vdev %s buffers in css: %d\n",
+				__func__,
+				asd->video_out_capture.vdev.name,
+				asd->video_out_capture.
+				buffers_in_css);
+			dev_err(isp->dev,
+				"%s, vdev %s buffers in css: %d\n",
+				__func__,
+				asd->video_out_vf.vdev.name,
+				asd->video_out_vf.
+				buffers_in_css);
+			dev_err(isp->dev,
+				"%s, vdev %s buffers in css: %d\n",
+				__func__,
+				asd->video_out_preview.vdev.name,
+				asd->video_out_preview.
+				buffers_in_css);
+			dev_err(isp->dev,
+				"%s, vdev %s buffers in css: %d\n",
+				__func__,
+				asd->video_out_video_capture.vdev.name,
+				asd->video_out_video_capture.
+				buffers_in_css);
+			dev_err(isp->dev,
+				"%s, s3a buffers in css preview pipe:%d\n",
+				__func__,
+				asd->s3a_bufs_in_css[CSS_PIPE_ID_PREVIEW]);
+			dev_err(isp->dev,
+				"%s, s3a buffers in css capture pipe:%d\n",
+				__func__,
+				asd->s3a_bufs_in_css[CSS_PIPE_ID_CAPTURE]);
+			dev_err(isp->dev,
+				"%s, s3a buffers in css video pipe:%d\n",
+				__func__,
+				asd->s3a_bufs_in_css[CSS_PIPE_ID_VIDEO]);
+			dev_err(isp->dev,
+				"%s, dis buffers in css: %d\n",
+				__func__, asd->dis_bufs_in_css);
+			dev_err(isp->dev,
+				"%s, metadata buffers in css preview pipe:%d\n",
+				__func__,
+				asd->metadata_bufs_in_css
+				[ATOMISP_INPUT_STREAM_GENERAL]
+				[CSS_PIPE_ID_PREVIEW]);
+			dev_err(isp->dev,
+				"%s, metadata buffers in css capture pipe:%d\n",
+				__func__,
+				asd->metadata_bufs_in_css
+				[ATOMISP_INPUT_STREAM_GENERAL]
+				[CSS_PIPE_ID_CAPTURE]);
+			dev_err(isp->dev,
+				"%s, metadata buffers in css video pipe:%d\n",
+				__func__,
+				asd->metadata_bufs_in_css
+				[ATOMISP_INPUT_STREAM_GENERAL]
+				[CSS_PIPE_ID_VIDEO]);
+			if (asd->enable_raw_buffer_lock->val) {
+				unsigned int j;
+
+				dev_err(isp->dev, "%s, raw_buffer_locked_count %d\n",
+					__func__, asd->raw_buffer_locked_count);
+				for (j = 0; j <= ATOMISP_MAX_EXP_ID/32; j++)
+					dev_err(isp->dev, "%s, raw_buffer_bitmap[%d]: 0x%x\n",
+						__func__, j,
+						asd->raw_buffer_bitmap[j]);
+			}
+		}
+
+		/*sh_css_dump_sp_state();*/
+		/*sh_css_dump_isp_state();*/
+	} else {
+		for (i = 0; i < isp->num_of_streams; i++) {
+			struct atomisp_sub_device *asd = &isp->asd[i];
+			if (asd->streaming ==
+			    ATOMISP_DEVICE_STREAMING_ENABLED) {
+				atomisp_clear_css_buffer_counters(asd);
+				atomisp_flush_bufs_and_wakeup(asd);
+				complete(&asd->init_done);
+			}
+#ifdef ISP2401
+			atomisp_wdt_stop(asd, false);
+#endif
+		}
+
+#ifndef ISP2401
+		atomic_set(&isp->wdt_count, 0);
+#endif
+		isp->isp_fatal_error = true;
+		atomic_set(&isp->wdt_work_queued, 0);
+
+		rt_mutex_unlock(&isp->mutex);
+		return;
+	}
+
+	__atomisp_css_recover(isp, true);
+#ifdef ISP2401
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		if (asd->streaming ==
+			ATOMISP_DEVICE_STREAMING_ENABLED) {
+			atomisp_wdt_refresh(asd,
+				isp->sw_contex.file_input ?
+				ATOMISP_ISP_FILE_TIMEOUT_DURATION :
+				ATOMISP_ISP_TIMEOUT_DURATION);
+		}
+	}
+#endif
+	dev_err(isp->dev, "timeout recovery handling done\n");
+	atomic_set(&isp->wdt_work_queued, 0);
+
+	rt_mutex_unlock(&isp->mutex);
+}
+
+void atomisp_css_flush(struct atomisp_device *isp)
+{
+	int i;
+
+	if (!atomisp_streaming_count(isp))
+		return;
+
+	/* Disable wdt */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		atomisp_wdt_stop(asd, true);
+	}
+
+	/* Start recover */
+	__atomisp_css_recover(isp, false);
+	/* Restore wdt */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		if (asd->streaming !=
+				ATOMISP_DEVICE_STREAMING_ENABLED)
+			continue;
+
+		atomisp_wdt_refresh(asd,
+				    isp->sw_contex.file_input ?
+				    ATOMISP_ISP_FILE_TIMEOUT_DURATION :
+				    ATOMISP_ISP_TIMEOUT_DURATION);
+	}
+	dev_dbg(isp->dev, "atomisp css flush done\n");
+}
+
+#ifndef ISP2401
+void atomisp_wdt(unsigned long isp_addr)
+#else
+void atomisp_wdt(unsigned long pipe_addr)
+#endif
+{
+#ifndef ISP2401
+	struct atomisp_device *isp = (struct atomisp_device *)isp_addr;
+#else
+	struct atomisp_video_pipe *pipe =
+		(struct atomisp_video_pipe *)pipe_addr;
+	struct atomisp_sub_device *asd = pipe->asd;
+	struct atomisp_device *isp = asd->isp;
+#endif
+
+#ifdef ISP2401
+	atomic_inc(&pipe->wdt_count);
+	dev_warn(isp->dev,
+		"[WARNING]asd %d pipe %s ISP timeout %d!\n",
+			asd->index, pipe->vdev.name,
+			atomic_read(&pipe->wdt_count));
+#endif
+	if (atomic_read(&isp->wdt_work_queued)) {
+		dev_dbg(isp->dev, "ISP watchdog was put into workqueue\n");
+		return;
+	}
+	atomic_set(&isp->wdt_work_queued, 1);
+	queue_work(isp->wdt_work_queue, &isp->wdt_work);
+}
+
+#ifndef ISP2401
+void atomisp_wdt_refresh(struct atomisp_sub_device *asd, unsigned int delay)
+#else
+void atomisp_wdt_refresh_pipe(struct atomisp_video_pipe *pipe,
+				unsigned int delay)
+#endif
+{
+	unsigned long next;
+
+	if (delay != ATOMISP_WDT_KEEP_CURRENT_DELAY)
+#ifndef ISP2401
+		asd->wdt_duration = delay;
+#else
+		pipe->wdt_duration = delay;
+#endif
+
+#ifndef ISP2401
+	next = jiffies + asd->wdt_duration;
+#else
+	next = jiffies + pipe->wdt_duration;
+#endif
+
+	/* Override next if it has been pushed beyon the "next" time */
+#ifndef ISP2401
+	if (atomisp_is_wdt_running(asd) && time_after(asd->wdt_expires, next))
+		next = asd->wdt_expires;
+#else
+	if (atomisp_is_wdt_running(pipe) && time_after(pipe->wdt_expires, next))
+		next = pipe->wdt_expires;
+#endif
+
+#ifndef ISP2401
+	asd->wdt_expires = next;
+#else
+	pipe->wdt_expires = next;
+#endif
+
+#ifndef ISP2401
+	if (atomisp_is_wdt_running(asd))
+		dev_dbg(asd->isp->dev, "WDT will hit after %d ms\n",
+			((int)(next - jiffies) * 1000 / HZ));
+#else
+	if (atomisp_is_wdt_running(pipe))
+		dev_dbg(pipe->asd->isp->dev, "WDT will hit after %d ms (%s)\n",
+			((int)(next - jiffies) * 1000 / HZ), pipe->vdev.name);
+#endif
+	else
+#ifndef ISP2401
+		dev_dbg(asd->isp->dev, "WDT starts with %d ms period\n",
+			((int)(next - jiffies) * 1000 / HZ));
+#else
+		dev_dbg(pipe->asd->isp->dev, "WDT starts with %d ms period (%s)\n",
+			((int)(next - jiffies) * 1000 / HZ), pipe->vdev.name);
+#endif
+
+#ifndef ISP2401
+	mod_timer(&asd->wdt, next);
+	atomic_set(&asd->isp->wdt_count, 0);
+#else
+	mod_timer(&pipe->wdt, next);
+#endif
+}
+
+#ifndef ISP2401
+void atomisp_wdt_stop(struct atomisp_sub_device *asd, bool sync)
+#else
+void atomisp_wdt_refresh(struct atomisp_sub_device *asd, unsigned int delay)
+{
+	dev_dbg(asd->isp->dev, "WDT refresh all:\n");
+	if (atomisp_is_wdt_running(&asd->video_out_capture))
+		atomisp_wdt_refresh_pipe(&asd->video_out_capture, delay);
+	if (atomisp_is_wdt_running(&asd->video_out_preview))
+		atomisp_wdt_refresh_pipe(&asd->video_out_preview, delay);
+	if (atomisp_is_wdt_running(&asd->video_out_vf))
+		atomisp_wdt_refresh_pipe(&asd->video_out_vf, delay);
+	if (atomisp_is_wdt_running(&asd->video_out_video_capture))
+		atomisp_wdt_refresh_pipe(&asd->video_out_video_capture, delay);
+}
+
+
+void atomisp_wdt_stop_pipe(struct atomisp_video_pipe *pipe, bool sync)
+#endif
+{
+#ifndef ISP2401
+	dev_dbg(asd->isp->dev, "WDT stop\n");
+#else
+	if (!atomisp_is_wdt_running(pipe))
+		return;
+
+	dev_dbg(pipe->asd->isp->dev,
+		"WDT stop asd %d (%s)\n", pipe->asd->index, pipe->vdev.name);
+
+#endif
+	if (sync) {
+#ifndef ISP2401
+		del_timer_sync(&asd->wdt);
+		cancel_work_sync(&asd->isp->wdt_work);
+#else
+		del_timer_sync(&pipe->wdt);
+		cancel_work_sync(&pipe->asd->isp->wdt_work);
+#endif
+	} else {
+#ifndef ISP2401
+		del_timer(&asd->wdt);
+#else
+		del_timer(&pipe->wdt);
+#endif
+	}
+}
+
+#ifndef ISP2401
+void atomisp_wdt_start(struct atomisp_sub_device *asd)
+#else
+void atomisp_wdt_stop(struct atomisp_sub_device *asd, bool sync)
+{
+	dev_dbg(asd->isp->dev, "WDT stop all:\n");
+	atomisp_wdt_stop_pipe(&asd->video_out_capture, sync);
+	atomisp_wdt_stop_pipe(&asd->video_out_preview, sync);
+	atomisp_wdt_stop_pipe(&asd->video_out_vf, sync);
+	atomisp_wdt_stop_pipe(&asd->video_out_video_capture, sync);
+}
+
+void atomisp_wdt_start(struct atomisp_video_pipe *pipe)
+#endif
+{
+#ifndef ISP2401
+	atomisp_wdt_refresh(asd, ATOMISP_ISP_TIMEOUT_DURATION);
+#else
+	atomisp_wdt_refresh_pipe(pipe, ATOMISP_ISP_TIMEOUT_DURATION);
+#endif
+}
+
+void atomisp_setup_flash(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct v4l2_control ctrl;
+
+	if (isp->flash == NULL)
+		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;
+		}
+
+		atomisp_css_request_flash(asd);
+		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 = &isp->asd[0];
+
+	dev_dbg(isp->dev, ">%s\n", __func__);
+
+	spin_lock_irqsave(&isp->lock, flags);
+
+	if (!atomisp_streaming_count(isp) && !atomisp_is_acc_enabled(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_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
+	 */
+	rt_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:
+	rt_mutex_unlock(&isp->mutex);
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED
+		    && css_pipe_done[asd->index]
+		    && isp->sw_contex.file_input)
+			v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					 video, s_stream, 1);
+		/* FIXME! FIX ACC implementation */
+		if (asd->acc.pipeline && css_pipe_done[asd->index])
+			atomisp_css_acc_done(asd);
+	}
+	dev_dbg(isp->dev, "<%s\n", __func__);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * utils for buffer allocation/free
+ */
+
+int atomisp_get_frame_pgnr(struct atomisp_device *isp,
+			   const struct atomisp_css_frame *frame, u32 *p_pgnr)
+{
+	if (!frame) {
+		dev_err(isp->dev, "%s: NULL frame pointer ERROR.\n", __func__);
+		return -EINVAL;
+	}
+
+	*p_pgnr = DIV_ROUND_UP(frame->data_bytes, PAGE_SIZE);
+	return 0;
+}
+
+/*
+ * Get internal fmt according to V4L2 fmt
+ */
+static enum atomisp_css_frame_format
+v4l2_fmt_to_sh_fmt(u32 fmt)
+{
+	switch (fmt) {
+	case V4L2_PIX_FMT_YUV420:
+		return CSS_FRAME_FORMAT_YUV420;
+	case V4L2_PIX_FMT_YVU420:
+		return CSS_FRAME_FORMAT_YV12;
+	case V4L2_PIX_FMT_YUV422P:
+		return CSS_FRAME_FORMAT_YUV422;
+	case V4L2_PIX_FMT_YUV444:
+		return CSS_FRAME_FORMAT_YUV444;
+	case V4L2_PIX_FMT_NV12:
+		return CSS_FRAME_FORMAT_NV12;
+	case V4L2_PIX_FMT_NV21:
+		return CSS_FRAME_FORMAT_NV21;
+	case V4L2_PIX_FMT_NV16:
+		return CSS_FRAME_FORMAT_NV16;
+	case V4L2_PIX_FMT_NV61:
+		return CSS_FRAME_FORMAT_NV61;
+	case V4L2_PIX_FMT_UYVY:
+		return CSS_FRAME_FORMAT_UYVY;
+	case V4L2_PIX_FMT_YUYV:
+		return CSS_FRAME_FORMAT_YUYV;
+	case V4L2_PIX_FMT_RGB24:
+		return CSS_FRAME_FORMAT_PLANAR_RGB888;
+	case V4L2_PIX_FMT_RGB32:
+		return CSS_FRAME_FORMAT_RGBA888;
+	case V4L2_PIX_FMT_RGB565:
+		return CSS_FRAME_FORMAT_RGB565;
+	case V4L2_PIX_FMT_JPEG:
+	case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
+		return CSS_FRAME_FORMAT_BINARY_8;
+	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 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 == CSS_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 == CSS_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 == CSS_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 == CSS_FORMAT_RAW_16) && (output == V4L2_PIX_FMT_SBGGR16))
+		return 0;
+
+	return -EINVAL;
+}
+
+static u32 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, CSS_CAPTURE_MODE_ADVANCED);
+	else if (asd->params.low_light)
+		atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_LOW_LIGHT);
+	else if (asd->video_out_capture.sh_fmt == CSS_FRAME_FORMAT_RAW)
+		atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_RAW);
+	else
+		atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_PRIMARY);
+}
+
+#ifdef 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;
+	struct v4l2_streamparm p = {0};
+	int ret;
+	int modes[] = { CI_MODE_NONE,
+			CI_MODE_VIDEO,
+			CI_MODE_STILL_CAPTURE,
+			CI_MODE_CONTINUOUS,
+			CI_MODE_PREVIEW };
+
+	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);
+	} else {
+		p.parm.capture.capturemode = modes[runmode->mode];
+		ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				       video, s_parm, &p);
+	}
+
+	mutex_unlock(asd->ctrl_handler.lock);
+	return ret;
+}
+
+#endif
+/*
+ * 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) {
+		atomisp_css_set_morph_table(asd,
+					    asd->params.css_param.morph_table);
+	} else {
+		atomisp_css_set_morph_table(asd, 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 atomisp_css_nr_config));
+		atomisp_css_set_nr_config(asd, &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 atomisp_css_tnr_config));
+		atomisp_css_set_tnr_config(asd, &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 atomisp_css_ob_config));
+		atomisp_css_set_ob_config(asd, &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));
+		atomisp_css_set_ee_config(asd, &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));
+		atomisp_css_set_gamma_table(asd, &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));
+		atomisp_css_set_gc_config(asd, &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));
+		atomisp_css_set_formats_config(asd, &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) {
+		atomisp_css_frame_free(asd->raw_output_frame);
+		asd->raw_output_frame = NULL;
+	}
+}
+
+static void atomisp_update_grid_info(struct atomisp_sub_device *asd,
+				     enum atomisp_css_pipe_id pipe_id,
+				     int source_pad)
+{
+	struct atomisp_device *isp = asd->isp;
+	int err;
+	uint16_t stream_id = atomisp_source_pad_to_stream_id(asd, source_pad);
+
+	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, stream_id);
+	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 atomisp_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 atomisp_css_morph_table tab = {0};
+		atomisp_css_get_morph_table(asd, &tab);
+
+		config->width = tab.width;
+		config->height = tab.height;
+
+		for (i = 0; i < 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 atomisp_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 < 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)
+			atomisp_css_set_morph_table(asd, tab);
+	}
+
+	return 0;
+}
+
+int atomisp_macc_table(struct atomisp_sub_device *asd, int flag,
+		       struct atomisp_macc_config *config)
+{
+	struct atomisp_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 atomisp_css_macc_table));
+	} else {
+		memcpy(macc_table, &config->table,
+		       sizeof(struct atomisp_css_macc_table));
+		if (config->color_effect == asd->params.color_effect)
+			atomisp_css_set_macc_table(asd, 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[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_config *stream_config;
+	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_config = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		stream_config;
+	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 == NULL)
+		return -EINVAL;
+
+	if (mipi_info->metadata_effective_width != NULL) {
+		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_config *stream_config;
+	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_config = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+		stream_config;
+	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 == NULL)
+		return -EINVAL;
+
+	if (mipi_info->metadata_effective_width != NULL) {
+		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 atomisp_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;
+#ifdef ISP2401
+	int w_offset, h_offset;
+#endif
+
+	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.
+	 */
+#ifdef ISP2401
+	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;
+	}
+#endif
+	dz_config->zoom_region.origin.x =
+			dz_config->zoom_region.origin.x
+			* eff_res.width
+#ifndef ISP2401
+			/ asd->sensor_array_res.width;
+#else
+			/ (asd->sensor_array_res.width -
+			2 * w_offset);
+#endif
+	dz_config->zoom_region.origin.y =
+			dz_config->zoom_region.origin.y
+			* eff_res.height
+#ifndef ISP2401
+			/ asd->sensor_array_res.height;
+#else
+			/ (asd->sensor_array_res.height -
+			2 * h_offset);
+#endif
+	dz_config->zoom_region.resolution.width =
+			dz_config->zoom_region.resolution.width
+			* eff_res.width
+#ifndef ISP2401
+			/ asd->sensor_array_res.width;
+#else
+			/ (asd->sensor_array_res.width -
+			2 * w_offset);
+#endif
+	dz_config->zoom_region.resolution.height =
+			dz_config->zoom_region.resolution.height
+			* eff_res.height
+#ifndef ISP2401
+			/ asd->sensor_array_res.height;
+#else
+			/ (asd->sensor_array_res.height -
+			2 * h_offset);
+#endif
+
+	/*
+	  * Set same ratio of crop region resolution and current pipe output
+	  * resolution
+	  */
+#ifndef ISP2401
+	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;
+	}
+
+#endif
+	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)
+		atomisp_css_set_wb_config(asd, &css_param->wb_config);
+
+	if (css_param->update_flag.ob_config)
+		atomisp_css_set_ob_config(asd, &css_param->ob_config);
+
+	if (css_param->update_flag.dp_config)
+		atomisp_css_set_dp_config(asd, &css_param->dp_config);
+
+	if (css_param->update_flag.nr_config)
+		atomisp_css_set_nr_config(asd, &css_param->nr_config);
+
+	if (css_param->update_flag.ee_config)
+		atomisp_css_set_ee_config(asd, &css_param->ee_config);
+
+	if (css_param->update_flag.tnr_config)
+		atomisp_css_set_tnr_config(asd, &css_param->tnr_config);
+
+	if (css_param->update_flag.a3a_config)
+		atomisp_css_set_3a_config(asd, &css_param->s3a_config);
+
+	if (css_param->update_flag.ctc_config)
+		atomisp_css_set_ctc_config(asd, &css_param->ctc_config);
+
+	if (css_param->update_flag.cnr_config)
+		atomisp_css_set_cnr_config(asd, &css_param->cnr_config);
+
+	if (css_param->update_flag.ecd_config)
+		atomisp_css_set_ecd_config(asd, &css_param->ecd_config);
+
+	if (css_param->update_flag.ynr_config)
+		atomisp_css_set_ynr_config(asd, &css_param->ynr_config);
+
+	if (css_param->update_flag.fc_config)
+		atomisp_css_set_fc_config(asd, &css_param->fc_config);
+
+	if (css_param->update_flag.macc_config)
+		atomisp_css_set_macc_config(asd, &css_param->macc_config);
+
+	if (css_param->update_flag.aa_config)
+		atomisp_css_set_aa_config(asd, &css_param->aa_config);
+
+	if (css_param->update_flag.anr_config)
+		atomisp_css_set_anr_config(asd, &css_param->anr_config);
+
+	if (css_param->update_flag.xnr_config)
+		atomisp_css_set_xnr_config(asd, &css_param->xnr_config);
+
+	if (css_param->update_flag.yuv2rgb_cc_config)
+		atomisp_css_set_yuv2rgb_cc_config(asd,
+					&css_param->yuv2rgb_cc_config);
+
+	if (css_param->update_flag.rgb2yuv_cc_config)
+		atomisp_css_set_rgb2yuv_cc_config(asd,
+					&css_param->rgb2yuv_cc_config);
+
+	if (css_param->update_flag.macc_table)
+		atomisp_css_set_macc_table(asd, &css_param->macc_table);
+
+	if (css_param->update_flag.xnr_table)
+		atomisp_css_set_xnr_table(asd, &css_param->xnr_table);
+
+	if (css_param->update_flag.r_gamma_table)
+		atomisp_css_set_r_gamma_table(asd, &css_param->r_gamma_table);
+
+	if (css_param->update_flag.g_gamma_table)
+		atomisp_css_set_g_gamma_table(asd, &css_param->g_gamma_table);
+
+	if (css_param->update_flag.b_gamma_table)
+		atomisp_css_set_b_gamma_table(asd, &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)
+		atomisp_css_set_shading_table(asd, css_param->shading_table);
+
+	if (css_param->update_flag.morph_table && asd->params.gdc_cac_en)
+		atomisp_css_set_morph_table(asd, css_param->morph_table);
+
+	if (css_param->update_flag.dvs2_coefs) {
+		struct atomisp_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, 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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_css_shading_table *shading_table;
+	struct atomisp_css_shading_table *old_table;
+#ifdef ISP2401
+	struct atomisp_shading_table st;
+#endif
+
+	if (!source_st)
+		return 0;
+
+	if (!css_param)
+		return -EINVAL;
+
+	if (!from_user && css_param->update_flag.shading_table)
+		return 0;
+
+#ifdef ISP2401
+	if (copy_from_compatible(&st, source_st,
+				 sizeof(struct atomisp_shading_table),
+				 from_user)) {
+		dev_err(asd->isp->dev, "copy shading table failed!");
+		return -EFAULT;
+	}
+
+#endif
+	old_table = css_param->shading_table;
+
+#ifdef ISP2401
+
+#endif
+	/* user config is to disable the shading table. */
+#ifndef ISP2401
+	if (!source_st->enable) {
+#else
+	if (!st.enable) {
+#endif
+		/* 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++) {
+#ifndef ISP2401
+		if (!source_st->data[i])
+#else
+		if (!st.data[i]) {
+			dev_err(asd->isp->dev, "shading table validate failed");
+#endif
+			return -EINVAL;
+#ifdef ISP2401
+		}
+#endif
+	}
+
+	/* Shading table size per color */
+#ifndef ISP2401
+	if (source_st->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
+		source_st->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR)
+#else
+	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!");
+#endif
+		return -EINVAL;
+#ifdef ISP2401
+	}
+#endif
+
+#ifndef ISP2401
+	shading_table = atomisp_css_shading_table_alloc(source_st->width,
+							source_st->height);
+	if (!shading_table)
+			return -ENOMEM;
+#else
+	shading_table = atomisp_css_shading_table_alloc(st.width,
+							st.height);
+	if (!shading_table) {
+		dev_err(asd->isp->dev, "shading table alloc failed!");
+		return -ENOMEM;
+	}
+#endif
+
+#ifndef ISP2401
+	len_table = source_st->width * source_st->height * ATOMISP_SC_TYPE_SIZE;
+#else
+	len_table = st.width * st.height * ATOMISP_SC_TYPE_SIZE;
+#endif
+	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
+		if (copy_from_compatible(shading_table->data[i],
+#ifndef ISP2401
+			source_st->data[i], len_table, from_user)) {
+#else
+			st.data[i], len_table, from_user)) {
+#endif
+			atomisp_css_shading_table_free(shading_table);
+			return -EFAULT;
+		}
+
+	}
+#ifndef ISP2401
+	shading_table->sensor_width = source_st->sensor_width;
+	shading_table->sensor_height = source_st->sensor_height;
+	shading_table->fraction_bits = source_st->fraction_bits;
+	shading_table->enable = source_st->enable;
+#else
+	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;
+#endif
+
+	/* No need to update shading table if it is the same */
+	if (old_table != NULL &&
+		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 != NULL;
+
+	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 atomisp_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;
+#ifdef ISP2401
+	struct ia_css_dvs2_coefficients dvs2_coefs;
+#endif
+
+	if (!coefs || !cur)
+		return 0;
+
+	if (!from_user && css_param->update_flag.dvs2_coefs)
+		return 0;
+
+#ifndef ISP2401
+	if (sizeof(*cur) != sizeof(coefs->grid) ||
+	    memcmp(&coefs->grid, cur, sizeof(coefs->grid))) {
+#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))) {
+#endif
+		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;
+	}
+
+#ifndef ISP2401
+	if (coefs->hor_coefs.odd_real == NULL ||
+	    coefs->hor_coefs.odd_imag == NULL ||
+	    coefs->hor_coefs.even_real == NULL ||
+	    coefs->hor_coefs.even_imag == NULL ||
+	    coefs->ver_coefs.odd_real == NULL ||
+	    coefs->ver_coefs.odd_imag == NULL ||
+	    coefs->ver_coefs.even_real == NULL ||
+	    coefs->ver_coefs.even_imag == NULL)
+#else
+	if (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)
+#endif
+		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,
+#ifndef ISP2401
+	    coefs->hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
+#ifndef ISP2401
+	    coefs->hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
+#ifndef ISP2401
+	    coefs->hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
+#ifndef ISP2401
+	    coefs->hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
+#ifndef ISP2401
+	    coefs->ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
+#ifndef ISP2401
+	    coefs->ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
+#ifndef ISP2401
+	    coefs->ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
+#else
+	    dvs2_coefs.ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
+#endif
+	    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
+#ifndef ISP2401
+	    coefs->ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
+#else
+	    dvs2_coefs.ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
+#endif
+		ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
+		css_param->dvs2_coeff = NULL;
+		return -EFAULT;
+	}
+
+	css_param->update_flag.dvs2_coefs =
+	    (struct atomisp_dvs2_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 atomisp_css_dvs_6axis_config *dvs_6axis_config;
+	struct atomisp_css_dvs_6axis_config *old_6axis_config;
+#ifdef ISP2401
+	struct atomisp_css_dvs_6axis_config t_6axis_config;
+#endif
+	struct ia_css_stream *stream =
+			asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
+	struct atomisp_css_dvs_grid_info *dvs_grid_info =
+		atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	int ret = -EFAULT;
+
+	if (stream == NULL) {
+		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;
+#ifdef ISP2401
+
+	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;
+	}
+
+#endif
+	if (old_6axis_config &&
+#ifndef ISP2401
+	    (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)) {
+#else
+	    (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)) {
+#endif
+		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;
+	}
+
+#ifndef ISP2401
+	dvs_6axis_config->exp_id = source_6axis_config->exp_id;
+#else
+	dvs_6axis_config->exp_id = t_6axis_config.exp_id;
+#endif
+
+	if (copy_from_compatible(dvs_6axis_config->xcoords_y,
+#ifndef ISP2401
+			   source_6axis_config->xcoords_y,
+			   source_6axis_config->width_y *
+			   source_6axis_config->height_y *
+			   sizeof(*source_6axis_config->xcoords_y),
+#else
+			   t_6axis_config.xcoords_y,
+			   t_6axis_config.width_y *
+			   t_6axis_config.height_y *
+			   sizeof(*dvs_6axis_config->xcoords_y),
+#endif
+			   from_user))
+		goto error;
+	if (copy_from_compatible(dvs_6axis_config->ycoords_y,
+#ifndef ISP2401
+			   source_6axis_config->ycoords_y,
+			   source_6axis_config->width_y *
+			   source_6axis_config->height_y *
+			   sizeof(*source_6axis_config->ycoords_y),
+#else
+			   t_6axis_config.ycoords_y,
+			   t_6axis_config.width_y *
+			   t_6axis_config.height_y *
+			   sizeof(*dvs_6axis_config->ycoords_y),
+#endif
+			   from_user))
+		goto error;
+	if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
+#ifndef ISP2401
+			   source_6axis_config->xcoords_uv,
+			   source_6axis_config->width_uv *
+			   source_6axis_config->height_uv *
+			   sizeof(*source_6axis_config->xcoords_uv),
+#else
+			   t_6axis_config.xcoords_uv,
+			   t_6axis_config.width_uv *
+			   t_6axis_config.height_uv *
+			   sizeof(*dvs_6axis_config->xcoords_uv),
+#endif
+			   from_user))
+		goto error;
+	if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
+#ifndef ISP2401
+			   source_6axis_config->ycoords_uv,
+			   source_6axis_config->width_uv *
+			   source_6axis_config->height_uv *
+			   sizeof(*source_6axis_config->ycoords_uv),
+#else
+			   t_6axis_config.ycoords_uv,
+			   t_6axis_config.width_uv *
+			   t_6axis_config.height_uv *
+			   sizeof(*dvs_6axis_config->ycoords_uv),
+#endif
+			   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 atomisp_css_morph_table *morph_table;
+#ifdef ISP2401
+	struct atomisp_css_morph_table mtbl;
+#endif
+	struct atomisp_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;
+
+#ifdef ISP2401
+	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;
+	}
+
+#endif
+	morph_table = atomisp_css_morph_table_allocate(
+#ifndef ISP2401
+		source_morph_table->width,
+		source_morph_table->height);
+#else
+		mtbl.width,
+		mtbl.height);
+#endif
+	if (!morph_table)
+		return -ENOMEM;
+
+	for (i = 0; i < CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		if (copy_from_compatible(morph_table->coordinates_x[i],
+			source_morph_table->coordinates_x[i],
+#ifndef ISP2401
+			source_morph_table->height * source_morph_table->width *
+			sizeof(*source_morph_table->coordinates_x[i]),
+#else
+			mtbl.height * mtbl.width *
+			sizeof(*morph_table->coordinates_x[i]),
+#endif
+			from_user))
+			goto error;
+
+		if (copy_from_compatible(morph_table->coordinates_y[i],
+			source_morph_table->coordinates_y[i],
+#ifndef ISP2401
+			source_morph_table->height * source_morph_table->width *
+			sizeof(*source_morph_table->coordinates_y[i]),
+#else
+			mtbl.height * mtbl.width *
+			sizeof(*morph_table->coordinates_y[i]),
+#endif
+			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;
+
+	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) {
+		atomisp_qbuffers_to_css(asd);
+#ifndef ISP2401
+		if (!atomisp_is_wdt_running(asd) && atomisp_buffers_queued(asd))
+			atomisp_wdt_start(asd);
+#else
+		if (atomisp_buffers_queued_pipe(pipe)) {
+			if (!atomisp_is_wdt_running(pipe))
+				atomisp_wdt_start(pipe);
+			else
+				atomisp_wdt_refresh_pipe(pipe,
+					ATOMISP_WDT_KEEP_CURRENT_DELAY);
+		}
+#endif
+	}
+}
+
+/*
+* 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;
+
+	if (asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream == NULL) {
+		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);
+#ifdef 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;
+	}
+
+#endif
+	if (arg->per_frame_setting && !atomisp_is_vf_pipe(pipe)) {
+		/*
+		 * Per-frame setting enabled, we allocate a new paramter
+		 * 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 = atomisp_kernel_zalloc(sizeof(*param), true);
+		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);
+	if (param)
+		atomisp_kernel_free(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 atomisp_device *isp = asd->isp;
+	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 atomisp_css_dvs_grid_info *dvs_grid_info =
+			atomisp_css_get_dvs_grid_info(
+				&asd->params.curr_grid_info);
+
+		if (&config->info == NULL) {
+			dev_err(isp->dev, "ERROR: NULL pointer in grid_info\n");
+			return -EINVAL;
+		}
+		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 atomisp_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 atomisp_css_wb_config));
+	memcpy(&asd->params.css_param.ob_config, &config->ob_config,
+	       sizeof(struct atomisp_css_ob_config));
+	memcpy(&asd->params.css_param.dp_config, &config->dp_config,
+	       sizeof(struct atomisp_css_dp_config));
+	memcpy(&asd->params.css_param.de_config, &config->de_config,
+	       sizeof(struct atomisp_css_de_config));
+	memcpy(&asd->params.css_param.dz_config, &config->dz_config,
+	       sizeof(struct atomisp_css_dz_config));
+	memcpy(&asd->params.css_param.ce_config, &config->ce_config,
+	       sizeof(struct atomisp_css_ce_config));
+	memcpy(&asd->params.css_param.nr_config, &config->nr_config,
+	       sizeof(struct atomisp_css_nr_config));
+	memcpy(&asd->params.css_param.ee_config, &config->ee_config,
+	       sizeof(struct atomisp_css_ee_config));
+	memcpy(&asd->params.css_param.tnr_config, &config->tnr_config,
+	       sizeof(struct atomisp_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 atomisp_css_cc_config));
+		atomisp_css_set_cc_config(asd, &asd->params.css_param.cc_config);
+	}
+
+	atomisp_css_set_wb_config(asd, &asd->params.css_param.wb_config);
+	atomisp_css_set_ob_config(asd, &asd->params.css_param.ob_config);
+	atomisp_css_set_de_config(asd, &asd->params.css_param.de_config);
+	atomisp_css_set_dz_config(asd, &asd->params.css_param.dz_config);
+	atomisp_css_set_ce_config(asd, &asd->params.css_param.ce_config);
+	atomisp_css_set_dp_config(asd, &asd->params.css_param.dp_config);
+	atomisp_css_set_nr_config(asd, &asd->params.css_param.nr_config);
+	atomisp_css_set_ee_config(asd, &asd->params.css_param.ee_config);
+	atomisp_css_set_tnr_config(asd, &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 atomisp_css_cc_config *cc_config = NULL;
+	struct atomisp_css_macc_table *macc_table = NULL;
+	struct atomisp_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)
+		atomisp_css_set_cc_config(asd, cc_config);
+	if (macc_table)
+		atomisp_css_set_macc_table(asd, 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));
+		atomisp_css_set_dp_config(asd, &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 = 0;
+		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 atomisp_css_frame_format format)
+{
+	switch (format) {
+	case CSS_FRAME_FORMAT_UYVY:
+	case CSS_FRAME_FORMAT_YUYV:
+	case CSS_FRAME_FORMAT_RAW:
+	case CSS_FRAME_FORMAT_RGB565:
+		return bytesperline/2;
+	case 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 CSS_FRAME_FORMAT_NV11:
+	case CSS_FRAME_FORMAT_NV12:
+	case CSS_FRAME_FORMAT_NV16:
+	case CSS_FRAME_FORMAT_NV21:
+	case CSS_FRAME_FORMAT_NV61:
+	case CSS_FRAME_FORMAT_YV12:
+	case CSS_FRAME_FORMAT_YV16:
+	case CSS_FRAME_FORMAT_YUV420:
+	case CSS_FRAME_FORMAT_YUV420_16:
+	case CSS_FRAME_FORMAT_YUV422:
+	case CSS_FRAME_FORMAT_YUV422_16:
+	case CSS_FRAME_FORMAT_YUV444:
+	case CSS_FRAME_FORMAT_YUV_LINE:
+	case CSS_FRAME_FORMAT_PLANAR_RGB888:
+	case CSS_FRAME_FORMAT_QPLANE6:
+	case CSS_FRAME_FORMAT_BINARY_8:
+	default:
+		return bytesperline;
+	}
+}
+
+static int
+atomisp_v4l2_framebuffer_to_css_frame(const struct v4l2_framebuffer *arg,
+					 struct atomisp_css_frame **result)
+{
+	struct atomisp_css_frame *res = NULL;
+	unsigned int padded_width;
+	enum atomisp_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 atomisp_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 (atomisp_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)
+		atomisp_css_frame_free(res);
+	if (tmp_buf)
+		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 atomisp_css_frame *raw_black_frame = NULL;
+	int ret;
+
+	if (arg == NULL)
+		return -EINVAL;
+
+	ret = atomisp_v4l2_framebuffer_to_css_frame(arg, &raw_black_frame);
+	if (ret)
+		return ret;
+	if (atomisp_css_set_black_frame(asd, raw_black_frame))
+		ret = -ENOMEM;
+
+	atomisp_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) {
+		atomisp_css_set_default_de_config(asd);
+	} else {
+		asd->params.css_param.de_config.pixelnoise = 0;
+		atomisp_css_set_de_config(asd, &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));
+		atomisp_css_set_de_config(asd, &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));
+		atomisp_css_set_wb_config(asd, &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));
+		atomisp_css_set_3a_config(asd, &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 == NULL)
+		return -EINVAL;
+
+	memcpy(config, &mipi_info->data, sizeof(*config));
+	return 0;
+}
+
+int atomisp_get_fmt(struct video_device *vdev, struct v4l2_format *f)
+{
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+
+	f->fmt.pix = pipe->pix;
+
+	return 0;
+}
+
+static void __atomisp_update_stream_env(struct atomisp_sub_device *asd,
+	uint16_t stream_index, struct atomisp_input_stream_info *stream_info)
+{
+	int i;
+
+#if defined(ISP2401_NEW_INPUT_SYSTEM)
+	/* assign virtual channel id return from sensor driver query */
+	asd->stream_env[stream_index].ch_id = stream_info->ch_id;
+#endif
+	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(uint16_t 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_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_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;
+	uint16_t stream_index;
+	int source_pad = atomisp_subdev_source_pad(vdev);
+	int ret;
+
+	if (isp->inputs[asd->input_curr].camera == NULL)
+		return -EINVAL;
+
+	stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
+	fmt = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+	if (fmt == NULL) {
+		dev_err(isp->dev, "unsupported pixelformat!\n");
+		fmt = atomisp_output_fmts;
+	}
+
+#ifdef ISP2401
+	if (f->fmt.pix.width <= 0 || f->fmt.pix.height <= 0)
+		return -EINVAL;
+
+#endif
+	snr_mbus_fmt->code = fmt->mbus_code;
+	snr_mbus_fmt->width = f->fmt.pix.width;
+	snr_mbus_fmt->height = f->fmt.pix.height;
+
+	__atomisp_init_stream_info(stream_index, 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_cfg, &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 == NULL) {
+		dev_err(isp->dev, "unknown sensor format 0x%8.8x\n",
+			snr_mbus_fmt->code);
+		return -EINVAL;
+	}
+
+	f->fmt.pix.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->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG ||
+	    f->fmt.pix.pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
+		f->fmt.pix.width = snr_mbus_fmt->width;
+		f->fmt.pix.height = snr_mbus_fmt->height;
+		return 0;
+	}
+
+	if (snr_mbus_fmt->width < f->fmt.pix.width
+	    && snr_mbus_fmt->height < f->fmt.pix.height) {
+		f->fmt.pix.width = snr_mbus_fmt->width;
+		f->fmt.pix.height = snr_mbus_fmt->height;
+		/* Set the flag when resolution requested is
+		 * beyond the max value supported by sensor
+		 */
+		if (res_overflow != NULL)
+			*res_overflow = true;
+	}
+
+	/* app vs isp */
+	f->fmt.pix.width = rounddown(
+		clamp_t(u32, f->fmt.pix.width, ATOM_ISP_MIN_WIDTH,
+			ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
+	f->fmt.pix.height = rounddown(
+		clamp_t(u32, f->fmt.pix.height, ATOM_ISP_MIN_HEIGHT,
+			ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
+
+	return 0;
+}
+
+static int
+atomisp_try_fmt_file(struct atomisp_device *isp, struct v4l2_format *f)
+{
+	u32 width = f->fmt.pix.width;
+	u32 height = f->fmt.pix.height;
+	u32 pixelformat = f->fmt.pix.pixelformat;
+	enum v4l2_field field = f->fmt.pix.field;
+	u32 depth;
+
+	if (!atomisp_get_format_bridge(pixelformat)) {
+		dev_err(isp->dev, "Wrong output pixelformat\n");
+		return -EINVAL;
+	}
+
+	depth = get_pixel_depth(pixelformat);
+
+	if (field == V4L2_FIELD_ANY)
+		field = V4L2_FIELD_NONE;
+	else if (field != V4L2_FIELD_NONE) {
+		dev_err(isp->dev, "Wrong output field\n");
+		return -EINVAL;
+	}
+
+	f->fmt.pix.field = field;
+	f->fmt.pix.width = clamp_t(u32,
+				   rounddown(width, (u32)ATOM_ISP_STEP_WIDTH),
+				   ATOM_ISP_MIN_WIDTH, ATOM_ISP_MAX_WIDTH);
+	f->fmt.pix.height = clamp_t(u32, rounddown(height,
+				    (u32)ATOM_ISP_STEP_HEIGHT),
+				    ATOM_ISP_MIN_HEIGHT, ATOM_ISP_MAX_HEIGHT);
+	f->fmt.pix.bytesperline = (width * depth) >> 3;
+
+	return 0;
+}
+
+mipi_port_ID_t __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;
+		/* go through down for else case */
+	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->css_stream_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->css_stream_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->css_stream_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);
+#ifndef ISP2401
+	atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_PRIMARY);
+#else
+	atomisp_update_capture_mode(asd);
+#endif
+	/* 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);
+
+	if (atomisp_css_continuous_set_num_raw_frames(asd,
+			asd->continuous_raw_buffer_size->val)) {
+		dev_err(isp->dev, "css_continuous_set_num_raw_frames failed\n");
+		return -EINVAL;
+	}
+
+	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, CSS_INPUT_MODE_SENSOR);
+
+	return atomisp_update_run_mode(asd);
+}
+
+int configure_pp_input_nop(struct atomisp_sub_device *asd,
+			   unsigned int width, unsigned int height)
+{
+	return 0;
+}
+
+int configure_output_nop(struct atomisp_sub_device *asd,
+			 unsigned int width, unsigned int height,
+			 unsigned int min_width,
+			 enum atomisp_css_frame_format sh_fmt)
+{
+	return 0;
+}
+
+int get_frame_info_nop(struct atomisp_sub_device *asd,
+		       struct atomisp_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 defined(ISP2401_NEW_INPUT_SYSTEM)
+	atomisp_css_input_set_two_pixels_per_clock(asd, false);
+#else
+	atomisp_css_input_set_two_pixels_per_clock(asd, true);
+#endif
+	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 atomisp_css_frame_info *output_info,
+			struct atomisp_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 atomisp_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 atomisp_css_frame_format sh_fmt) =
+							configure_output_nop;
+	int (*get_frame_info)(struct atomisp_sub_device *asd,
+			      struct atomisp_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;
+	uint16_t stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
+	const struct atomisp_in_fmt_conv *fc;
+	int ret;
+
+	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 == NULL)
+		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, stream_index,
+					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 == CSS_FRAME_FORMAT_RAW &&
+		     raw_output_format_match_input(fc->css_stream_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.pad,
+					     V4L2_SUBDEV_FORMAT_ACTIVE,
+					     ATOMISP_SUBDEV_PAD_SOURCE_VF,
+					     V4L2_SEL_TGT_COMPOSE, 0, &vf_size);
+		atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
+					V4L2_SUBDEV_FORMAT_ACTIVE,
+					ATOMISP_SUBDEV_PAD_SOURCE_VF, &vf_ffmt);
+		asd->video_out_vf.sh_fmt = 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, CSS_INPUT_MODE_SENSOR);
+	atomisp_css_disable_vf_pp(asd,
+			asd->vfpp->val != ATOMISP_VFPP_ENABLE);
+
+	/* ISP2401 new input system need to use copy pipe */
+	if (asd->copy_mode) {
+		pipe_id = CSS_PIPE_ID_COPY;
+		atomisp_css_capture_enable_online(asd, stream_index, 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 = 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 = 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 = 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 = CSS_PIPE_ID_CAPTURE;
+
+				atomisp_update_capture_mode(asd);
+				atomisp_css_capture_enable_online(asd, stream_index, 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 = 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 == CSS_FRAME_FORMAT_RAW) {
+			atomisp_css_capture_set_mode(asd, 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, stream_index,
+					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 = 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 = CSS_PIPE_ID_YUVPP;
+
+	if (asd->copy_mode)
+		ret = atomisp_css_copy_configure_output(asd, stream_index,
+				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;
+	}
+
+	if (asd->continuous_mode->val &&
+	    (configure_pp_input == atomisp_css_preview_configure_pp_input ||
+	     configure_pp_input == atomisp_css_video_configure_pp_input)) {
+		/* for isp 2.2, configure pp input is available for continuous
+		 * mode */
+		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;
+		}
+	} else {
+		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, stream_index,
+				output_info);
+	else
+		ret = get_frame_info(asd, output_info);
+	if (ret) {
+		dev_err(isp->dev, "get_frame_info %ux%u (padded to %u)\n",
+			pix->width, pix->height, pix->bytesperline);
+		return -EINVAL;
+	}
+
+	atomisp_update_grid_info(asd, pipe_id, source_pad);
+
+	/* Free the raw_dump buffer first */
+	atomisp_css_frame_free(asd->raw_output_frame);
+	asd->raw_output_frame = NULL;
+
+	if (!asd->continuous_mode->val &&
+	    !asd->params.online_process && !isp->sw_contex.file_input &&
+		atomisp_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 = 0;
+
+	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_format *f)
+{
+#if defined(ISP2401_NEW_INPUT_SYSTEM)
+	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);
+
+	if ((sink->code == src->code &&
+	    sink->width == f->fmt.pix.width &&
+	    sink->height == f->fmt.pix.height) ||
+	    ((asd->isp->inputs[asd->input_curr].type == SOC_CAMERA) &&
+	    (asd->isp->inputs[asd->input_curr].camera_caps->
+	    sensor[asd->sensor_curr].stream_num > 1)))
+		asd->copy_mode = true;
+	else
+#endif
+		/* Only used for the new input system */
+		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_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_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	const struct atomisp_format_bridge *format;
+	struct v4l2_subdev_pad_config 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 = asd->isp;
+	struct atomisp_input_stream_info *stream_info =
+	    (struct atomisp_input_stream_info *)ffmt->reserved;
+	uint16_t stream_index = ATOMISP_INPUT_STREAM_GENERAL;
+	int source_pad = atomisp_subdev_source_pad(vdev);
+	struct v4l2_subdev_fh fh;
+	int ret;
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
+
+	format = atomisp_get_format_bridge(pixelformat);
+	if (format == NULL)
+		return -EINVAL;
+
+	v4l2_fill_mbus_format(ffmt, &f->fmt.pix, 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(stream_index, 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_cfg, &vformat);
+		if (ret)
+			return ret;
+		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 = 0;
+		}
+	}
+	dev_dbg(isp->dev, "sensor width: %d, height: %d\n",
+		ffmt->width, ffmt->height);
+	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, stream_index, 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 = 0;
+	}
+
+	atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+				ATOMISP_SUBDEV_PAD_SINK, ffmt);
+
+	return css_input_resolution_changed(asd, ffmt);
+}
+
+int atomisp_set_fmt(struct video_device *vdev, struct v4l2_format *f)
+{
+	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 atomisp_css_frame_info output_info, raw_output_info;
+	struct v4l2_format snr_fmt = *f;
+	struct v4l2_format backup_fmt = *f, s_fmt = *f;
+	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_rect isp_sink_crop;
+	uint16_t source_pad = atomisp_subdev_source_pad(vdev);
+	struct v4l2_subdev_fh fh;
+	int ret;
+
+	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);
+
+	if (source_pad >= ATOMISP_SUBDEV_PADS_NUM)
+		return -EINVAL;
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
+		dev_warn(isp->dev, "ISP does not support set format while at streaming!\n");
+		return -EBUSY;
+	}
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+	if (format_bridge == NULL)
+		return -EINVAL;
+
+	pipe->sh_fmt = format_bridge->sh_fmt;
+	pipe->pix.pixelformat = f->fmt.pix.pixelformat;
+
+	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.pad,
+				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 &&
+		    (asd->isp->inputs[asd->input_curr].type == SOC_CAMERA) &&
+		    (asd->isp->inputs[asd->input_curr].camera_caps->
+		    sensor[asd->sensor_curr].stream_num > 1)) {
+			/* For M10MO outputing YUV preview images. */
+			uint16_t video_index =
+				atomisp_source_pad_to_stream_id(asd,
+					ATOMISP_SUBDEV_PAD_SOURCE_VIDEO);
+
+			ret = atomisp_css_copy_get_output_frame_info(asd,
+				video_index, &output_info);
+			if (ret) {
+				dev_err(isp->dev,
+				      "copy_get_output_frame_info ret %i", ret);
+				return -EINVAL;
+			}
+			if (!asd->yuvpp_mode) {
+				/*
+				 * If viewfinder was configured into copy_mode,
+				 * we switch to using yuvpp pipe instead.
+				 */
+				asd->yuvpp_mode = true;
+				ret = atomisp_css_copy_configure_output(
+					asd, video_index, 0, 0, 0, 0);
+				if (ret) {
+					dev_err(isp->dev,
+						"failed to disable copy pipe");
+					return -EINVAL;
+				}
+				ret = atomisp_css_yuvpp_configure_output(
+					asd, video_index,
+					output_info.res.width,
+					output_info.res.height,
+					output_info.padded_width,
+					output_info.format);
+				if (ret) {
+					dev_err(isp->dev,
+						"failed to set up yuvpp pipe\n");
+					return -EINVAL;
+				}
+				atomisp_css_video_enable_online(asd, false);
+				atomisp_css_preview_enable_online(asd,
+					ATOMISP_INPUT_STREAM_GENERAL, false);
+			}
+			atomisp_css_yuvpp_configure_viewfinder(asd, video_index,
+				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_yuvpp_get_viewfinder_frame_info(
+				asd, video_index, &output_info);
+		} else 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)
+		return ret;
+	f->fmt.pix.width = snr_fmt.fmt.pix.width;
+	f->fmt.pix.height = snr_fmt.fmt.pix.height;
+
+	snr_format_bridge =
+		atomisp_get_format_bridge(snr_fmt.fmt.pix.pixelformat);
+	if (!snr_format_bridge)
+		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.pad,
+				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;
+		if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG ||
+		    f->fmt.pix.pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
+			s_fmt.fmt.pix.width = backup_fmt.fmt.pix.width;
+			s_fmt.fmt.pix.height = backup_fmt.fmt.pix.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)
+			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.pad,
+					     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.pad,
+					     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.pad,
+				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;
+
+#ifndef ISP2401
+		/* 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 (1 ||
+		    crop_needs_override) {
+#else
+		if (crop_needs_override) {
+#endif
+			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.pad,
+				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.pad,
+				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)
+		return -EINVAL;
+done:
+	pipe->pix.width = f->fmt.pix.width;
+	pipe->pix.height = f->fmt.pix.height;
+	pipe->pix.pixelformat = f->fmt.pix.pixelformat;
+	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);
+
+	}
+	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;
+
+	return 0;
+}
+
+int atomisp_set_fmt_file(struct video_device *vdev, struct v4l2_format *f)
+{
+	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_mbus_framefmt ffmt = {0};
+	const struct atomisp_format_bridge *format_bridge;
+	struct v4l2_subdev_fh fh;
+	int ret;
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	dev_dbg(isp->dev, "setting fmt %ux%u 0x%x for file inject\n",
+		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
+	ret = atomisp_try_fmt_file(isp, f);
+	if (ret) {
+		dev_err(isp->dev, "atomisp_try_fmt_file err: %d\n", ret);
+		return ret;
+	}
+
+	format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
+	if (format_bridge == NULL) {
+		dev_dbg(isp->dev, "atomisp_get_format_bridge err! fmt:0x%x\n",
+				f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+
+	pipe->pix = f->fmt.pix;
+	atomisp_css_input_set_mode(asd, CSS_INPUT_MODE_FIFO);
+	atomisp_css_input_configure_port(asd,
+		__get_mipi_port(isp, ATOMISP_CAMERA_PORT_PRIMARY), 2, 0xffff4,
+		0, 0, 0, 0);
+	ffmt.width = f->fmt.pix.width;
+	ffmt.height = f->fmt.pix.height;
+	ffmt.code = format_bridge->mbus_code;
+
+	atomisp_subdev_set_ffmt(&asd->subdev, fh.pad, V4L2_SUBDEV_FORMAT_ACTIVE,
+				ATOMISP_SUBDEV_PAD_SINK, &ffmt);
+
+	return 0;
+}
+
+int atomisp_set_shading_table(struct atomisp_sub_device *asd,
+		struct atomisp_shading_table *user_shading_table)
+{
+	struct atomisp_css_shading_table *shading_table;
+	struct atomisp_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) {
+		atomisp_css_set_shading_table(asd, NULL);
+		asd->params.sc_en = 0;
+		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],
+				     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;
+	atomisp_css_set_shading_table(asd, shading_table);
+	asd->params.sc_en = 1;
+
+out:
+	if (free_table != NULL)
+		atomisp_css_shading_table_free(free_table);
+
+	return ret;
+}
+
+/*Turn off ISP dphy */
+int atomisp_ospm_dphy_down(struct atomisp_device *isp)
+{
+	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(isp->pdev, MRFLD_PCI_CSI_CONTROL, &reg);
+	reg |= MRFLD_ALL_CSI_PORTS_OFF_MASK;
+	pci_write_config_dword(isp->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;
+
+	/*
+	* 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;
+}
+
+int atomisp_source_pad_to_stream_id(struct atomisp_sub_device *asd,
+					   uint16_t source_pad)
+{
+	int stream_id;
+	struct atomisp_device *isp = asd->isp;
+
+	if (isp->inputs[asd->input_curr].camera_caps->
+			sensor[asd->sensor_curr].stream_num == 1)
+		return ATOMISP_INPUT_STREAM_GENERAL;
+
+	switch (source_pad) {
+	case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
+		stream_id = ATOMISP_INPUT_STREAM_CAPTURE;
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_VF:
+		stream_id = ATOMISP_INPUT_STREAM_POSTVIEW;
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
+		stream_id = ATOMISP_INPUT_STREAM_PREVIEW;
+		break;
+	case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO:
+		stream_id = ATOMISP_INPUT_STREAM_VIDEO;
+		break;
+	default:
+		stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+	}
+
+	return stream_id;
+}
+
+bool atomisp_is_vf_pipe(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_sub_device *asd = pipe->asd;
+
+	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);
+}
+
+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;
+
+	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;
+
+	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 == NULL)
+		return -EINVAL;
+
+	value = *enable > 0 ? true : false;
+
+	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;
+
+	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;
+}
+
+int atomisp_get_pipe_id(struct atomisp_video_pipe *pipe)
+{
+	struct atomisp_sub_device *asd = pipe->asd;
+
+	if (ATOMISP_USE_YUVPP(asd))
+		return CSS_PIPE_ID_YUVPP;
+	else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER)
+		return CSS_PIPE_ID_VIDEO;
+	else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT)
+		return CSS_PIPE_ID_CAPTURE;
+	else if (pipe == &asd->video_out_video_capture)
+		return CSS_PIPE_ID_VIDEO;
+	else if (pipe == &asd->video_out_vf)
+		return CSS_PIPE_ID_CAPTURE;
+	else if (pipe == &asd->video_out_preview) {
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			return CSS_PIPE_ID_VIDEO;
+		else
+			return CSS_PIPE_ID_PREVIEW;
+	} else if (pipe == &asd->video_out_capture) {
+		if (asd->copy_mode)
+			return IA_CSS_PIPE_ID_COPY;
+		else
+			return CSS_PIPE_ID_CAPTURE;
+	}
+
+	/* fail through */
+	dev_warn(asd->isp->dev, "%s failed to find proper pipe\n",
+		__func__);
+	return 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 atomisp_css_pipe_id pipe_id;
+	struct ia_css_pipe_info p_info;
+	int ret;
+
+	if (asd->isp->inputs[asd->input_curr].camera_caps->
+		sensor[asd->sensor_curr].stream_num > 1) {
+		/* External ISP */
+		*invalid_frame_num = 0;
+		return 0;
+	}
+
+	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 == IA_CSS_SUCCESS) {
+		*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/atomisp2/atomisp_cmd.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.h
new file mode 100644
index 000000000000..8e6d9df7ad1a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.h
@@ -0,0 +1,457 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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 atomisp_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
+#ifdef 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;
+#endif
+
+/*
+ * 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);
+struct atomisp_acc_pipe *atomisp_to_acc_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);
+#ifndef ISP2401
+bool atomisp_buffers_queued(struct atomisp_sub_device *asd);
+#else
+bool atomisp_buffers_queued_pipe(struct atomisp_video_pipe *pipe);
+#endif
+
+/* TODO:should be here instead of atomisp_helper.h
+extern void __iomem *atomisp_io_base;
+
+static inline void __iomem *atomisp_get_io_virt_addr(unsigned int address)
+{
+	void __iomem *ret = atomisp_io_base + (address & 0x003FFFFF);
+	return ret;
+}
+*/
+void *atomisp_kernel_malloc(size_t bytes);
+void *atomisp_kernel_zalloc(size_t bytes, bool zero_mem);
+void atomisp_kernel_free(void *ptr);
+
+/*
+ * Interrupt functions
+ */
+void atomisp_msi_irq_init(struct atomisp_device *isp, struct pci_dev *dev);
+void atomisp_msi_irq_uninit(struct atomisp_device *isp, struct pci_dev *dev);
+void atomisp_wdt_work(struct work_struct *work);
+#ifndef ISP2401
+void atomisp_wdt(unsigned long isp_addr);
+#else
+void atomisp_wdt(unsigned long pipe_addr);
+#endif
+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);
+int atomisp_get_frame_pgnr(struct atomisp_device *isp,
+			   const struct atomisp_css_frame *frame, u32 *p_pgnr);
+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
+ */
+
+/*
+#ifdef ISP2401
+ * 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);
+/*
+#endif
+ * 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 atomisp_css_dz_config   *dz_config,
+			enum atomisp_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);
+
+int atomisp_get_fmt(struct video_device *vdev, struct v4l2_format *f);
+
+
+/* This function looks up the closest available resolution. */
+int atomisp_try_fmt(struct video_device *vdev, struct v4l2_format *f,
+						bool *res_overflow);
+
+int atomisp_set_fmt(struct video_device *vdev, struct v4l2_format *f);
+int atomisp_set_fmt_file(struct video_device *vdev, 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 atomisp_css_buffer_type buf_type,
+		      enum atomisp_css_pipe_id css_pipe_id,
+		      bool q_buffers, enum atomisp_input_stream_id stream_id);
+
+void atomisp_css_flush(struct atomisp_device *isp);
+int atomisp_source_pad_to_stream_id(struct atomisp_sub_device *asd,
+					   uint16_t source_pad);
+
+/*
+ * Events. Only one event has to be exported for now.
+ */
+void atomisp_eof_event(struct atomisp_sub_device *asd, uint8_t exp_id);
+
+mipi_port_ID_t __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);
+
+/*
+ * Function to update Raw Buffer bitmap
+ */
+int atomisp_set_raw_buffer_bitmap(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);
+
+/*
+ * 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/atomisp2/atomisp_common.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_common.h
new file mode 100644
index 000000000000..69d1526da362
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_common.h
@@ -0,0 +1,79 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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
+#ifdef ISP2401
+#define ATOMISP_CSS_ISP_PIPE_VERSION_2_2    0
+#define ATOMISP_CSS_ISP_PIPE_VERSION_2_7    1
+#endif
+
+#define IS_ISP2401(isp)							\
+	(((isp)->media_dev.hw_revision & ATOMISP_HW_REVISION_MASK)	\
+	 >= (ATOMISP_HW_REVISION_ISP2401_LEGACY << ATOMISP_HW_REVISION_SHIFT))
+
+struct atomisp_format_bridge {
+	unsigned int pixelformat;
+	unsigned int depth;
+	u32 mbus_code;
+	enum atomisp_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/atomisp2/atomisp_compat.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat.h
new file mode 100644
index 000000000000..fb8b8fab4e92
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat.h
@@ -0,0 +1,668 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __ATOMISP_COMPAT_H__
+#define __ATOMISP_COMPAT_H__
+
+#include "atomisp_compat_css20.h"
+
+#include "../../include/linux/atomisp.h"
+#include <media/videobuf-vmalloc.h>
+
+#define CSS_RX_IRQ_INFO_BUFFER_OVERRUN \
+	CSS_ID(CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
+#define CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE \
+	CSS_ID(CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE)
+#define CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE \
+	CSS_ID(CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE)
+#define CSS_RX_IRQ_INFO_ECC_CORRECTED \
+	CSS_ID(CSS_RX_IRQ_INFO_ECC_CORRECTED)
+#define CSS_RX_IRQ_INFO_ERR_SOT \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_SOT)
+#define CSS_RX_IRQ_INFO_ERR_SOT_SYNC \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
+#define CSS_RX_IRQ_INFO_ERR_CONTROL \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_CONTROL)
+#define CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
+#define CSS_RX_IRQ_INFO_ERR_CRC \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_CRC)
+#define CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
+#define CSS_RX_IRQ_INFO_ERR_FRAME_SYNC \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
+#define CSS_RX_IRQ_INFO_ERR_FRAME_DATA \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
+#define CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
+#define CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
+#define CSS_RX_IRQ_INFO_ERR_LINE_SYNC \
+	CSS_ID(CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
+#define CSS_RX_IRQ_INFO_INIT_TIMEOUT \
+	CSS_ID(CSS_RX_IRQ_INFO_INIT_TIMEOUT)
+
+#define CSS_IRQ_INFO_CSS_RECEIVER_SOF	CSS_ID(CSS_IRQ_INFO_CSS_RECEIVER_SOF)
+#define CSS_IRQ_INFO_CSS_RECEIVER_EOF	CSS_ID(CSS_IRQ_INFO_CSS_RECEIVER_EOF)
+#define CSS_IRQ_INFO_CSS_RECEIVER_FIFO_OVERFLOW \
+	CSS_ID(CSS_IRQ_INFO_CSS_RECEIVER_FIFO_OVERFLOW)
+#define CSS_EVENT_OUTPUT_FRAME_DONE	CSS_EVENT(OUTPUT_FRAME_DONE)
+#define CSS_EVENT_SEC_OUTPUT_FRAME_DONE	CSS_EVENT(SECOND_OUTPUT_FRAME_DONE)
+#define CSS_EVENT_VF_OUTPUT_FRAME_DONE	CSS_EVENT(VF_OUTPUT_FRAME_DONE)
+#define CSS_EVENT_SEC_VF_OUTPUT_FRAME_DONE	CSS_EVENT(SECOND_VF_OUTPUT_FRAME_DONE)
+#define CSS_EVENT_3A_STATISTICS_DONE	CSS_EVENT(3A_STATISTICS_DONE)
+#define CSS_EVENT_DIS_STATISTICS_DONE	CSS_EVENT(DIS_STATISTICS_DONE)
+#define CSS_EVENT_PIPELINE_DONE		CSS_EVENT(PIPELINE_DONE)
+#define CSS_EVENT_METADATA_DONE		CSS_EVENT(METADATA_DONE)
+#define CSS_EVENT_ACC_STAGE_COMPLETE	CSS_EVENT(ACC_STAGE_COMPLETE)
+#define CSS_EVENT_TIMER			CSS_EVENT(TIMER)
+
+#define CSS_BUFFER_TYPE_METADATA	CSS_ID(CSS_BUFFER_TYPE_METADATA)
+#define CSS_BUFFER_TYPE_3A_STATISTICS	CSS_ID(CSS_BUFFER_TYPE_3A_STATISTICS)
+#define CSS_BUFFER_TYPE_DIS_STATISTICS	CSS_ID(CSS_BUFFER_TYPE_DIS_STATISTICS)
+#define CSS_BUFFER_TYPE_INPUT_FRAME	CSS_ID(CSS_BUFFER_TYPE_INPUT_FRAME)
+#define CSS_BUFFER_TYPE_OUTPUT_FRAME	CSS_ID(CSS_BUFFER_TYPE_OUTPUT_FRAME)
+#define CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME	CSS_ID(CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME)
+#define CSS_BUFFER_TYPE_VF_OUTPUT_FRAME	CSS_ID(CSS_BUFFER_TYPE_VF_OUTPUT_FRAME)
+#define CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME	CSS_ID(CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME)
+#define CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME \
+	CSS_ID(CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME)
+
+#define CSS_FORMAT_RAW_8	CSS_FORMAT(RAW_8)
+#define CSS_FORMAT_RAW_10	CSS_FORMAT(RAW_10)
+#define CSS_FORMAT_RAW_12	CSS_FORMAT(RAW_12)
+#define CSS_FORMAT_RAW_16	CSS_FORMAT(RAW_16)
+
+#define CSS_CAPTURE_MODE_RAW		CSS_ID(CSS_CAPTURE_MODE_RAW)
+#define CSS_CAPTURE_MODE_BAYER		CSS_ID(CSS_CAPTURE_MODE_BAYER)
+#define CSS_CAPTURE_MODE_PRIMARY	CSS_ID(CSS_CAPTURE_MODE_PRIMARY)
+#define CSS_CAPTURE_MODE_ADVANCED	CSS_ID(CSS_CAPTURE_MODE_ADVANCED)
+#define CSS_CAPTURE_MODE_LOW_LIGHT	CSS_ID(CSS_CAPTURE_MODE_LOW_LIGHT)
+
+#define CSS_MORPH_TABLE_NUM_PLANES	CSS_ID(CSS_MORPH_TABLE_NUM_PLANES)
+
+#define CSS_FRAME_FORMAT_NV11		CSS_ID(CSS_FRAME_FORMAT_NV11)
+#define CSS_FRAME_FORMAT_NV12		CSS_ID(CSS_FRAME_FORMAT_NV12)
+#define CSS_FRAME_FORMAT_NV16		CSS_ID(CSS_FRAME_FORMAT_NV16)
+#define CSS_FRAME_FORMAT_NV21		CSS_ID(CSS_FRAME_FORMAT_NV21)
+#define CSS_FRAME_FORMAT_NV61		CSS_ID(CSS_FRAME_FORMAT_NV61)
+#define CSS_FRAME_FORMAT_YV12		CSS_ID(CSS_FRAME_FORMAT_YV12)
+#define CSS_FRAME_FORMAT_YV16		CSS_ID(CSS_FRAME_FORMAT_YV16)
+#define CSS_FRAME_FORMAT_YUV420		CSS_ID(CSS_FRAME_FORMAT_YUV420)
+#define CSS_FRAME_FORMAT_YUV420_16	CSS_ID(CSS_FRAME_FORMAT_YUV420_16)
+#define CSS_FRAME_FORMAT_YUV422		CSS_ID(CSS_FRAME_FORMAT_YUV422)
+#define CSS_FRAME_FORMAT_YUV422_16	CSS_ID(CSS_FRAME_FORMAT_YUV422_16)
+#define CSS_FRAME_FORMAT_UYVY		CSS_ID(CSS_FRAME_FORMAT_UYVY)
+#define CSS_FRAME_FORMAT_YUYV		CSS_ID(CSS_FRAME_FORMAT_YUYV)
+#define CSS_FRAME_FORMAT_YUV444		CSS_ID(CSS_FRAME_FORMAT_YUV444)
+#define CSS_FRAME_FORMAT_YUV_LINE	CSS_ID(CSS_FRAME_FORMAT_YUV_LINE)
+#define CSS_FRAME_FORMAT_RAW		CSS_ID(CSS_FRAME_FORMAT_RAW)
+#define CSS_FRAME_FORMAT_RGB565		CSS_ID(CSS_FRAME_FORMAT_RGB565)
+#define CSS_FRAME_FORMAT_PLANAR_RGB888	CSS_ID(CSS_FRAME_FORMAT_PLANAR_RGB888)
+#define CSS_FRAME_FORMAT_RGBA888	CSS_ID(CSS_FRAME_FORMAT_RGBA888)
+#define CSS_FRAME_FORMAT_QPLANE6	CSS_ID(CSS_FRAME_FORMAT_QPLANE6)
+#define CSS_FRAME_FORMAT_BINARY_8	CSS_ID(CSS_FRAME_FORMAT_BINARY_8)
+
+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_css_debug_dump_sp_sw_debug_info(void);
+void atomisp_css_debug_dump_debug_info(const char *context);
+void atomisp_css_debug_set_dtrace_level(const unsigned int trace_level);
+
+void atomisp_store_uint32(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 ia_css_csi2_port port,
+					unsigned int *infos);
+
+void atomisp_css_rx_clear_irq_info(enum ia_css_csi2_port port,
+					unsigned int infos);
+
+int atomisp_css_irq_enable(struct atomisp_device *isp,
+			   enum atomisp_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 atomisp_css_buffer_type css_buf_type,
+			enum atomisp_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 atomisp_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 atomisp_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 atomisp_css_pipe_id css_pipe_id);
+
+void atomisp_css_mmu_invalidate_cache(void);
+
+void atomisp_css_mmu_invalidate_tlb(void);
+
+void atomisp_css_mmu_set_page_table_base_index(unsigned long base_index);
+
+int atomisp_css_start(struct atomisp_sub_device *asd,
+		      enum atomisp_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 atomisp_css_pipe_id pipe_id,
+			     enum atomisp_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 atomisp_css_pipe_id pipe_id,
+				enum atomisp_css_buffer_type buf_type,
+				struct atomisp_css_buffer *isp_css_buffer);
+
+int atomisp_css_allocate_stat_buffers(struct atomisp_sub_device *asd,
+				      uint16_t 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 atomisp_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_get_dis_statistics(struct atomisp_sub_device *asd,
+				    struct atomisp_css_buffer *isp_css_buffer,
+				    struct ia_css_isp_dvs_statistics_map *dvs_map);
+
+int atomisp_css_dequeue_event(struct atomisp_css_event *current_event);
+
+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_css_stream_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_css_stream_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_css_stream_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_css_stream_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 atomisp_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_css_stream_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 atomisp_css_capture_mode mode);
+
+void atomisp_css_input_set_mode(struct atomisp_sub_device *asd,
+				enum atomisp_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,
+				mipi_port_ID_t port,
+				unsigned int num_lanes,
+				unsigned int timeout,
+				unsigned int mipi_freq,
+				enum atomisp_css_stream_format metadata_format,
+				unsigned int metadata_width,
+				unsigned int metadata_height);
+
+int atomisp_css_frame_allocate(struct atomisp_css_frame **frame,
+				unsigned int width, unsigned int height,
+				enum atomisp_css_frame_format format,
+				unsigned int padded_width,
+				unsigned int raw_bit_depth);
+
+int atomisp_css_frame_allocate_from_info(struct atomisp_css_frame **frame,
+				const struct atomisp_css_frame_info *info);
+
+void atomisp_css_frame_free(struct atomisp_css_frame *frame);
+
+int atomisp_css_frame_map(struct atomisp_css_frame **frame,
+				const struct atomisp_css_frame_info *info,
+				const void *data, uint16_t attribute,
+				void *context);
+
+int atomisp_css_set_black_frame(struct atomisp_sub_device *asd,
+			const struct atomisp_css_frame *raw_black_frame);
+
+int atomisp_css_allocate_continuous_frames(bool init_time,
+			struct atomisp_sub_device *asd);
+
+void atomisp_css_update_continuous_frames(struct atomisp_sub_device *asd);
+
+void atomisp_create_pipes_stream(struct atomisp_sub_device *asd);
+void atomisp_destroy_pipes_stream_force(struct atomisp_sub_device *asd);
+
+int atomisp_css_stop(struct atomisp_sub_device *asd,
+			enum atomisp_css_pipe_id pipe_id, bool in_reset);
+
+int atomisp_css_continuous_set_num_raw_frames(
+					struct atomisp_sub_device *asd,
+					int num_frames);
+
+void atomisp_css_disable_vf_pp(struct atomisp_sub_device *asd,
+			       bool disable);
+
+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 atomisp_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 atomisp_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 atomisp_css_frame_format format);
+
+int atomisp_css_yuvpp_get_output_frame_info(
+					struct atomisp_sub_device *asd,
+					unsigned int stream_index,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_yuvpp_get_viewfinder_frame_info(
+					struct atomisp_sub_device *asd,
+					unsigned int stream_index,
+					struct atomisp_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 atomisp_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 atomisp_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 atomisp_css_frame_format format);
+
+int atomisp_get_css_frame_info(struct atomisp_sub_device *asd,
+				uint16_t source_pad,
+				struct atomisp_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 atomisp_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 atomisp_css_frame_format format);
+
+int atomisp_css_video_get_viewfinder_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_capture_get_viewfinder_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_copy_get_output_frame_info(
+					struct atomisp_sub_device *asd,
+					unsigned int stream_index,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_capture_get_output_raw_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_preview_get_output_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_capture_get_output_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_css_frame_info *info);
+
+int atomisp_css_video_get_output_frame_info(
+					struct atomisp_sub_device *asd,
+					struct atomisp_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_send_input_frame(struct atomisp_sub_device *asd,
+				  unsigned short *data, unsigned int width,
+				  unsigned int height);
+
+bool atomisp_css_isp_has_started(void);
+
+void atomisp_css_request_flash(struct atomisp_sub_device *asd);
+
+void atomisp_css_set_wb_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_wb_config *wb_config);
+
+void atomisp_css_set_ob_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ob_config *ob_config);
+
+void atomisp_css_set_dp_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_dp_config *dp_config);
+
+void atomisp_css_set_de_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_de_config *de_config);
+
+void atomisp_css_set_dz_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_dz_config *dz_config);
+
+void atomisp_css_set_default_de_config(struct atomisp_sub_device *asd);
+
+void atomisp_css_set_ce_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ce_config *ce_config);
+
+void atomisp_css_set_nr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_nr_config *nr_config);
+
+void atomisp_css_set_ee_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ee_config *ee_config);
+
+void atomisp_css_set_tnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_tnr_config *tnr_config);
+
+void atomisp_css_set_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *cc_config);
+
+void atomisp_css_set_macc_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_macc_table *macc_table);
+
+void atomisp_css_set_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_gamma_table *gamma_table);
+
+void atomisp_css_set_ctc_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_ctc_table *ctc_table);
+
+void atomisp_css_set_gc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_gc_config *gc_config);
+
+void atomisp_css_set_3a_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_3a_config *s3a_config);
+
+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 atomisp_css_formats_config *formats_config);
+
+int atomisp_css_get_zoom_factor(struct atomisp_sub_device *asd,
+					unsigned int *zoom);
+
+struct atomisp_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 atomisp_css_shading_table *table);
+
+void atomisp_css_shading_table_free(struct atomisp_css_shading_table *table);
+
+struct atomisp_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 atomisp_css_morph_table *table);
+
+void atomisp_css_get_morph_table(struct atomisp_sub_device *asd,
+				struct atomisp_css_morph_table *table);
+
+void atomisp_css_morph_table_free(struct atomisp_css_morph_table *table);
+
+void atomisp_css_set_cont_prev_start_time(struct atomisp_device *isp,
+					unsigned int overlap);
+
+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_create_acc_pipe(struct atomisp_sub_device *asd);
+
+int atomisp_css_start_acc_pipe(struct atomisp_sub_device *asd);
+
+int atomisp_css_stop_acc_pipe(struct atomisp_sub_device *asd);
+
+void atomisp_css_destroy_acc_pipe(struct atomisp_sub_device *asd);
+
+int atomisp_css_load_acc_extension(struct atomisp_sub_device *asd,
+					struct atomisp_css_fw_info *fw,
+					enum atomisp_css_pipe_id pipe_id,
+					unsigned int type);
+
+void atomisp_css_unload_acc_extension(struct atomisp_sub_device *asd,
+					struct atomisp_css_fw_info *fw,
+					enum atomisp_css_pipe_id pipe_id);
+
+int atomisp_css_wait_acc_finish(struct atomisp_sub_device *asd);
+
+void atomisp_css_acc_done(struct atomisp_sub_device *asd);
+
+int atomisp_css_load_acc_binary(struct atomisp_sub_device *asd,
+					struct atomisp_css_fw_info *fw,
+					unsigned int index);
+
+void atomisp_css_unload_acc_binary(struct atomisp_sub_device *asd);
+
+struct atomisp_acc_fw;
+int atomisp_css_set_acc_parameters(struct atomisp_acc_fw *acc_fw);
+
+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/atomisp2/atomisp_compat_css20.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c
new file mode 100644
index 000000000000..b830b241e2e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.c
@@ -0,0 +1,4722 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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 "atomisp_acc.h"
+
+#include "hrt/hive_isp_css_mm_hrt.h"
+
+#include <asm/intel-mid.h>
+
+#include "ia_css_debug.h"
+#include "ia_css_isp_param.h"
+#include "sh_css_hrt.h"
+#include "ia_css_isys.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;
+};
+
+void atomisp_css_debug_dump_sp_sw_debug_info(void)
+{
+	ia_css_debug_dump_sp_sw_debug_info();
+}
+
+void atomisp_css_debug_dump_debug_info(const char *context)
+{
+	ia_css_debug_dump_debug_info(context);
+}
+
+void atomisp_css_debug_set_dtrace_level(const unsigned int trace_level)
+{
+	ia_css_debug_set_dtrace_level(trace_level);
+}
+
+unsigned int atomisp_css_debug_get_dtrace_level(void)
+{
+	return ia_css_debug_trace_level;
+}
+
+void atomisp_css2_hw_store_8(hrt_address addr, uint8_t data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	_hrt_master_port_store_8(addr, data);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static void atomisp_css2_hw_store_16(hrt_address addr, uint16_t data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	_hrt_master_port_store_16(addr, data);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static void atomisp_css2_hw_store_32(hrt_address addr, uint32_t data)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	_hrt_master_port_store_32(addr, data);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static uint8_t atomisp_css2_hw_load_8(hrt_address addr)
+{
+	unsigned long flags;
+	uint8_t ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = _hrt_master_port_load_8(addr);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+
+uint16_t atomisp_css2_hw_load_16(hrt_address addr)
+{
+	unsigned long flags;
+	uint16_t ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = _hrt_master_port_load_16(addr);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+uint32_t atomisp_css2_hw_load_32(hrt_address addr)
+{
+	unsigned long flags;
+	uint32_t ret;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	ret = _hrt_master_port_load_32(addr);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+	return ret;
+}
+
+static void atomisp_css2_hw_store(hrt_address addr,
+				  const void *from, uint32_t n)
+{
+	unsigned long flags;
+	unsigned i;
+	unsigned int _to = (unsigned int)addr;
+	const char *_from = (const char *)from;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	for (i = 0; i < n; i++, _to++, _from++)
+		_hrt_master_port_store_8(_to , *_from);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static void atomisp_css2_hw_load(hrt_address addr, void *to, uint32_t n)
+{
+	unsigned long flags;
+	unsigned i;
+	char *_to = (char *)to;
+	unsigned int _from = (unsigned int)addr;
+
+	spin_lock_irqsave(&mmio_lock, flags);
+	for (i = 0; i < n; i++, _to++, _from++)
+		*_to = _hrt_master_port_load_8(_from);
+	spin_unlock_irqrestore(&mmio_lock, flags);
+}
+
+static int atomisp_css2_dbg_print(const char *fmt, va_list args)
+{
+	vprintk(fmt, args);
+	return 0;
+}
+
+static int atomisp_css2_dbg_ftrace_print(const char *fmt, va_list args)
+{
+	ftrace_vprintk(fmt, args);
+	return 0;
+}
+
+static int atomisp_css2_err_print(const char *fmt, va_list args)
+{
+	vprintk(fmt, args);
+	return 0;
+}
+
+void atomisp_store_uint32(hrt_address addr, uint32_t data)
+{
+	atomisp_css2_hw_store_32(addr, data);
+}
+
+void atomisp_load_uint32(hrt_address addr, uint32_t *data)
+{
+	*data = atomisp_css2_hw_load_32(addr);
+}
+static int hmm_get_mmu_base_addr(unsigned int *mmu_base_addr)
+{
+	if (sh_mmu_mrfld.get_pd_base == NULL) {
+		dev_err(atomisp_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 atomisp_isp_parameters_clean_up(
+				struct atomisp_css_isp_config *config)
+{
+	/*
+	 * Set NULL to configs pointer to avoid they are set into isp again when
+	 * some configs are changed and need to be updated later.
+	 */
+	memset(config, 0, sizeof(*config));
+}
+
+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, bool force)
+{
+	struct atomisp_device *isp = asd->isp;
+	int i;
+	unsigned long timeout;
+
+	if (!stream_env->stream)
+		return 0;
+
+	if (!force) {
+		for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+			if (stream_env->update_pipe[i])
+				break;
+
+		if (i == IA_CSS_PIPE_ID_NUM)
+			return 0;
+	}
+
+	if (stream_env->stream_state == CSS_STREAM_STARTED
+	    && ia_css_stream_stop(stream_env->stream) != IA_CSS_SUCCESS) {
+		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) != IA_CSS_SUCCESS) {
+		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, bool force)
+{
+	int ret, i;
+	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++) {
+		ret = __destroy_stream(asd, &asd->stream_env[i], force);
+		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) != IA_CSS_SUCCESS)
+		return -EINVAL;
+	if (ia_css_stream_get_info(stream_env->stream,
+				&stream_env->stream_info) != IA_CSS_SUCCESS) {
+		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], true);
+	return ret;
+}
+
+static int __destroy_stream_pipes(struct atomisp_sub_device *asd,
+				  struct atomisp_stream_env *stream_env,
+				  bool force)
+{
+	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] ||
+		    !(force || stream_env->update_pipe[i]))
+			continue;
+		if (ia_css_pipe_destroy(stream_env->pipes[i])
+		    != IA_CSS_SUCCESS) {
+			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, bool force)
+{
+	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], force);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+void atomisp_destroy_pipes_stream_force(struct atomisp_sub_device *asd)
+{
+	__destroy_streams(asd, true);
+	__destroy_pipes(asd, true);
+}
+
+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 == CSS_CAPTURE_MODE_RAW)
+			stream_env->pipe_configs[pipe_id].enable_dz = false;
+#ifdef ISP2401
+
+		/* the isp default to use ISP2.2 and the camera hal will
+		 * control whether use isp2.7 */
+		if (asd->select_isp_version->val ==
+			ATOMISP_CSS_ISP_PIPE_VERSION_2_7)
+			stream_env->pipe_configs[pipe_id].isp_pipe_version =
+				SH_CSS_ISP_PIPE_VERSION_2_7;
+		else
+			stream_env->pipe_configs[pipe_id].isp_pipe_version =
+				SH_CSS_ISP_PIPE_VERSION_2_2;
+#endif
+		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 == CSS_PIPE_ID_ACC || pipe_id == 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;
+		else
+			return false;
+	case ATOMISP_RUN_MODE_PREVIEW:
+		if (!asd->continuous_mode->val) {
+			if (pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+				return true;
+			else
+				return false;
+		}
+		/* fall through to ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE */
+	case ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE:
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE ||
+		    pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			return true;
+		else
+			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;
+		}
+		/* fall through to ATOMISP_RUN_MODE_SDV */
+	case ATOMISP_RUN_MODE_SDV:
+		if (pipe_id == IA_CSS_PIPE_ID_CAPTURE ||
+		    pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			return true;
+		else
+			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;
+	enum ia_css_err ret;
+
+	if (pipe_id >= IA_CSS_PIPE_ID_NUM)
+		return -EINVAL;
+
+	if (pipe_id != CSS_PIPE_ID_ACC &&
+	    !stream_env->pipe_configs[pipe_id].output_info[0].res.width)
+		return 0;
+
+	if (pipe_id == 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 != IA_CSS_SUCCESS)
+		dev_err(isp->dev, "create pipe[%d] error.\n", pipe_id);
+	return ret;
+}
+
+static int __create_pipes(struct atomisp_sub_device *asd)
+{
+	enum ia_css_err 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 != IA_CSS_SUCCESS)
+				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;
+}
+
+void atomisp_create_pipes_stream(struct atomisp_sub_device *asd)
+{
+	__create_pipes(asd);
+	__create_streams(asd);
+}
+
+int atomisp_css_update_stream(struct atomisp_sub_device *asd)
+{
+	int ret;
+	struct atomisp_device *isp = asd->isp;
+
+	if (__destroy_streams(asd, true) != IA_CSS_SUCCESS)
+		dev_warn(isp->dev, "destroy stream failed.\n");
+
+	if (__destroy_pipes(asd, true) != IA_CSS_SUCCESS)
+		dev_warn(isp->dev, "destroy pipe failed.\n");
+
+	ret = __create_pipes(asd);
+	if (ret != IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "create pipe failed %d.\n", ret);
+		return -EIO;
+	}
+
+	ret = __create_streams(asd);
+	if (ret != IA_CSS_SUCCESS) {
+		dev_warn(isp->dev, "create stream failed %d.\n", ret);
+		__destroy_pipes(asd, true);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int atomisp_css_init(struct atomisp_device *isp)
+{
+	unsigned int mmu_base_addr;
+	int ret;
+	enum ia_css_err err;
+
+	ret = hmm_get_mmu_base_addr(&mmu_base_addr);
+	if (ret)
+		return ret;
+
+	/* Init ISP */
+	err = ia_css_init(&isp->css_env.isp_css_env, NULL,
+			  (uint32_t)mmu_base_addr, IA_CSS_IRQ_TYPE_PULSE);
+	if (err != IA_CSS_SUCCESS) {
+		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 (0 == opt)
+		isp->css_env.isp_css_env.print_env.debug_print = NULL;
+	else if (1 == opt)
+		isp->css_env.isp_css_env.print_env.debug_print =
+			atomisp_css2_dbg_ftrace_print;
+	else if (2 == opt)
+		isp->css_env.isp_css_env.print_env.debug_print =
+			atomisp_css2_dbg_print;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+int atomisp_css_check_firmware_version(struct atomisp_device *isp)
+{
+	if (!sh_css_check_firmware_version((void *)isp->firmware->data)) {
+		dev_err(isp->dev, "Fw version check failed.\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int atomisp_css_load_firmware(struct atomisp_device *isp)
+{
+	enum ia_css_err 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_css2_err_print;
+
+	/* load isp fw into ISP memory */
+	err = ia_css_load_firmware(&isp->css_env.isp_css_env,
+				   &isp->css_env.isp_css_fw);
+	if (err != IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "css load fw failed.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void atomisp_css_unload_firmware(struct atomisp_device *isp)
+{
+	ia_css_unload_firmware();
+}
+
+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];
+		atomisp_isp_parameters_clean_up(&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(&mmu_base_addr);
+	if (ret) {
+		dev_err(isp->dev, "get base address error.\n");
+		return -EINVAL;
+	}
+
+	ret = ia_css_init(&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 != IA_CSS_SUCCESS) {
+		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 ia_css_csi2_port port,
+					unsigned int *infos)
+{
+#ifndef ISP2401_NEW_INPUT_SYSTEM
+	ia_css_isys_rx_get_irq_info(port, infos);
+#else
+	*infos = 0;
+#endif
+}
+
+void atomisp_css_rx_clear_irq_info(enum ia_css_csi2_port port,
+					unsigned int infos)
+{
+#ifndef ISP2401_NEW_INPUT_SYSTEM
+	ia_css_isys_rx_clear_irq_info(port, infos);
+#endif
+}
+
+int atomisp_css_irq_enable(struct atomisp_device *isp,
+			    enum atomisp_css_irq_info info, bool enable)
+{
+	if (ia_css_irq_enable(info, enable) != IA_CSS_SUCCESS) {
+		dev_warn(isp->dev, "%s:Invalid irq info.\n", __func__);
+		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 atomisp_css_buffer_type css_buf_type,
+			enum atomisp_css_pipe_id css_pipe_id)
+{
+	struct atomisp_stream_env *stream_env = &asd->stream_env[stream_id];
+	struct ia_css_buffer css_buf = {0};
+	enum ia_css_err 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 != IA_CSS_SUCCESS)
+		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 atomisp_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 atomisp_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 atomisp_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;
+}
+
+void atomisp_css_mmu_invalidate_cache(void)
+{
+	ia_css_mmu_invalidate_cache();
+}
+
+void atomisp_css_mmu_invalidate_tlb(void)
+{
+	ia_css_mmu_invalidate_cache();
+}
+
+void atomisp_css_mmu_set_page_table_base_index(unsigned long base_index)
+{
+}
+
+/*
+ * Check whether currently running MIPI buffer size fulfill
+ * the requirement of the stream to be run
+ */
+bool __need_realloc_mipi_buffer(struct atomisp_device *isp)
+{
+	unsigned int i;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+
+		if (asd->streaming !=
+				ATOMISP_DEVICE_STREAMING_ENABLED)
+			continue;
+		if (asd->mipi_frame_size < isp->mipi_frame_size)
+			return true;
+	}
+
+	return false;
+}
+
+int atomisp_css_start(struct atomisp_sub_device *asd,
+			enum atomisp_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) {
+		if (__destroy_streams(asd, true))
+			dev_warn(isp->dev, "destroy stream failed.\n");
+
+		if (__destroy_pipes(asd, true))
+			dev_warn(isp->dev, "destroy pipe failed.\n");
+
+		if (__create_pipes(asd)) {
+			dev_err(isp->dev, "create pipe error.\n");
+			return -EINVAL;
+		}
+		if (__create_streams(asd)) {
+			dev_err(isp->dev, "create stream error.\n");
+			ret = -EINVAL;
+			goto stream_err;
+		}
+		/* in_reset == true, extension firmwares are reloaded after the recovery */
+		atomisp_acc_load_extensions(asd);
+	}
+
+	/*
+	 * 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 == false) {
+		if (__create_pipes(asd)) {
+			dev_err(isp->dev, "create pipe error.\n");
+			return -EINVAL;
+		}
+		if (__create_streams(asd)) {
+			dev_err(isp->dev, "create stream error.\n");
+			ret = -EINVAL;
+			goto stream_err;
+		}
+	}
+	/*
+	 * 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() != IA_CSS_SUCCESS) {
+			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) != IA_CSS_SUCCESS) {
+				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:
+	__destroy_streams(asd, true);
+stream_err:
+	__destroy_pipes(asd, true);
+
+	/* 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);
+	atomisp_isp_parameters_clean_up(&asd->params.config);
+}
+
+
+void atomisp_css_update_isp_params_on_pipe(struct atomisp_sub_device *asd,
+					struct ia_css_pipe *pipe)
+{
+	enum ia_css_err 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 != IA_CSS_SUCCESS)
+		dev_warn(asd->isp->dev, "%s: ia_css_stream_set_isp_config_on_pipe failed %d\n",
+			__func__, ret);
+	atomisp_isp_parameters_clean_up(&asd->params.config);
+}
+
+int atomisp_css_queue_buffer(struct atomisp_sub_device *asd,
+			     enum atomisp_input_stream_id stream_id,
+			     enum atomisp_css_pipe_id pipe_id,
+			     enum atomisp_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)
+					!= IA_CSS_SUCCESS)
+		return -EINVAL;
+
+	return 0;
+}
+
+int atomisp_css_dequeue_buffer(struct atomisp_sub_device *asd,
+				enum atomisp_input_stream_id stream_id,
+				enum atomisp_css_pipe_id pipe_id,
+				enum atomisp_css_buffer_type buf_type,
+				struct atomisp_css_buffer *isp_css_buffer)
+{
+	struct atomisp_device *isp = asd->isp;
+	enum ia_css_err err;
+
+	err = ia_css_pipe_dequeue_buffer(
+		asd->stream_env[stream_id].pipes[pipe_id],
+					&isp_css_buffer->css_buffer);
+	if (err != IA_CSS_SUCCESS) {
+		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,
+				      uint16_t 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 atomisp_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 atomisp_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 atomisp_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 stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
+	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[stream_index].pipes[pipe_id],
+		&p_info) != IA_CSS_SUCCESS) {
+		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 != 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 atomisp_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] = atomisp_kernel_malloc(
+				asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
+				stream_info.metadata_info.size);
+		if (!asd->params.metadata_user[i]) {
+			while (--i >= 0) {
+				atomisp_kernel_free(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]) {
+			atomisp_kernel_free(asd->params.metadata_user[i]);
+			asd->params.metadata_user[i] = NULL;
+		}
+	}
+}
+
+void atomisp_css_get_dis_statistics(struct atomisp_sub_device *asd,
+				    struct atomisp_css_buffer *isp_css_buffer,
+				    struct ia_css_isp_dvs_statistics_map *dvs_map)
+{
+	if (asd->params.dvs_stat) {
+		if (dvs_map)
+			ia_css_translate_dvs2_statistics(
+				asd->params.dvs_stat, dvs_map);
+		else
+			ia_css_get_dvs2_statistics(asd->params.dvs_stat,
+				isp_css_buffer->css_buffer.data.stats_dvs);
+
+	}
+}
+
+int atomisp_css_dequeue_event(struct atomisp_css_event *current_event)
+{
+	if (ia_css_dequeue_event(&current_event->event) != IA_CSS_SUCCESS)
+		return -EINVAL;
+
+	return 0;
+}
+
+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 atomisp_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_css_stream_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_css_stream_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_css_stream_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 =
+		IA_CSS_STREAM_FORMAT_USER_DEF1;
+	s_config->isys_config[IA_CSS_STREAM_ISYS_STREAM_1].format =
+		IA_CSS_STREAM_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_css_stream_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_css_stream_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 atomisp_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 atomisp_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) != IA_CSS_SUCCESS) {
+			if (intel_mid_identify_cpu() ==
+				INTEL_MID_CPU_CHIP_TANGIER)
+				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,
+		mipi_port_ID_t port,
+		unsigned int num_lanes,
+		unsigned int timeout,
+		unsigned int mipi_freq,
+		enum atomisp_css_stream_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;
+}
+
+int atomisp_css_frame_allocate(struct atomisp_css_frame **frame,
+				unsigned int width, unsigned int height,
+				enum atomisp_css_frame_format format,
+				unsigned int padded_width,
+				unsigned int raw_bit_depth)
+{
+	if (ia_css_frame_allocate(frame, width, height, format,
+			padded_width, raw_bit_depth) != IA_CSS_SUCCESS)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int atomisp_css_frame_allocate_from_info(struct atomisp_css_frame **frame,
+				const struct atomisp_css_frame_info *info)
+{
+	if (ia_css_frame_allocate_from_info(frame, info) != IA_CSS_SUCCESS)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void atomisp_css_frame_free(struct atomisp_css_frame *frame)
+{
+	ia_css_frame_free(frame);
+}
+
+int atomisp_css_frame_map(struct atomisp_css_frame **frame,
+				const struct atomisp_css_frame_info *info,
+				const void *data, uint16_t attribute,
+				void *context)
+{
+	if (ia_css_frame_map(frame, info, data, attribute, context)
+	    != IA_CSS_SUCCESS)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int atomisp_css_set_black_frame(struct atomisp_sub_device *asd,
+				const struct atomisp_css_frame *raw_black_frame)
+{
+	if (sh_css_set_black_frame(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		raw_black_frame) != IA_CSS_SUCCESS)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int atomisp_css_allocate_continuous_frames(bool init_time,
+				struct atomisp_sub_device *asd)
+{
+	if (ia_css_alloc_continuous_frame_remain(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream)
+			!= IA_CSS_SUCCESS)
+		return -EINVAL;
+	return 0;
+}
+
+void atomisp_css_update_continuous_frames(struct atomisp_sub_device *asd)
+{
+	ia_css_update_continuous_frames(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream);
+}
+
+int atomisp_css_stop(struct atomisp_sub_device *asd,
+			enum atomisp_css_pipe_id pipe_id, bool in_reset)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_s3a_buf *s3a_buf;
+	struct atomisp_dis_buf *dis_buf;
+	struct atomisp_metadata_buf *md_buf;
+	unsigned long irqflags;
+	unsigned int i;
+
+	/* if is called in atomisp_reset(), force destroy stream */
+	if (__destroy_streams(asd, true))
+		dev_err(isp->dev, "destroy stream failed.\n");
+
+	/* if is called in atomisp_reset(), force destroy all pipes */
+	if (__destroy_pipes(asd, true))
+		dev_err(isp->dev, "destroy pipes failed.\n");
+
+	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);
+		}
+		atomisp_isp_parameters_clean_up(&asd->params.config);
+		asd->params.css_update_params_needed = false;
+	}
+
+	/* move stats buffers to free queue list */
+	while (!list_empty(&asd->s3a_stats_in_css)) {
+		s3a_buf = list_entry(asd->s3a_stats_in_css.next,
+				struct atomisp_s3a_buf, list);
+		list_del(&s3a_buf->list);
+		list_add_tail(&s3a_buf->list, &asd->s3a_stats);
+	}
+	while (!list_empty(&asd->s3a_stats_ready)) {
+		s3a_buf = list_entry(asd->s3a_stats_ready.next,
+				struct atomisp_s3a_buf, list);
+		list_del(&s3a_buf->list);
+		list_add_tail(&s3a_buf->list, &asd->s3a_stats);
+	}
+
+	spin_lock_irqsave(&asd->dis_stats_lock, irqflags);
+	while (!list_empty(&asd->dis_stats_in_css)) {
+		dis_buf = list_entry(asd->dis_stats_in_css.next,
+				struct atomisp_dis_buf, list);
+		list_del(&dis_buf->list);
+		list_add_tail(&dis_buf->list, &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++) {
+		while (!list_empty(&asd->metadata_in_css[i])) {
+			md_buf = list_entry(asd->metadata_in_css[i].next,
+					struct atomisp_metadata_buf, list);
+			list_del(&md_buf->list);
+			list_add_tail(&md_buf->list, &asd->metadata[i]);
+		}
+		while (!list_empty(&asd->metadata_ready[i])) {
+			md_buf = list_entry(asd->metadata_ready[i].next,
+					struct atomisp_metadata_buf, list);
+			list_del(&md_buf->list);
+			list_add_tail(&md_buf->list, &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));
+	return 0;
+}
+
+int 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;
+	return 0;
+}
+
+void atomisp_css_disable_vf_pp(struct atomisp_sub_device *asd,
+			       bool disable)
+{
+	int i;
+
+	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.pipe_extra_configs[i].disable_vf_pp = !!disable;
+}
+
+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;
+#define CEIL_DIV(a, b)       ((b) ? ((a) + (b) - 1) / (b) : 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;
+
+	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};
+	 */
+	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: arbitary 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;
+
+	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 atomisp_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 atomisp_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 atomisp_css_frame_info *info,
+				enum frame_info_type type,
+				enum ia_css_pipe_id pipe_id)
+{
+	struct atomisp_device *isp = asd->isp;
+	enum ia_css_err ret;
+	struct ia_css_pipe_info p_info;
+
+	/* FIXME! No need to destroy/recreate all streams */
+	if (__destroy_streams(asd, true))
+		dev_warn(isp->dev, "destroy stream failed.\n");
+
+	if (__destroy_pipes(asd, true))
+		dev_warn(isp->dev, "destroy pipe failed.\n");
+
+	if (__create_pipes(asd))
+		return -EINVAL;
+
+	if (__create_streams(asd))
+		goto stream_err;
+
+	ret = ia_css_pipe_get_info(
+		asd->stream_env[stream_index]
+		.pipes[pipe_id], &p_info);
+	if (ret == IA_CSS_SUCCESS) {
+		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;
+		case ATOMISP_CSS_RAW_FRAME:
+			*info = p_info.raw_output_info;
+			dev_dbg(isp->dev, "getting raw frame info.\n");
+		}
+		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;
+	}
+
+stream_err:
+	__destroy_pipes(asd, true);
+	return -EINVAL;
+}
+
+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;
+		else
+			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;
+	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;
+		else
+			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,
+				uint16_t source_pad,
+				struct atomisp_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_source_pad_to_stream_id(asd, source_pad);
+	else {
+		stream_index = (pipe_index == IA_CSS_PIPE_ID_YUVPP) ?
+			   ATOMISP_INPUT_STREAM_VIDEO :
+			   atomisp_source_pad_to_stream_id(asd, source_pad);
+	}
+
+	if (IA_CSS_SUCCESS != 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 atomisp_css_frame_format format)
+{
+	asd->stream_env[stream_index].pipe_configs[IA_CSS_PIPE_ID_COPY].
+					default_capture_config.mode =
+					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 atomisp_css_frame_format format)
+{
+	asd->stream_env[stream_index].pipe_configs[IA_CSS_PIPE_ID_YUVPP].
+					default_capture_config.mode =
+					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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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)
+{
+	enum ia_css_err ret;
+
+#ifdef ISP2401
+	dev_dbg(asd->isp->dev, "%s num_capture:%d skip:%d offset:%d\n",
+			__func__, num_captures, skip, offset);
+#endif
+	ret = ia_css_stream_capture(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		num_captures, skip, offset);
+	if (ret != IA_CSS_SUCCESS)
+		return -EINVAL;
+
+	return 0;
+}
+
+int atomisp_css_exp_id_capture(struct atomisp_sub_device *asd, int exp_id)
+{
+	enum ia_css_err ret;
+
+	ret = ia_css_stream_capture_frame(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		exp_id);
+	if (ret == IA_CSS_ERR_QUEUE_IS_FULL) {
+		/* capture cmd queue is full */
+		return -EBUSY;
+	} else if (ret != IA_CSS_SUCCESS) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int atomisp_css_exp_id_unlock(struct atomisp_sub_device *asd, int exp_id)
+{
+	enum ia_css_err ret;
+
+	ret = ia_css_unlock_raw_frame(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		exp_id);
+	if (ret == IA_CSS_ERR_QUEUE_IS_FULL)
+		return -EAGAIN;
+	else if (ret != IA_CSS_SUCCESS)
+		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_send_input_frame(struct atomisp_sub_device *asd,
+				  unsigned short *data, unsigned int width,
+				  unsigned int height)
+{
+	ia_css_stream_send_input_frame(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
+		data, width, height);
+}
+
+bool atomisp_css_isp_has_started(void)
+{
+	return ia_css_isp_has_started();
+}
+
+void atomisp_css_request_flash(struct atomisp_sub_device *asd)
+{
+	ia_css_stream_request_flash(
+		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream);
+}
+
+void atomisp_css_set_wb_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_wb_config *wb_config)
+{
+	asd->params.config.wb_config = wb_config;
+}
+
+void atomisp_css_set_ob_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ob_config *ob_config)
+{
+	asd->params.config.ob_config = ob_config;
+}
+
+void atomisp_css_set_dp_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_dp_config *dp_config)
+{
+	asd->params.config.dp_config = dp_config;
+}
+
+void atomisp_css_set_de_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_de_config *de_config)
+{
+	asd->params.config.de_config = de_config;
+}
+
+void atomisp_css_set_dz_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_dz_config *dz_config)
+{
+	asd->params.config.dz_config = dz_config;
+}
+
+void atomisp_css_set_default_de_config(struct atomisp_sub_device *asd)
+{
+	asd->params.config.de_config = NULL;
+}
+
+void atomisp_css_set_ce_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ce_config *ce_config)
+{
+	asd->params.config.ce_config = ce_config;
+}
+
+void atomisp_css_set_nr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_nr_config *nr_config)
+{
+	asd->params.config.nr_config = nr_config;
+}
+
+void atomisp_css_set_ee_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ee_config *ee_config)
+{
+	asd->params.config.ee_config = ee_config;
+}
+
+void atomisp_css_set_tnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_tnr_config *tnr_config)
+{
+	asd->params.config.tnr_config = tnr_config;
+}
+
+void atomisp_css_set_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *cc_config)
+{
+	asd->params.config.cc_config = cc_config;
+}
+
+void atomisp_css_set_macc_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_macc_table *macc_table)
+{
+	asd->params.config.macc_table = macc_table;
+}
+
+void atomisp_css_set_macc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_macc_config *macc_config)
+{
+	asd->params.config.macc_config = macc_config;
+}
+
+void atomisp_css_set_ecd_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ecd_config *ecd_config)
+{
+	asd->params.config.ecd_config = ecd_config;
+}
+
+void atomisp_css_set_ynr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ynr_config *ynr_config)
+{
+	asd->params.config.ynr_config = ynr_config;
+}
+
+void atomisp_css_set_fc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_fc_config *fc_config)
+{
+	asd->params.config.fc_config = fc_config;
+}
+
+void atomisp_css_set_ctc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ctc_config *ctc_config)
+{
+	asd->params.config.ctc_config = ctc_config;
+}
+
+void atomisp_css_set_cnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cnr_config *cnr_config)
+{
+	asd->params.config.cnr_config = cnr_config;
+}
+
+void atomisp_css_set_aa_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_aa_config *aa_config)
+{
+	asd->params.config.aa_config = aa_config;
+}
+
+void atomisp_css_set_baa_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_baa_config *baa_config)
+{
+	asd->params.config.baa_config = baa_config;
+}
+
+void atomisp_css_set_anr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_anr_config *anr_config)
+{
+	asd->params.config.anr_config = anr_config;
+}
+
+void atomisp_css_set_xnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_xnr_config *xnr_config)
+{
+	asd->params.config.xnr_config = xnr_config;
+}
+
+void atomisp_css_set_yuv2rgb_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *yuv2rgb_cc_config)
+{
+	asd->params.config.yuv2rgb_cc_config = yuv2rgb_cc_config;
+}
+
+void atomisp_css_set_rgb2yuv_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *rgb2yuv_cc_config)
+{
+	asd->params.config.rgb2yuv_cc_config = rgb2yuv_cc_config;
+}
+
+void atomisp_css_set_xnr_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_xnr_table *xnr_table)
+{
+	asd->params.config.xnr_table = xnr_table;
+}
+
+void atomisp_css_set_r_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *r_gamma_table)
+{
+	asd->params.config.r_gamma_table = r_gamma_table;
+}
+
+void atomisp_css_set_g_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *g_gamma_table)
+{
+	asd->params.config.g_gamma_table = g_gamma_table;
+}
+
+void atomisp_css_set_b_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *b_gamma_table)
+{
+	asd->params.config.b_gamma_table = b_gamma_table;
+}
+
+void atomisp_css_set_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_gamma_table *gamma_table)
+{
+	asd->params.config.gamma_table = gamma_table;
+}
+
+void atomisp_css_set_ctc_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_ctc_table *ctc_table)
+{
+	int i;
+	uint16_t *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 atomisp_css_anr_thres *anr_thres)
+{
+	asd->params.config.anr_thres = anr_thres;
+}
+
+void atomisp_css_set_dvs_6axis(struct atomisp_sub_device *asd,
+			struct atomisp_css_dvs_6axis *dvs_6axis)
+{
+	asd->params.config.dvs_6axis_config = dvs_6axis;
+}
+
+void atomisp_css_set_gc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_gc_config *gc_config)
+{
+	asd->params.config.gc_config = gc_config;
+}
+
+void atomisp_css_set_3a_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_3a_config *s3a_config)
+{
+	asd->params.config.s3a_config = s3a_config;
+}
+
+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 atomisp_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 == NULL ||
+	    coefs->hor_coefs.odd_imag == NULL ||
+	    coefs->hor_coefs.even_real == NULL ||
+	    coefs->hor_coefs.even_imag == NULL ||
+	    coefs->ver_coefs.odd_real == NULL ||
+	    coefs->ver_coefs.odd_imag == NULL ||
+	    coefs->ver_coefs.even_real == NULL ||
+	    coefs->ver_coefs.even_imag == NULL ||
+	    asd->params.css_param.dvs2_coeff->hor_coefs.odd_real == NULL ||
+	    asd->params.css_param.dvs2_coeff->hor_coefs.odd_imag == NULL ||
+	    asd->params.css_param.dvs2_coeff->hor_coefs.even_real == NULL ||
+	    asd->params.css_param.dvs2_coeff->hor_coefs.even_imag == NULL ||
+	    asd->params.css_param.dvs2_coeff->ver_coefs.odd_real == NULL ||
+	    asd->params.css_param.dvs2_coeff->ver_coefs.odd_imag == NULL ||
+	    asd->params.css_param.dvs2_coeff->ver_coefs.even_real == NULL ||
+	    asd->params.css_param.dvs2_coeff->ver_coefs.even_imag == NULL)
+		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_dvs2_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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;
+
+	if (asd->params.dvs_stat->hor_prod.odd_real == NULL ||
+	    asd->params.dvs_stat->hor_prod.odd_imag == NULL ||
+	    asd->params.dvs_stat->hor_prod.even_real == NULL ||
+	    asd->params.dvs_stat->hor_prod.even_imag == NULL ||
+	    asd->params.dvs_stat->ver_prod.odd_real == NULL ||
+	    asd->params.dvs_stat->ver_prod.odd_imag == NULL ||
+	    asd->params.dvs_stat->ver_prod.even_real == NULL ||
+	    asd->params.dvs_stat->ver_prod.even_imag == NULL)
+		return -EINVAL;
+
+	/* isp needs to be streaming to get DIS statistics */
+	spin_lock_irqsave(&isp->lock, flags);
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED) {
+		spin_unlock_irqrestore(&isp->lock, flags);
+		return -EINVAL;
+	}
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	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 atomisp_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 atomisp_css_shading_table *table)
+{
+	asd->params.config.shading_table = table;
+}
+
+void atomisp_css_shading_table_free(struct atomisp_css_shading_table *table)
+{
+	ia_css_shading_table_free(table);
+}
+
+struct atomisp_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 atomisp_css_morph_table *table)
+{
+	asd->params.config.morph_table = table;
+}
+
+void atomisp_css_get_morph_table(struct atomisp_sub_device *asd,
+				struct atomisp_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 atomisp_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 atomisp_css_morph_table *table)
+{
+	ia_css_morph_table_free(table);
+}
+
+void atomisp_css_set_cont_prev_start_time(struct atomisp_device *isp,
+					unsigned int overlap)
+{
+	/* CSS 2.0 doesn't support this API. */
+	dev_dbg(isp->dev, "set cont prev start time is not supported.\n");
+	return;
+}
+
+void atomisp_css_acc_done(struct atomisp_sub_device *asd)
+{
+	complete(&asd->acc.acc_done);
+}
+
+int atomisp_css_wait_acc_finish(struct atomisp_sub_device *asd)
+{
+	int ret = 0;
+	struct atomisp_device *isp = asd->isp;
+
+	/* Unlock the isp mutex taken in IOCTL handler before sleeping! */
+	rt_mutex_unlock(&isp->mutex);
+	if (wait_for_completion_interruptible_timeout(&asd->acc.acc_done,
+					ATOMISP_ISP_TIMEOUT_DURATION) == 0) {
+		dev_err(isp->dev, "<%s: completion timeout\n", __func__);
+		atomisp_css_debug_dump_sp_sw_debug_info();
+		atomisp_css_debug_dump_debug_info(__func__);
+		ret = -EIO;
+	}
+	rt_mutex_lock(&isp->mutex);
+
+	return ret;
+}
+
+/* Set the ACC binary arguments */
+int atomisp_css_set_acc_parameters(struct atomisp_acc_fw *acc_fw)
+{
+	unsigned int mem;
+
+	for (mem = 0; mem < ATOMISP_ACC_NR_MEMORY; mem++) {
+		if (acc_fw->args[mem].length == 0)
+			continue;
+
+		ia_css_isp_param_set_css_mem_init(&acc_fw->fw->mem_initializers,
+						IA_CSS_PARAM_CLASS_PARAM, mem,
+						acc_fw->args[mem].css_ptr,
+						acc_fw->args[mem].length);
+	}
+
+	return 0;
+}
+
+/* Load acc binary extension */
+int atomisp_css_load_acc_extension(struct atomisp_sub_device *asd,
+				   struct atomisp_css_fw_info *fw,
+				   enum atomisp_css_pipe_id pipe_id,
+				   unsigned int type)
+{
+	struct atomisp_css_fw_info **hd;
+
+	fw->next = NULL;
+	hd = &(asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+			.pipe_configs[pipe_id].acc_extension);
+	while (*hd)
+		hd = &(*hd)->next;
+	*hd = fw;
+
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.update_pipe[pipe_id] = true;
+	return 0;
+}
+
+/* Unload acc binary extension */
+void atomisp_css_unload_acc_extension(struct atomisp_sub_device *asd,
+					struct atomisp_css_fw_info *fw,
+					enum atomisp_css_pipe_id pipe_id)
+{
+	struct atomisp_css_fw_info **hd;
+
+	hd = &(asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+			.pipe_configs[pipe_id].acc_extension);
+	while (*hd && *hd != fw)
+		hd = &(*hd)->next;
+	if (!*hd) {
+		dev_err(asd->isp->dev, "did not find acc fw for removal\n");
+		return;
+	}
+	*hd = fw->next;
+	fw->next = NULL;
+
+	asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+		.update_pipe[pipe_id] = true;
+}
+
+int atomisp_css_create_acc_pipe(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct ia_css_pipe_config *pipe_config;
+	struct atomisp_stream_env *stream_env =
+		&asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+
+	if (stream_env->acc_stream) {
+		if (stream_env->acc_stream_state == CSS_STREAM_STARTED) {
+			if (ia_css_stream_stop(stream_env->acc_stream)
+				!= IA_CSS_SUCCESS) {
+				dev_err(isp->dev, "stop acc_stream failed.\n");
+				return -EBUSY;
+			}
+		}
+
+		if (ia_css_stream_destroy(stream_env->acc_stream)
+			!= IA_CSS_SUCCESS) {
+			dev_err(isp->dev, "destroy acc_stream failed.\n");
+			return -EBUSY;
+		}
+		stream_env->acc_stream = NULL;
+	}
+
+	pipe_config = &stream_env->pipe_configs[CSS_PIPE_ID_ACC];
+	ia_css_pipe_config_defaults(pipe_config);
+	asd->acc.acc_stages = kzalloc(MAX_ACC_STAGES *
+				sizeof(void *), GFP_KERNEL);
+	if (!asd->acc.acc_stages)
+		return -ENOMEM;
+	pipe_config->acc_stages = asd->acc.acc_stages;
+	pipe_config->mode = IA_CSS_PIPE_MODE_ACC;
+	pipe_config->num_acc_stages = 0;
+
+	/*
+	 * We delay the ACC pipeline creation to atomisp_css_start_acc_pipe,
+	 * because pipe configuration will soon be changed by
+	 * atomisp_css_load_acc_binary()
+	 */
+	return 0;
+}
+
+int atomisp_css_start_acc_pipe(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+	struct atomisp_stream_env *stream_env =
+		&asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	struct ia_css_pipe_config *pipe_config =
+			&stream_env->pipe_configs[IA_CSS_PIPE_ID_ACC];
+
+	if (ia_css_pipe_create(pipe_config,
+		&stream_env->pipes[IA_CSS_PIPE_ID_ACC]) != IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "%s: ia_css_pipe_create failed\n",
+				__func__);
+		return -EBADE;
+	}
+
+	memset(&stream_env->acc_stream_config, 0,
+		sizeof(struct ia_css_stream_config));
+	if (ia_css_stream_create(&stream_env->acc_stream_config, 1,
+				&stream_env->pipes[IA_CSS_PIPE_ID_ACC],
+				&stream_env->acc_stream) != IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "%s: create acc_stream error.\n", __func__);
+		return -EINVAL;
+	}
+	stream_env->acc_stream_state = CSS_STREAM_CREATED;
+
+	init_completion(&asd->acc.acc_done);
+	asd->acc.pipeline = stream_env->pipes[IA_CSS_PIPE_ID_ACC];
+
+	atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_MAX, false);
+
+	if (ia_css_start_sp() != IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "start sp error.\n");
+		return -EIO;
+	}
+
+	if (ia_css_stream_start(stream_env->acc_stream)
+		!= IA_CSS_SUCCESS) {
+		dev_err(isp->dev, "acc_stream start error.\n");
+		return -EIO;
+	}
+
+	stream_env->acc_stream_state = CSS_STREAM_STARTED;
+	return 0;
+}
+
+int atomisp_css_stop_acc_pipe(struct atomisp_sub_device *asd)
+{
+	struct atomisp_stream_env *stream_env =
+		&asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	if (stream_env->acc_stream_state == CSS_STREAM_STARTED) {
+		ia_css_stream_stop(stream_env->acc_stream);
+		stream_env->acc_stream_state = CSS_STREAM_STOPPED;
+	}
+	return 0;
+}
+
+void atomisp_css_destroy_acc_pipe(struct atomisp_sub_device *asd)
+{
+	struct atomisp_stream_env *stream_env =
+		&asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
+	if (stream_env->acc_stream) {
+		if (ia_css_stream_destroy(stream_env->acc_stream)
+		    != IA_CSS_SUCCESS)
+			dev_warn(asd->isp->dev,
+				"destroy acc_stream failed.\n");
+		stream_env->acc_stream = NULL;
+	}
+
+	if (stream_env->pipes[IA_CSS_PIPE_ID_ACC]) {
+		if (ia_css_pipe_destroy(stream_env->pipes[IA_CSS_PIPE_ID_ACC])
+			!= IA_CSS_SUCCESS)
+			dev_warn(asd->isp->dev,
+				"destroy ACC pipe failed.\n");
+		stream_env->pipes[IA_CSS_PIPE_ID_ACC] = NULL;
+		stream_env->update_pipe[IA_CSS_PIPE_ID_ACC] = false;
+		ia_css_pipe_config_defaults(
+			&stream_env->pipe_configs[IA_CSS_PIPE_ID_ACC]);
+		ia_css_pipe_extra_config_defaults(
+			&stream_env->pipe_extra_configs[IA_CSS_PIPE_ID_ACC]);
+	}
+	asd->acc.pipeline = NULL;
+
+	/* css 2.0 API limitation: ia_css_stop_sp() could be only called after
+	 * destroy all pipes
+	 */
+	ia_css_stop_sp();
+
+	kfree(asd->acc.acc_stages);
+	asd->acc.acc_stages = NULL;
+
+	atomisp_freq_scaling(asd->isp, ATOMISP_DFS_MODE_LOW, false);
+}
+
+int atomisp_css_load_acc_binary(struct atomisp_sub_device *asd,
+					struct atomisp_css_fw_info *fw,
+					unsigned int index)
+{
+	struct ia_css_pipe_config *pipe_config =
+			&asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
+			.pipe_configs[IA_CSS_PIPE_ID_ACC];
+
+	if (index >= MAX_ACC_STAGES) {
+		dev_dbg(asd->isp->dev, "%s: index(%d) out of range\n",
+				__func__, index);
+		return -ENOMEM;
+	}
+
+	pipe_config->acc_stages[index] = fw;
+	pipe_config->num_acc_stages = index + 1;
+	pipe_config->acc_num_execs = 1;
+
+	return 0;
+}
+
+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 &&
+		    !asd->acc.pipeline)
+			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 = &isp->asd[0];
+#ifndef ISP2401
+	bool reset_wdt_timer[MAX_STREAM_NUM] = {false};
+#endif
+	int i;
+
+	while (!atomisp_css_dequeue_event(&current_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);
+			for (i = 0; i < isp->num_of_streams; i++)
+				atomisp_wdt_stop(&isp->asd[i], 0);
+#ifndef ISP2401
+			atomisp_wdt((unsigned long)isp);
+#else
+			queue_work(isp->wdt_work_queue, &isp->wdt_work);
+#endif
+			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 == CSS_EVENT_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 CSS_EVENT_OUTPUT_FRAME_DONE:
+			frame_done_found[asd->index] = true;
+			atomisp_buf_done(asd, 0, CSS_BUFFER_TYPE_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+#ifndef ISP2401
+			reset_wdt_timer[asd->index] = true; /* ISP running */
+#endif
+			break;
+		case CSS_EVENT_SEC_OUTPUT_FRAME_DONE:
+			frame_done_found[asd->index] = true;
+			atomisp_buf_done(asd, 0, CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+#ifndef ISP2401
+			reset_wdt_timer[asd->index] = true; /* ISP running */
+#endif
+			break;
+		case CSS_EVENT_3A_STATISTICS_DONE:
+			atomisp_buf_done(asd, 0,
+					 CSS_BUFFER_TYPE_3A_STATISTICS,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case CSS_EVENT_METADATA_DONE:
+			atomisp_buf_done(asd, 0,
+					 CSS_BUFFER_TYPE_METADATA,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case CSS_EVENT_VF_OUTPUT_FRAME_DONE:
+			atomisp_buf_done(asd, 0,
+					 CSS_BUFFER_TYPE_VF_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+#ifndef ISP2401
+			reset_wdt_timer[asd->index] = true; /* ISP running */
+#endif
+			break;
+		case CSS_EVENT_SEC_VF_OUTPUT_FRAME_DONE:
+			atomisp_buf_done(asd, 0,
+					 CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME,
+					 current_event.pipe, true, stream_id);
+#ifndef ISP2401
+			reset_wdt_timer[asd->index] = true; /* ISP running */
+#endif
+			break;
+		case CSS_EVENT_DIS_STATISTICS_DONE:
+			atomisp_buf_done(asd, 0,
+					 CSS_BUFFER_TYPE_DIS_STATISTICS,
+					 current_event.pipe,
+					 false, stream_id);
+			break;
+		case CSS_EVENT_PIPELINE_DONE:
+			css_pipe_done[asd->index] = true;
+			break;
+		case CSS_EVENT_ACC_STAGE_COMPLETE:
+			atomisp_acc_done(asd, current_event.event.fw_handle);
+			break;
+		default:
+			dev_dbg(isp->dev, "unhandled css stored event: 0x%x\n",
+					current_event.event.type);
+			break;
+		}
+	}
+#ifndef ISP2401
+	/* If there are no buffers queued then
+	 * delete wdt timer. */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		asd = &isp->asd[i];
+		if (!asd)
+			continue;
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+			continue;
+		if (!atomisp_buffers_queued(asd))
+			atomisp_wdt_stop(asd, false);
+		else if (reset_wdt_timer[i])
+		/* SOF irq should not reset wdt timer. */
+			atomisp_wdt_refresh(asd,
+					ATOMISP_WDT_KEEP_CURRENT_DELAY);
+	}
+#endif
+
+	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 &&
+				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;
+}
+
+int atomisp_css_dump_blob_infor(void)
+{
+	struct ia_css_blob_descr *bd = sh_css_blob_info;
+	unsigned i, nm = sh_css_num_binaries;
+
+	if (nm == 0)
+		return -EPERM;
+	if (bd == NULL)
+		return -EPERM;
+
+	for (i = 1; i < sh_css_num_binaries; i++)
+		dev_dbg(atomisp_dev, "Num%d binary id is %d, name is %s\n", i,
+			bd[i-1].header.info.isp.sp.id, bd[i-1].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 atomisp_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 (0 == ret)
+		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 atomisp_css_dvs_grid_info *atomisp_css_get_dvs_grid_info(
+	struct atomisp_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/atomisp2/atomisp_compat_css20.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.h
new file mode 100644
index 000000000000..b62ad9082018
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_css20.h
@@ -0,0 +1,282 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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 atomisp_css_pipe_id ia_css_pipe_id
+#define atomisp_css_pipeline	ia_css_pipe
+#define atomisp_css_buffer_type ia_css_buffer_type
+#define atomisp_css_dis_data ia_css_isp_dvs_statistics
+#define atomisp_css_irq_info  ia_css_irq_info
+#define atomisp_css_isp_config ia_css_isp_config
+#define atomisp_css_bayer_order ia_css_bayer_order
+#define atomisp_css_stream_format ia_css_stream_format
+#define atomisp_css_capture_mode ia_css_capture_mode
+#define atomisp_css_input_mode ia_css_input_mode
+#define atomisp_css_frame ia_css_frame
+#define atomisp_css_frame_format ia_css_frame_format
+#define atomisp_css_frame_info ia_css_frame_info
+#define atomisp_css_dp_config	ia_css_dp_config
+#define atomisp_css_wb_config	ia_css_wb_config
+#define atomisp_css_cc_config	ia_css_cc_config
+#define atomisp_css_nr_config	ia_css_nr_config
+#define atomisp_css_ee_config	ia_css_ee_config
+#define atomisp_css_ob_config	ia_css_ob_config
+#define atomisp_css_de_config	ia_css_de_config
+#define atomisp_css_dz_config	ia_css_dz_config
+#define atomisp_css_ce_config	ia_css_ce_config
+#define atomisp_css_gc_config	ia_css_gc_config
+#define atomisp_css_tnr_config	ia_css_tnr_config
+#define atomisp_css_cnr_config	ia_css_cnr_config
+#define atomisp_css_ctc_config	ia_css_ctc_config
+#define atomisp_css_3a_config	ia_css_3a_config
+#define atomisp_css_ecd_config	ia_css_ecd_config
+#define atomisp_css_ynr_config	ia_css_ynr_config
+#define atomisp_css_fc_config	ia_css_fc_config
+#define atomisp_css_aa_config	ia_css_aa_config
+#define atomisp_css_baa_config	ia_css_aa_config
+#define atomisp_css_anr_config	ia_css_anr_config
+#define atomisp_css_xnr_config	ia_css_xnr_config
+#define atomisp_css_macc_config	ia_css_macc_config
+#define atomisp_css_gamma_table	ia_css_gamma_table
+#define atomisp_css_ctc_table	ia_css_ctc_table
+#define atomisp_css_macc_table	ia_css_macc_table
+#define atomisp_css_xnr_table	ia_css_xnr_table
+#define atomisp_css_rgb_gamma_table	ia_css_rgb_gamma_table
+#define atomisp_css_anr_thres	ia_css_anr_thres
+#define atomisp_css_dvs_6axis	ia_css_dvs_6axis_config
+#define atomisp_css_grid_info	ia_css_grid_info
+#define atomisp_css_3a_grid_info	ia_css_3a_grid_info
+#define atomisp_css_dvs_grid_info	ia_css_dvs_grid_info
+#define atomisp_css_shading_table	ia_css_shading_table
+#define atomisp_css_morph_table	ia_css_morph_table
+#define atomisp_css_dvs_6axis_config	ia_css_dvs_6axis_config
+#define atomisp_css_fw_info	ia_css_fw_info
+#define atomisp_css_formats_config	ia_css_formats_config
+
+#define CSS_PIPE_ID_PREVIEW	IA_CSS_PIPE_ID_PREVIEW
+#define CSS_PIPE_ID_COPY	IA_CSS_PIPE_ID_COPY
+#define CSS_PIPE_ID_VIDEO	IA_CSS_PIPE_ID_VIDEO
+#define CSS_PIPE_ID_CAPTURE	IA_CSS_PIPE_ID_CAPTURE
+#define CSS_PIPE_ID_ACC		IA_CSS_PIPE_ID_ACC
+#define CSS_PIPE_ID_YUVPP	IA_CSS_PIPE_ID_YUVPP
+#define CSS_PIPE_ID_NUM		IA_CSS_PIPE_ID_NUM
+
+#define CSS_INPUT_MODE_SENSOR	IA_CSS_INPUT_MODE_BUFFERED_SENSOR
+#define CSS_INPUT_MODE_FIFO	IA_CSS_INPUT_MODE_FIFO
+#define CSS_INPUT_MODE_TPG	IA_CSS_INPUT_MODE_TPG
+#define CSS_INPUT_MODE_PRBS	IA_CSS_INPUT_MODE_PRBS
+#define CSS_INPUT_MODE_MEMORY	IA_CSS_INPUT_MODE_MEMORY
+
+#define CSS_IRQ_INFO_CSS_RECEIVER_ERROR	IA_CSS_IRQ_INFO_CSS_RECEIVER_ERROR
+#define CSS_IRQ_INFO_EVENTS_READY	IA_CSS_IRQ_INFO_EVENTS_READY
+#define CSS_IRQ_INFO_INPUT_SYSTEM_ERROR \
+	IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR
+#define CSS_IRQ_INFO_IF_ERROR	IA_CSS_IRQ_INFO_IF_ERROR
+
+#define CSS_BUFFER_TYPE_NUM	IA_CSS_BUFFER_TYPE_NUM
+
+#define CSS_FRAME_FLASH_STATE_NONE	IA_CSS_FRAME_FLASH_STATE_NONE
+#define CSS_FRAME_FLASH_STATE_PARTIAL	IA_CSS_FRAME_FLASH_STATE_PARTIAL
+#define CSS_FRAME_FLASH_STATE_FULL	IA_CSS_FRAME_FLASH_STATE_FULL
+
+#define CSS_BAYER_ORDER_GRBG	IA_CSS_BAYER_ORDER_GRBG
+#define CSS_BAYER_ORDER_RGGB	IA_CSS_BAYER_ORDER_RGGB
+#define CSS_BAYER_ORDER_BGGR	IA_CSS_BAYER_ORDER_BGGR
+#define CSS_BAYER_ORDER_GBRG	IA_CSS_BAYER_ORDER_GBRG
+
+/*
+ * Hide IA_ naming difference in otherwise common CSS macros.
+ */
+#define CSS_ID(val)	(IA_ ## val)
+#define CSS_EVENT(val)	(IA_CSS_EVENT_TYPE_ ## val)
+#define CSS_FORMAT(val)	(IA_CSS_STREAM_FORMAT_ ## val)
+
+#define CSS_EVENT_PORT_EOF	CSS_EVENT(PORT_EOF)
+#define CSS_EVENT_FRAME_TAGGED	CSS_EVENT(FRAME_TAGGED)
+
+#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 atomisp_css_dis_data *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 atomisp_css_pipe_id pipe;
+	struct ia_css_event event;
+};
+
+void atomisp_css_set_macc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_macc_config *macc_config);
+
+void atomisp_css_set_ecd_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ecd_config *ecd_config);
+
+void atomisp_css_set_ynr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ynr_config *ynr_config);
+
+void atomisp_css_set_fc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_fc_config *fc_config);
+
+void atomisp_css_set_aa_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_aa_config *aa_config);
+
+void atomisp_css_set_baa_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_baa_config *baa_config);
+
+void atomisp_css_set_anr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_anr_config *anr_config);
+
+void atomisp_css_set_xnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_xnr_config *xnr_config);
+
+void atomisp_css_set_cnr_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cnr_config *cnr_config);
+
+void atomisp_css_set_ctc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_ctc_config *ctc_config);
+
+void atomisp_css_set_yuv2rgb_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *yuv2rgb_cc_config);
+
+void atomisp_css_set_rgb2yuv_cc_config(struct atomisp_sub_device *asd,
+			struct atomisp_css_cc_config *rgb2yuv_cc_config);
+
+void atomisp_css_set_xnr_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_xnr_table *xnr_table);
+
+void atomisp_css_set_r_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *r_gamma_table);
+
+void atomisp_css_set_g_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *g_gamma_table);
+
+void atomisp_css_set_b_gamma_table(struct atomisp_sub_device *asd,
+			struct atomisp_css_rgb_gamma_table *b_gamma_table);
+
+void atomisp_css_set_anr_thres(struct atomisp_sub_device *asd,
+			struct atomisp_css_anr_thres *anr_thres);
+
+int atomisp_css_check_firmware_version(struct atomisp_device *isp);
+
+int atomisp_css_load_firmware(struct atomisp_device *isp);
+
+void atomisp_css_unload_firmware(struct atomisp_device *isp);
+
+void atomisp_css_set_dvs_6axis(struct atomisp_sub_device *asd,
+			struct atomisp_css_dvs_6axis *dvs_6axis);
+
+unsigned int atomisp_css_debug_get_dtrace_level(void);
+
+int atomisp_css_debug_dump_isp_binary(void);
+
+int atomisp_css_dump_sp_raw_copy_linecount(bool reduced);
+
+int atomisp_css_dump_blob_infor(void);
+
+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 atomisp_css_frame *output_frame);
+
+int atomisp_get_css_dbgfunc(void);
+
+int atomisp_set_css_dbgfunc(struct atomisp_device *isp, int opt);
+struct atomisp_css_dvs_grid_info *atomisp_css_get_dvs_grid_info(
+	struct atomisp_css_grid_info *grid_info);
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.c
new file mode 100644
index 000000000000..0592ac1f2832
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.c
@@ -0,0 +1,1263 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifdef CONFIG_COMPAT
+#include <linux/compat.h>
+
+#include <linux/videodev2.h>
+
+#include "atomisp_internal.h"
+#include "atomisp_compat.h"
+#include "atomisp_compat_ioctl32.h"
+
+static int get_atomisp_histogram32(struct atomisp_histogram *kp,
+					struct atomisp_histogram32 __user *up)
+{
+	compat_uptr_t tmp;
+
+	if (!access_ok(VERIFY_READ, up, sizeof(struct atomisp_histogram32)) ||
+		get_user(kp->num_elements, &up->num_elements) ||
+		get_user(tmp, &up->data))
+			return -EFAULT;
+
+	kp->data = compat_ptr(tmp);
+	return 0;
+}
+
+static int put_atomisp_histogram32(struct atomisp_histogram *kp,
+					struct atomisp_histogram32 __user *up)
+{
+	compat_uptr_t tmp = (compat_uptr_t)((uintptr_t)kp->data);
+
+	if (!access_ok(VERIFY_WRITE, up, sizeof(struct atomisp_histogram32)) ||
+		put_user(kp->num_elements, &up->num_elements) ||
+		put_user(tmp, &up->data))
+			return -EFAULT;
+
+	return 0;
+}
+
+static inline int get_v4l2_pix_format(struct v4l2_pix_format *kp,
+					struct v4l2_pix_format __user *up)
+{
+	if (copy_from_user(kp, up, sizeof(struct v4l2_pix_format)))
+		return -EFAULT;
+	return 0;
+}
+
+static inline int put_v4l2_pix_format(struct v4l2_pix_format *kp,
+					struct v4l2_pix_format __user *up)
+{
+	if (copy_to_user(up, kp, sizeof(struct v4l2_pix_format)))
+		return -EFAULT;
+	return 0;
+}
+
+static int get_v4l2_framebuffer32(struct v4l2_framebuffer *kp,
+					struct v4l2_framebuffer32 __user *up)
+{
+	compat_uptr_t tmp;
+
+	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_framebuffer32)) ||
+		get_user(tmp, &up->base) ||
+		get_user(kp->capability, &up->capability) ||
+		get_user(kp->flags, &up->flags))
+			return -EFAULT;
+
+	kp->base = compat_ptr(tmp);
+	get_v4l2_pix_format((struct v4l2_pix_format *)&kp->fmt, &up->fmt);
+	return 0;
+}
+
+static int get_atomisp_dis_statistics32(struct atomisp_dis_statistics *kp,
+				struct atomisp_dis_statistics32 __user *up)
+{
+	compat_uptr_t hor_prod_odd_real;
+	compat_uptr_t hor_prod_odd_imag;
+	compat_uptr_t hor_prod_even_real;
+	compat_uptr_t hor_prod_even_imag;
+	compat_uptr_t ver_prod_odd_real;
+	compat_uptr_t ver_prod_odd_imag;
+	compat_uptr_t ver_prod_even_real;
+	compat_uptr_t ver_prod_even_imag;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_dis_statistics32)) ||
+		copy_from_user(kp, up, sizeof(struct atomisp_dvs_grid_info)) ||
+		get_user(hor_prod_odd_real,
+				&up->dvs2_stat.hor_prod.odd_real) ||
+		get_user(hor_prod_odd_imag,
+				&up->dvs2_stat.hor_prod.odd_imag) ||
+		get_user(hor_prod_even_real,
+				&up->dvs2_stat.hor_prod.even_real) ||
+		get_user(hor_prod_even_imag,
+				&up->dvs2_stat.hor_prod.even_imag) ||
+		get_user(ver_prod_odd_real,
+				&up->dvs2_stat.ver_prod.odd_real) ||
+		get_user(ver_prod_odd_imag,
+				&up->dvs2_stat.ver_prod.odd_imag) ||
+		get_user(ver_prod_even_real,
+				&up->dvs2_stat.ver_prod.even_real) ||
+		get_user(ver_prod_even_imag,
+				&up->dvs2_stat.ver_prod.even_imag) ||
+		get_user(kp->exp_id, &up->exp_id))
+			return -EFAULT;
+
+	kp->dvs2_stat.hor_prod.odd_real = compat_ptr(hor_prod_odd_real);
+	kp->dvs2_stat.hor_prod.odd_imag = compat_ptr(hor_prod_odd_imag);
+	kp->dvs2_stat.hor_prod.even_real = compat_ptr(hor_prod_even_real);
+	kp->dvs2_stat.hor_prod.even_imag = compat_ptr(hor_prod_even_imag);
+	kp->dvs2_stat.ver_prod.odd_real = compat_ptr(ver_prod_odd_real);
+	kp->dvs2_stat.ver_prod.odd_imag = compat_ptr(ver_prod_odd_imag);
+	kp->dvs2_stat.ver_prod.even_real = compat_ptr(ver_prod_even_real);
+	kp->dvs2_stat.ver_prod.even_imag = compat_ptr(ver_prod_even_imag);
+	return 0;
+}
+
+static int put_atomisp_dis_statistics32(struct atomisp_dis_statistics *kp,
+				struct atomisp_dis_statistics32 __user *up)
+{
+	compat_uptr_t hor_prod_odd_real =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.hor_prod.odd_real);
+	compat_uptr_t hor_prod_odd_imag =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.hor_prod.odd_imag);
+	compat_uptr_t hor_prod_even_real =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.hor_prod.even_real);
+	compat_uptr_t hor_prod_even_imag =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.hor_prod.even_imag);
+	compat_uptr_t ver_prod_odd_real =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.ver_prod.odd_real);
+	compat_uptr_t ver_prod_odd_imag =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.ver_prod.odd_imag);
+	compat_uptr_t ver_prod_even_real =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.ver_prod.even_real);
+	compat_uptr_t ver_prod_even_imag =
+		(compat_uptr_t)((uintptr_t)kp->dvs2_stat.ver_prod.even_imag);
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_dis_statistics32)) ||
+		copy_to_user(up, kp, sizeof(struct atomisp_dvs_grid_info)) ||
+		put_user(hor_prod_odd_real,
+				&up->dvs2_stat.hor_prod.odd_real) ||
+		put_user(hor_prod_odd_imag,
+				&up->dvs2_stat.hor_prod.odd_imag) ||
+		put_user(hor_prod_even_real,
+				&up->dvs2_stat.hor_prod.even_real) ||
+		put_user(hor_prod_even_imag,
+				&up->dvs2_stat.hor_prod.even_imag) ||
+		put_user(ver_prod_odd_real,
+				&up->dvs2_stat.ver_prod.odd_real) ||
+		put_user(ver_prod_odd_imag,
+				&up->dvs2_stat.ver_prod.odd_imag) ||
+		put_user(ver_prod_even_real,
+				&up->dvs2_stat.ver_prod.even_real) ||
+		put_user(ver_prod_even_imag,
+				&up->dvs2_stat.ver_prod.even_imag) ||
+		put_user(kp->exp_id, &up->exp_id))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_dis_coefficients32(struct atomisp_dis_coefficients *kp,
+				struct atomisp_dis_coefficients32 __user *up)
+{
+	compat_uptr_t hor_coefs_odd_real;
+	compat_uptr_t hor_coefs_odd_imag;
+	compat_uptr_t hor_coefs_even_real;
+	compat_uptr_t hor_coefs_even_imag;
+	compat_uptr_t ver_coefs_odd_real;
+	compat_uptr_t ver_coefs_odd_imag;
+	compat_uptr_t ver_coefs_even_real;
+	compat_uptr_t ver_coefs_even_imag;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_dis_coefficients32)) ||
+		copy_from_user(kp, up, sizeof(struct atomisp_dvs_grid_info)) ||
+		get_user(hor_coefs_odd_real, &up->hor_coefs.odd_real) ||
+		get_user(hor_coefs_odd_imag, &up->hor_coefs.odd_imag) ||
+		get_user(hor_coefs_even_real, &up->hor_coefs.even_real) ||
+		get_user(hor_coefs_even_imag, &up->hor_coefs.even_imag) ||
+		get_user(ver_coefs_odd_real, &up->ver_coefs.odd_real) ||
+		get_user(ver_coefs_odd_imag, &up->ver_coefs.odd_imag) ||
+		get_user(ver_coefs_even_real, &up->ver_coefs.even_real) ||
+		get_user(ver_coefs_even_imag, &up->ver_coefs.even_imag))
+			return -EFAULT;
+
+	kp->hor_coefs.odd_real = compat_ptr(hor_coefs_odd_real);
+	kp->hor_coefs.odd_imag = compat_ptr(hor_coefs_odd_imag);
+	kp->hor_coefs.even_real = compat_ptr(hor_coefs_even_real);
+	kp->hor_coefs.even_imag = compat_ptr(hor_coefs_even_imag);
+	kp->ver_coefs.odd_real = compat_ptr(ver_coefs_odd_real);
+	kp->ver_coefs.odd_imag = compat_ptr(ver_coefs_odd_imag);
+	kp->ver_coefs.even_real = compat_ptr(ver_coefs_even_real);
+	kp->ver_coefs.even_imag = compat_ptr(ver_coefs_even_imag);
+	return 0;
+}
+
+static int get_atomisp_dvs_6axis_config32(struct atomisp_dvs_6axis_config *kp,
+				struct atomisp_dvs_6axis_config32 __user *up)
+{	compat_uptr_t xcoords_y;
+	compat_uptr_t ycoords_y;
+	compat_uptr_t xcoords_uv;
+	compat_uptr_t ycoords_uv;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_dvs_6axis_config32)) ||
+		get_user(kp->exp_id, &up->exp_id) ||
+		get_user(kp->width_y, &up->width_y) ||
+		get_user(kp->height_y, &up->height_y) ||
+		get_user(kp->width_uv, &up->width_uv) ||
+		get_user(kp->height_uv, &up->height_uv) ||
+		get_user(xcoords_y, &up->xcoords_y) ||
+		get_user(ycoords_y, &up->ycoords_y) ||
+		get_user(xcoords_uv, &up->xcoords_uv) ||
+		get_user(ycoords_uv, &up->ycoords_uv))
+			return -EFAULT;
+
+	kp->xcoords_y = compat_ptr(xcoords_y);
+	kp->ycoords_y = compat_ptr(ycoords_y);
+	kp->xcoords_uv = compat_ptr(xcoords_uv);
+	kp->ycoords_uv = compat_ptr(ycoords_uv);
+	return 0;
+}
+
+static int get_atomisp_3a_statistics32(struct atomisp_3a_statistics *kp,
+				struct atomisp_3a_statistics32 __user *up)
+{
+	compat_uptr_t data;
+	compat_uptr_t rgby_data;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_3a_statistics32)) ||
+		copy_from_user(kp, up, sizeof(struct atomisp_grid_info)) ||
+		get_user(rgby_data, &up->rgby_data) ||
+		get_user(data, &up->data) ||
+		get_user(kp->exp_id, &up->exp_id) ||
+		get_user(kp->isp_config_id, &up->isp_config_id))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	kp->rgby_data = compat_ptr(rgby_data);
+
+	return 0;
+}
+
+static int put_atomisp_3a_statistics32(struct atomisp_3a_statistics *kp,
+				struct atomisp_3a_statistics32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+	compat_uptr_t rgby_data = (compat_uptr_t)((uintptr_t)kp->rgby_data);
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_3a_statistics32)) ||
+		copy_to_user(up, kp, sizeof(struct atomisp_grid_info)) ||
+		put_user(rgby_data, &up->rgby_data) ||
+		put_user(data, &up->data) ||
+		put_user(kp->exp_id, &up->exp_id) ||
+		put_user(kp->isp_config_id, &up->isp_config_id))
+			return -EFAULT;
+
+	return 0;
+}
+
+
+static int get_atomisp_metadata_stat32(struct atomisp_metadata *kp,
+				struct atomisp_metadata32 __user *up)
+{
+	compat_uptr_t data;
+	compat_uptr_t effective_width;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_metadata32)) ||
+		get_user(data, &up->data) ||
+		get_user(kp->width, &up->width) ||
+		get_user(kp->height, &up->height) ||
+		get_user(kp->stride, &up->stride) ||
+		get_user(kp->exp_id, &up->exp_id) ||
+		get_user(effective_width, &up->effective_width))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	kp->effective_width = compat_ptr(effective_width);
+	return 0;
+}
+
+
+static int put_atomisp_metadata_stat32(struct atomisp_metadata *kp,
+				struct atomisp_metadata32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+	compat_uptr_t effective_width =
+		(compat_uptr_t)((uintptr_t)kp->effective_width);
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_metadata32)) ||
+		put_user(data, &up->data) ||
+		put_user(kp->width, &up->width) ||
+		put_user(kp->height, &up->height) ||
+		put_user(kp->stride, &up->stride) ||
+		put_user(kp->exp_id, &up->exp_id) ||
+		put_user(effective_width, &up->effective_width))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int put_atomisp_metadata_by_type_stat32(
+				struct atomisp_metadata_with_type *kp,
+				struct atomisp_metadata_with_type32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+	compat_uptr_t effective_width =
+		(compat_uptr_t)((uintptr_t)kp->effective_width);
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_metadata_with_type32)) ||
+		put_user(data, &up->data) ||
+		put_user(kp->width, &up->width) ||
+		put_user(kp->height, &up->height) ||
+		put_user(kp->stride, &up->stride) ||
+		put_user(kp->exp_id, &up->exp_id) ||
+		put_user(effective_width, &up->effective_width) ||
+		put_user(kp->type, &up->type))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_metadata_by_type_stat32(
+				struct atomisp_metadata_with_type *kp,
+				struct atomisp_metadata_with_type32 __user *up)
+{
+	compat_uptr_t data;
+	compat_uptr_t effective_width;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_metadata_with_type32)) ||
+		get_user(data, &up->data) ||
+		get_user(kp->width, &up->width) ||
+		get_user(kp->height, &up->height) ||
+		get_user(kp->stride, &up->stride) ||
+		get_user(kp->exp_id, &up->exp_id) ||
+		get_user(effective_width, &up->effective_width) ||
+		get_user(kp->type, &up->type))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	kp->effective_width = compat_ptr(effective_width);
+	return 0;
+}
+
+static int get_atomisp_morph_table32(struct atomisp_morph_table *kp,
+				struct atomisp_morph_table32 __user *up)
+{
+	unsigned int n = ATOMISP_MORPH_TABLE_NUM_PLANES;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_morph_table32)) ||
+		get_user(kp->enabled, &up->enabled) ||
+		get_user(kp->width, &up->width) ||
+		get_user(kp->height, &up->height))
+			return -EFAULT;
+
+	while (n-- > 0) {
+		uintptr_t *coord_kp = (uintptr_t *)&kp->coordinates_x[n];
+
+		if (get_user((*coord_kp), &up->coordinates_x[n]))
+			return -EFAULT;
+
+		coord_kp = (uintptr_t *)&kp->coordinates_y[n];
+		if (get_user((*coord_kp), &up->coordinates_y[n]))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static int put_atomisp_morph_table32(struct atomisp_morph_table *kp,
+				struct atomisp_morph_table32 __user *up)
+{
+	unsigned int n = ATOMISP_MORPH_TABLE_NUM_PLANES;
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_morph_table32)) ||
+		put_user(kp->enabled, &up->enabled) ||
+		put_user(kp->width, &up->width) ||
+		put_user(kp->height, &up->height))
+			return -EFAULT;
+
+	while (n-- > 0) {
+		uintptr_t *coord_kp = (uintptr_t *)&kp->coordinates_x[n];
+
+		if (put_user((*coord_kp), &up->coordinates_x[n]))
+			return -EFAULT;
+
+		coord_kp = (uintptr_t *)&kp->coordinates_y[n];
+		if (put_user((*coord_kp), &up->coordinates_y[n]))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static int get_atomisp_overlay32(struct atomisp_overlay *kp,
+					struct atomisp_overlay32 __user *up)
+{
+	compat_uptr_t frame;
+	if (!access_ok(VERIFY_READ, up, sizeof(struct atomisp_overlay32)) ||
+		get_user(frame, &up->frame) ||
+		get_user(kp->bg_y, &up->bg_y) ||
+		get_user(kp->bg_u, &up->bg_u) ||
+		get_user(kp->bg_v, &up->bg_v) ||
+		get_user(kp->blend_input_perc_y, &up->blend_input_perc_y) ||
+		get_user(kp->blend_input_perc_u, &up->blend_input_perc_u) ||
+		get_user(kp->blend_input_perc_v, &up->blend_input_perc_v) ||
+		get_user(kp->blend_overlay_perc_y,
+				&up->blend_overlay_perc_y) ||
+		get_user(kp->blend_overlay_perc_u,
+				&up->blend_overlay_perc_u) ||
+		get_user(kp->blend_overlay_perc_v,
+				&up->blend_overlay_perc_v) ||
+		get_user(kp->blend_overlay_perc_u,
+				&up->blend_overlay_perc_u) ||
+		get_user(kp->overlay_start_x, &up->overlay_start_y))
+			return -EFAULT;
+
+	kp->frame = compat_ptr(frame);
+	return 0;
+}
+
+static int put_atomisp_overlay32(struct atomisp_overlay *kp,
+					struct atomisp_overlay32 __user *up)
+{
+	compat_uptr_t frame = (compat_uptr_t)((uintptr_t)kp->frame);
+
+	if (!access_ok(VERIFY_WRITE, up, sizeof(struct atomisp_overlay32)) ||
+		put_user(frame, &up->frame) ||
+		put_user(kp->bg_y, &up->bg_y) ||
+		put_user(kp->bg_u, &up->bg_u) ||
+		put_user(kp->bg_v, &up->bg_v) ||
+		put_user(kp->blend_input_perc_y, &up->blend_input_perc_y) ||
+		put_user(kp->blend_input_perc_u, &up->blend_input_perc_u) ||
+		put_user(kp->blend_input_perc_v, &up->blend_input_perc_v) ||
+		put_user(kp->blend_overlay_perc_y,
+				&up->blend_overlay_perc_y) ||
+		put_user(kp->blend_overlay_perc_u,
+				&up->blend_overlay_perc_u) ||
+		put_user(kp->blend_overlay_perc_v,
+				&up->blend_overlay_perc_v) ||
+		put_user(kp->blend_overlay_perc_u,
+				&up->blend_overlay_perc_u) ||
+		put_user(kp->overlay_start_x, &up->overlay_start_y))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_calibration_group32(
+				struct atomisp_calibration_group *kp,
+				struct atomisp_calibration_group32 __user *up)
+{
+	compat_uptr_t calb_grp_values;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_calibration_group32)) ||
+		get_user(kp->size, &up->size) ||
+		get_user(kp->type, &up->type) ||
+		get_user(calb_grp_values, &up->calb_grp_values))
+			return -EFAULT;
+
+	kp->calb_grp_values = compat_ptr(calb_grp_values);
+	return 0;
+}
+
+static int put_atomisp_calibration_group32(
+				struct atomisp_calibration_group *kp,
+				struct atomisp_calibration_group32 __user *up)
+{
+	compat_uptr_t calb_grp_values =
+			(compat_uptr_t)((uintptr_t)kp->calb_grp_values);
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_calibration_group32)) ||
+		put_user(kp->size, &up->size) ||
+		put_user(kp->type, &up->type) ||
+		put_user(calb_grp_values, &up->calb_grp_values))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_acc_fw_load32(struct atomisp_acc_fw_load *kp,
+				struct atomisp_acc_fw_load32 __user *up)
+{
+	compat_uptr_t data;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_acc_fw_load32)) ||
+		get_user(kp->size, &up->size) ||
+		get_user(kp->fw_handle, &up->fw_handle) ||
+		get_user(data, &up->data))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	return 0;
+}
+
+static int put_atomisp_acc_fw_load32(struct atomisp_acc_fw_load *kp,
+				struct atomisp_acc_fw_load32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_acc_fw_load32)) ||
+		put_user(kp->size, &up->size) ||
+		put_user(kp->fw_handle, &up->fw_handle) ||
+		put_user(data, &up->data))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_acc_fw_arg32(struct atomisp_acc_fw_arg *kp,
+					struct atomisp_acc_fw_arg32 __user *up)
+{
+	compat_uptr_t value;
+
+	if (!access_ok(VERIFY_READ, up, sizeof(struct atomisp_acc_fw_arg32)) ||
+		get_user(kp->fw_handle, &up->fw_handle) ||
+		get_user(kp->index, &up->index) ||
+		get_user(value, &up->value) ||
+		get_user(kp->size, &up->size))
+			return -EFAULT;
+
+	kp->value = compat_ptr(value);
+	return 0;
+}
+
+static int put_atomisp_acc_fw_arg32(struct atomisp_acc_fw_arg *kp,
+					struct atomisp_acc_fw_arg32 __user *up)
+{
+	compat_uptr_t value = (compat_uptr_t)((uintptr_t)kp->value);
+
+	if (!access_ok(VERIFY_WRITE, up, sizeof(struct atomisp_acc_fw_arg32)) ||
+		put_user(kp->fw_handle, &up->fw_handle) ||
+		put_user(kp->index, &up->index) ||
+		put_user(value, &up->value) ||
+		put_user(kp->size, &up->size))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_v4l2_private_int_data32(struct v4l2_private_int_data *kp,
+					struct v4l2_private_int_data32 __user *up)
+{
+	compat_uptr_t data;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct v4l2_private_int_data32)) ||
+		get_user(kp->size, &up->size) ||
+		get_user(data, &up->data) ||
+		get_user(kp->reserved[0], &up->reserved[0]) ||
+		get_user(kp->reserved[1], &up->reserved[1]))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	return 0;
+}
+
+static int put_v4l2_private_int_data32(struct v4l2_private_int_data *kp,
+				struct v4l2_private_int_data32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct v4l2_private_int_data32)) ||
+		put_user(kp->size, &up->size) ||
+		put_user(data, &up->data) ||
+		put_user(kp->reserved[0], &up->reserved[0]) ||
+		put_user(kp->reserved[1], &up->reserved[1]))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_shading_table32(struct atomisp_shading_table *kp,
+				struct atomisp_shading_table32 __user *up)
+{
+	unsigned int n = ATOMISP_NUM_SC_COLORS;
+
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_shading_table32)) ||
+		get_user(kp->enable, &up->enable) ||
+		get_user(kp->sensor_width, &up->sensor_width) ||
+		get_user(kp->sensor_height, &up->sensor_height) ||
+		get_user(kp->width, &up->width) ||
+		get_user(kp->height, &up->height) ||
+		get_user(kp->fraction_bits, &up->fraction_bits))
+			return -EFAULT;
+
+	while (n-- > 0) {
+		uintptr_t *data_p = (uintptr_t *)&kp->data[n];
+
+		if (get_user((*data_p), &up->data[n]))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static int get_atomisp_acc_map32(struct atomisp_acc_map *kp,
+					struct atomisp_acc_map32 __user *up)
+{
+	compat_uptr_t user_ptr;
+
+	if (!access_ok(VERIFY_READ, up, sizeof(struct atomisp_acc_map32)) ||
+		get_user(kp->flags, &up->flags) ||
+		get_user(kp->length, &up->length) ||
+		get_user(user_ptr, &up->user_ptr) ||
+		get_user(kp->css_ptr, &up->css_ptr) ||
+		get_user(kp->reserved[0], &up->reserved[0]) ||
+		get_user(kp->reserved[1], &up->reserved[1]) ||
+		get_user(kp->reserved[2], &up->reserved[2]) ||
+		get_user(kp->reserved[3], &up->reserved[3]))
+			return -EFAULT;
+
+	kp->user_ptr = compat_ptr(user_ptr);
+	return 0;
+}
+
+static int put_atomisp_acc_map32(struct atomisp_acc_map *kp,
+					struct atomisp_acc_map32 __user *up)
+{
+	compat_uptr_t user_ptr = (compat_uptr_t)((uintptr_t)kp->user_ptr);
+
+	if (!access_ok(VERIFY_WRITE, up, sizeof(struct atomisp_acc_map32)) ||
+		put_user(kp->flags, &up->flags) ||
+		put_user(kp->length, &up->length) ||
+		put_user(user_ptr, &up->user_ptr) ||
+		put_user(kp->css_ptr, &up->css_ptr) ||
+		put_user(kp->reserved[0], &up->reserved[0]) ||
+		put_user(kp->reserved[1], &up->reserved[1]) ||
+		put_user(kp->reserved[2], &up->reserved[2]) ||
+		put_user(kp->reserved[3], &up->reserved[3]))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_acc_s_mapped_arg32(struct atomisp_acc_s_mapped_arg *kp,
+				struct atomisp_acc_s_mapped_arg32 __user *up)
+{
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_acc_s_mapped_arg32)) ||
+		get_user(kp->fw_handle, &up->fw_handle) ||
+		get_user(kp->memory, &up->memory) ||
+		get_user(kp->length, &up->length) ||
+		get_user(kp->css_ptr, &up->css_ptr))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int put_atomisp_acc_s_mapped_arg32(struct atomisp_acc_s_mapped_arg *kp,
+				struct atomisp_acc_s_mapped_arg32 __user *up)
+{
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_acc_s_mapped_arg32)) ||
+		put_user(kp->fw_handle, &up->fw_handle) ||
+		put_user(kp->memory, &up->memory) ||
+		put_user(kp->length, &up->length) ||
+		put_user(kp->css_ptr, &up->css_ptr))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_parameters32(struct atomisp_parameters *kp,
+					struct atomisp_parameters32 __user *up)
+{
+	int n = offsetof(struct atomisp_parameters32, output_frame) /
+				sizeof(compat_uptr_t);
+	unsigned int size, offset = 0;
+	void  __user *user_ptr;
+#ifdef ISP2401
+	unsigned int stp, mtp, dcp, dscp = 0;
+
+#endif
+	if (!access_ok(VERIFY_READ, up, sizeof(struct atomisp_parameters32)))
+			return -EFAULT;
+
+	while (n >= 0) {
+		compat_uptr_t *src = (compat_uptr_t *)up + n;
+		uintptr_t *dst = (uintptr_t *)kp + n;
+
+		if (get_user((*dst), src))
+			return -EFAULT;
+		n--;
+	}
+	if (get_user(kp->isp_config_id, &up->isp_config_id) ||
+#ifndef ISP2401
+	    get_user(kp->per_frame_setting, &up->per_frame_setting))
+#else
+	    get_user(kp->per_frame_setting, &up->per_frame_setting) ||
+	    get_user(stp, &up->shading_table) ||
+	    get_user(mtp, &up->morph_table) ||
+	    get_user(dcp, &up->dvs2_coefs) ||
+	    get_user(dscp, &up->dvs_6axis_config))
+#endif
+		return -EFAULT;
+
+	{
+		union {
+			struct atomisp_shading_table shading_table;
+			struct atomisp_morph_table   morph_table;
+			struct atomisp_dis_coefficients dvs2_coefs;
+			struct atomisp_dvs_6axis_config dvs_6axis_config;
+		} karg;
+
+		size = sizeof(struct atomisp_shading_table) +
+				sizeof(struct atomisp_morph_table) +
+				sizeof(struct atomisp_dis_coefficients) +
+				sizeof(struct atomisp_dvs_6axis_config);
+		user_ptr = compat_alloc_user_space(size);
+
+		/* handle shading table */
+#ifndef ISP2401
+		if (up->shading_table != 0) {
+#else
+		if (stp != 0) {
+#endif
+			if (get_atomisp_shading_table32(&karg.shading_table,
+				(struct atomisp_shading_table32 __user *)
+#ifndef ISP2401
+						(uintptr_t)up->shading_table))
+#else
+						(uintptr_t)stp))
+#endif
+				return -EFAULT;
+
+			kp->shading_table = user_ptr + offset;
+			offset = sizeof(struct atomisp_shading_table);
+			if (!kp->shading_table)
+				return -EFAULT;
+
+			if (copy_to_user(kp->shading_table,
+					 &karg.shading_table,
+					 sizeof(struct atomisp_shading_table)))
+				return -EFAULT;
+		}
+
+		/* handle morph table */
+#ifndef ISP2401
+		if (up->morph_table != 0) {
+#else
+		if (mtp != 0) {
+#endif
+			if (get_atomisp_morph_table32(&karg.morph_table,
+					(struct atomisp_morph_table32 __user *)
+#ifndef ISP2401
+						(uintptr_t)up->morph_table))
+#else
+						(uintptr_t)mtp))
+#endif
+				return -EFAULT;
+
+			kp->morph_table = user_ptr + offset;
+			offset += sizeof(struct atomisp_morph_table);
+			if (!kp->morph_table)
+				return -EFAULT;
+
+			if (copy_to_user(kp->morph_table, &karg.morph_table,
+					   sizeof(struct atomisp_morph_table)))
+				return -EFAULT;
+		}
+
+		/* handle dvs2 coefficients */
+#ifndef ISP2401
+		if (up->dvs2_coefs != 0) {
+#else
+		if (dcp != 0) {
+#endif
+			if (get_atomisp_dis_coefficients32(&karg.dvs2_coefs,
+				(struct atomisp_dis_coefficients32 __user *)
+#ifndef ISP2401
+						(uintptr_t)up->dvs2_coefs))
+#else
+						(uintptr_t)dcp))
+#endif
+				return -EFAULT;
+
+			kp->dvs2_coefs = user_ptr + offset;
+			offset += sizeof(struct atomisp_dis_coefficients);
+			if (!kp->dvs2_coefs)
+				return -EFAULT;
+
+			if (copy_to_user(kp->dvs2_coefs, &karg.dvs2_coefs,
+				sizeof(struct atomisp_dis_coefficients)))
+				return -EFAULT;
+		}
+		/* handle dvs 6axis configuration */
+#ifndef ISP2401
+		if (up->dvs_6axis_config != 0) {
+#else
+		if (dscp != 0) {
+#endif
+			if (get_atomisp_dvs_6axis_config32(&karg.dvs_6axis_config,
+				(struct atomisp_dvs_6axis_config32 __user *)
+#ifndef ISP2401
+						(uintptr_t)up->dvs_6axis_config))
+#else
+						(uintptr_t)dscp))
+#endif
+				return -EFAULT;
+
+			kp->dvs_6axis_config = user_ptr + offset;
+			offset += sizeof(struct atomisp_dvs_6axis_config);
+			if (!kp->dvs_6axis_config)
+				return -EFAULT;
+
+			if (copy_to_user(kp->dvs_6axis_config, &karg.dvs_6axis_config,
+				sizeof(struct atomisp_dvs_6axis_config)))
+				return -EFAULT;
+		}
+	}
+	return 0;
+}
+
+static int get_atomisp_acc_fw_load_to_pipe32(
+			struct atomisp_acc_fw_load_to_pipe *kp,
+			struct atomisp_acc_fw_load_to_pipe32 __user *up)
+{
+	compat_uptr_t data;
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_acc_fw_load_to_pipe32)) ||
+		get_user(kp->flags, &up->flags) ||
+		get_user(kp->fw_handle, &up->fw_handle) ||
+		get_user(kp->size, &up->size) ||
+		get_user(kp->type, &up->type) ||
+		get_user(kp->reserved[0], &up->reserved[0]) ||
+		get_user(kp->reserved[1], &up->reserved[1]) ||
+		get_user(kp->reserved[2], &up->reserved[2]) ||
+		get_user(data, &up->data))
+			return -EFAULT;
+
+	kp->data = compat_ptr(data);
+	return 0;
+}
+
+static int put_atomisp_acc_fw_load_to_pipe32(
+			struct atomisp_acc_fw_load_to_pipe *kp,
+			struct atomisp_acc_fw_load_to_pipe32 __user *up)
+{
+	compat_uptr_t data = (compat_uptr_t)((uintptr_t)kp->data);
+	if (!access_ok(VERIFY_WRITE, up,
+			sizeof(struct atomisp_acc_fw_load_to_pipe32)) ||
+		put_user(kp->flags, &up->flags) ||
+		put_user(kp->fw_handle, &up->fw_handle) ||
+		put_user(kp->size, &up->size) ||
+		put_user(kp->type, &up->type) ||
+		put_user(kp->reserved[0], &up->reserved[0]) ||
+		put_user(kp->reserved[1], &up->reserved[1]) ||
+		put_user(kp->reserved[2], &up->reserved[2]) ||
+		put_user(data, &up->data))
+			return -EFAULT;
+
+	return 0;
+}
+
+static int get_atomisp_sensor_ae_bracketing_lut(
+			struct atomisp_sensor_ae_bracketing_lut *kp,
+			struct atomisp_sensor_ae_bracketing_lut32 __user *up)
+{
+	compat_uptr_t lut;
+	if (!access_ok(VERIFY_READ, up,
+			sizeof(struct atomisp_sensor_ae_bracketing_lut32)) ||
+		get_user(kp->lut_size, &up->lut_size) ||
+		get_user(lut, &up->lut))
+			return -EFAULT;
+
+	kp->lut = compat_ptr(lut);
+	return 0;
+}
+
+static long native_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	long ret = -ENOIOCTLCMD;
+
+	if (file->f_op->unlocked_ioctl)
+		ret = file->f_op->unlocked_ioctl(file, cmd, arg);
+
+	return ret;
+}
+
+long atomisp_do_compat_ioctl(struct file *file,
+			    unsigned int cmd, unsigned long arg)
+{
+	union {
+		struct atomisp_histogram his;
+		struct atomisp_dis_statistics dis_s;
+		struct atomisp_dis_coefficients dis_c;
+		struct atomisp_dvs_6axis_config dvs_c;
+		struct atomisp_3a_statistics s3a_s;
+		struct atomisp_morph_table mor_t;
+		struct v4l2_framebuffer v4l2_buf;
+		struct atomisp_overlay overlay;
+		struct atomisp_calibration_group cal_grp;
+		struct atomisp_acc_fw_load acc_fw_load;
+		struct atomisp_acc_fw_arg acc_fw_arg;
+		struct v4l2_private_int_data v4l2_pri_data;
+		struct atomisp_shading_table shd_tbl;
+		struct atomisp_acc_map acc_map;
+		struct atomisp_acc_s_mapped_arg acc_map_arg;
+		struct atomisp_parameters param;
+		struct atomisp_acc_fw_load_to_pipe acc_fw_to_pipe;
+		struct atomisp_metadata md;
+		struct atomisp_metadata_with_type md_with_type;
+		struct atomisp_sensor_ae_bracketing_lut lut;
+	} karg;
+	mm_segment_t old_fs;
+	void __user *up = compat_ptr(arg);
+	long err = -ENOIOCTLCMD;
+
+	/* First, convert the command. */
+	switch (cmd) {
+	case ATOMISP_IOC_G_HISTOGRAM32:
+		cmd = ATOMISP_IOC_G_HISTOGRAM;
+		break;
+	case ATOMISP_IOC_S_HISTOGRAM32:
+		cmd = ATOMISP_IOC_S_HISTOGRAM;
+		break;
+	case ATOMISP_IOC_G_DIS_STAT32:
+		cmd = ATOMISP_IOC_G_DIS_STAT;
+		break;
+	case ATOMISP_IOC_S_DIS_COEFS32:
+		cmd = ATOMISP_IOC_S_DIS_COEFS;
+		break;
+	case ATOMISP_IOC_S_DIS_VECTOR32:
+		cmd = ATOMISP_IOC_S_DIS_VECTOR;
+		break;
+	case ATOMISP_IOC_G_3A_STAT32:
+		cmd = ATOMISP_IOC_G_3A_STAT;
+		break;
+	case ATOMISP_IOC_G_ISP_GDC_TAB32:
+		cmd = ATOMISP_IOC_G_ISP_GDC_TAB;
+		break;
+	case ATOMISP_IOC_S_ISP_GDC_TAB32:
+		cmd = ATOMISP_IOC_S_ISP_GDC_TAB;
+		break;
+	case ATOMISP_IOC_S_ISP_FPN_TABLE32:
+		cmd = ATOMISP_IOC_S_ISP_FPN_TABLE;
+		break;
+	case ATOMISP_IOC_G_ISP_OVERLAY32:
+		cmd = ATOMISP_IOC_G_ISP_OVERLAY;
+		break;
+	case ATOMISP_IOC_S_ISP_OVERLAY32:
+		cmd = ATOMISP_IOC_S_ISP_OVERLAY;
+		break;
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP32:
+		cmd = ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP;
+		break;
+	case ATOMISP_IOC_ACC_LOAD32:
+		cmd = ATOMISP_IOC_ACC_LOAD;
+		break;
+	case ATOMISP_IOC_ACC_S_ARG32:
+		cmd = ATOMISP_IOC_ACC_S_ARG;
+		break;
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA32:
+		cmd = ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA;
+		break;
+	case ATOMISP_IOC_S_ISP_SHD_TAB32:
+		cmd = ATOMISP_IOC_S_ISP_SHD_TAB;
+		break;
+	case ATOMISP_IOC_ACC_DESTAB32:
+		cmd = ATOMISP_IOC_ACC_DESTAB;
+		break;
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA32:
+		cmd = ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA;
+		break;
+	case ATOMISP_IOC_ACC_MAP32:
+		cmd = ATOMISP_IOC_ACC_MAP;
+		break;
+	case ATOMISP_IOC_ACC_UNMAP32:
+		cmd = ATOMISP_IOC_ACC_UNMAP;
+		break;
+	case ATOMISP_IOC_ACC_S_MAPPED_ARG32:
+		cmd = ATOMISP_IOC_ACC_S_MAPPED_ARG;
+		break;
+	case ATOMISP_IOC_S_PARAMETERS32:
+		cmd = ATOMISP_IOC_S_PARAMETERS;
+		break;
+	case ATOMISP_IOC_ACC_LOAD_TO_PIPE32:
+		cmd = ATOMISP_IOC_ACC_LOAD_TO_PIPE;
+		break;
+	case ATOMISP_IOC_G_METADATA32:
+		cmd = ATOMISP_IOC_G_METADATA;
+		break;
+	case ATOMISP_IOC_G_METADATA_BY_TYPE32:
+		cmd = ATOMISP_IOC_G_METADATA_BY_TYPE;
+		break;
+	case ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT32:
+		cmd = ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT;
+		break;
+	}
+
+	switch (cmd) {
+	case ATOMISP_IOC_G_HISTOGRAM:
+	case ATOMISP_IOC_S_HISTOGRAM:
+		err = get_atomisp_histogram32(&karg.his, up);
+		break;
+	case ATOMISP_IOC_G_DIS_STAT:
+		err = get_atomisp_dis_statistics32(&karg.dis_s, up);
+		break;
+	case ATOMISP_IOC_S_DIS_COEFS:
+		err = get_atomisp_dis_coefficients32(&karg.dis_c, up);
+		break;
+	case ATOMISP_IOC_S_DIS_VECTOR:
+		err = get_atomisp_dvs_6axis_config32(&karg.dvs_c, up);
+		break;
+	case ATOMISP_IOC_G_3A_STAT:
+		err = get_atomisp_3a_statistics32(&karg.s3a_s, up);
+		break;
+	case ATOMISP_IOC_G_ISP_GDC_TAB:
+	case ATOMISP_IOC_S_ISP_GDC_TAB:
+		err = get_atomisp_morph_table32(&karg.mor_t, up);
+		break;
+	case ATOMISP_IOC_S_ISP_FPN_TABLE:
+		err = get_v4l2_framebuffer32(&karg.v4l2_buf, up);
+		break;
+	case ATOMISP_IOC_G_ISP_OVERLAY:
+	case ATOMISP_IOC_S_ISP_OVERLAY:
+		err = get_atomisp_overlay32(&karg.overlay, up);
+		break;
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP:
+		err = get_atomisp_calibration_group32(&karg.cal_grp, up);
+		break;
+	case ATOMISP_IOC_ACC_LOAD:
+		err = get_atomisp_acc_fw_load32(&karg.acc_fw_load, up);
+		break;
+	case ATOMISP_IOC_ACC_S_ARG:
+	case ATOMISP_IOC_ACC_DESTAB:
+		err = get_atomisp_acc_fw_arg32(&karg.acc_fw_arg, up);
+		break;
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA:
+		err = get_v4l2_private_int_data32(&karg.v4l2_pri_data, up);
+		break;
+	case ATOMISP_IOC_S_ISP_SHD_TAB:
+		err = get_atomisp_shading_table32(&karg.shd_tbl, up);
+		break;
+	case ATOMISP_IOC_ACC_MAP:
+	case ATOMISP_IOC_ACC_UNMAP:
+		err = get_atomisp_acc_map32(&karg.acc_map, up);
+		break;
+	case ATOMISP_IOC_ACC_S_MAPPED_ARG:
+		err = get_atomisp_acc_s_mapped_arg32(&karg.acc_map_arg, up);
+		break;
+	case ATOMISP_IOC_S_PARAMETERS:
+		err = get_atomisp_parameters32(&karg.param, up);
+		break;
+	case ATOMISP_IOC_ACC_LOAD_TO_PIPE:
+		err = get_atomisp_acc_fw_load_to_pipe32(&karg.acc_fw_to_pipe,
+							up);
+		break;
+	case ATOMISP_IOC_G_METADATA:
+		err = get_atomisp_metadata_stat32(&karg.md, up);
+		break;
+	case ATOMISP_IOC_G_METADATA_BY_TYPE:
+		err = get_atomisp_metadata_by_type_stat32(&karg.md_with_type,
+							up);
+		break;
+	case ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT:
+		err = get_atomisp_sensor_ae_bracketing_lut(&karg.lut, up);
+		break;
+	}
+	if (err)
+		return err;
+
+	old_fs = get_fs();
+	set_fs(KERNEL_DS);
+	err = native_ioctl(file, cmd, (unsigned long)&karg);
+	set_fs(old_fs);
+	if (err)
+		return err;
+
+	switch (cmd) {
+	case ATOMISP_IOC_G_HISTOGRAM:
+		err = put_atomisp_histogram32(&karg.his, up);
+		break;
+	case ATOMISP_IOC_G_DIS_STAT:
+		err = put_atomisp_dis_statistics32(&karg.dis_s, up);
+		break;
+	case ATOMISP_IOC_G_3A_STAT:
+		err = put_atomisp_3a_statistics32(&karg.s3a_s, up);
+		break;
+	case ATOMISP_IOC_G_ISP_GDC_TAB:
+		err = put_atomisp_morph_table32(&karg.mor_t, up);
+		break;
+	case ATOMISP_IOC_G_ISP_OVERLAY:
+		err = put_atomisp_overlay32(&karg.overlay, up);
+		break;
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP:
+		err = put_atomisp_calibration_group32(&karg.cal_grp, up);
+		break;
+	case ATOMISP_IOC_ACC_LOAD:
+		err = put_atomisp_acc_fw_load32(&karg.acc_fw_load, up);
+		break;
+	case ATOMISP_IOC_ACC_S_ARG:
+	case ATOMISP_IOC_ACC_DESTAB:
+		err = put_atomisp_acc_fw_arg32(&karg.acc_fw_arg, up);
+		break;
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA:
+		err = put_v4l2_private_int_data32(&karg.v4l2_pri_data, up);
+		break;
+	case ATOMISP_IOC_ACC_MAP:
+	case ATOMISP_IOC_ACC_UNMAP:
+		err = put_atomisp_acc_map32(&karg.acc_map, up);
+		break;
+	case ATOMISP_IOC_ACC_S_MAPPED_ARG:
+		err = put_atomisp_acc_s_mapped_arg32(&karg.acc_map_arg, up);
+		break;
+	case ATOMISP_IOC_ACC_LOAD_TO_PIPE:
+		err = put_atomisp_acc_fw_load_to_pipe32(&karg.acc_fw_to_pipe,
+							up);
+		break;
+	case ATOMISP_IOC_G_METADATA:
+		err = put_atomisp_metadata_stat32(&karg.md, up);
+		break;
+	case ATOMISP_IOC_G_METADATA_BY_TYPE:
+		err = put_atomisp_metadata_by_type_stat32(&karg.md_with_type,
+							up);
+		break;
+	}
+
+	return err;
+}
+
+long atomisp_compat_ioctl32(struct file *file,
+			    unsigned int cmd, unsigned long arg)
+{
+
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	long ret = -ENOIOCTLCMD;
+
+	if (!file->f_op->unlocked_ioctl)
+		return ret;
+
+	switch (cmd) {
+	case ATOMISP_IOC_G_XNR:
+	case ATOMISP_IOC_S_XNR:
+	case ATOMISP_IOC_G_NR:
+	case ATOMISP_IOC_S_NR:
+	case ATOMISP_IOC_G_TNR:
+	case ATOMISP_IOC_S_TNR:
+	case ATOMISP_IOC_G_BLACK_LEVEL_COMP:
+	case ATOMISP_IOC_S_BLACK_LEVEL_COMP:
+	case ATOMISP_IOC_G_EE:
+	case ATOMISP_IOC_S_EE:
+	case ATOMISP_IOC_S_DIS_VECTOR:
+	case ATOMISP_IOC_G_ISP_PARM:
+	case ATOMISP_IOC_S_ISP_PARM:
+	case ATOMISP_IOC_G_ISP_GAMMA:
+	case ATOMISP_IOC_S_ISP_GAMMA:
+	case ATOMISP_IOC_ISP_MAKERNOTE:
+	case ATOMISP_IOC_G_ISP_MACC:
+	case ATOMISP_IOC_S_ISP_MACC:
+	case ATOMISP_IOC_G_ISP_BAD_PIXEL_DETECTION:
+	case ATOMISP_IOC_S_ISP_BAD_PIXEL_DETECTION:
+	case ATOMISP_IOC_G_ISP_FALSE_COLOR_CORRECTION:
+	case ATOMISP_IOC_S_ISP_FALSE_COLOR_CORRECTION:
+	case ATOMISP_IOC_G_ISP_CTC:
+	case ATOMISP_IOC_S_ISP_CTC:
+	case ATOMISP_IOC_G_ISP_WHITE_BALANCE:
+	case ATOMISP_IOC_S_ISP_WHITE_BALANCE:
+	case ATOMISP_IOC_CAMERA_BRIDGE:
+	case ATOMISP_IOC_G_SENSOR_MODE_DATA:
+	case ATOMISP_IOC_S_EXPOSURE:
+	case ATOMISP_IOC_G_3A_CONFIG:
+	case ATOMISP_IOC_S_3A_CONFIG:
+	case ATOMISP_IOC_ACC_UNLOAD:
+	case ATOMISP_IOC_ACC_START:
+	case ATOMISP_IOC_ACC_WAIT:
+	case ATOMISP_IOC_ACC_ABORT:
+	case ATOMISP_IOC_G_ISP_GAMMA_CORRECTION:
+	case ATOMISP_IOC_S_ISP_GAMMA_CORRECTION:
+	case ATOMISP_IOC_S_CONT_CAPTURE_CONFIG:
+	case ATOMISP_IOC_G_DVS2_BQ_RESOLUTIONS:
+	case ATOMISP_IOC_EXT_ISP_CTRL:
+	case ATOMISP_IOC_EXP_ID_UNLOCK:
+	case ATOMISP_IOC_EXP_ID_CAPTURE:
+	case ATOMISP_IOC_S_ENABLE_DZ_CAPT_PIPE:
+	case ATOMISP_IOC_G_FORMATS_CONFIG:
+	case ATOMISP_IOC_S_FORMATS_CONFIG:
+	case ATOMISP_IOC_S_EXPOSURE_WINDOW:
+	case ATOMISP_IOC_S_ACC_STATE:
+	case ATOMISP_IOC_G_ACC_STATE:
+	case ATOMISP_IOC_INJECT_A_FAKE_EVENT:
+	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_G_INVALID_FRAME_NUM:
+	case ATOMISP_IOC_S_ARRAY_RESOLUTION:
+#ifdef ISP2401
+	case ATOMISP_IOC_S_SENSOR_RUNMODE:
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+#endif
+		ret = native_ioctl(file, cmd, arg);
+		break;
+
+	case ATOMISP_IOC_G_HISTOGRAM32:
+	case ATOMISP_IOC_S_HISTOGRAM32:
+	case ATOMISP_IOC_G_DIS_STAT32:
+	case ATOMISP_IOC_S_DIS_COEFS32:
+	case ATOMISP_IOC_S_DIS_VECTOR32:
+	case ATOMISP_IOC_G_3A_STAT32:
+	case ATOMISP_IOC_G_ISP_GDC_TAB32:
+	case ATOMISP_IOC_S_ISP_GDC_TAB32:
+	case ATOMISP_IOC_S_ISP_FPN_TABLE32:
+	case ATOMISP_IOC_G_ISP_OVERLAY32:
+	case ATOMISP_IOC_S_ISP_OVERLAY32:
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP32:
+	case ATOMISP_IOC_ACC_LOAD32:
+	case ATOMISP_IOC_ACC_S_ARG32:
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA32:
+	case ATOMISP_IOC_S_ISP_SHD_TAB32:
+	case ATOMISP_IOC_ACC_DESTAB32:
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA32:
+	case ATOMISP_IOC_ACC_MAP32:
+	case ATOMISP_IOC_ACC_UNMAP32:
+	case ATOMISP_IOC_ACC_S_MAPPED_ARG32:
+	case ATOMISP_IOC_S_PARAMETERS32:
+	case ATOMISP_IOC_ACC_LOAD_TO_PIPE32:
+	case ATOMISP_IOC_G_METADATA32:
+	case ATOMISP_IOC_G_METADATA_BY_TYPE32:
+	case ATOMISP_IOC_S_SENSOR_AE_BRACKETING_LUT32:
+		ret = atomisp_do_compat_ioctl(file, cmd, arg);
+		break;
+
+	default:
+		dev_warn(isp->dev,
+			"%s: unknown ioctl '%c', dir=%d, #%d (0x%08x)\n",
+			__func__, _IOC_TYPE(cmd), _IOC_DIR(cmd), _IOC_NR(cmd),
+			cmd);
+		break;
+	}
+	return ret;
+}
+#endif /* CONFIG_COMPAT */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.h
new file mode 100644
index 000000000000..750478f614d6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_compat_ioctl32.h
@@ -0,0 +1,369 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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;
+	uint32_t 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;
+	uint32_t exp_id;
+	uint32_t 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;
+	uint32_t width;
+	uint32_t height;
+	uint32_t stride; /* in bytes */
+	uint32_t exp_id; /* exposure ID */
+	compat_uptr_t effective_width;
+};
+
+struct atomisp_metadata32 {
+	compat_uptr_t data;
+	uint32_t width;
+	uint32_t height;
+	uint32_t stride;
+	uint32_t 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 atomisp_acc_fw_load32 {
+	unsigned int size;
+	unsigned int fw_handle;
+	compat_uptr_t data;
+};
+
+struct atomisp_acc_fw_arg32 {
+	unsigned int fw_handle;
+	unsigned int index;
+	compat_uptr_t value;
+	compat_size_t size;
+};
+
+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_acc_map32 {
+	__u32 flags;			/* Flags, see list below */
+	__u32 length;			/* Length of data in bytes */
+	compat_uptr_t user_ptr;		/* Pointer into user space */
+	compat_ulong_t css_ptr;		/* Pointer into CSS address space */
+	__u32 reserved[4];		/* Set to zero */
+};
+
+struct atomisp_acc_s_mapped_arg32 {
+	unsigned int fw_handle;
+	__u32 memory;			/* one of enum atomisp_acc_memory */
+	compat_size_t length;
+	compat_ulong_t css_ptr;
+};
+
+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.
+	 */
+	uint32_t	isp_config_id;
+	uint32_t	per_frame_setting;
+};
+
+struct atomisp_acc_fw_load_to_pipe32 {
+	__u32 flags;			/* Flags, see below for valid values */
+	unsigned int fw_handle;		/* Handle, filled by kernel. */
+	__u32 size;			/* Firmware binary size */
+	compat_uptr_t data;		/* Pointer to firmware */
+	__u32 type;			/* Binary type */
+	__u32 reserved[3];		/* Set to zero */
+};
+
+struct atomisp_dvs_6axis_config32 {
+	uint32_t exp_id;
+	uint32_t width_y;
+	uint32_t height_y;
+	uint32_t width_uv;
+	uint32_t 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_ACC_LOAD32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 24, struct atomisp_acc_fw_load32)
+
+#define ATOMISP_IOC_ACC_S_ARG32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 24, struct atomisp_acc_fw_arg32)
+
+#define ATOMISP_IOC_ACC_DESTAB32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 25, struct atomisp_acc_fw_arg32)
+
+#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_ACC_MAP32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_map32)
+
+#define ATOMISP_IOC_ACC_UNMAP32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_map32)
+
+#define ATOMISP_IOC_ACC_S_MAPPED_ARG32 \
+	_IOW('v', BASE_VIDIOC_PRIVATE + 30, struct atomisp_acc_s_mapped_arg32)
+
+#define ATOMISP_IOC_ACC_LOAD_TO_PIPE32 \
+	_IOWR('v', BASE_VIDIOC_PRIVATE + 31, struct atomisp_acc_fw_load_to_pipe32)
+
+#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/atomisp2/atomisp_csi2.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_csi2.c
new file mode 100644
index 000000000000..2c5036685447
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_csi2.c
@@ -0,0 +1,446 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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_pad_config *cfg,
+						    enum
+						    v4l2_subdev_format_whence
+						    which, unsigned int pad)
+{
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return v4l2_subdev_get_try_format(&csi2->subdev, cfg, 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_pad_config *cfg,
+			       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 sucess
+*/
+static int csi2_get_format(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_pad_config *cfg,
+			   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, cfg, fmt->which, fmt->pad);
+
+	fmt->format = *format;
+
+	return 0;
+}
+
+int atomisp_csi2_set_ffmt(struct v4l2_subdev *sd,
+			  struct v4l2_subdev_pad_config *cfg,
+			  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 =
+#ifndef ISP2401
+		__csi2_get_format(csi2, cfg, which, pad);
+#else
+	    __csi2_get_format(csi2, cfg, which, pad);
+#endif
+
+	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, cfg, which, CSI2_PAD_SOURCE,
+					     &tmp_ffmt);
+	}
+
+	/* FIXME: DPCM decompression */
+	*actual_ffmt = *ffmt =
+#ifndef ISP2401
+		*__csi2_get_format(csi2, cfg, which, CSI2_PAD_SINK);
+#else
+	    *__csi2_get_format(csi2, cfg, which, CSI2_PAD_SINK);
+#endif
+
+	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_pad_config *cfg,
+			   struct v4l2_subdev_format *fmt)
+{
+	return atomisp_csi2_set_ffmt(sd, cfg, 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,
+};
+
+#ifndef ISP2401
+
+#endif
+/*
+ * 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 =
+		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_store_uint32(base + CSI2_REG_RX_CSI_DLY_CNT_TERMEN,
+				     n == 0 ? clk_termen : dat_termen);
+		atomisp_store_uint32(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)
+{
+}
+
+#ifndef ISP2401
+
+#endif
+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/atomisp2/atomisp_csi2.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_csi2.h
new file mode 100644
index 000000000000..faa9cf7e05c0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_csi2.h
@@ -0,0 +1,61 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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	(1 << 0)
+#define CSI2_OUTPUT_MEMORY	(1 << 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_pad_config *cfg,
+			  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/atomisp2/atomisp_dfs_tables.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_dfs_tables.h
new file mode 100644
index 000000000000..204d941cdb6c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_dfs_tables.h
@@ -0,0 +1,412 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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;
+};
+
+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,
+#ifndef ISP2401
+		.isp_freq = ISP_FREQ_266MHZ,
+#else
+		.isp_freq = ISP_FREQ_400MHZ,
+#endif
+		.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 const 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_byt_cr[] = {
+	{
+		.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_320MHZ,
+		.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_320MHZ,
+		.run_mode = ATOMISP_RUN_MODE_SDV,
+	},
+};
+
+static const struct atomisp_dfs_config dfs_config_byt_cr = {
+	.lowest_freq = ISP_FREQ_200MHZ,
+	.max_freq_at_vmin = ISP_FREQ_320MHZ,
+	.highest_freq = ISP_FREQ_320MHZ,
+	.dfs_table = dfs_rules_byt_cr,
+	.dfs_table_size = ARRAY_SIZE(dfs_rules_byt_cr),
+};
+
+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),
+};
+
+static 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),
+};
+
+#endif /* __ATOMISP_DFS_TABLES_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.c
new file mode 100644
index 000000000000..1ae2358de8d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.c
@@ -0,0 +1,209 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+
+#include "atomisp_compat.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+#include "hmm/hmm.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 pci_driver	*drv;
+	struct atomisp_device	*isp;
+	unsigned int		dbglvl;
+	unsigned int		dbgfun;
+	unsigned int		dbgopt;
+};
+
+#define OPTION_BIN_LIST			(1<<0)
+#define OPTION_BIN_RUN			(1<<1)
+#define OPTION_MEM_STAT			(1<<2)
+#define OPTION_VALID			(OPTION_BIN_LIST \
+					| OPTION_BIN_RUN \
+					| OPTION_MEM_STAT)
+
+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();
+			if (ret) {
+				dev_err(atomisp_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(atomisp_dev, "%s dump running bin err[ret:%d]\n",
+					__func__, ret);
+				goto opt_err;
+			}
+		}
+
+		if (opt & OPTION_MEM_STAT)
+			hmm_show_mem_stat(__func__, __LINE__);
+	} else {
+		ret = -EINVAL;
+		dev_err(atomisp_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 = atomisp_css_debug_get_dtrace_level();
+	return sprintf(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;
+	}
+	atomisp_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 sprintf(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 sprintf(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 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 pci_driver *drv)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < ARRAY_SIZE(iunit_drvfs_attrs); i++)
+		ret |= driver_create_file(&(drv->driver),
+					&iunit_drvfs_attrs[i]);
+
+	return ret;
+}
+
+static void iunit_drvfs_remove_files(struct pci_driver *drv)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(iunit_drvfs_attrs); i++)
+		driver_remove_file(&(drv->driver), &iunit_drvfs_attrs[i]);
+}
+
+int atomisp_drvfs_init(struct pci_driver *drv, struct atomisp_device *isp)
+{
+	int ret;
+
+	iunit_debug.isp = isp;
+	iunit_debug.drv = drv;
+
+	ret = iunit_drvfs_create_files(iunit_debug.drv);
+	if (ret) {
+		dev_err(atomisp_dev, "drvfs_create_files error: %d\n", ret);
+		iunit_drvfs_remove_files(drv);
+	}
+
+	return ret;
+}
+
+void atomisp_drvfs_exit(void)
+{
+	iunit_drvfs_remove_files(iunit_debug.drv);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.h
new file mode 100644
index 000000000000..5cb717b0c1c2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_drvfs.h
@@ -0,0 +1,29 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef	__ATOMISP_DRVFS_H__
+#define	__ATOMISP_DRVFS_H__
+
+extern int atomisp_drvfs_init(struct pci_driver *drv, struct atomisp_device
+				*isp);
+extern void atomisp_drvfs_exit(void);
+
+#endif /* __ATOMISP_DRVFS_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.c
new file mode 100644
index 000000000000..c766119bf798
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.c
@@ -0,0 +1,245 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mediabus.h>
+
+#include <media/videobuf-vmalloc.h>
+#include <linux/delay.h>
+
+#include "ia_css.h"
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_file.h"
+#include "atomisp_internal.h"
+#include "atomisp_ioctl.h"
+
+static void file_work(struct work_struct *work)
+{
+	struct atomisp_file_device *file_dev =
+			container_of(work, struct atomisp_file_device, work);
+	struct atomisp_device *isp = file_dev->isp;
+	/* only support file injection on subdev0 */
+	struct atomisp_sub_device *asd = &isp->asd[0];
+	struct atomisp_video_pipe *out_pipe = &asd->video_in;
+	unsigned short *buf = videobuf_to_vmalloc(out_pipe->outq.bufs[0]);
+	struct v4l2_mbus_framefmt isp_sink_fmt;
+
+	if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+		return;
+
+	dev_dbg(isp->dev, ">%s: ready to start streaming\n", __func__);
+	isp_sink_fmt = *atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+						V4L2_SUBDEV_FORMAT_ACTIVE,
+						ATOMISP_SUBDEV_PAD_SINK);
+
+	while (!atomisp_css_isp_has_started())
+		usleep_range(1000, 1500);
+
+	atomisp_css_send_input_frame(asd, buf, isp_sink_fmt.width,
+				     isp_sink_fmt.height);
+	dev_dbg(isp->dev, "<%s: streaming done\n", __func__);
+}
+
+static int file_input_s_stream(struct v4l2_subdev *sd, int enable)
+{
+	struct atomisp_file_device *file_dev = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = file_dev->isp;
+	/* only support file injection on subdev0 */
+	struct atomisp_sub_device *asd = &isp->asd[0];
+
+	dev_dbg(isp->dev, "%s: enable %d\n", __func__, enable);
+	if (enable) {
+		if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
+			return 0;
+
+		queue_work(file_dev->work_queue, &file_dev->work);
+		return 0;
+	}
+	cancel_work_sync(&file_dev->work);
+	return 0;
+}
+
+static int file_input_g_parm(struct v4l2_subdev *sd,
+		struct v4l2_streamparm *param)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int file_input_s_parm(struct v4l2_subdev *sd,
+		struct v4l2_streamparm *param)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int file_input_get_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	struct atomisp_file_device *file_dev = v4l2_get_subdevdata(sd);
+	struct atomisp_device *isp = file_dev->isp;
+	/* only support file injection on subdev0 */
+	struct atomisp_sub_device *asd = &isp->asd[0];
+	struct v4l2_mbus_framefmt *isp_sink_fmt;
+	if (format->pad)
+		return -EINVAL;
+	isp_sink_fmt = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
+					       V4L2_SUBDEV_FORMAT_ACTIVE,
+					       ATOMISP_SUBDEV_PAD_SINK);
+
+	fmt->width = isp_sink_fmt->width;
+	fmt->height = isp_sink_fmt->height;
+	fmt->code = isp_sink_fmt->code;
+
+	return 0;
+}
+
+static int file_input_set_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_pad_config *cfg,
+			      struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt = &format->format;
+	if (format->pad)
+		return -EINVAL;
+	file_input_get_fmt(sd, cfg, format);
+	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
+		cfg->try_fmt = *fmt;
+	return 0;
+}
+
+static int file_input_log_status(struct v4l2_subdev *sd)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int file_input_s_power(struct v4l2_subdev *sd, int on)
+{
+	/* to fake */
+	return 0;
+}
+
+static int file_input_enum_mbus_code(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_pad_config *cfg,
+				     struct v4l2_subdev_mbus_code_enum *code)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int file_input_enum_frame_size(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_pad_config *cfg,
+				      struct v4l2_subdev_frame_size_enum *fse)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int file_input_enum_frame_ival(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_pad_config *cfg,
+				      struct v4l2_subdev_frame_interval_enum
+				      *fie)
+{
+	/*to fake*/
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops file_input_video_ops = {
+	.s_stream = file_input_s_stream,
+	.g_parm = file_input_g_parm,
+	.s_parm = file_input_s_parm,
+};
+
+static const struct v4l2_subdev_core_ops file_input_core_ops = {
+	.log_status = file_input_log_status,
+	.s_power = file_input_s_power,
+};
+
+static const struct v4l2_subdev_pad_ops file_input_pad_ops = {
+	.enum_mbus_code = file_input_enum_mbus_code,
+	.enum_frame_size = file_input_enum_frame_size,
+	.enum_frame_interval = file_input_enum_frame_ival,
+	.get_fmt = file_input_get_fmt,
+	.set_fmt = file_input_set_fmt,
+};
+
+static const struct v4l2_subdev_ops file_input_ops = {
+	.core = &file_input_core_ops,
+	.video = &file_input_video_ops,
+	.pad = &file_input_pad_ops,
+};
+
+void
+atomisp_file_input_unregister_entities(struct atomisp_file_device *file_dev)
+{
+	media_entity_cleanup(&file_dev->sd.entity);
+	v4l2_device_unregister_subdev(&file_dev->sd);
+}
+
+int atomisp_file_input_register_entities(struct atomisp_file_device *file_dev,
+			struct v4l2_device *vdev)
+{
+	/* Register the subdev and video nodes. */
+	return  v4l2_device_register_subdev(vdev, &file_dev->sd);
+}
+
+void atomisp_file_input_cleanup(struct atomisp_device *isp)
+{
+	struct atomisp_file_device *file_dev = &isp->file_dev;
+
+	if (file_dev->work_queue) {
+		destroy_workqueue(file_dev->work_queue);
+		file_dev->work_queue = NULL;
+	}
+}
+
+int atomisp_file_input_init(struct atomisp_device *isp)
+{
+	struct atomisp_file_device *file_dev = &isp->file_dev;
+	struct v4l2_subdev *sd = &file_dev->sd;
+	struct media_pad *pads = file_dev->pads;
+	struct media_entity *me = &sd->entity;
+
+	file_dev->isp = isp;
+	file_dev->work_queue = alloc_workqueue(isp->v4l2_dev.name, 0, 1);
+	if (file_dev->work_queue == NULL) {
+		dev_err(isp->dev, "Failed to initialize file inject workq\n");
+		return -ENOMEM;
+	}
+
+	INIT_WORK(&file_dev->work, file_work);
+
+	v4l2_subdev_init(sd, &file_input_ops);
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	strcpy(sd->name, "file_input_subdev");
+	v4l2_set_subdevdata(sd, file_dev);
+
+	pads[0].flags = MEDIA_PAD_FL_SINK;
+	me->function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+
+	return media_entity_pads_init(me, 1, pads);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.h
new file mode 100644
index 000000000000..1b86abd35c38
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_file.h
@@ -0,0 +1,47 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __ATOMISP_FILE_H__
+#define __ATOMISP_FILE_H__
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+struct atomisp_device;
+
+struct atomisp_file_device {
+	struct v4l2_subdev sd;
+	struct atomisp_device *isp;
+	struct media_pad pads[1];
+
+	struct workqueue_struct *work_queue;
+	struct work_struct work;
+};
+
+void atomisp_file_input_cleanup(struct atomisp_device *isp);
+int atomisp_file_input_init(struct atomisp_device *isp);
+void atomisp_file_input_unregister_entities(
+				struct atomisp_file_device *file_dev);
+int atomisp_file_input_register_entities(struct atomisp_file_device *file_dev,
+			struct v4l2_device *vdev);
+#endif /* __ATOMISP_FILE_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c
new file mode 100644
index 000000000000..7ce8803cf6f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.c
@@ -0,0 +1,1304 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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 "hrt/hive_isp_css_mm_hrt.h"
+
+#include "type_support.h"
+#include "device_access/device_access.h"
+#include "memory_access/memory_access.h"
+
+#include "atomisp_acc.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 atomisp_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;
+}
+
+int atomisp_q_one_s3a_buffer(struct atomisp_sub_device *asd,
+				enum atomisp_input_stream_id stream_id,
+				enum atomisp_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;
+}
+
+int atomisp_q_one_dis_buffer(struct atomisp_sub_device *asd,
+				enum atomisp_input_stream_id stream_id,
+				enum atomisp_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 atomisp_css_buffer_type css_buf_type,
+			     enum atomisp_css_pipe_id css_pipe_id)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct atomisp_css_params_with_list *param;
+	struct atomisp_css_dvs_grid_info *dvs_grid =
+		 atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
+	unsigned long irqflags;
+	int err = 0;
+
+	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)
+					atomisp_css_set_dz_config(asd,
+						&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 atomisp_css_dvs_6axis *)
+				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 inculde
+			 * 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 == 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 == 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 == CSS_PIPE_ID_VIDEO &&
+			css_buf_type == 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 atomisp_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 CSS_BUFFER_TYPE_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
+				return CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME;
+			else
+				return 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 CSS_BUFFER_TYPE_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
+				return CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME;
+			else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO)
+				return CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME;
+			else
+				return 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 CSS_BUFFER_TYPE_OUTPUT_FRAME;
+		else
+			return CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+	}
+
+	if (pipe_id == 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 CSS_BUFFER_TYPE_OUTPUT_FRAME;
+	else
+		return CSS_BUFFER_TYPE_VF_OUTPUT_FRAME;
+}
+
+static int atomisp_qbuffers_to_css_for_all_pipes(struct atomisp_sub_device *asd)
+{
+	enum atomisp_css_buffer_type buf_type;
+	enum atomisp_css_pipe_id css_capture_pipe_id = CSS_PIPE_ID_COPY;
+	enum atomisp_css_pipe_id css_preview_pipe_id = CSS_PIPE_ID_COPY;
+	enum atomisp_css_pipe_id css_video_pipe_id = CSS_PIPE_ID_COPY;
+	enum atomisp_input_stream_id input_stream_id;
+	struct atomisp_video_pipe *capture_pipe;
+	struct atomisp_video_pipe *preview_pipe;
+	struct atomisp_video_pipe *video_pipe;
+
+	capture_pipe = &asd->video_out_capture;
+	preview_pipe = &asd->video_out_preview;
+	video_pipe = &asd->video_out_video_capture;
+
+	buf_type = atomisp_get_css_buf_type(
+			asd, css_preview_pipe_id,
+			atomisp_subdev_source_pad(&preview_pipe->vdev));
+	input_stream_id = ATOMISP_INPUT_STREAM_PREVIEW;
+	atomisp_q_video_buffers_to_css(asd, preview_pipe,
+				       input_stream_id,
+				       buf_type, css_preview_pipe_id);
+
+	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;
+	atomisp_q_video_buffers_to_css(asd, capture_pipe,
+					       input_stream_id,
+					       buf_type, css_capture_pipe_id);
+
+	buf_type = atomisp_get_css_buf_type(asd, css_video_pipe_id,
+			atomisp_subdev_source_pad(&video_pipe->vdev));
+	input_stream_id = ATOMISP_INPUT_STREAM_VIDEO;
+	atomisp_q_video_buffers_to_css(asd, video_pipe,
+					       input_stream_id,
+					       buf_type, css_video_pipe_id);
+	return 0;
+}
+
+
+/* queue all available buffers to css */
+int atomisp_qbuffers_to_css(struct atomisp_sub_device *asd)
+{
+	enum atomisp_css_buffer_type buf_type;
+	enum atomisp_css_pipe_id css_capture_pipe_id = CSS_PIPE_ID_NUM;
+	enum atomisp_css_pipe_id css_preview_pipe_id = CSS_PIPE_ID_NUM;
+	enum atomisp_css_pipe_id css_video_pipe_id = 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->isp->inputs[asd->input_curr].camera_caps->
+	    sensor[asd->sensor_curr].stream_num == 2 &&
+	    !asd->yuvpp_mode)
+		return atomisp_qbuffers_to_css_for_all_pipes(asd);
+
+	if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
+		video_pipe = &asd->video_out_video_capture;
+		css_video_pipe_id = CSS_PIPE_ID_VIDEO;
+	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
+		preview_pipe = &asd->video_out_capture;
+		css_preview_pipe_id = 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 = CSS_PIPE_ID_CAPTURE;
+		}
+		video_pipe = &asd->video_out_video_capture;
+		preview_pipe = &asd->video_out_preview;
+		css_video_pipe_id = CSS_PIPE_ID_VIDEO;
+		css_preview_pipe_id = 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 = CSS_PIPE_ID_PREVIEW;
+		css_capture_pipe_id = CSS_PIPE_ID_CAPTURE;
+	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
+		preview_pipe = &asd->video_out_preview;
+		css_preview_pipe_id = 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 = CSS_PIPE_ID_CAPTURE;
+	}
+
+#ifdef ISP2401_NEW_INPUT_SYSTEM
+	if (asd->copy_mode) {
+		css_capture_pipe_id = CSS_PIPE_ID_COPY;
+		css_preview_pipe_id = CSS_PIPE_ID_COPY;
+		css_video_pipe_id = CSS_PIPE_ID_COPY;
+	}
+#endif
+
+	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 = CSS_PIPE_ID_COPY;
+		css_video_pipe_id   = CSS_PIPE_ID_YUVPP;
+		css_preview_pipe_id = 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 = 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 = 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 == CSS_PIPE_ID_YUVPP)
+			input_stream_id = ATOMISP_INPUT_STREAM_GENERAL;
+		 /* else for ext isp use case */
+		else if (css_preview_pipe_id == 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 = 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 = 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 int atomisp_buf_setup_output(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_output(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 void atomisp_buf_queue_output(struct videobuf_queue *vq,
+				     struct videobuf_buffer *vb)
+{
+	struct atomisp_video_pipe *pipe = vq->priv_data;
+
+	list_add_tail(&vb->queue, &pipe->activeq_out);
+	vb->state = VIDEOBUF_QUEUED;
+}
+
+static void atomisp_buf_release_output(struct videobuf_queue *vq,
+				       struct videobuf_buffer *vb)
+{
+	videobuf_vmalloc_free(vb);
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static 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 struct videobuf_queue_ops videobuf_qops_output = {
+	.buf_setup	= atomisp_buf_setup_output,
+	.buf_prepare	= atomisp_buf_prepare_output,
+	.buf_queue	= atomisp_buf_queue_output,
+	.buf_release	= atomisp_buf_release_output,
+};
+
+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 */
+
+	videobuf_queue_vmalloc_init(&pipe->outq, &videobuf_qops_output, NULL,
+				    &pipe->irq_lock,
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT,
+				    V4L2_FIELD_NONE,
+				    sizeof(struct atomisp_buffer), pipe,
+				    NULL);	/* ext_lock: NULL */
+
+	INIT_LIST_HEAD(&pipe->activeq);
+	INIT_LIST_HEAD(&pipe->activeq_out);
+	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->sw_contex.file_input = 0;
+	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 = 1;
+	asd->params.gdc_cac_en = 0;
+	asd->params.video_dis_en = 0;
+	asd->params.sc_en = 0;
+	asd->params.fpn_en = 0;
+	asd->params.xnr_en = 0;
+	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 = 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
+ */
+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 +
+	       asd->video_acc.users +
+	       asd->video_in.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 = NULL;
+	struct atomisp_acc_pipe *acc_pipe = NULL;
+	struct atomisp_sub_device *asd;
+	bool acc_node = false;
+	int ret;
+
+	dev_dbg(isp->dev, "open device %s\n", vdev->name);
+
+	rt_mutex_lock(&isp->mutex);
+
+	acc_node = !strncmp(vdev->name, "ATOMISP ISP ACC",
+			sizeof(vdev->name));
+	if (acc_node) {
+		acc_pipe = atomisp_to_acc_pipe(vdev);
+		asd = acc_pipe->asd;
+	} else {
+		pipe = atomisp_to_video_pipe(vdev);
+		asd = pipe->asd;
+	}
+	asd->subdev.devnode = vdev;
+	/* Deferred firmware loading case. */
+	if (isp->css_env.isp_css_fw.bytes == 0) {
+		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 (acc_node && acc_pipe->users) {
+		dev_dbg(isp->dev, "acc node already opened\n");
+		rt_mutex_unlock(&isp->mutex);
+		return -EBUSY;
+	} else if (acc_node) {
+		goto dev_init;
+	}
+
+	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");
+		rt_mutex_unlock(&isp->mutex);
+		return -EBUSY;
+	}
+
+	ret = atomisp_init_pipe(pipe);
+	if (ret)
+		goto error;
+
+dev_init:
+	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_get_sync(vdev->v4l2_dev->dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "Failed to power on device\n");
+		goto error;
+	}
+
+	if (dypool_enable) {
+		ret = hmm_pool_register(dypool_pgnr, HMM_POOL_TYPE_DYNAMIC);
+		if (ret)
+			dev_err(isp->dev, "Failed to register dynamic memory pool.\n");
+	}
+
+	/* 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);
+
+done:
+
+	if (acc_node)
+		acc_pipe->users++;
+	else
+		pipe->users++;
+	rt_mutex_unlock(&isp->mutex);
+	return 0;
+
+css_error:
+	atomisp_css_uninit(isp);
+error:
+	hmm_pool_unregister(HMM_POOL_TYPE_DYNAMIC);
+	pm_runtime_put(vdev->v4l2_dev->dev);
+	rt_mutex_unlock(&isp->mutex);
+	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;
+	struct atomisp_acc_pipe *acc_pipe;
+	struct atomisp_sub_device *asd;
+	bool acc_node;
+	struct v4l2_requestbuffers req;
+	struct v4l2_subdev_fh fh;
+	struct v4l2_rect clear_compose = {0};
+	int ret = 0;
+
+	v4l2_fh_init(&fh.vfh, vdev);
+
+	req.count = 0;
+	if (isp == NULL)
+		return -EBADF;
+
+	mutex_lock(&isp->streamoff_mutex);
+	rt_mutex_lock(&isp->mutex);
+
+	dev_dbg(isp->dev, "release device %s\n", vdev->name);
+	acc_node = !strncmp(vdev->name, "ATOMISP ISP ACC",
+			sizeof(vdev->name));
+	if (acc_node) {
+		acc_pipe = atomisp_to_acc_pipe(vdev);
+		asd = acc_pipe->asd;
+	} else {
+		pipe = atomisp_to_video_pipe(vdev);
+		asd = pipe->asd;
+	}
+	asd->subdev.devnode = vdev;
+	if (acc_node) {
+		acc_pipe->users--;
+		goto subdev_uninit;
+	}
+	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;
+	}
+
+	if (pipe->outq.bufs[0]) {
+		mutex_lock(&pipe->outq.vb_lock);
+		videobuf_queue_cancel(&pipe->outq);
+		mutex_unlock(&pipe->outq.vb_lock);
+	}
+
+	/*
+	 * 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 (!isp->sw_contex.file_input && asd->fmt_auto->val) {
+		struct v4l2_mbus_framefmt isp_sink_fmt = { 0 };
+		atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
+					V4L2_SUBDEV_FORMAT_ACTIVE,
+					ATOMISP_SUBDEV_PAD_SINK, &isp_sink_fmt);
+	}
+subdev_uninit:
+	if (atomisp_subdev_users(asd))
+		goto done;
+
+	/* clear the sink pad for file input */
+	if (isp->sw_contex.file_input && asd->fmt_auto->val) {
+		struct v4l2_mbus_framefmt isp_sink_fmt = { 0 };
+		atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
+					V4L2_SUBDEV_FORMAT_ACTIVE,
+					ATOMISP_SUBDEV_PAD_SINK, &isp_sink_fmt);
+	}
+
+	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;
+	asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+
+	if (atomisp_dev_users(isp))
+		goto done;
+
+	atomisp_acc_release(asd);
+
+	atomisp_destroy_pipes_stream_force(asd);
+	atomisp_css_uninit(isp);
+
+	if (defer_fw_load) {
+		atomisp_css_unload_firmware(isp);
+		isp->css_env.isp_css_fw.data = NULL;
+		isp->css_env.isp_css_fw.bytes = 0;
+	}
+
+	hmm_pool_unregister(HMM_POOL_TYPE_DYNAMIC);
+
+	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:
+	if (!acc_node) {
+		atomisp_subdev_set_selection(&asd->subdev, fh.pad,
+				V4L2_SUBDEV_FORMAT_ACTIVE,
+				atomisp_subdev_source_pad(vdev),
+				V4L2_SEL_TGT_COMPOSE, 0,
+				&clear_compose);
+	}
+	rt_mutex_unlock(&isp->mutex);
+	mutex_unlock(&isp->streamoff_mutex);
+
+	return 0;
+}
+
+/*
+ * 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 atomisp_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;
+	atomisp_get_frame_pgnr(isp, frame, &pgnr);
+
+	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 == NULL)
+			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 atomisp_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(width*sizeof(load), GFP_KERNEL);
+	if (!buffer) {
+		dev_err(isp->dev, "out of memory.\n");
+		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 atomisp_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 (!(vma->vm_flags & (VM_WRITE | VM_READ)))
+		return -EACCES;
+
+	rt_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);
+		rt_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 == NULL) {
+			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;
+		rt_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;
+	}
+	rt_mutex_unlock(&isp->mutex);
+
+	return atomisp_videobuf_mmap_mapper(&pipe->capq, vma);
+
+error:
+	rt_mutex_unlock(&isp->mutex);
+
+	return ret;
+}
+
+static int atomisp_file_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
+
+	return videobuf_mmap_mapper(&pipe->outq, vma);
+}
+
+static unsigned int 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);
+
+	rt_mutex_lock(&isp->mutex);
+	if (pipe->capq.streaming != 1) {
+		rt_mutex_unlock(&isp->mutex);
+		return POLLERR;
+	}
+	rt_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
+	.compat_ioctl32 = atomisp_compat_ioctl32,
+#endif
+	.poll = atomisp_poll,
+};
+
+const struct v4l2_file_operations atomisp_file_fops = {
+	.owner = THIS_MODULE,
+	.open = atomisp_open,
+	.release = atomisp_release,
+	.mmap = atomisp_file_mmap,
+	.unlocked_ioctl = video_ioctl2,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl32 = atomisp_compat_ioctl32,
+#endif
+	.poll = atomisp_poll,
+};
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.h
new file mode 100644
index 000000000000..8471e391501a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_fops.h
@@ -0,0 +1,54 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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 atomisp_css_buffer_type css_buf_type,
+			     enum atomisp_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/atomisp2/atomisp_helper.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_helper.h
new file mode 100644
index 000000000000..e9650cb75ba5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_helper.h
@@ -0,0 +1,33 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef _atomisp_helper_h_
+#define _atomisp_helper_h_
+extern void __iomem *atomisp_io_base;
+
+static inline void __iomem *atomisp_get_io_virt_addr(unsigned int address)
+{
+	void __iomem *ret = atomisp_io_base + (address & 0x003FFFFF);
+	return ret;
+}
+#endif
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_internal.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_internal.h
new file mode 100644
index 000000000000..d3667132851b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_internal.h
@@ -0,0 +1,331 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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 <asm/intel-mid.h>
+#include "../../include/asm/intel_mid_pcihelpers.h"
+
+#include <media/media-device.h>
+#include <media/v4l2-subdev.h>
+
+#ifndef ISP2401
+#include "ia_css_types.h"
+#include "sh_css_legacy.h"
+#else
+/*#include "ia_css_types.h"*/
+/*#include "sh_css_legacy.h"*/
+#endif
+
+#include "atomisp_csi2.h"
+#include "atomisp_file.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 ATOMISP_MAJOR		0
+#define ATOMISP_MINOR		5
+#define ATOMISP_PATCHLEVEL	1
+
+#define DRIVER_VERSION_STR	__stringify(ATOMISP_MAJOR) \
+	"." __stringify(ATOMISP_MINOR) "." __stringify(ATOMISP_PATCHLEVEL)
+#define DRIVER_VERSION		KERNEL_VERSION(ATOMISP_MAJOR, \
+	ATOMISP_MINOR, ATOMISP_PATCHLEVEL)
+
+#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	4
+
+#define ATOMISP_SC_TYPE_SIZE	2
+
+#define ATOMISP_ISP_TIMEOUT_DURATION		(2 * HZ)
+#define ATOMISP_EXT_ISP_TIMEOUT_DURATION        (6 * HZ)
+#define ATOMISP_ISP_FILE_TIMEOUT_DURATION	(60 * 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 ATOMISP_CALC_CSS_PREV_OVERLAP(lines) \
+	((lines) * 38 / 100 & 0xfffffe)
+
+/*
+ * 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 \
+	&& asd->isp->inputs[asd->input_curr].camera_caps-> \
+	   sensor[asd->sensor_curr].stream_num == 1)
+
+#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
+
+#ifdef 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)
+
+#endif
+#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;
+
+	const struct atomisp_camera_caps *camera_caps;
+	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 {
+	bool file_input;
+	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 pci_dev *pdev;
+	struct device *dev;
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct atomisp_platform_data *pdata;
+	void *mmu_l1_base;
+	struct pci_dev *pci_root;
+	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 assiged 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;
+	struct atomisp_file_device file_dev;
+
+	/* Purpose of mutex is to protect and serialize use of isp data
+	 * structures and css API calls. */
+	struct rt_mutex mutex;
+	/*
+	 * Serialise streamoff: mutex is dropped during streamoff to
+	 * cancel the watchdog queue. MUST be acquired BEFORE
+	 * "mutex".
+	 */
+	struct mutex streamoff_mutex;
+
+	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 workqueue_struct *wdt_work_queue;
+	struct work_struct wdt_work;
+#ifndef ISP2401
+	atomic_t wdt_count;
+#endif
+	atomic_t wdt_work_queued;
+
+	spinlock_t lock; /* Just for streaming below */
+
+	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;
+
+extern void *atomisp_kernel_malloc(size_t bytes);
+
+extern void atomisp_kernel_free(void *ptr);
+
+#define atomisp_is_wdt_running(a) timer_pending(&(a)->wdt)
+#ifdef ISP2401
+extern void atomisp_wdt_refresh_pipe(struct atomisp_video_pipe *pipe,
+					unsigned int delay);
+#endif
+extern void atomisp_wdt_refresh(struct atomisp_sub_device *asd, unsigned int delay);
+#ifndef ISP2401
+extern void atomisp_wdt_start(struct atomisp_sub_device *asd);
+#else
+extern void atomisp_wdt_start(struct atomisp_video_pipe *pipe);
+extern void atomisp_wdt_stop_pipe(struct atomisp_video_pipe *pipe, bool sync);
+#endif
+extern void atomisp_wdt_stop(struct atomisp_sub_device *asd, bool sync);
+
+#endif /* __ATOMISP_INTERNAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c
new file mode 100644
index 000000000000..6064bb823a47
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.c
@@ -0,0 +1,3130 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/pci.h>
+
+#include <asm/intel-mid.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf-vmalloc.h>
+
+#include "atomisp_acc.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"
+
+#include "hrt/hive_isp_css_mm_hrt.h"
+
+/* for v4l2_capability */
+static const char *DRIVER = "atomisp";	/* max size 15 */
+static const char *CARD = "ATOM ISP";	/* max size 31 */
+static const char *BUS_INFO = "PCI-3";	/* max size 31 */
+static const u32 VERSION = DRIVER_VERSION;
+
+/*
+ * 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 = 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 = 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 = 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 = CSS_FRAME_FORMAT_YUV444,
+		.description = "YUV444"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_NV12,
+		.depth = 12,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_NV12,
+		.sh_fmt = 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 = 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 = 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 = CSS_FRAME_FORMAT_YUYV,
+		.description = "YUYV, interleaved"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.sh_fmt = 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 = CSS_FRAME_FORMAT_UYVY,
+		.description = "UYVY, interleaved"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR16,
+		.depth = 16,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_SBGGR16,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 16"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB8,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 8"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB10,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 10"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SBGGR12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGBRG12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SGRBG12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_SRGGB12,
+		.depth = 16,
+		.mbus_code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.sh_fmt = CSS_FRAME_FORMAT_RAW,
+		.description = "Bayer 12"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_RGB32,
+		.depth = 32,
+		.mbus_code = V4L2_MBUS_FMT_CUSTOM_RGB32,
+		.sh_fmt = 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 = CSS_FRAME_FORMAT_RGB565,
+		.description = "16 RGB 5-6-5"
+	}, {
+		.pixelformat = V4L2_PIX_FMT_JPEG,
+		.depth = 8,
+		.mbus_code = MEDIA_BUS_FMT_JPEG_1X8,
+		.sh_fmt = 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 = CSS_FRAME_FORMAT_BINARY_8,
+		.description = "Custom RAW for M10MO"
+	},
+};
+
+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;
+}
+
+/*
+ * v4l2 ioctls
+ * return ISP capabilities
+ *
+ * FIXME: capabilities should be different for video0/video2/video3
+ */
+static int atomisp_querycap(struct file *file, void *fh,
+			    struct v4l2_capability *cap)
+{
+	memset(cap, 0, sizeof(struct v4l2_capability));
+
+	WARN_ON(sizeof(DRIVER) > sizeof(cap->driver) ||
+		sizeof(CARD) > sizeof(cap->card) ||
+		sizeof(BUS_INFO) > sizeof(cap->bus_info));
+
+	strncpy(cap->driver, DRIVER, sizeof(cap->driver) - 1);
+	strncpy(cap->card, CARD, sizeof(cap->card) - 1);
+	strncpy(cap->bus_info, BUS_INFO, sizeof(cap->card) - 1);
+
+	cap->version = VERSION;
+
+	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE |
+	    V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_OUTPUT;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	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;
+
+	if (index >= isp->input_cnt)
+		return -EINVAL;
+
+	if (!isp->inputs[index].camera)
+		return -EINVAL;
+
+	memset(input, 0, sizeof(struct v4l2_input));
+	strncpy(input->name, isp->inputs[index].camera->name,
+		sizeof(input->name) - 1);
+
+	/*
+	 * 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.
+	 */
+#ifndef ISP2401
+	if (isp->inputs[index].motor &&
+	    strlen(isp->inputs[index].motor->name) > 0) {
+#else
+	if (isp->motor &&
+	    strlen(isp->motor->name) > 0) {
+#endif
+		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] = '+';
+			strncpy(&input->name[cur_len + 1],
+#ifndef ISP2401
+				isp->inputs[index].motor->name, max_size - 1);
+#else
+				isp->motor->name, max_size - 1);
+#endif
+		}
+	}
+
+	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
+		+ asd->video_in.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;
+}
+
+unsigned int atomisp_is_acc_enabled(struct atomisp_device *isp)
+{
+	unsigned int i;
+
+	for (i = 0; i < isp->num_of_streams; i++)
+		if (isp->asd[i].acc.pipeline)
+			return 1;
+
+	return 0;
+}
+/*
+ * 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_device *isp = video_get_drvdata(vdev);
+	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+
+	rt_mutex_lock(&isp->mutex);
+	*input = asd->input_curr;
+	rt_mutex_unlock(&isp->mutex);
+
+	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_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
+	struct v4l2_subdev *camera = NULL;
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	if (input >= ATOM_ISP_MAX_INPUTS || input > isp->input_cnt) {
+		dev_dbg(isp->dev, "input_cnt: %d\n", isp->input_cnt);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * 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 != NULL && isp->inputs[input].asd != asd) {
+		dev_err(isp->dev,
+			 "%s, camera is already used by stream: %d\n", __func__,
+			 isp->inputs[input].asd->index);
+		ret = -EBUSY;
+		goto error;
+	}
+
+	camera = isp->inputs[input].camera;
+	if (!camera) {
+		dev_err(isp->dev, "%s, no camera\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if (atomisp_subdev_streaming_count(asd)) {
+		dev_err(isp->dev,
+			 "ISP is still streaming, stop first\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* 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");
+		goto error;
+	}
+	/*
+	 * 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");
+		goto error;
+	}
+
+#ifndef ISP2401
+	if (!isp->sw_contex.file_input && isp->inputs[input].motor)
+		ret = v4l2_subdev_call(isp->inputs[input].motor, core,
+				       init, 1);
+#else
+	if (isp->motor)
+		ret = v4l2_subdev_call(isp->motor, core, s_power, 1);
+
+	if (!isp->sw_contex.file_input && isp->motor)
+		ret = v4l2_subdev_call(isp->motor, core, init, 1);
+#endif
+
+	asd->input_curr = input;
+	/* mark this camera is used by the current stream */
+	isp->inputs[input].asd = asd;
+	rt_mutex_unlock(&isp->mutex);
+
+	return 0;
+
+error:
+	rt_mutex_unlock(&isp->mutex);
+
+	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 = { 0 };
+	unsigned int i, fi = 0;
+	int rval;
+
+	rt_mutex_lock(&isp->mutex);
+	rval = v4l2_subdev_call(isp->inputs[asd->input_curr].camera, pad,
+				enum_mbus_code, NULL, &code);
+	if (rval == -ENOIOCTLCMD) {
+		dev_warn(isp->dev, "enum_mbus_code pad op not supported. Please fix your sensor driver!\n");
+	//	rval = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+	//				video, enum_mbus_fmt, 0, &code.code);
+	}
+	rt_mutex_unlock(&isp->mutex);
+
+	if (rval)
+		return rval;
+
+	for (i = 0; i < ARRAY_SIZE(atomisp_output_fmts); i++) {
+		const struct atomisp_format_bridge *format =
+			&atomisp_output_fmts[i];
+
+		/*
+		 * Is the atomisp-supported format is valid for the
+		 * sensor (configuration)? If not, skip it.
+		 */
+		if (format->sh_fmt == CSS_FRAME_FORMAT_RAW
+		    && format->mbus_code != code.code)
+			continue;
+
+		/* Found a match. Now let's pick f->index'th one. */
+		if (fi < f->index) {
+			fi++;
+			continue;
+		}
+
+		strlcpy(f->description, format->description,
+			sizeof(f->description));
+		f->pixelformat = format->pixelformat;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int atomisp_g_fmt_cap(struct file *file, void *fh,
+	struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	ret = atomisp_get_fmt(vdev, f);
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+static int atomisp_g_fmt_file(struct file *file, void *fh,
+		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);
+
+	rt_mutex_lock(&isp->mutex);
+	f->fmt.pix = pipe->pix;
+	rt_mutex_unlock(&isp->mutex);
+
+	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);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	ret = atomisp_try_fmt(vdev, f, NULL);
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+static int atomisp_s_fmt_cap(struct file *file, void *fh,
+	struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	if (isp->isp_fatal_error) {
+		ret = -EIO;
+		rt_mutex_unlock(&isp->mutex);
+		return ret;
+	}
+	ret = atomisp_set_fmt(vdev, f);
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+static int atomisp_s_fmt_file(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	ret = atomisp_set_fmt_file(vdev, f);
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+/*
+ * Free videobuffer buffer priv data
+ */
+void atomisp_videobuf_free_buf(struct videobuf_buffer *vb)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+
+	if (vb == NULL)
+		return;
+
+	vm_mem = vb->priv;
+	if (vm_mem && vm_mem->vaddr) {
+		atomisp_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 atomisp_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) {
+				dev_err(isp->dev, "s3a stat buf alloc failed\n");
+				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) {
+				dev_err(isp->dev, "dis stat buf alloc failed\n");
+				kfree(s3a_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) {
+					dev_err(isp->dev, "metadata buf alloc failed\n");
+					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 atomisp_css_frame_info frame_info;
+	struct atomisp_css_frame *frame;
+	struct videobuf_vmalloc_memory *vm_mem;
+	uint16_t source_pad = atomisp_subdev_source_pad(vdev);
+	uint16_t stream_id = atomisp_source_pad_to_stream_id(asd, source_pad);
+	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, stream_id);
+
+	/*
+	 * for user pointer type, buffers are not really allcated 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 (atomisp_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;
+		atomisp_css_frame_free(vm_mem->vaddr);
+	}
+
+	if (asd->vf_frame)
+		atomisp_css_frame_free(asd->vf_frame);
+
+	return -ENOMEM;
+}
+
+int atomisp_reqbufs(struct file *file, void *fh,
+	struct v4l2_requestbuffers *req)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	ret = __atomisp_reqbufs(file, fh, req);
+	rt_mutex_unlock(&isp->mutex);
+
+	return ret;
+}
+
+static int atomisp_reqbufs_file(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);
+
+	if (req->count == 0) {
+		mutex_lock(&pipe->outq.vb_lock);
+		atomisp_videobuf_free_queue(&pipe->outq);
+		mutex_unlock(&pipe->outq.vb_lock);
+		return 0;
+	}
+
+	return videobuf_reqbufs(&pipe->outq, req);
+}
+
+/* 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);
+}
+
+static int atomisp_querybuf_file(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->outq, 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 atomisp_css_frame_info frame_info;
+	struct atomisp_css_frame *handle = NULL;
+	u32 length;
+	u32 pgnr;
+	int ret = 0;
+
+	rt_mutex_lock(&isp->mutex);
+	if (isp->isp_fatal_error) {
+		ret = -EIO;
+		goto error;
+	}
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_STOPPING) {
+		dev_err(isp->dev, "%s: reject, as ISP at stopping.\n",
+				__func__);
+		ret = -EIO;
+		goto error;
+	}
+
+	if (!buf || buf->index >= VIDEO_MAX_FRAME ||
+		!pipe->capq.bufs[buf->index]) {
+		dev_err(isp->dev, "Invalid index for qbuf.\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * For userptr type frame, we convert user space address to physic
+	 * address and reprograme out page table properly
+	 */
+	if (buf->memory == V4L2_MEMORY_USERPTR) {
+		struct hrt_userbuffer_attr attributes;
+		vb = pipe->capq.bufs[buf->index];
+		vm_mem = vb->priv;
+		if (!vm_mem) {
+			ret = -EINVAL;
+			goto error;
+		}
+
+		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)) {
+			ret = -EIO;
+			goto error;
+		}
+
+		attributes.pgnr = pgnr;
+#ifdef CONFIG_ION
+#ifndef ISP2401
+		attributes.type = buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_ION
+					? HRT_USR_ION : HRT_USR_PTR;
+#else
+		if (buf->reserved & ATOMISP_BUFFER_TYPE_IS_ION) {
+			attributes.type = HRT_USR_ION;
+			if (asd->ion_dev_fd->val !=  ION_FD_UNSET) {
+				dev_dbg(isp->dev, "ION buffer queued, share_fd=%lddev_fd=%d.\n",
+				buf->m.userptr, asd->ion_dev_fd->val);
+				/*
+				 * Make sure the shared fd we just got
+				 * from user space isn't larger than
+				 * the space we have for it.
+				 */
+				if ((buf->m.userptr &
+				(ATOMISP_ION_DEVICE_FD_MASK)) != 0) {
+					dev_err(isp->dev,
+							"Error: v4l2 buffer fd:0X%0lX > 0XFFFF.\n",
+							buf->m.userptr);
+					ret = -EINVAL;
+					goto error;
+				}
+				buf->m.userptr |= asd->ion_dev_fd->val <<
+					ATOMISP_ION_DEVICE_FD_OFFSET;
+			} else {
+				dev_err(isp->dev, "v4l2 buffer type is ION, \
+						but no dev fd set from userspace.\n");
+				ret = -EINVAL;
+				goto error;
+			}
+		} else {
+			attributes.type = HRT_USR_PTR;
+		}
+#endif
+#else
+		attributes.type = HRT_USR_PTR;
+#endif
+		ret = atomisp_css_frame_map(&handle, &frame_info,
+				       (void *)buf->m.userptr,
+				       0, &attributes);
+		if (ret) {
+			dev_err(isp->dev, "Failed to map user buffer\n");
+			goto error;
+		}
+
+		if (vm_mem->vaddr) {
+			mutex_lock(&pipe->capq.vb_lock);
+			atomisp_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;
+	}
+
+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;
+
+	rt_mutex_unlock(&isp->mutex);
+
+	ret = videobuf_qbuf(&pipe->capq, buf);
+	rt_mutex_lock(&isp->mutex);
+	if (ret)
+		goto error;
+
+	/* 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);
+
+#ifndef ISP2401
+			if (!atomisp_is_wdt_running(asd) && atomisp_buffers_queued(asd))
+				atomisp_wdt_start(asd);
+#else
+			if (!atomisp_is_wdt_running(pipe) &&
+				atomisp_buffers_queued_pipe(pipe))
+				atomisp_wdt_start(pipe);
+#endif
+		}
+	}
+
+	/* 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) {
+#ifndef ISP2401
+		asd->pending_capture_request++;
+		dev_dbg(isp->dev, "Add one pending capture request.\n");
+#else
+	    if (asd->re_trigger_capture) {
+			ret = atomisp_css_offline_capture_configure(asd,
+				asd->params.offline_parm.num_captures,
+				asd->params.offline_parm.skip_frames,
+				asd->params.offline_parm.offset);
+			asd->re_trigger_capture = false;
+			dev_dbg(isp->dev, "%s Trigger capture again ret=%d\n",
+				__func__, ret);
+
+	    } else {
+			asd->pending_capture_request++;
+			asd->re_trigger_capture = false;
+			dev_dbg(isp->dev, "Add one pending capture request.\n");
+	    }
+#endif
+	}
+	rt_mutex_unlock(&isp->mutex);
+
+	dev_dbg(isp->dev, "qbuf buffer %d (%s) for asd%d\n", buf->index,
+		vdev->name, asd->index);
+
+	return ret;
+
+error:
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+static int atomisp_qbuf_file(struct file *file, void *fh,
+					struct v4l2_buffer *buf)
+{
+	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);
+	int ret;
+
+	rt_mutex_lock(&isp->mutex);
+	if (isp->isp_fatal_error) {
+		ret = -EIO;
+		goto error;
+	}
+
+	if (!buf || buf->index >= VIDEO_MAX_FRAME ||
+		!pipe->outq.bufs[buf->index]) {
+		dev_err(isp->dev, "Invalid index for qbuf.\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if (buf->memory != V4L2_MEMORY_MMAP) {
+		dev_err(isp->dev, "Unsupported memory method\n");
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		dev_err(isp->dev, "Unsupported buffer type\n");
+		ret = -EINVAL;
+		goto error;
+	}
+	rt_mutex_unlock(&isp->mutex);
+
+	return videobuf_qbuf(&pipe->outq, buf);
+
+error:
+	rt_mutex_unlock(&isp->mutex);
+
+	return ret;
+}
+
+static int __get_frame_exp_id(struct atomisp_video_pipe *pipe,
+		struct v4l2_buffer *buf)
+{
+	struct videobuf_vmalloc_memory *vm_mem;
+	struct atomisp_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 = 0;
+
+	rt_mutex_lock(&isp->mutex);
+
+	if (isp->isp_fatal_error) {
+		rt_mutex_unlock(&isp->mutex);
+		return -EIO;
+	}
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_STOPPING) {
+		rt_mutex_unlock(&isp->mutex);
+		dev_err(isp->dev, "%s: reject, as ISP at stopping.\n",
+				__func__);
+		return -EIO;
+	}
+
+	rt_mutex_unlock(&isp->mutex);
+
+	ret = videobuf_dqbuf(&pipe->capq, buf, file->f_flags & O_NONBLOCK);
+	if (ret) {
+		dev_dbg(isp->dev, "<%s: %d\n", __func__, ret);
+		return ret;
+	}
+	rt_mutex_lock(&isp->mutex);
+	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];
+	rt_mutex_unlock(&isp->mutex);
+
+	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 atomisp_css_pipe_id atomisp_get_css_pipe_id(struct atomisp_sub_device *asd)
+{
+	if (ATOMISP_USE_YUVPP(asd))
+		return CSS_PIPE_ID_YUVPP;
+
+	if (asd->continuous_mode->val) {
+		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
+			return CSS_PIPE_ID_VIDEO;
+		else
+			return 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 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 CSS_PIPE_ID_CAPTURE;
+
+	switch (asd->run_mode->val) {
+	case ATOMISP_RUN_MODE_PREVIEW:
+		return CSS_PIPE_ID_PREVIEW;
+	case ATOMISP_RUN_MODE_VIDEO:
+		return CSS_PIPE_ID_VIDEO;
+	case ATOMISP_RUN_MODE_STILL_CAPTURE:
+		/* fall through */
+	default:
+		return CSS_PIPE_ID_CAPTURE;
+	}
+}
+
+static unsigned int atomisp_sensor_start_stream(struct atomisp_sub_device *asd)
+{
+	struct atomisp_device *isp = asd->isp;
+
+	if (isp->inputs[asd->input_curr].camera_caps->
+	    sensor[asd->sensor_curr].stream_num > 1) {
+		if (asd->high_speed_mode)
+			return 1;
+		else
+			return 2;
+	}
+
+	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 = -1, slave = -1, delay_slave = 0;
+	int i, ret;
+
+	/*
+	 * ISP only support 2 streams now so ignore multiple master/slave
+	 * case to reduce the delay between 2 stream_on calls.
+	 */
+	for (i = 0; i < isp->num_of_streams; i++) {
+		int sensor_index = isp->asd[i].input_curr;
+		if (isp->inputs[sensor_index].camera_caps->
+				sensor[isp->asd[i].sensor_curr].is_slave)
+			slave = sensor_index;
+		else
+			master = sensor_index;
+	}
+
+	if (master == -1 || slave == -1) {
+		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;
+}
+
+/* FIXME! */
+#ifndef ISP2401
+void __wdt_on_master_slave_sensor(struct atomisp_device *isp, unsigned int wdt_duration)
+#else
+void __wdt_on_master_slave_sensor(struct atomisp_video_pipe *pipe,
+				unsigned int wdt_duration, bool enable)
+#endif
+{
+#ifndef ISP2401
+	if (atomisp_buffers_queued(&isp->asd[0]))
+		atomisp_wdt_refresh(&isp->asd[0], wdt_duration);
+	if (atomisp_buffers_queued(&isp->asd[1]))
+		atomisp_wdt_refresh(&isp->asd[1], wdt_duration);
+#else
+	static struct atomisp_video_pipe *pipe0;
+
+	if (enable) {
+		if (atomisp_buffers_queued_pipe(pipe0))
+			atomisp_wdt_refresh_pipe(pipe0, wdt_duration);
+		if (atomisp_buffers_queued_pipe(pipe))
+			atomisp_wdt_refresh_pipe(pipe, wdt_duration);
+	} else {
+		pipe0 = pipe;
+	}
+#endif
+}
+
+static void atomisp_pause_buffer_event(struct atomisp_device *isp)
+{
+	struct v4l2_event event = {0};
+	int i;
+
+	event.type = V4L2_EVENT_ATOMISP_PAUSE_BUFFER;
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		int sensor_index = isp->asd[i].input_curr;
+		if (isp->inputs[sensor_index].camera_caps->
+				sensor[isp->asd[i].sensor_curr].is_slave) {
+			v4l2_event_queue(isp->asd[i].subdev.devnode, &event);
+			break;
+		}
+	}
+}
+
+/* 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_store_uint32(DMA_BURST_SIZE_REG, 0x7F);
+	else
+		atomisp_store_uint32(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);
+	enum atomisp_css_pipe_id css_pipe_id;
+	unsigned int sensor_start_stream;
+	unsigned int wdt_duration = ATOMISP_ISP_TIMEOUT_DURATION;
+	int ret = 0;
+	unsigned long irqflags;
+
+	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;
+	}
+
+	rt_mutex_lock(&isp->mutex);
+	if (isp->isp_fatal_error) {
+		ret = -EIO;
+		goto out;
+	}
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_STOPPING) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	if (pipe->capq.streaming)
+		goto out;
+
+	/* 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");
+		ret = -EINVAL;
+		goto out;
+	}
+	spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
+
+	ret = videobuf_streamon(&pipe->capq);
+	if (ret)
+		goto out;
+
+	/* Reset pending capture request count. */
+	asd->pending_capture_request = 0;
+#ifdef ISP2401
+	asd->re_trigger_capture = false;
+#endif
+
+	if ((atomisp_subdev_streaming_count(asd) > sensor_start_stream) &&
+	    (!isp->inputs[asd->input_curr].camera_caps->multi_stream_ctrl)) {
+		/* 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);
+				rt_mutex_unlock(&isp->mutex);
+				if (wait_for_completion_interruptible(
+						&asd->init_done) != 0)
+					return -ERESTARTSYS;
+				rt_mutex_lock(&isp->mutex);
+			}
+
+			/* 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) {
+					ret = -EINVAL;
+					goto out;
+				}
+				if (asd->depth_mode->val)
+					atomisp_pause_buffer_event(isp);
+			}
+		}
+		atomisp_qbuffers_to_css(asd);
+		goto out;
+	}
+
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
+		atomisp_qbuffers_to_css(asd);
+		goto start_sensor;
+	}
+
+	css_pipe_id = atomisp_get_css_pipe_id(asd);
+
+	ret = atomisp_acc_load_extensions(asd);
+	if (ret < 0) {
+		dev_err(isp->dev, "acc extension failed to load\n");
+		goto out;
+	}
+
+	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)
+			atomisp_css_set_dz_config(asd,
+				&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)
+		goto out;
+
+	asd->streaming = ATOMISP_DEVICE_STREAMING_ENABLED;
+	atomic_set(&asd->sof_count, -1);
+	atomic_set(&asd->sequence, -1);
+	atomic_set(&asd->sequence_temp, -1);
+	if (isp->sw_contex.file_input)
+		wdt_duration = ATOMISP_ISP_FILE_TIMEOUT_DURATION;
+
+	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)
+		goto out;
+
+start_sensor:
+	if (isp->flash) {
+		asd->params.num_flash_frames = 0;
+		asd->params.flash_state = ATOMISP_FLASH_IDLE;
+		atomisp_setup_flash(asd);
+	}
+
+	if (!isp->sw_contex.file_input) {
+		atomisp_css_irq_enable(isp, 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 failed!\n");
+		} else {
+			if (atomisp_freq_scaling(isp,
+				ATOMISP_DFS_MODE_AUTO, false) < 0)
+				dev_dbg(isp->dev, "dfs failed!\n");
+		}
+	} else {
+		if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_MAX, false) < 0)
+			dev_dbg(isp->dev, "dfs 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");
+			goto out;
+		}
+#ifndef ISP2401
+		__wdt_on_master_slave_sensor(isp, wdt_duration);
+#else
+		__wdt_on_master_slave_sensor(pipe, wdt_duration, true);
+#endif
+		goto start_delay_wq;
+	} else if (asd->depth_mode->val && (atomisp_streaming_count(isp) <
+		   ATOMISP_DEPTH_SENSOR_STREAMON_COUNT)) {
+#ifdef ISP2401
+		__wdt_on_master_slave_sensor(pipe, wdt_duration, false);
+#endif
+		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(isp->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) {
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+		ret = -EINVAL;
+		goto out;
+	}
+
+#ifndef ISP2401
+	if (atomisp_buffers_queued(asd))
+		atomisp_wdt_refresh(asd, wdt_duration);
+#else
+	if (atomisp_buffers_queued_pipe(pipe))
+		atomisp_wdt_refresh_pipe(pipe, wdt_duration);
+#endif
+
+start_delay_wq:
+	if (asd->continuous_mode->val) {
+		struct v4l2_mbus_framefmt *sink;
+
+		sink = 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);
+		atomisp_css_set_cont_prev_start_time(isp,
+				ATOMISP_CALC_CSS_PREV_OVERLAP(sink->height));
+	} else {
+		asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
+	}
+out:
+	rt_mutex_unlock(&isp->mutex);
+	return ret;
+}
+
+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 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 atomisp_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);
+
+	BUG_ON(!rt_mutex_is_locked(&isp->mutex));
+	BUG_ON(!mutex_is_locked(&isp->streamoff_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 ||
+	    isp->inputs[asd->input_curr].camera_caps->multi_stream_ctrl) &&
+	    atomisp_subdev_source_pad(vdev) !=
+		ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW &&
+	    atomisp_subdev_source_pad(vdev) !=
+		ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
+
+		if (isp->inputs[asd->input_curr].camera_caps->multi_stream_ctrl) {
+			v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+				video, s_stream, 0);
+		} else 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;
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
+		asd->streaming = ATOMISP_DEVICE_STREAMING_STOPPING;
+		first_streamoff = true;
+	}
+	spin_unlock_irqrestore(&isp->lock, flags);
+
+	if (first_streamoff) {
+		/* if other streams are running, should not disable watch dog */
+		rt_mutex_unlock(&isp->mutex);
+		atomisp_wdt_stop(asd, true);
+
+		/*
+		 * must stop sending pixels into GP_FIFO before stop
+		 * the pipeline.
+		 */
+		if (isp->sw_contex.file_input)
+			v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					video, s_stream, 0);
+
+		rt_mutex_lock(&isp->mutex);
+		atomisp_acc_unload_extensions(asd);
+	}
+
+	spin_lock_irqsave(&isp->lock, flags);
+	if (atomisp_subdev_streaming_count(asd) == 1)
+		asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
+	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);
+
+	if (!isp->sw_contex.file_input)
+		atomisp_css_irq_enable(isp, 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;
+	}
+	if (first_streamoff) {
+		css_pipe_id = atomisp_get_css_pipe_id(asd);
+		ret = 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;
+
+	if (!isp->sw_contex.file_input)
+		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(isp->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(isp->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])
+				atomisp_create_pipes_stream(&isp->asd[i]);
+		}
+		isp->isp_timeout = false;
+	}
+	return ret;
+}
+
+static 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);
+	int rval;
+
+	mutex_lock(&isp->streamoff_mutex);
+	rt_mutex_lock(&isp->mutex);
+	rval = __atomisp_streamoff(file, fh, type);
+	rt_mutex_unlock(&isp->mutex);
+	mutex_unlock(&isp->streamoff_mutex);
+
+	return rval;
+}
+
+/*
+ * 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;
+
+	rt_mutex_lock(&isp->mutex);
+
+	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:
+		rt_mutex_unlock(&isp->mutex);
+		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;
+	}
+
+	rt_mutex_unlock(&isp->mutex);
+	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;
+
+	rt_mutex_lock(&isp->mutex);
+	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:
+		rt_mutex_unlock(&isp->mutex);
+		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;
+	}
+	rt_mutex_unlock(&isp->mutex);
+	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:
+#ifndef ISP2401
+		return v4l2_queryctrl(isp->inputs[asd->input_curr].camera->
+				      ctrl_handler, qc);
+#else
+		if (isp->motor)
+			return v4l2_queryctrl(isp->motor->ctrl_handler, qc);
+		else
+			return v4l2_queryctrl(isp->inputs[asd->input_curr].
+					      camera->ctrl_handler, qc);
+#endif
+	}
+
+	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_control ctrl;
+	int i;
+	int ret = 0;
+
+	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:
+#ifndef ISP2401
+			if (isp->inputs[asd->input_curr].motor)
+#else
+			if (isp->motor)
+#endif
+				ret =
+#ifndef ISP2401
+				    v4l2_g_ctrl(isp->inputs[asd->input_curr].
+						motor->ctrl_handler, &ctrl);
+#else
+				    v4l2_g_ctrl(isp->motor->ctrl_handler,
+						&ctrl);
+#endif
+			else
+				ret =
+				    v4l2_g_ctrl(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_g_ctrl(isp->flash->ctrl_handler,
+						&ctrl);
+			break;
+		case V4L2_CID_ZOOM_ABSOLUTE:
+			rt_mutex_lock(&isp->mutex);
+			ret = atomisp_digital_zoom(asd, 0, &ctrl.value);
+			rt_mutex_unlock(&isp->mutex);
+			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 releated 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_control ctrl;
+	int i;
+	int ret = 0;
+
+	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_TIMEING:
+		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:
+#ifndef ISP2401
+			if (isp->inputs[asd->input_curr].motor)
+#else
+			if (isp->motor)
+#endif
+				ret = v4l2_s_ctrl(NULL,
+#ifndef ISP2401
+						  isp->inputs[asd->input_curr].
+						  motor->ctrl_handler, &ctrl);
+#else
+						  isp->motor->ctrl_handler,
+						  &ctrl);
+#endif
+			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:
+			rt_mutex_lock(&isp->mutex);
+			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;
+				}
+			}
+			rt_mutex_unlock(&isp->mutex);
+			break;
+		case V4L2_CID_ZOOM_ABSOLUTE:
+			rt_mutex_lock(&isp->mutex);
+			ret = atomisp_digital_zoom(asd, 1, &ctrl.value);
+			rt_mutex_unlock(&isp->mutex);
+			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 releated 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, "unsupport v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	rt_mutex_lock(&isp->mutex);
+	parm->parm.capture.capturemode = asd->run_mode->val;
+	rt_mutex_unlock(&isp->mutex);
+
+	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, "unsupport v4l2 buf type\n");
+		return -EINVAL;
+	}
+
+	rt_mutex_lock(&isp->mutex);
+
+	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;
+		}
+
+		goto out;
+	}
+	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:
+		rval = -EINVAL;
+		goto out;
+	}
+
+	rval = v4l2_ctrl_s_ctrl(asd->run_mode, mode);
+
+out:
+	rt_mutex_unlock(&isp->mutex);
+
+	return rval == -ENOIOCTLCMD ? 0 : rval;
+}
+
+static int atomisp_s_parm_file(struct file *file, void *fh,
+				struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct atomisp_device *isp = video_get_drvdata(vdev);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		dev_err(isp->dev, "unsupport v4l2 buf type for output\n");
+		return -EINVAL;
+	}
+
+	rt_mutex_lock(&isp->mutex);
+	isp->sw_contex.file_input = 1;
+	rt_mutex_unlock(&isp->mutex);
+
+	return 0;
+}
+
+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;
+	bool acc_node;
+	int err;
+
+	acc_node = !strncmp(vdev->name, "ATOMISP ISP ACC",
+			sizeof(vdev->name));
+	if (acc_node)
+		asd = atomisp_to_acc_pipe(vdev)->asd;
+	else
+		asd = atomisp_to_video_pipe(vdev)->asd;
+
+	switch (cmd) {
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_S_EXPOSURE:
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP:
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_EXT_ISP_CTRL:
+	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:
+	case ATOMISP_IOC_S_SENSOR_EE_CONFIG:
+#ifdef ISP2401
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+#endif
+		/* we do not need take isp->mutex for these IOCTLs */
+		break;
+	default:
+		rt_mutex_lock(&isp->mutex);
+		break;
+	}
+	switch (cmd) {
+#ifdef ISP2401
+	case ATOMISP_IOC_S_SENSOR_RUNMODE:
+		err = atomisp_set_sensor_runmode(asd, arg);
+		break;
+
+#endif
+	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:
+#ifndef ISP2401
+		if (isp->inputs[asd->input_curr].motor)
+#else
+		if (isp->motor)
+#endif
+			err = v4l2_subdev_call(
+#ifndef ISP2401
+					isp->inputs[asd->input_curr].motor,
+#else
+					isp->motor,
+#endif
+					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:
+#ifdef ISP2401
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+#endif
+		err = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
+					core, ioctl, cmd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_LOAD:
+		err = atomisp_acc_load(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_LOAD_TO_PIPE:
+		err = atomisp_acc_load_to_pipe(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_UNLOAD:
+		err = atomisp_acc_unload(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_START:
+		err = atomisp_acc_start(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_WAIT:
+		err = atomisp_acc_wait(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_MAP:
+		err = atomisp_acc_map(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_UNMAP:
+		err = atomisp_acc_unmap(asd, arg);
+		break;
+
+	case ATOMISP_IOC_ACC_S_MAPPED_ARG:
+		err = atomisp_acc_s_mapped_arg(asd, arg);
+		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_S_ACC_STATE:
+		err = atomisp_acc_set_state(asd, arg);
+		break;
+	case ATOMISP_IOC_G_ACC_STATE:
+		err = atomisp_acc_get_state(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;
+	}
+
+	switch (cmd) {
+	case ATOMISP_IOC_G_MOTOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_S_EXPOSURE:
+	case ATOMISP_IOC_G_SENSOR_CALIBRATION_GROUP:
+	case ATOMISP_IOC_G_SENSOR_PRIV_INT_DATA:
+	case ATOMISP_IOC_EXT_ISP_CTRL:
+	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:
+#ifdef ISP2401
+	case ATOMISP_IOC_G_UPDATE_EXPOSURE:
+#endif
+		break;
+	default:
+		rt_mutex_unlock(&isp->mutex);
+		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_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_s_fmt_cap,
+	.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,
+};
+
+const struct v4l2_ioctl_ops atomisp_file_ioctl_ops = {
+	.vidioc_querycap = atomisp_querycap,
+	.vidioc_g_fmt_vid_out = atomisp_g_fmt_file,
+	.vidioc_s_fmt_vid_out = atomisp_s_fmt_file,
+	.vidioc_s_parm = atomisp_s_parm_file,
+	.vidioc_reqbufs = atomisp_reqbufs_file,
+	.vidioc_querybuf = atomisp_querybuf_file,
+	.vidioc_qbuf = atomisp_qbuf_file,
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.h
new file mode 100644
index 000000000000..fb5fadb5332b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_ioctl.h
@@ -0,0 +1,73 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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);
+#ifndef ISP2401
+const struct atomisp_format_bridge *atomisp_get_format_bridge_from_mbus(
+	u32 mbus_code);
+#else
+const struct atomisp_format_bridge *atomisp_get_format_bridge_from_mbus(u32
+									mbus_code);
+#endif
+
+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);
+
+int atomisp_reqbufs(struct file *file, void *fh,
+			struct v4l2_requestbuffers *req);
+
+enum atomisp_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_file_operations atomisp_file_fops;
+
+extern const struct v4l2_ioctl_ops atomisp_ioctl_ops;
+
+extern const struct v4l2_ioctl_ops atomisp_file_ioctl_ops;
+
+unsigned int atomisp_streaming_count(struct atomisp_device *isp);
+
+unsigned int atomisp_is_acc_enabled(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/atomisp2/atomisp_subdev.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_subdev.c
new file mode 100644
index 000000000000..3d6bb166927c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_subdev.c
@@ -0,0 +1,1437 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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 <asm/intel-mid.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, CSS_BAYER_ORDER_BGGR, CSS_FORMAT_RAW_8 },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, CSS_BAYER_ORDER_GBRG, CSS_FORMAT_RAW_8 },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, CSS_BAYER_ORDER_GRBG, CSS_FORMAT_RAW_8 },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, 8, ATOMISP_INPUT_FORMAT_RAW_8, CSS_BAYER_ORDER_RGGB, CSS_FORMAT_RAW_8 },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, CSS_BAYER_ORDER_BGGR, CSS_FORMAT_RAW_10 },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, CSS_BAYER_ORDER_GBRG, CSS_FORMAT_RAW_10 },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, CSS_BAYER_ORDER_GRBG, CSS_FORMAT_RAW_10 },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, 10, ATOMISP_INPUT_FORMAT_RAW_10, CSS_BAYER_ORDER_RGGB, CSS_FORMAT_RAW_10 },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, CSS_BAYER_ORDER_BGGR, CSS_FORMAT_RAW_12 },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, CSS_BAYER_ORDER_GBRG, CSS_FORMAT_RAW_12 },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, CSS_BAYER_ORDER_GRBG, CSS_FORMAT_RAW_12 },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, 12, ATOMISP_INPUT_FORMAT_RAW_12, CSS_BAYER_ORDER_RGGB, CSS_FORMAT_RAW_12 },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, 8, ATOMISP_INPUT_FORMAT_YUV422_8, 0, IA_CSS_STREAM_FORMAT_YUV422_8 },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, 8, ATOMISP_INPUT_FORMAT_YUV422_8, 0, IA_CSS_STREAM_FORMAT_YUV422_8 },
+	{ MEDIA_BUS_FMT_JPEG_1X8, 8, 8, CSS_FRAME_FORMAT_BINARY_8, 0, IA_CSS_STREAM_FORMAT_BINARY_8 },
+	{ V4L2_MBUS_FMT_CUSTOM_NV12, 12, 12, CSS_FRAME_FORMAT_NV12, 0, CSS_FRAME_FORMAT_NV12 },
+	{ V4L2_MBUS_FMT_CUSTOM_NV21, 12, 12, CSS_FRAME_FORMAT_NV21, 0, CSS_FRAME_FORMAT_NV21 },
+	{ V4L2_MBUS_FMT_CUSTOM_YUV420, 12, 12, ATOMISP_INPUT_FORMAT_YUV420_8_LEGACY, 0, IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY },
+	{ V4L2_MBUS_FMT_CUSTOM_M10MO_RAW, 8, 8, CSS_FRAME_FORMAT_BINARY_8, 0, IA_CSS_STREAM_FORMAT_BINARY_8 },
+	/* no valid V4L2 MBUS code for metadata format, so leave it 0. */
+	{ 0, 0, 0, ATOMISP_INPUT_FORMAT_EMBEDDED, 0, IA_CSS_STREAM_FORMAT_EMBEDDED },
+	{}
+};
+
+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_css_stream_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;
+	uint16_t 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_pad_config *cfg,
+				     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_pad_config *cfg,
+					  uint32_t 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, cfg, pad);
+		case V4L2_SEL_TGT_COMPOSE:
+			return v4l2_subdev_get_try_compose(sd, cfg, 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_pad_config *cfg, 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, cfg, pad);
+
+	return &isp_sd->fmt[pad].fmt;
+}
+
+static void isp_get_fmt_rect(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg, 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, cfg, which, i);
+		crop[i] = atomisp_subdev_get_rect(sd, cfg, which, i,
+						  V4L2_SEL_TGT_CROP);
+		comp[i] = atomisp_subdev_get_rect(sd, cfg, which, i,
+						  V4L2_SEL_TGT_COMPOSE);
+	}
+}
+
+static void isp_subdev_propagate(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 uint32_t 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, cfg, 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, cfg, which, pad,
+					     target, flags, &r);
+		break;
+	}
+	}
+}
+
+static int isp_subdev_get_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_pad_config *cfg,
+				    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, cfg, sel->which, sel->pad,
+					sel->target);
+	if (!rec)
+		return -EINVAL;
+
+	sel->r = *rec;
+	return 0;
+}
+
+static char *atomisp_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"};
+
+int atomisp_subdev_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 uint32_t which, uint32_t pad, uint32_t target,
+				 uint32_t 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];
+	uint16_t vdev_pad = atomisp_subdev_source_pad(sd->devnode);
+	struct v4l2_rect *crop[ATOMISP_SUBDEV_PADS_NUM],
+		*comp[ATOMISP_SUBDEV_PADS_NUM];
+	enum atomisp_input_stream_id stream_id;
+	unsigned int i;
+	unsigned int padding_w = pad_w;
+	unsigned int padding_h = pad_h;
+
+	stream_id = atomisp_source_pad_to_stream_id(isp_sd, vdev_pad);
+
+	isp_get_fmt_rect(sd, cfg, 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, cfg, 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, stream_id,
+					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,
+				stream_id,
+				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,
+				stream_id,
+				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, cfg, which, pad, target))
+		return -EINVAL;
+	*r = *atomisp_subdev_get_rect(sd, cfg, 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_pad_config *cfg,
+				    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, cfg, 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_pad_config *cfg, uint32_t which,
+			     uint32_t 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, cfg, which, pad);
+	uint16_t vdev_pad = atomisp_subdev_source_pad(sd->devnode);
+	enum atomisp_input_stream_id stream_id;
+
+	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");
+
+	stream_id = atomisp_source_pad_to_stream_id(isp_sd, vdev_pad);
+
+	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, cfg, which, pad,
+				     V4L2_SEL_TGT_CROP, 0);
+
+		if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+			atomisp_css_input_set_resolution(isp_sd,
+				stream_id, ffmt);
+			atomisp_css_input_set_binning_factor(isp_sd,
+				stream_id,
+				atomisp_get_sensor_bin_factor(isp_sd));
+			atomisp_css_input_set_bayer_order(isp_sd, stream_id,
+							  fc->bayer_order);
+			atomisp_css_input_set_format(isp_sd, stream_id,
+						fc->css_stream_fmt);
+			atomisp_css_set_default_isys_config(isp_sd, stream_id,
+							    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_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
+{
+	fmt->format = *atomisp_subdev_get_ffmt(sd, cfg, 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_pad_config *cfg,
+				 struct v4l2_subdev_format *fmt)
+{
+	atomisp_subdev_set_ffmt(sd, cfg, 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;
+	struct v4l2_streamparm p = {0};
+	int modes[] = { CI_MODE_NONE,
+			CI_MODE_VIDEO,
+			CI_MODE_STILL_CAPTURE,
+			CI_MODE_CONTINUOUS,
+			CI_MODE_PREVIEW };
+	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);
+
+	/* Fall back to obsolete s_parm */
+	p.parm.capture.capturemode = modes[mode];
+
+	return v4l2_subdev_call(
+		isp->inputs[asd->input_curr].camera, video, s_parm, &p);
+}
+
+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);
+
+	switch (ctrl->id) {
+	case V4L2_CID_RUN_MODE:
+		return __atomisp_update_run_mode(asd);
+	case V4L2_CID_DEPTH_MODE:
+		if (asd->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 configuation 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,
+};
+
+#ifdef ISP2401
+/*
+ * Control for selectting ISP version
+ *
+ * When enabled, that means ISP version will be used ISP2.7. when disable, the
+ * isp will default to use ISP2.2.
+ * Note: Make sure set this configuration before creating stream.
+ */
+static const struct v4l2_ctrl_config ctrl_select_isp_version = {
+	.ops = &ctrl_ops,
+	.id = V4L2_CID_ATOMISP_SELECT_ISP_VERSION,
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.name = "Select Isp version",
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 0,
+};
+
+#ifdef CONFIG_ION
+/*
+ * Control for ISP ion device fd
+ *
+ * userspace will open ion device and pass the fd to kernel.
+ * this fd will be used to map shared fd to buffer.
+ */
+static const struct v4l2_ctrl_config ctrl_ion_dev_fd = {
+		.ops = &ctrl_ops,
+		.id = V4L2_CID_ATOMISP_ION_DEVICE_FD,
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.name = "Ion Device Fd",
+		.min = -1,
+		.max = 1024,
+		.step = 1,
+		.def = ION_FD_UNSET
+};
+#endif
+
+#endif
+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->activeq_out);
+	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 *));
+}
+
+static void atomisp_init_acc_pipe(struct atomisp_sub_device *asd,
+		struct atomisp_acc_pipe *pipe)
+{
+	pipe->asd = asd;
+	pipe->isp = asd->isp;
+	INIT_LIST_HEAD(&asd->acc.fw);
+	INIT_LIST_HEAD(&asd->acc.memory_maps);
+	ida_init(&asd->acc.ida);
+}
+
+/*
+ * 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_in,
+				 V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	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);
+
+	atomisp_init_acc_pipe(asd, &asd->video_acc);
+
+	ret = atomisp_video_init(&asd->video_in, "MEMORY");
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_capture, "CAPTURE");
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_vf, "VIEWFINDER");
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_preview, "PREVIEW");
+	if (ret < 0)
+		return ret;
+
+	ret = atomisp_video_init(&asd->video_out_video_capture, "VIDEO");
+	if (ret < 0)
+		return ret;
+
+	atomisp_acc_init(&asd->video_acc, "ACC");
+
+	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);
+#ifdef ISP2401
+	asd->select_isp_version =
+			v4l2_ctrl_new_custom(&asd->ctrl_handler,
+					     &ctrl_select_isp_version,
+					     NULL);
+
+#ifdef CONFIG_ION
+	asd->ion_dev_fd =
+			v4l2_ctrl_new_custom(&asd->ctrl_handler,
+						&ctrl_ion_dev_fd,
+						 NULL);
+#endif
+#endif
+
+	/* 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 = isp->media_dev.driver_version >=
+	    ATOMISP_CSS_VERSION_20 ? 2 : 1;
+	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 - 2; i++) {
+		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;
+		/*
+		 * file input only supported on subdev0
+		 * so do not create pad link for subdevs other then subdev0
+		 */
+		if (asd->index)
+			return 0;
+		ret = media_create_pad_link(&asd->video_in.vdev.entity,
+					    0, &asd->subdev.entity,
+					    ATOMISP_SUBDEV_PAD_SINK, 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_in);
+	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);
+	atomisp_acc_unregister(&asd->video_acc);
+}
+
+int atomisp_subdev_register_entities(struct atomisp_sub_device *asd,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	/* Register the subdev and video node. */
+	ret = v4l2_device_register_subdev(vdev, &asd->subdev);
+	if (ret < 0)
+		goto error;
+
+	ret = atomisp_video_register(&asd->video_out_capture, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = atomisp_video_register(&asd->video_out_vf, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = atomisp_video_register(&asd->video_out_preview, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = atomisp_video_register(&asd->video_out_video_capture, vdev);
+	if (ret < 0)
+		goto error;
+
+	ret = atomisp_acc_register(&asd->video_acc, vdev);
+	if (ret < 0)
+		goto error;
+
+	/*
+	 * file input only supported on subdev0
+	 * so do not create video node for subdevs other then subdev0
+	 */
+	if (asd->index)
+		return 0;
+	ret = atomisp_video_register(&asd->video_in, vdev);
+	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 = isp->media_dev.driver_version >=
+	    ATOMISP_CSS_VERSION_20 ? 2 : 1;
+	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];
+		spin_lock_init(&asd->lock);
+		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/atomisp2/atomisp_subdev.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_subdev.h
new file mode 100644
index 000000000000..ba5c2ab14253
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_subdev.h
@@ -0,0 +1,471 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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;
+	uint8_t bpp; /* bits per pixel */
+	uint8_t depth; /* uncompressed */
+	enum atomisp_css_stream_format atomisp_in_fmt;
+	enum atomisp_css_bayer_order bayer_order;
+	enum ia_css_stream_format css_stream_fmt;
+};
+
+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 videobuf_queue outq;
+	struct list_head activeq;
+	struct list_head activeq_out;
+	/*
+	 * 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;
+
+	unsigned int buffers_in_css;
+
+	/* irq_lock is used to protect video buffer state change operations and
+	 * also to make activeq, activeq_out, capq and outq list
+	 * operations atomic. */
+	spinlock_t irq_lock;
+	unsigned int users;
+
+	struct atomisp_device *isp;
+	struct v4l2_pix_format pix;
+	uint32_t 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];
+#ifdef ISP2401
+
+	/*
+	* move wdt from asd struct to create wdt for each pipe
+	*/
+	struct timer_list wdt;
+	unsigned int wdt_duration;	/* in jiffies */
+	unsigned long wdt_expires;
+	atomic_t wdt_count;
+#endif
+};
+
+struct atomisp_acc_pipe {
+	struct video_device vdev;
+	unsigned int users;
+	bool running;
+	struct atomisp_sub_device *asd;
+	struct atomisp_device *isp;
+};
+
+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;
+	uint32_t	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 atomisp_css_grid_info curr_grid_info;
+	enum atomisp_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 atomisp_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];
+	uint32_t metadata_width_size;
+
+	struct ia_css_dvs2_statistics *dvs_stat;
+	struct atomisp_css_dvs_6axis *dvs_6axis;
+	uint32_t 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_acc_fw {
+	struct atomisp_css_fw_info *fw;
+	unsigned int handle;
+	unsigned int flags;
+	unsigned int type;
+	struct {
+		size_t length;
+		unsigned long css_ptr;
+	} args[ATOMISP_ACC_NR_MEMORY];
+	struct list_head list;
+};
+
+struct atomisp_map {
+	ia_css_ptr ptr;
+	size_t length;
+	struct list_head list;
+	/* FIXME: should keep book which maps are currently used
+	 * by binaries and not allow releasing those
+	 * which are in use. Implement by reference counting.
+	 */
+};
+
+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];
+	uint16_t capture_pad; /* main capture pad; defines much of isp config */
+
+	enum atomisp_subdev_input_entity input;
+	unsigned int output;
+	struct atomisp_video_pipe video_in;
+	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 */
+	struct atomisp_acc_pipe video_acc;
+	/* video pipe main output */
+	struct atomisp_video_pipe video_out_video_capture;
+	/* struct isp_subdev_params params; */
+	spinlock_t lock;
+	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;
+#ifdef ISP2401
+	struct v4l2_ctrl *ion_dev_fd;
+#endif
+	struct v4l2_ctrl *disable_dz;
+#ifdef ISP2401
+	struct v4l2_ctrl *select_isp_version;
+#endif
+
+	struct {
+		struct list_head fw;
+		struct list_head memory_maps;
+		struct atomisp_css_pipeline *pipeline;
+		bool extension_mode;
+		struct ida ida;
+		struct completion acc_done;
+		void *acc_stages;
+	} acc;
+
+	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[CSS_PIPE_ID_NUM];
+	unsigned int dis_bufs_in_css;
+
+	unsigned int metadata_bufs_in_css
+		[ATOMISP_INPUT_STREAM_NUM][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 atomisp_css_frame *vf_frame; /* TODO: needed? */
+	struct atomisp_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;
+	/* This field specifies which sensor is being selected when there
+	   are multiple sensors connected to the same MIPI port. */
+	int sensor_curr;
+
+	atomic_t sof_count;
+	atomic_t sequence;      /* Sequence value that is assigned to buffer. */
+	atomic_t sequence_temp;
+
+	unsigned int streaming; /* Hold both mutex and lock to change this */
+	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;
+
+#ifndef ISP2401
+	struct timer_list wdt;
+	unsigned int wdt_duration;	/* in jiffies */
+	unsigned long wdt_expires;
+
+#endif
+	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. */
+#ifndef ISP2401
+
+#else
+	bool re_trigger_capture;
+#endif
+	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);
+#ifndef ISP2401
+const struct atomisp_in_fmt_conv *atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
+	enum atomisp_css_stream_format atomisp_in_fmt);
+#else
+const struct atomisp_in_fmt_conv
+    *atomisp_find_in_fmt_conv_by_atomisp_in_fmt(enum atomisp_css_stream_format
+						atomisp_in_fmt);
+#endif
+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_pad_config *cfg, uint32_t which,
+			 uint32_t pad);
+struct v4l2_rect *atomisp_subdev_get_rect(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_pad_config *cfg,
+					  uint32_t which, uint32_t pad,
+					  uint32_t target);
+int atomisp_subdev_set_selection(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_pad_config *cfg,
+				 uint32_t which, uint32_t pad, uint32_t target,
+				 uint32_t flags, struct v4l2_rect *r);
+/* Actually set the format */
+void atomisp_subdev_set_ffmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_pad_config *cfg, uint32_t which,
+			     uint32_t 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_entities(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/atomisp2/atomisp_tables.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tables.h
new file mode 100644
index 000000000000..af09218d8b71
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tables.h
@@ -0,0 +1,191 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef	__ATOMISP_TABLES_H__
+#define	__ATOMISP_TABLES_H__
+
+#include "sh_css_params.h"
+
+/*Sepia image effect table*/
+static struct atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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 atomisp_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/atomisp2/atomisp_tpg.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c
new file mode 100644
index 000000000000..996d1bdebad4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.c
@@ -0,0 +1,181 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *param)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *param)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_get_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_format *format)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_set_fmt(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       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) {
+		cfg->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_pad_config *cfg,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_enum_frame_size(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	/*to fake*/
+	return 0;
+}
+
+static int tpg_enum_frame_ival(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_pad_config *cfg,
+			       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,
+	.g_parm = tpg_g_parm,
+	.s_parm = tpg_s_parm,
+};
+
+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;
+	strcpy(sd->name, "tpg_subdev");
+	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/atomisp2/atomisp_tpg.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.h
new file mode 100644
index 000000000000..64ab60f02e85
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_tpg.h
@@ -0,0 +1,42 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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/atomisp2/atomisp_trace_event.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_trace_event.h
new file mode 100644
index 000000000000..5ce282d6c939
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_trace_event.h
@@ -0,0 +1,133 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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(
+		strlcpy(__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(
+		strlcpy(__entry->name, name, 24);
+		strlcpy(__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/atomisp2/atomisp_v4l2.c b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
new file mode 100644
index 000000000000..e3fdbdba0b34
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.c
@@ -0,0 +1,1610 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#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/interrupt.h>
+
+#include "../../include/linux/atomisp_gmin_platform.h"
+
+#include "atomisp_cmd.h"
+#include "atomisp_common.h"
+#include "atomisp_fops.h"
+#include "atomisp_file.h"
+#include "atomisp_ioctl.h"
+#include "atomisp_internal.h"
+#include "atomisp_acc.h"
+#include "atomisp-regs.h"
+#include "atomisp_dfs_tables.h"
+#include "atomisp_drvfs.h"
+#include "hmm/hmm.h"
+#include "atomisp_trace_event.h"
+
+#include "hrt/hive_isp_css_mm_hrt.h"
+
+#include "device_access.h"
+#include <asm/intel-mid.h>
+
+/* G-Min addition: pull this in from intel_mid_pm.h */
+#define CSTATE_EXIT_LATENCY_C1  1
+
+static uint skip_fwload = 0;
+module_param(skip_fwload, uint, 0644);
+MODULE_PARM_DESC(skip_fwload, "Skip atomisp firmware load");
+
+/* set reserved memory pool size in page */
+unsigned int repool_pgnr;
+module_param(repool_pgnr, uint, 0644);
+MODULE_PARM_DESC(repool_pgnr,
+		"Set the reserved memory pool size in page (default:0)");
+
+/* set dynamic memory pool size in page */
+unsigned int dypool_pgnr = UINT_MAX;
+module_param(dypool_pgnr, uint, 0644);
+MODULE_PARM_DESC(dypool_pgnr,
+		"Set the dynamic memory pool size in page (default:0)");
+
+bool dypool_enable;
+module_param(dypool_enable, bool, 0644);
+MODULE_PARM_DESC(dypool_enable,
+		"dynamic memory pool enable/disable (default:disable)");
+
+/* 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 on/off (default:off)");
+
+/* 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");
+
+/*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");
+
+struct device *atomisp_dev;
+
+void __iomem *atomisp_io_base;
+
+int atomisp_video_init(struct atomisp_video_pipe *video, const char *name)
+{
+	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;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		direction = "input";
+		video->pad.flags = MEDIA_PAD_FL_SOURCE;
+		video->vdev.fops = &atomisp_file_fops;
+		video->vdev.ioctl_ops = &atomisp_file_ioctl_ops;
+		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);
+
+	return 0;
+}
+
+void atomisp_acc_init(struct atomisp_acc_pipe *video, const char *name)
+{
+	video->vdev.fops = &atomisp_fops;
+	video->vdev.ioctl_ops = &atomisp_ioctl_ops;
+
+	/* Initialize the video device. */
+	snprintf(video->vdev.name, sizeof(video->vdev.name),
+		 "ATOMISP ISP %s", name);
+	video->vdev.release = video_device_release_empty;
+	video_set_drvdata(&video->vdev, video->isp);
+}
+
+int atomisp_video_register(struct atomisp_video_pipe *video,
+	struct v4l2_device *vdev)
+{
+	int ret;
+
+	video->vdev.v4l2_dev = vdev;
+
+	ret = video_register_device(&video->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0)
+		dev_err(vdev->dev, "%s: could not register video device (%d)\n",
+			__func__, ret);
+
+	return ret;
+}
+
+int atomisp_acc_register(struct atomisp_acc_pipe *video,
+		struct v4l2_device *vdev)
+{
+	int ret;
+
+	video->vdev.v4l2_dev = vdev;
+
+	ret = video_register_device(&video->vdev, VFL_TYPE_GRABBER, -1);
+	if (ret < 0)
+		dev_err(vdev->dev, "%s: could not register video device (%d)\n",
+			__func__, ret);
+
+	return ret;
+}
+
+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);
+	}
+}
+
+void atomisp_acc_unregister(struct atomisp_acc_pipe *video)
+{
+	if (video_is_registered(&video->vdev))
+		video_unregister_device(&video->vdev);
+}
+
+static int atomisp_save_iunit_reg(struct atomisp_device *isp)
+{
+	struct pci_dev *dev = isp->pdev;
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	pci_read_config_word(dev, PCI_COMMAND, &isp->saved_regs.pcicmdsts);
+	/* isp->saved_regs.ispmmadr is set from the atomisp_pci_probe() */
+	pci_read_config_dword(dev, PCI_MSI_CAPID, &isp->saved_regs.msicap);
+	pci_read_config_dword(dev, PCI_MSI_ADDR, &isp->saved_regs.msi_addr);
+	pci_read_config_word(dev, PCI_MSI_DATA,  &isp->saved_regs.msi_data);
+	pci_read_config_byte(dev, PCI_INTERRUPT_LINE, &isp->saved_regs.intr);
+	pci_read_config_dword(dev, PCI_INTERRUPT_CTRL,
+			      &isp->saved_regs.interrupt_control);
+
+	pci_read_config_dword(dev, MRFLD_PCI_PMCS,
+			      &isp->saved_regs.pmcs);
+	/* Ensure read/write combining is enabled. */
+	pci_read_config_dword(dev, 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(dev, MRFLD_PCI_CSI_ACCESS_CTRL_VIOL,
+			      &isp->saved_regs.csi_access_viol);
+	pci_read_config_dword(dev, 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(dev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL,
+			      &isp->saved_regs.csi_afe_dly);
+	pci_read_config_dword(dev, 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(dev, MRFLD_PCI_CSI_AFE_RCOMP_CONTROL,
+			      &isp->saved_regs.csi_afe_rcomp_config);
+	pci_read_config_dword(dev, MRFLD_PCI_CSI_AFE_HS_CONTROL,
+			      &isp->saved_regs.csi_afe_hs_control);
+	pci_read_config_dword(dev, 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 *dev = isp->pdev;
+
+	dev_dbg(isp->dev, "%s\n", __func__);
+
+	pci_write_config_word(dev, PCI_COMMAND, isp->saved_regs.pcicmdsts);
+	pci_write_config_dword(dev, PCI_BASE_ADDRESS_0,
+			       isp->saved_regs.ispmmadr);
+	pci_write_config_dword(dev, PCI_MSI_CAPID, isp->saved_regs.msicap);
+	pci_write_config_dword(dev, PCI_MSI_ADDR, isp->saved_regs.msi_addr);
+	pci_write_config_word(dev, PCI_MSI_DATA, isp->saved_regs.msi_data);
+	pci_write_config_byte(dev, PCI_INTERRUPT_LINE, isp->saved_regs.intr);
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL,
+			       isp->saved_regs.interrupt_control);
+	pci_write_config_dword(dev, PCI_I_CONTROL,
+					isp->saved_regs.i_control);
+
+	pci_write_config_dword(dev, MRFLD_PCI_PMCS,
+					isp->saved_regs.pmcs);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_ACCESS_CTRL_VIOL,
+			      isp->saved_regs.csi_access_viol);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_RCOMP_CONTROL,
+			      isp->saved_regs.csi_rcomp_config);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL,
+			      isp->saved_regs.csi_afe_dly);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_CONTROL,
+			      isp->saved_regs.csi_control);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_AFE_RCOMP_CONTROL,
+			      isp->saved_regs.csi_afe_rcomp_config);
+	pci_write_config_dword(dev, MRFLD_PCI_CSI_AFE_HS_CONTROL,
+			      isp->saved_regs.csi_afe_hs_control);
+	pci_write_config_dword(dev, 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_store_uint32(MRFLD_CSI_RECEIVER_SELECTION_REG, 1);
+	return 0;
+}
+
+static int atomisp_mrfld_pre_power_down(struct atomisp_device *isp)
+{
+	struct pci_dev *dev = isp->pdev;
+	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(dev, PCI_INTERRUPT_CTRL, &irq);
+	irq = irq & 1 << INTR_IIR;
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, irq);
+
+	pci_read_config_dword(dev, PCI_INTERRUPT_CTRL, &irq);
+	if (!(irq & (1 << INTR_IIR)))
+		goto done;
+
+	atomisp_store_uint32(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(dev, PCI_INTERRUPT_CTRL, &irq);
+		irq = irq & 1 << INTR_IIR;
+		pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, irq);
+
+		pci_read_config_dword(dev, PCI_INTERRUPT_CTRL, &irq);
+		if (!(irq & (1 << INTR_IIR))) {
+			atomisp_store_uint32(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(dev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= ~(1 << INTR_IER);
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, irq);
+
+	atomisp_msi_irq_uninit(isp, dev);
+	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
+ */
+void punit_ddr_dvfs_enable(bool enable)
+{
+	int reg = intel_mid_msgbus_read32(PUNIT_PORT, MRFLD_ISPSSDVFS);
+	int door_bell = 1 << 8;
+	int max_wait = 30;
+
+	if (enable) {
+		reg &= ~(MRFLD_BIT0 | MRFLD_BIT1);
+	} else {
+		reg |= (MRFLD_BIT1 | door_bell);
+		reg &= ~(MRFLD_BIT0);
+	}
+
+	intel_mid_msgbus_write32(PUNIT_PORT, MRFLD_ISPSSDVFS, reg);
+
+	/*Check Req_ACK to see freq status, wait until door_bell is cleared*/
+	if (reg & door_bell) {
+		while (max_wait--) {
+			if (0 == (intel_mid_msgbus_read32(PUNIT_PORT,
+				MRFLD_ISPSSDVFS) & door_bell))
+					break;
+
+			usleep_range(100, 500);
+		}
+	}
+
+	if (max_wait == -1)
+		pr_info("DDR DVFS, door bell is not cleared within 3ms\n");
+}
+
+/* Workaround for pmu_nc_set_power_state not ready in MRFLD */
+int atomisp_mrfld_power_down(struct atomisp_device *isp)
+{
+	unsigned long timeout;
+	u32 reg_value;
+
+	/* writing 0x3 to ISPSSPM0 bit[1:0] to power off the IUNIT */
+	reg_value = intel_mid_msgbus_read32(PUNIT_PORT, MRFLD_ISPSSPM0);
+	reg_value &= ~MRFLD_ISPSSPM0_ISPSSC_MASK;
+	reg_value |= MRFLD_ISPSSPM0_IUNIT_POWER_OFF;
+	intel_mid_msgbus_write32(PUNIT_PORT, MRFLD_ISPSSPM0, reg_value);
+
+	/*WA:Enable DVFS*/
+	if (IS_CHT)
+		punit_ddr_dvfs_enable(true);
+
+	/*
+	 * There should be no iunit access while power-down is
+	 * in progress HW sighting: 4567865
+	 * FIXME: msecs_to_jiffies(50)- experienced value
+	 */
+	timeout = jiffies + msecs_to_jiffies(50);
+	while (1) {
+		reg_value = intel_mid_msgbus_read32(PUNIT_PORT,
+							MRFLD_ISPSSPM0);
+		dev_dbg(isp->dev, "power-off in progress, ISPSSPM0: 0x%x\n",
+				reg_value);
+		/* wait until ISPSSPM0 bit[25:24] shows 0x3 */
+		if ((reg_value >> MRFLD_ISPSSPM0_ISPSSS_OFFSET) ==
+			MRFLD_ISPSSPM0_IUNIT_POWER_OFF) {
+			trace_ipu_cstate(0);
+			return 0;
+		}
+
+		if (time_after(jiffies, timeout)) {
+			dev_err(isp->dev, "power-off iunit timeout.\n");
+			return -EBUSY;
+		}
+		/* FIXME: experienced value for delay */
+		usleep_range(100, 150);
+	}
+}
+
+
+/* Workaround for pmu_nc_set_power_state not ready in MRFLD */
+int atomisp_mrfld_power_up(struct atomisp_device *isp)
+{
+	unsigned long timeout;
+	u32 reg_value;
+
+	/*WA for PUNIT, if DVFS enabled, ISP timeout observed*/
+	if (IS_CHT)
+		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)
+		msleep(10);
+
+	/* writing 0x0 to ISPSSPM0 bit[1:0] to power off the IUNIT */
+	reg_value = intel_mid_msgbus_read32(PUNIT_PORT, MRFLD_ISPSSPM0);
+	reg_value &= ~MRFLD_ISPSSPM0_ISPSSC_MASK;
+	intel_mid_msgbus_write32(PUNIT_PORT, MRFLD_ISPSSPM0, reg_value);
+
+	/* FIXME: experienced value for delay */
+	timeout = jiffies + msecs_to_jiffies(50);
+	while (1) {
+		reg_value = intel_mid_msgbus_read32(PUNIT_PORT, MRFLD_ISPSSPM0);
+		dev_dbg(isp->dev, "power-on in progress, ISPSSPM0: 0x%x\n",
+				reg_value);
+		/* wait until ISPSSPM0 bit[25:24] shows 0x0 */
+		if ((reg_value >> MRFLD_ISPSSPM0_ISPSSS_OFFSET) ==
+			MRFLD_ISPSSPM0_IUNIT_POWER_ON) {
+			trace_ipu_cstate(1);
+			return 0;
+		}
+
+		if (time_after(jiffies, timeout)) {
+			dev_err(isp->dev, "power-on iunit timeout.\n");
+			return -EBUSY;
+		}
+		/* FIXME: experienced value for delay */
+		usleep_range(100, 150);
+	}
+}
+
+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;
+	pm_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;
+
+	pm_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;
+	}
+	pm_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;
+
+	pm_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)
+{
+	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(isp->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(isp->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;
+
+	pdata = atomisp_get_platform_data();
+	if (pdata == NULL) {
+		dev_err(isp->dev, "no platform data available\n");
+		return 0;
+	}
+
+	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);
+		struct camera_sensor_platform_data *sensor_pdata;
+		int sensor_num, i;
+
+		if (adapter == NULL) {
+			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);
+		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 == NULL) {
+			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:
+			raw_index = isp->input_cnt;
+			dev_dbg(isp->dev, "raw_index: %d\n", raw_index);
+		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;
+			sensor_pdata = (struct camera_sensor_platform_data *)
+					board_info->platform_data;
+			if (sensor_pdata->get_camera_caps)
+				isp->inputs[isp->input_cnt].camera_caps =
+					sensor_pdata->get_camera_caps();
+			else
+				isp->inputs[isp->input_cnt].camera_caps =
+					atomisp_get_default_camera_caps();
+			sensor_num = isp->inputs[isp->input_cnt]
+				.camera_caps->sensor_num;
+			isp->input_cnt++;
+			for (i = 1; i < sensor_num; i++) {
+				if (isp->input_cnt >= ATOM_ISP_MAX_INPUTS) {
+					dev_warn(isp->dev,
+						"atomisp inputs out of range\n");
+					break;
+				}
+				isp->inputs[isp->input_cnt] =
+					isp->inputs[isp->input_cnt - 1];
+				isp->inputs[isp->input_cnt].sensor_index = i;
+				isp->input_cnt++;
+			}
+			break;
+		case CAMERA_MOTOR:
+			isp->motor = subdev;
+			break;
+		case LED_FLASH:
+		case XENON_FLASH:
+			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->inputs[0].camera)
+		dev_warn(isp->dev, "no camera attached or fail to detect\n");
+
+	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);
+	atomisp_file_input_unregister_entities(&isp->file_dev);
+	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);
+}
+
+static int atomisp_register_entities(struct atomisp_device *isp)
+{
+	int ret = 0;
+	unsigned int i;
+
+	isp->media_dev.dev = isp->dev;
+
+	strlcpy(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_file_input_register_entities(&isp->file_dev, &isp->v4l2_dev);
+	if (ret < 0) {
+		dev_err(isp->dev, "atomisp_file_input_register_entities\n");
+		goto file_input_register_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_entities(asd, &isp->v4l2_dev);
+		if (ret < 0) {
+			dev_err(isp->dev,
+				"atomisp_subdev_register_entities 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 == NULL) {
+			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;
+		}
+	}
+
+	dev_dbg(isp->dev,
+		"FILE_INPUT enable, camera_cnt: %d\n", isp->input_cnt);
+	isp->inputs[isp->input_cnt].type = FILE_INPUT;
+	isp->inputs[isp->input_cnt].port = -1;
+	isp->inputs[isp->input_cnt].camera_caps =
+		    atomisp_get_default_camera_caps();
+	isp->inputs[isp->input_cnt++].camera = &isp->file_dev.sd;
+
+	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_caps =
+		    atomisp_get_default_camera_caps();
+		isp->inputs[isp->input_cnt++].camera = &isp->tpg.sd;
+	} else {
+		dev_warn(isp->dev, "too many atomisp inputs, TPG ignored.\n");
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+	if (ret < 0)
+		goto link_failed;
+
+	return media_device_register(&isp->media_dev);
+
+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:
+	atomisp_file_input_unregister_entities(&isp->file_dev);
+file_input_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_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_file_input_init(isp);
+	if (ret < 0) {
+		dev_err(isp->dev,
+			"file input device initialization failed\n");
+		goto error_file_input;
+	}
+
+	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_file_input:
+	atomisp_file_input_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_file_input_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 (isp->media_dev.driver_version == ATOMISP_CSS_VERSION_21) {
+		if (isp->media_dev.hw_revision ==
+		    ((ATOMISP_HW_REVISION_ISP2401 << ATOMISP_HW_REVISION_SHIFT)
+		     | ATOMISP_HW_STEPPING_A0))
+			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 driver_version 0x%x, hw_revision 0x%x\n",
+			isp->media_dev.driver_version,
+			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 *dev,
+			    const struct pci_device_id *id)
+{
+	unsigned int a0_max_id;
+
+	switch (id->device & ATOMISP_PCI_DEVICE_SOC_MASK) {
+	case ATOMISP_PCI_DEVICE_SOC_MRFLD:
+		a0_max_id = ATOMISP_PCI_REV_MRFLD_A0_MAX;
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_BYT:
+		a0_max_id = ATOMISP_PCI_REV_BYT_A0_MAX;
+		break;
+	default:
+		return true;
+	}
+
+	return dev->revision > a0_max_id;
+}
+
+static int init_atomisp_wdts(struct atomisp_device *isp)
+{
+	int i, err;
+
+	atomic_set(&isp->wdt_work_queued, 0);
+	isp->wdt_work_queue = alloc_workqueue(isp->v4l2_dev.name, 0, 1);
+	if (isp->wdt_work_queue == NULL) {
+		dev_err(isp->dev, "Failed to initialize wdt work queue\n");
+		err = -ENOMEM;
+		goto alloc_fail;
+	}
+	INIT_WORK(&isp->wdt_work, atomisp_wdt_work);
+
+	for (i = 0; i < isp->num_of_streams; i++) {
+		struct atomisp_sub_device *asd = &isp->asd[i];
+		asd = &isp->asd[i];
+#ifndef ISP2401
+		setup_timer(&asd->wdt, atomisp_wdt, (unsigned long)isp);
+#else
+		setup_timer(&asd->video_out_capture.wdt,
+			atomisp_wdt, (unsigned long)&asd->video_out_capture);
+		setup_timer(&asd->video_out_preview.wdt,
+			atomisp_wdt, (unsigned long)&asd->video_out_preview);
+		setup_timer(&asd->video_out_vf.wdt,
+			atomisp_wdt, (unsigned long)&asd->video_out_vf);
+		setup_timer(&asd->video_out_video_capture.wdt,
+			atomisp_wdt,
+			(unsigned long)&asd->video_out_video_capture);
+#endif
+	}
+	return 0;
+alloc_fail:
+	return err;
+}
+
+static struct pci_driver atomisp_pci_driver;
+
+#define ATOM_ISP_PCI_BAR	0
+
+static int atomisp_pci_probe(struct pci_dev *dev,
+				       const struct pci_device_id *id)
+{
+	const struct atomisp_platform_data *pdata;
+	struct atomisp_device *isp;
+	unsigned int start;
+	void __iomem *base;
+	int err, val;
+	u32 irq;
+
+	if (!dev) {
+		dev_err(&dev->dev, "atomisp: error device ptr\n");
+		return -EINVAL;
+	}
+
+	if (!is_valid_device(dev, id))
+		return -ENODEV;
+	/* Pointer to struct device. */
+	atomisp_dev = &dev->dev;
+
+	pdata = atomisp_get_platform_data();
+	if (pdata == NULL)
+		dev_warn(&dev->dev, "no platform data available\n");
+
+	err = pcim_enable_device(dev);
+	if (err) {
+		dev_err(&dev->dev, "Failed to enable CI ISP device (%d)\n",
+			err);
+		return err;
+	}
+
+	start = pci_resource_start(dev, ATOM_ISP_PCI_BAR);
+	dev_dbg(&dev->dev, "start: 0x%x\n", start);
+
+	err = pcim_iomap_regions(dev, 1 << ATOM_ISP_PCI_BAR, pci_name(dev));
+	if (err) {
+		dev_err(&dev->dev, "Failed to I/O memory remapping (%d)\n",
+			err);
+		return err;
+	}
+
+	base = pcim_iomap_table(dev)[ATOM_ISP_PCI_BAR];
+	dev_dbg(&dev->dev, "base: %p\n", base);
+
+	atomisp_io_base = base;
+
+	dev_dbg(&dev->dev, "atomisp_io_base: %p\n", atomisp_io_base);
+
+	isp = devm_kzalloc(&dev->dev, sizeof(struct atomisp_device), GFP_KERNEL);
+	if (!isp) {
+		dev_err(&dev->dev, "Failed to alloc CI ISP structure\n");
+		return -ENOMEM;
+	}
+	isp->pdev = dev;
+	isp->dev = &dev->dev;
+	isp->sw_contex.power_state = ATOM_ISP_POWER_UP;
+	isp->pci_root = pci_get_bus_and_slot(0, 0);
+	if (!isp->pci_root) {
+		dev_err(&dev->dev, "Unable to find PCI host\n");
+		return -ENODEV;
+	}
+	isp->saved_regs.ispmmadr = start;
+
+	rt_mutex_init(&isp->mutex);
+	mutex_init(&isp->streamoff_mutex);
+	spin_lock_init(&isp->lock);
+
+	/* This is not a true PCI device on SoC, so the delay is not needed. */
+	isp->pdev->d3_delay = 0;
+
+	isp->media_dev.driver_version = ATOMISP_CSS_VERSION_21;
+	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;
+#ifdef FIXME			
+		if (INTEL_MID_BOARD(3, TABLET, BYT, BLK, PRO, CRV2) ||
+			INTEL_MID_BOARD(3, TABLET, BYT, BLK, ENG, CRV2)) {
+			isp->dfs = &dfs_config_byt_cr;
+			isp->hpll_freq = HPLL_FREQ_2000MHZ;
+		} else
+#endif		
+		{
+			isp->dfs = &dfs_config_byt;
+			isp->hpll_freq = HPLL_FREQ_1600MHZ;
+		}
+		/* 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(&dev->dev, "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.
+		 */
+		isp->pdev->d3cold_delay = 0;
+		break;
+	case ATOMISP_PCI_DEVICE_SOC_ANN:
+		isp->media_dev.hw_revision = (
+#ifdef ISP2401_NEW_INPUT_SYSTEM
+			 ATOMISP_HW_REVISION_ISP2401
+#else
+			 ATOMISP_HW_REVISION_ISP2401_LEGACY
+#endif
+			 << ATOMISP_HW_REVISION_SHIFT);
+		isp->media_dev.hw_revision |= isp->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 = (
+#ifdef ISP2401_NEW_INPUT_SYSTEM
+			 ATOMISP_HW_REVISION_ISP2401
+#else
+			 ATOMISP_HW_REVISION_ISP2401_LEGACY
+#endif
+			<< ATOMISP_HW_REVISION_SHIFT);
+		isp->media_dev.hw_revision |= isp->pdev->revision < 2 ?
+			 ATOMISP_HW_STEPPING_A0 : ATOMISP_HW_STEPPING_B0;
+
+		isp->dfs = &dfs_config_cht;
+		isp->pdev->d3cold_delay = 0;
+
+		val = intel_mid_msgbus_read32(CCK_PORT, CCK_FUSE_REG_0);
+		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(isp->dev,
+				 "read HPLL from cck failed.default 1600MHz.\n");
+		}
+		break;
+	default:
+		dev_err(&dev->dev, "un-supported IUNIT device\n");
+		return -ENODEV;
+	}
+
+	dev_info(&dev->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;
+			goto load_fw_fail;
+		}
+
+		err = atomisp_css_check_firmware_version(isp);
+		if (err) {
+			dev_dbg(&dev->dev, "Firmware version check failed\n");
+			goto fw_validation_fail;
+		}
+	}
+
+	pci_set_master(dev);
+	pci_set_drvdata(dev, isp);
+
+	err = pci_enable_msi(dev);
+	if (err) {
+		dev_err(&dev->dev, "Failed to enable msi (%d)\n", err);
+		goto enable_msi_fail;
+	}
+
+	atomisp_msi_irq_init(isp, dev);
+
+	pm_qos_add_request(&isp->pm_qos, PM_QOS_CPU_DMA_LATENCY,
+			   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_store_uint32(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(dev, 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(dev, MRFLD_PCI_CSI_AFE_TRIM_CONTROL,
+				      csi_afe_trim);
+	}
+
+	err = atomisp_initialize_modules(isp);
+	if (err < 0) {
+		dev_err(&dev->dev, "atomisp_initialize_modules (%d)\n", err);
+		goto initialize_modules_fail;
+	}
+
+	err = atomisp_register_entities(isp);
+	if (err < 0) {
+		dev_err(&dev->dev, "atomisp_register_entities failed (%d)\n",
+			err);
+		goto register_entities_fail;
+	}
+	err = atomisp_create_pads_links(isp);
+	if (err < 0)
+		goto register_entities_fail;
+	/* init atomisp wdts */
+	if (init_atomisp_wdts(isp) != 0)
+		goto wdt_work_queue_fail;
+
+	/* save the iunit context only once after all the values are init'ed. */
+	atomisp_save_iunit_reg(isp);
+
+	pm_runtime_put_noidle(&dev->dev);
+	pm_runtime_allow(&dev->dev);
+
+	hmm_init_mem_stat(repool_pgnr, dypool_enable, dypool_pgnr);
+	err = hmm_pool_register(repool_pgnr, HMM_POOL_TYPE_RESERVED);
+	if (err) {
+		dev_err(&dev->dev, "Failed to register reserved memory pool.\n");
+		goto hmm_pool_fail;
+	}
+
+	/* Init ISP memory management */
+	hmm_init();
+
+	err = devm_request_threaded_irq(&dev->dev, dev->irq,
+					atomisp_isr, atomisp_isr_thread,
+					IRQF_SHARED, "isp_irq", isp);
+	if (err) {
+		dev_err(&dev->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(&dev->dev, "Failed to init css.\n");
+			goto css_init_fail;
+		}
+	} else {
+		dev_dbg(&dev->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;
+
+	atomisp_drvfs_init(&atomisp_pci_driver, isp);
+
+	return 0;
+
+css_init_fail:
+	devm_free_irq(&dev->dev, dev->irq, isp);
+request_irq_fail:
+	hmm_cleanup();
+	hmm_pool_unregister(HMM_POOL_TYPE_RESERVED);
+hmm_pool_fail:
+	destroy_workqueue(isp->wdt_work_queue);
+wdt_work_queue_fail:
+	atomisp_acc_cleanup(isp);
+	atomisp_unregister_entities(isp);
+register_entities_fail:
+	atomisp_uninitialize_modules(isp);
+initialize_modules_fail:
+	pm_qos_remove_request(&isp->pm_qos);
+	atomisp_msi_irq_uninit(isp, dev);
+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(dev, PCI_INTERRUPT_CTRL, &irq);
+	irq = irq & 1 << INTR_IIR;
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, irq);
+
+	pci_read_config_dword(dev, PCI_INTERRUPT_CTRL, &irq);
+	irq &= ~(1 << INTR_IER);
+	pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, irq);
+
+	atomisp_msi_irq_uninit(isp, dev);
+
+	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(&dev->dev, "Failed to switch off ISP\n");
+	pci_dev_put(isp->pci_root);
+	return err;
+}
+
+static void atomisp_pci_remove(struct pci_dev *dev)
+{
+	struct atomisp_device *isp = (struct atomisp_device *)
+		pci_get_drvdata(dev);
+
+	atomisp_drvfs_exit();
+
+	atomisp_acc_cleanup(isp);
+
+	atomisp_css_unload_firmware(isp);
+	hmm_cleanup();
+
+	pm_runtime_forbid(&dev->dev);
+	pm_runtime_get_noresume(&dev->dev);
+	pm_qos_remove_request(&isp->pm_qos);
+
+	atomisp_msi_irq_uninit(isp, dev);
+	pci_dev_put(isp->pci_root);
+
+	atomisp_unregister_entities(isp);
+
+	destroy_workqueue(isp->wdt_work_queue);
+	atomisp_file_input_cleanup(isp);
+
+	release_firmware(isp->firmware);
+
+	hmm_pool_unregister(HMM_POOL_TYPE_RESERVED);
+}
+
+static const struct pci_device_id atomisp_pci_tbl[] = {
+#if defined(ISP2400) || defined(ISP2400B0)
+	/* Merrifield */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x1178)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x1179)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x117a)},
+	/* Baytrail */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0f38)},
+#elif defined(ISP2401)
+	/* Anniedale (Merrifield+ / Moorefield) */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x1478)},
+	/* Cherrytrail */
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x22b8)},
+#endif
+	{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,
+};
+
+static int __init atomisp_init(void)
+{
+	return pci_register_driver(&atomisp_pci_driver);
+}
+
+static void __exit atomisp_exit(void)
+{
+	pci_unregister_driver(&atomisp_pci_driver);
+}
+
+module_init(atomisp_init);
+module_exit(atomisp_exit);
+
+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/atomisp2/atomisp_v4l2.h b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.h
new file mode 100644
index 000000000000..191b2e57a810
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/atomisp_v4l2.h
@@ -0,0 +1,44 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef __ATOMISP_V4L2_H__
+#define __ATOMISP_V4L2_H__
+
+struct atomisp_video_pipe;
+struct atomisp_acc_pipe;
+struct v4l2_device;
+struct atomisp_device;
+struct firmware;
+
+int atomisp_video_init(struct atomisp_video_pipe *video, const char *name);
+void atomisp_acc_init(struct atomisp_acc_pipe *video, const char *name);
+void atomisp_video_unregister(struct atomisp_video_pipe *video);
+int atomisp_video_register(struct atomisp_video_pipe *video,
+	struct v4l2_device *vdev);
+void atomisp_acc_unregister(struct atomisp_acc_pipe *video);
+int atomisp_acc_register(struct atomisp_acc_pipe *video,
+	struct v4l2_device *vdev);
+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/atomisp2/css2400/Makefile b/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile
new file mode 100644
index 000000000000..04defaafa02c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/Makefile
@@ -0,0 +1,4 @@
+ccflags-y += -DISP2400B0
+ISP2400B0 := y
+
+include $(srctree)/$(src)/../Makefile.common
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf.h
new file mode 100644
index 000000000000..766218ed3649
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf.h
@@ -0,0 +1,377 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <storage_class.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().
+ */
+STORAGE_CLASS_EXTERN 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.
+ */
+STORAGE_CLASS_EXTERN 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.
+ */
+STORAGE_CLASS_EXTERN 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.
+ */
+STORAGE_CLASS_EXTERN 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.
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_elem_set_val(
+	ia_css_circbuf_elem_t *elem,
+	uint32_t val)
+{
+	OP___assert(elem != NULL);
+
+	elem->val = val;
+}
+
+/**
+ * @brief Initialize the element.
+ *
+ * @param elem The pointer to the element.
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_elem_init(
+		ia_css_circbuf_elem_t *elem)
+{
+	OP___assert(elem != NULL);
+	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.
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_elem_cpy(
+	ia_css_circbuf_elem_t *src,
+	ia_css_circbuf_elem_t *dest)
+{
+	OP___assert(src != NULL);
+	OP___assert(dest != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE uint8_t ia_css_circbuf_get_pos_at_offset(
+	ia_css_circbuf_t *cb,
+	uint32_t base,
+	int offset)
+{
+	uint8_t dest;
+
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+	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.
+ */
+STORAGE_CLASS_INLINE int ia_css_circbuf_get_offset(
+	ia_css_circbuf_t *cb,
+	uint32_t src_pos,
+	uint32_t dest_pos)
+{
+	int offset;
+
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE uint32_t ia_css_circbuf_get_size(
+		ia_css_circbuf_t *cb)
+{
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE uint32_t ia_css_circbuf_get_num_elems(
+		ia_css_circbuf_t *cb)
+{
+	int num;
+
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE bool ia_css_circbuf_is_empty(
+		ia_css_circbuf_t *cb)
+{
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE bool ia_css_circbuf_is_full(ia_css_circbuf_t *cb)
+{
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_write(
+	ia_css_circbuf_t *cb,
+	ia_css_circbuf_elem_t elem)
+{
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	/* 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.
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_push(
+	ia_css_circbuf_t *cb,
+	uint32_t val)
+{
+	ia_css_circbuf_elem_t elem;
+
+	OP___assert(cb != NULL);
+
+	/* 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.
+ */
+STORAGE_CLASS_INLINE uint32_t ia_css_circbuf_get_free_elems(
+		ia_css_circbuf_t *cb)
+{
+	OP___assert(cb != NULL);
+	OP___assert(cb->desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_EXTERN 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.
+ */
+STORAGE_CLASS_EXTERN 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 succesfully increasing the size
+ * 			false on failure
+ */
+STORAGE_CLASS_EXTERN 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/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_comm.h
new file mode 100644
index 000000000000..3fc0330b9526
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_comm.h
@@ -0,0 +1,56 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint8_t size;	/* the maximum number of elements*/
+	uint8_t step;   /* number of bytes per element */
+	uint8_t start;	/* index of the oldest element */
+	uint8_t 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 {
+	uint32_t 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/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_desc.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_desc.h
new file mode 100644
index 000000000000..a8447d409c31
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/interface/ia_css_circbuf_desc.h
@@ -0,0 +1,170 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <storage_class.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.
+ */
+STORAGE_CLASS_INLINE bool ia_css_circbuf_desc_is_empty(
+		ia_css_circbuf_desc_t *cb_desc)
+{
+	OP___assert(cb_desc != NULL);
+	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.
+ */
+STORAGE_CLASS_INLINE bool ia_css_circbuf_desc_is_full(
+		ia_css_circbuf_desc_t *cb_desc)
+{
+	OP___assert(cb_desc != NULL);
+	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
+ */
+STORAGE_CLASS_INLINE void ia_css_circbuf_desc_init(
+	ia_css_circbuf_desc_t *cb_desc,
+	int8_t size)
+{
+	OP___assert(cb_desc != NULL);
+	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.
+ */
+STORAGE_CLASS_INLINE uint8_t ia_css_circbuf_desc_get_pos_at_offset(
+	ia_css_circbuf_desc_t *cb_desc,
+	uint32_t base,
+	int offset)
+{
+	uint8_t dest;
+	OP___assert(cb_desc != NULL);
+	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.
+ */
+STORAGE_CLASS_INLINE int ia_css_circbuf_desc_get_offset(
+	ia_css_circbuf_desc_t *cb_desc,
+	uint32_t src_pos,
+	uint32_t dest_pos)
+{
+	int offset;
+	OP___assert(cb_desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE uint32_t ia_css_circbuf_desc_get_num_elems(
+		ia_css_circbuf_desc_t *cb_desc)
+{
+	int num;
+	OP___assert(cb_desc != NULL);
+
+	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.
+ */
+STORAGE_CLASS_INLINE uint32_t ia_css_circbuf_desc_get_free_elems(
+		ia_css_circbuf_desc_t *cb_desc)
+{
+	uint32_t num;
+	OP___assert(cb_desc != NULL);
+
+	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/atomisp2/css2400/base/circbuf/src/circbuf.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/src/circbuf.c
new file mode 100644
index 000000000000..19bae1610fb6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/circbuf/src/circbuf.c
@@ -0,0 +1,321 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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,
+						   uint32_t 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)
+{
+	uint32_t 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)
+{
+	uint32_t 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;
+	uint32_t val;
+	uint32_t pos;
+	uint32_t src_pos;
+	uint32_t 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)
+{
+	uint8_t curr_size;
+	uint8_t 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,
+				uint32_t 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/atomisp2/css2400/base/refcount/interface/ia_css_refcount.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/refcount/interface/ia_css_refcount.h
new file mode 100644
index 000000000000..20db4de6beeb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/refcount/interface/ia_css_refcount.h
@@ -0,0 +1,83 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.h>
+#include <ia_css_err.h>
+
+typedef void (*clear_func)(hrt_vaddress ptr);
+
+/*! \brief Function for initializing refcount list
+ *
+ * \param[in]	size		Size of the refcount list.
+ * \return				ia_css_err
+ */
+extern enum ia_css_err ia_css_refcount_init(uint32_t size);
+
+/*! \brief Function for de-initializing refcount list
+ *
+ * \return				None
+ */
+extern 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				hrt_vaddress (saved address)
+ */
+extern hrt_vaddress ia_css_refcount_increment(int32_t id, hrt_vaddress 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.
+ */
+extern bool ia_css_refcount_decrement(int32_t id, hrt_vaddress 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.
+ */
+extern bool ia_css_refcount_is_single(hrt_vaddress 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
+ */
+extern void ia_css_refcount_clear(int32_t 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
+ */
+extern bool ia_css_refcount_is_valid(hrt_vaddress ptr);
+
+#endif /* _IA_CSS_REFCOUNT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/refcount/src/refcount.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/refcount/src/refcount.c
new file mode 100644
index 000000000000..6e3bd773ee4c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/base/refcount/src/refcount.c
@@ -0,0 +1,281 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_refcount.h"
+#include "memory_access/memory_access.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 hrt_vaddress */
+struct ia_css_refcount_entry {
+	uint32_t count;
+	hrt_vaddress data;
+	int32_t id;
+};
+
+struct ia_css_refcount_list {
+	uint32_t size;
+	struct ia_css_refcount_entry *items;
+};
+
+static struct ia_css_refcount_list myrefcount;
+
+static struct ia_css_refcount_entry *refcount_find_entry(hrt_vaddress ptr,
+							 bool firstfree)
+{
+	uint32_t i;
+
+	if (ptr == 0)
+		return NULL;
+	if (myrefcount.items == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "refcount_find_entry(): Ref count not initiliazed!\n");
+		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;
+}
+
+enum ia_css_err ia_css_refcount_init(uint32_t size)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	if (size == 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_refcount_init(): Size of 0 for Ref count init!\n");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	if (myrefcount.items != NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				    "ia_css_refcount_init(): Ref count is already initialized\n");
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+	myrefcount.items =
+	    sh_css_malloc(sizeof(struct ia_css_refcount_entry) * size);
+	if (!myrefcount.items)
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	if (err == IA_CSS_SUCCESS) {
+		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;
+	uint32_t i;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_refcount_uninit() entry\n");
+	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;
+		}
+	}
+	sh_css_free(myrefcount.items);
+	myrefcount.items = NULL;
+	myrefcount.size = 0;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_refcount_uninit() leave\n");
+}
+
+hrt_vaddress ia_css_refcount_increment(int32_t id, hrt_vaddress 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,
+			    "ia_css_refcount_increment(%x) 0x%x\n", id, ptr);
+
+	if (!entry) {
+		entry = refcount_find_entry(ptr, true);
+		assert(entry != NULL);
+		if (entry == NULL)
+			return mmgr_NULL;
+		entry->id = id;
+	}
+
+	if (entry->id != id) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+			    "ia_css_refcount_increment(): Ref count IDS do not match!\n");
+		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(int32_t id, hrt_vaddress ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_refcount_decrement(%x) 0x%x\n", 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,
+					    "ia_css_refcount_decrement(): Ref count IDS do not match!\n");
+			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");
+#ifdef ISP2401
+	assert(false);
+#endif
+
+	return false;
+}
+
+bool ia_css_refcount_is_single(hrt_vaddress 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(int32_t id, clear_func clear_func_ptr)
+{
+	struct ia_css_refcount_entry *entry;
+	uint32_t i;
+	uint32_t count = 0;
+
+	assert(clear_func_ptr != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_refcount_clear(%x)\n",
+			    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,
+					    "ia_css_refcount_clear:"
+					    " %x: 0x%x\n", 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,
+						    "ia_css_refcount_clear: "
+						    "using hmm_free: "
+						    "no clear_func\n");
+				hmm_free(entry->data);
+			}
+#ifndef ISP2401
+
+#else
+			assert(entry->count == 0);
+#endif
+			if (entry->count != 0) {
+				IA_CSS_WARNING("Ref count for entry %x is not zero!", entry->id);
+			}
+			entry->data = mmgr_NULL;
+			entry->count = 0;
+			entry->id = 0;
+			count++;
+		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "ia_css_refcount_clear(%x): cleared %d\n", id,
+			    count);
+}
+
+bool ia_css_refcount_is_valid(hrt_vaddress ptr)
+{
+	struct ia_css_refcount_entry *entry;
+
+	if (ptr == mmgr_NULL)
+		return false;
+
+	entry = refcount_find_entry(ptr, false);
+
+	return entry != NULL;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_binarydesc.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_binarydesc.h
new file mode 100644
index 000000000000..616789d9b3f6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_binarydesc.h
@@ -0,0 +1,297 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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
+ *
+ */
+extern 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	IA_CSS_SUCCESS or error code upon error.
+ */
+enum ia_css_err 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/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_stagedesc.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_stagedesc.h
new file mode 100644
index 000000000000..38690ea093c2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_stagedesc.h
@@ -0,0 +1,52 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+extern 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);
+
+extern 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);
+
+extern 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);
+
+extern 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 max_input_width);
+
+#endif /*__IA_CSS_PIPE_STAGEDESC__H__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_util.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_util.h
new file mode 100644
index 000000000000..ba8858759b30
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/interface/ia_css_pipe_util.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+ *
+ */
+extern unsigned int ia_css_pipe_util_pipe_input_format_bpp(
+	const struct ia_css_pipe * const pipe);
+
+extern void ia_css_pipe_util_create_output_frames(
+	struct ia_css_frame *frames[]);
+
+extern 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/atomisp2/css2400/camera/pipe/src/pipe_binarydesc.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_binarydesc.c
new file mode 100644
index 000000000000..17d3b7de93ba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_binarydesc.c
@@ -0,0 +1,883 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+/* 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 != NULL);
+	assert(descr != NULL);
+	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;
+#ifdef ISP2401
+	descr->enable_luma_only = false;
+	descr->enable_tnr = false;
+#endif
+	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 != NULL);
+	assert(in_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	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}
+};
+
+enum ia_css_err sh_css_bds_factor_get_numerator_denominator(
+	unsigned int bds_factor,
+	unsigned int *bds_factor_numerator,
+	unsigned int *bds_factor_denominator)
+{
+	unsigned int i;
+	unsigned int bds_list_size = sizeof(bds_factors_list) /
+				sizeof(struct sh_css_bds_factor);
+
+	/* Loop over all bds factors until a match is found */
+	for (i = 0; i < bds_list_size; 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 IA_CSS_SUCCESS;
+		}
+	}
+
+	/* Throw an error since bds_factor cannot be found
+	in bds_factors_list */
+	return IA_CSS_ERR_INVALID_ARGUMENTS;
+}
+
+enum ia_css_err 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 bds_list_size = sizeof(bds_factors_list) /
+	    sizeof(struct sh_css_bds_factor);
+	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 < bds_list_size; i++) {
+		unsigned num = bds_factors_list[i].numerator;
+		unsigned 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 IA_CSS_SUCCESS;
+		}
+	}
+
+	/* Throw an error since a suitable bds_factor cannot be found */
+	return IA_CSS_ERR_INVALID_ARGUMENTS;
+}
+
+enum ia_css_err 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)
+{
+	enum ia_css_err 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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	assert(vf_info != NULL);
+	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 != IA_CSS_SUCCESS)
+				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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err 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];
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool stream_dz_config = false;
+
+	/* vf_info can be NULL */
+	assert(pipe != NULL);
+	assert(in_info != NULL);
+	/* 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;
+#ifdef ISP2401
+		video_descr->enable_luma_only =
+			pipe->config.enable_luma_only;
+		video_descr->enable_tnr =
+			pipe->config.enable_tnr;
+#endif
+
+		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 != IA_CSS_SUCCESS)
+					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 != NULL);
+	assert(in_info != NULL);
+	/* 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 != NULL);
+	assert(in_info != NULL);
+	assert(vf_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 !defined(HAS_NO_PACKED_RAW_PIXELS)
+	if (pipe->stream->config.pack_raw_pixels)
+		in_info->format = IA_CSS_FRAME_FORMAT_RAW_PACKED;
+	else
+#endif
+		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;
+#ifdef ISP2401
+		prim_descr->enable_luma_only =
+			pipe->config.enable_luma_only;
+#endif
+		/* 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
+#ifndef ISP2401
+			prim_descr->striped = prim_descr->continuous && (!pipe->stream->stop_copy_preview || !pipe->stream->disable_cont_vf);
+#else
+			prim_descr->striped = prim_descr->continuous && !pipe->stream->disable_cont_vf;
+
+		if ((pipe->config.default_capture_config.enable_xnr != 0) &&
+			(pipe->extra_config.enable_dvs_6axis == true))
+				prim_descr->enable_xnr = true;
+#endif
+	}
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	assert(vf_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	assert(vf_info != NULL);
+	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 != NULL);
+	assert(in_info != NULL);
+	assert(out_info != NULL);
+	IA_CSS_ENTER_PRIVATE("");
+
+#ifndef ISP2401
+	*in_info = *out_info;
+#else
+	if (pipe->out_yuv_ds_input_info.res.width)
+		*in_info = pipe->out_yuv_ds_input_info;
+	else
+		*in_info = *out_info;
+#endif
+	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/atomisp2/css2400/camera/pipe/src/pipe_stagedesc.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_stagedesc.c
new file mode 100644
index 000000000000..40af8daf5ad9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_stagedesc.c
@@ -0,0 +1,115 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL && binary != NULL && binary->info != NULL);
+	if (stage_desc == NULL || binary == NULL || binary->info == NULL) {
+		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 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/atomisp2/css2400/camera/pipe/src/pipe_util.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_util.c
new file mode 100644
index 000000000000..5fc1718cb2bd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/pipe/src/pipe_util.c
@@ -0,0 +1,51 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL);
+	assert(pipe->stream != NULL);
+
+	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 != NULL);
+	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/atomisp2/css2400/camera/util/interface/ia_css_util.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/util/interface/ia_css_util.h
new file mode 100644
index 000000000000..f8b2e458f876
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/util/interface/ia_css_util.h
@@ -0,0 +1,141 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <ia_css_err.h>
+#include <error_support.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
+ *
+ */
+enum ia_css_err ia_css_convert_errno(
+	int in_err);
+
+/** @brief check vf frame info.
+ *
+ * @param[in] info
+ * @return	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+extern enum ia_css_err ia_css_util_check_res(
+	unsigned int width,
+	unsigned int height);
+
+#ifdef 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
+ *
+ */
+extern bool ia_css_util_res_leq(
+	struct ia_css_resolution a,
+	struct ia_css_resolution b);
+
+/**
+ * @brief Check if resolution is zero
+ *
+ * @param[in] resolution The resolution to check
+ *
+ * @returns true if resolution is zero
+ */
+extern bool ia_css_util_resolution_is_zero(
+		const struct ia_css_resolution resolution);
+
+/**
+ * @brief Check if resolution is even
+ *
+ * @param[in] resolution The resolution to check
+ *
+ * @returns true if resolution is even
+ */
+extern bool ia_css_util_resolution_is_even(
+		const struct ia_css_resolution resolution);
+
+#endif
+/** @brief check width and height
+ *
+ * @param[in] stream_format
+ * @param[in] two_ppc
+ * @return bits per pixel based on given parameters.
+ *
+ */
+extern unsigned int ia_css_util_input_format_bpp(
+	enum ia_css_stream_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
+ *
+ */
+extern bool ia_css_util_is_input_format_raw(
+	enum ia_css_stream_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
+ *
+ */
+extern bool ia_css_util_is_input_format_yuv(
+	enum ia_css_stream_format stream_format);
+
+#endif /* __IA_CSS_UTIL_H__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/util/src/util.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/util/src/util.c
new file mode 100644
index 000000000000..08f486e20a65
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/camera/util/src/util.c
@@ -0,0 +1,227 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+
+enum ia_css_err ia_css_convert_errno(
+				int in_err)
+{
+	enum ia_css_err out_err;
+
+	switch (in_err) {
+		case 0:
+			out_err = IA_CSS_SUCCESS;
+			break;
+		case EINVAL:
+			out_err = IA_CSS_ERR_INVALID_ARGUMENTS;
+			break;
+		case ENODATA:
+			out_err = IA_CSS_ERR_QUEUE_IS_EMPTY;
+			break;
+		case ENOSYS:
+		case ENOTSUP:
+			out_err = IA_CSS_ERR_INTERNAL_ERROR;
+			break;
+		case ENOBUFS:
+			out_err = IA_CSS_ERR_QUEUE_IS_FULL;
+			break;
+		default:
+			out_err = IA_CSS_ERR_INTERNAL_ERROR;
+			break;
+	}
+	return out_err;
+}
+
+/* MW: Table look-up ??? */
+unsigned int ia_css_util_input_format_bpp(
+	enum ia_css_stream_format format,
+	bool two_ppc)
+{
+	unsigned int rval = 0;
+	switch (format) {
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_RGB_888:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+	case IA_CSS_STREAM_FORMAT_EMBEDDED:
+		rval = 8;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+		rval = 10;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+		rval = 16;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+		rval = 4;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+		rval = 5;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+		rval = 65;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+		rval = 6;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+		rval = 7;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+		rval = 12;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+		if (two_ppc)
+			rval = 14;
+		else
+			rval = 12;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+		if (two_ppc)
+			rval = 16;
+		else
+			rval = 12;
+		break;
+	default:
+		rval = 0;
+		break;
+
+	}
+return rval;
+}
+
+enum ia_css_err ia_css_util_check_vf_info(
+	const struct ia_css_frame_info * const info)
+{
+	enum ia_css_err err;
+	unsigned int max_vf_width;
+	assert(info != NULL);
+	err = ia_css_frame_check_info(info);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	max_vf_width = ia_css_binary_max_vf_width();
+	if (max_vf_width != 0 && info->res.width > max_vf_width*2)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err 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)
+{
+	enum ia_css_err err;
+
+	assert(out_info != NULL);
+	assert(vf_info != NULL);
+
+	err = ia_css_frame_check_info(out_info);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	err = ia_css_util_check_vf_info(vf_info);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	return IA_CSS_SUCCESS;
+}
+
+#ifdef 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;
+}
+
+bool ia_css_util_resolution_is_zero(const struct ia_css_resolution resolution)
+{
+	return (resolution.width == 0) || (resolution.height == 0);
+}
+
+bool ia_css_util_resolution_is_even(const struct ia_css_resolution resolution)
+{
+	return IS_EVEN(resolution.height) && IS_EVEN(resolution.width);
+}
+
+#endif
+bool ia_css_util_is_input_format_raw(enum ia_css_stream_format format)
+{
+	return ((format == IA_CSS_STREAM_FORMAT_RAW_6) ||
+		(format == IA_CSS_STREAM_FORMAT_RAW_7) ||
+		(format == IA_CSS_STREAM_FORMAT_RAW_8) ||
+		(format == IA_CSS_STREAM_FORMAT_RAW_10) ||
+		(format == IA_CSS_STREAM_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 ia_css_stream_format format)
+{
+	return format == IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY ||
+	    format == IA_CSS_STREAM_FORMAT_YUV420_8  ||
+	    format == IA_CSS_STREAM_FORMAT_YUV420_10 ||
+	    format == IA_CSS_STREAM_FORMAT_YUV420_16 ||
+	    format == IA_CSS_STREAM_FORMAT_YUV422_8  ||
+	    format == IA_CSS_STREAM_FORMAT_YUV422_10 ||
+	    format == IA_CSS_STREAM_FORMAT_YUV422_16;
+}
+
+enum ia_css_err 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 != NULL);
+
+	if (stream_config == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+#ifdef IS_ISP_2400_SYSTEM
+	if (stream_config->input_config.effective_res.width == 0 ||
+	    stream_config->input_config.effective_res.height == 0)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+#endif
+	if (must_be_raw &&
+	    !ia_css_util_is_input_format_raw(stream_config->input_config.format))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	if (must_be_yuv &&
+	    !ia_css_util_is_input_format_yuv(stream_config->input_config.format))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	return IA_CSS_SUCCESS;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.c
new file mode 100644
index 000000000000..325b821f276c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.c
@@ -0,0 +1,360 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.iterator.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.iterator.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.copy_output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.copy_output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.crop.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.crop.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.fpn.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.fpn.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.dvs.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.dvs.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.qplane.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.qplane.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output0.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output0.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output1.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output1.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#ifdef ISP2401
+
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.sc.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.sc.offset;
+		}
+		if (size) {
+			ia_css_sc_config((struct sh_css_isp_sc_isp_config *)
+					&binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+					config_dmem, size);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#endif
+
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.raw.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.raw.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.tnr.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.tnr.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.ref.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.ref.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_vf(
+	const struct ia_css_binary *binary,
+	const struct ia_css_vf_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.vf.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.vf.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() leave:\n");
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.h
new file mode 100644
index 000000000000..8aacd3dbc05a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_configs.h
@@ -0,0 +1,189 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "isp/kernels/sc/sc_1.0/ia_css_sc.host.h"
+#endif
+#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 /* IA_CSS_INCLUDE_CONFIGURATIONS */
+/* Generated code: do not edit or commmit. */
+
+#ifndef _IA_CSS_ISP_CONFIG_H
+#define _IA_CSS_ISP_CONFIG_H
+
+/* Code generated by genparam/gencode.c:gen_param_enum() */
+
+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,
+#ifdef ISP2401
+	IA_CSS_SC_CONFIG_ID,
+#endif
+	IA_CSS_RAW_CONFIG_ID,
+	IA_CSS_TNR_CONFIG_ID,
+	IA_CSS_REF_CONFIG_ID,
+	IA_CSS_VF_CONFIG_ID,
+	IA_CSS_NUM_CONFIGURATION_IDS
+};
+
+/* Code generated by genparam/gencode.c:gen_param_offsets() */
+
+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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter sc;
+#endif
+		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 */
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#ifdef ISP2401
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#endif
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+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/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.c
new file mode 100644
index 000000000000..d418e763b755
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.c
@@ -0,0 +1,3221 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.aa.size;
+	unsigned 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;
+	}
+	params->isp_params_changed = true;
+	params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] = true;
+}
+
+/* Code generated by genparam/gencode.c:gen_process_function() */
+
+static void
+ia_css_process_anr(
+	unsigned pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.anr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.anr2.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bh.size;
+
+		unsigned 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 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.cnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.crop.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.csc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.dp.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.de.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ecd.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.formats.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fpn.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.gc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem1.gc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ce.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yuv2rgb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.rgb2yuv.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem0.r_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.g_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem2.b_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.uds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.raa.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.s3a.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ob.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vmem.ob.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.output.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.tnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.macc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.wb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.nr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yee.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ynr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ctc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem0.ctc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.xnr_table.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr3.size;
+
+		unsigned 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");
+		}
+
+	}
+#ifdef ISP2401
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.size;
+
+		unsigned offset = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_xnr3() enter:\n");
+
+			ia_css_xnr3_vmem_encode((struct sh_css_isp_xnr3_vmem_params *)
+					&stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+					&params->xnr3_config,
+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_xnr3() leave:\n");
+		}
+
+	}
+#endif
+}
+
+/* Code generated by genparam/gencode.c:gen_param_process_table() */
+
+void (* ia_css_kernel_process_param[IA_CSS_NUM_PARAMETER_IDS])(
+			unsigned 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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DP_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_WB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_TNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR2_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ECD_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_MACC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CTC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_AA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YUV2RGB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_RGB2YUV_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CSC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_NR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_GC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_R_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_G_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_B_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_TABLE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FORMATS_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR3_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_S3A_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OUTPUT_ID] = true;
+
+#endif
+	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/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.h
new file mode 100644
index 000000000000..5b3deb7f74ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_params.h
@@ -0,0 +1,399 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter xnr3;
+#endif
+	} 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 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)
+;
+#ifdef ISP2401
+
+#endif
+/* 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)
+;
+#ifdef ISP2401
+
+#endif
+#endif /* IA_CSS_INCLUDE_PARAMETER */
+
+#endif /* _IA_CSS_ISP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.c
new file mode 100644
index 000000000000..fb3ba08f69c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.c
@@ -0,0 +1,214 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size   = binary->info->mem_offsets.offsets.state->vmem.aa.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr2.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.dp.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.de.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.tnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.ref.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.ynr.size;
+
+		unsigned 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/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.h
new file mode 100644
index 000000000000..732adafb0a63
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hive_isp_css_2400_system_generated/ia_css_isp_states.h
@@ -0,0 +1,72 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/css_2400_system/hrt/bits.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/bits.h
new file mode 100644
index 000000000000..e71e33d9d143
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/bits.h
@@ -0,0 +1,104 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/cell_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/cell_params.h
new file mode 100644
index 000000000000..b5756bfe8eb6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/cell_params.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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        (1<<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/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_common_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_common_defs.h
new file mode 100644
index 000000000000..f3054fe04d03
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_common_defs.h
@@ -0,0 +1,200 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 reseved 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/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_defs.h
new file mode 100644
index 000000000000..6f5b7d3d3715
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/css_receiver_2400_defs.h
@@ -0,0 +1,258 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 (1<<16)
+#define _HRT_CSS_RECEIVER_2400_VSYNC_VAL (1<<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/atomisp2/css2400/css_2400_system/hrt/defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/defs.h
new file mode 100644
index 000000000000..47505f41790c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/dma_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/dma_v2_defs.h
new file mode 100644
index 000000000000..d184a8b313c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/dma_v2_defs.h
@@ -0,0 +1,199 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/gdc_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gdc_v2_defs.h
new file mode 100644
index 000000000000..77722d205701
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gdc_v2_defs.h
@@ -0,0 +1,170 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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             (1 << 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/atomisp2/css2400/css_2400_system/hrt/gp_regs_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gp_regs_defs.h
new file mode 100644
index 000000000000..34e734f6648e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gp_regs_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_regs_defs_h
+#define _gp_regs_defs_h
+
+#define _HRT_GP_REGS_IS_FWD_REG_IDX 0
+
+#define _HRT_GP_REGS_REG_ALIGN 4
+
+#endif /* _gp_regs_defs_h */   
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gp_timer_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gp_timer_defs.h
new file mode 100644
index 000000000000..3082e2f5e014
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gp_timer_defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/gpio_block_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gpio_block_defs.h
new file mode 100644
index 000000000000..a807d4c99041
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/gpio_block_defs.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_defs.h
new file mode 100644
index 000000000000..39584996092e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_defs.h
@@ -0,0 +1,416 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CSS_IS_2400B0_SYSTEM
+
+#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     (1<<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
+#define HIVE_GP_REGS_VISA_REG_IDX                              21
+
+/* 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_PMEM_ERROR_IRQ                       19
+#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/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_host_ids_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_host_ids_hrt.h
new file mode 100644
index 000000000000..f4d033e221cc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_host_ids_hrt.h
@@ -0,0 +1,84 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_host_ids_hrt_h_
+#define _hive_isp_css_host_ids_hrt_h_
+
+/* ISP_CSS identifiers */
+#define INP_SYS       testbench_isp_inp_sys
+#define ISYS_GP_REGS  testbench_isp_inp_sys_gpreg
+#define ISYS_IRQ_CTRL testbench_isp_inp_sys_irq_ctrl
+#define ISYS_CAP_A    testbench_isp_inp_sys_capt_unit_a
+#define ISYS_CAP_B    testbench_isp_inp_sys_capt_unit_b
+#define ISYS_CAP_C    testbench_isp_inp_sys_capt_unit_c
+#define ISYS_INP_BUF  testbench_isp_inp_sys_input_buffer
+#define ISYS_INP_CTRL testbench_isp_inp_sys_inp_ctrl
+#define ISYS_ACQ      testbench_isp_inp_sys_acq_unit
+
+#define ISP           testbench_isp_isp
+#define SP            testbench_isp_scp
+
+#define IF_PRIM       testbench_isp_ifmt_ift_prim  
+#define IF_PRIM_B     testbench_isp_ifmt_ift_prim_b
+#define IF_SEC        testbench_isp_ifmt_ift_sec
+#define IF_SEC_MASTER testbench_isp_ifmt_ift_sec_mt_out
+#define STR_TO_MEM    testbench_isp_ifmt_mem_cpy
+#define IFMT_GP_REGS  testbench_isp_ifmt_gp_reg
+#define IFMT_IRQ_CTRL testbench_isp_ifmt_irq_ctrl
+
+#define CSS_RECEIVER  testbench_isp_inp_sys_csi_receiver
+
+#define TC            testbench_isp_gpd_tc
+#define GPTIMER       testbench_isp_gpd_gptimer
+#define DMA           testbench_isp_isp_dma
+#define GDC           testbench_isp_gdc1
+#define GDC2          testbench_isp_gdc2
+#define IRQ_CTRL      testbench_isp_gpd_irq_ctrl
+#define GPIO          testbench_isp_gpd_c_gpio
+#define GP_REGS       testbench_isp_gpd_gp_reg
+#define ISEL_GP_REGS  testbench_isp_isel_gpr
+#define ISEL_IRQ_CTRL testbench_isp_isel_irq_ctrl
+#define DATA_MMU      testbench_isp_data_out_sys_c_mmu
+#define ICACHE_MMU    testbench_isp_icache_out_sys_c_mmu
+
+/* next is actually not FIFO but FIFO adapter, or slave to streaming adapter */
+#define ISP_SP_FIFO   testbench_isp_fa_sp_isp
+#define ISEL_FIFO     testbench_isp_isel_sf_fa_in
+
+#define FIFO_GPF_SP   testbench_isp_sf_fa2sp_in
+#define FIFO_GPF_ISP  testbench_isp_sf_fa2isp_in
+#define FIFO_SP_GPF   testbench_isp_sf_sp2fa_in
+#define FIFO_ISP_GPF  testbench_isp_sf_isp2fa_in
+
+#define DATA_OCP_MASTER    testbench_isp_data_out_sys_cio2ocp_wide_data_out_mt
+#define ICACHE_OCP_MASTER  testbench_isp_icache_out_sys_cio2ocp_wide_data_out_mt
+
+#define SP_IN_FIFO    testbench_isp_sf_fa2sp_in
+#define SP_OUT_FIFO   testbench_isp_sf_sp2fa_out
+#define ISP_IN_FIFO   testbench_isp_sf_fa2isp_in
+#define ISP_OUT_FIFO  testbench_isp_sf_isp2fa_out
+#define GEN_SHORT_PACK_PORT testbench_isp_inp_sys_csi_str_mon_fa_gensh_out
+#define ISYS_GP_REGS  testbench_isp_inp_sys_gpreg
+
+/* Testbench identifiers */
+#define DDR             testbench_ddram
+#define DDR_SMALL       testbench_ddram_small
+#define XMEM            DDR
+#define GPIO_ADAPTER    testbench_gp_adapter
+#define SIG_MONITOR     testbench_sig_mon
+#define DDR_SLAVE       testbench_ddram_ip0
+#define DDR_SMALL_SLAVE testbench_ddram_small_ip0
+#define HOST_MASTER     host_op0
+
+#endif /* _hive_isp_css_host_ids_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_irq_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_irq_types_hrt.h
new file mode 100644
index 000000000000..04c237083835
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_irq_types_hrt.h
@@ -0,0 +1,72 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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      ,
+#ifdef _HIVE_ISP_CSS_2401_SYSTEM
+  hrt_isp_css_irq_is2401               = HIVE_GP_DEV_IRQ_IS2401_BIT_ID              ,           
+#else
+  hrt_isp_css_irq_isp_pmem_error       = HIVE_GP_DEV_IRQ_ISP_PMEM_ERROR_BIT_ID      ,           
+#endif
+  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/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
new file mode 100644
index 000000000000..b4211a0c631a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/hive_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_types.h
new file mode 100644
index 000000000000..58b0e6effbd0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/hive_types.h
@@ -0,0 +1,128 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+
+/* by default assume 32 bit master port (both data and address) */
+#ifndef HRT_DATA_WIDTH
+#define HRT_DATA_WIDTH 32
+#endif
+#ifndef HRT_ADDRESS_WIDTH
+#define HRT_ADDRESS_WIDTH 32
+#endif
+
+#define HRT_DATA_BYTES    (HRT_DATA_WIDTH/8)
+#define HRT_ADDRESS_BYTES (HRT_ADDRESS_WIDTH/8)
+
+#if HRT_DATA_WIDTH == 64
+typedef hive_uint64 hrt_data;
+#elif HRT_DATA_WIDTH == 32
+typedef hive_uint32 hrt_data;
+#else
+#error data width not supported
+#endif
+
+#if HRT_ADDRESS_WIDTH == 64
+typedef hive_uint64 hrt_address; 
+#elif HRT_ADDRESS_WIDTH == 32
+typedef hive_uint32 hrt_address;
+#else
+#error adddres width not supported
+#endif
+
+/* The SP side representation of an HMM virtual address */
+typedef hive_uint32 hrt_vaddress;
+
+/* 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/atomisp2/css2400/css_2400_system/hrt/if_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/if_defs.h
new file mode 100644
index 000000000000..7d39e45796ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/if_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/input_formatter_subsystem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_formatter_subsystem_defs.h
new file mode 100644
index 000000000000..16bfe1d80bc9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_formatter_subsystem_defs.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/input_selector_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_selector_defs.h
new file mode 100644
index 000000000000..87fbf82edb5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_selector_defs.h
@@ -0,0 +1,89 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/input_switch_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_switch_2400_defs.h
new file mode 100644
index 000000000000..20a13c4cdb56
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_switch_2400_defs.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/input_system_ctrl_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_system_ctrl_defs.h
new file mode 100644
index 000000000000..a7f0ca80bc9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_system_ctrl_defs.h
@@ -0,0 +1,254 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2400_system/hrt/input_system_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_system_defs.h
new file mode 100644
index 000000000000..ae62163034a6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/input_system_defs.h
@@ -0,0 +1,126 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/irq_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/irq_controller_defs.h
new file mode 100644
index 000000000000..ec6dd4487158
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/irq_controller_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/isp2400_mamoiada_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp2400_mamoiada_params.h
new file mode 100644
index 000000000000..669060d17c4f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp2400_mamoiada_params.h
@@ -0,0 +1,254 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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
+
+/* Cell name  */
+#define ISP_CELL_TYPE                          isp2400_mamoiada
+#define ISP_VMEM                               simd_vmem
+#define _HRT_ISP_VMEM                          isp2400_mamoiada_simd_vmem
+
+/* 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_VMEM_IS_BAMEM                      1
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_BAMEM_MAX_BOI_HEIGHT        8
+  #define ISP_VMEM_BAMEM_LATENCY               5
+  #define ISP_VMEM_BAMEM_BANK_NARROWING_FACTOR 2
+  #define ISP_VMEM_BAMEM_NR_DATA_PLANES        8
+  #define ISP_VMEM_BAMEM_NR_CFG_REGISTERS      16
+  #define ISP_VMEM_BAMEM_LININT                0
+  #define ISP_VMEM_BAMEM_DAP_BITS              3
+  #define ISP_VMEM_BAMEM_LININT_FRAC_BITS      0
+  #define ISP_VMEM_BAMEM_PID_BITS              3
+  #define ISP_VMEM_BAMEM_OFFSET_BITS           19
+  #define ISP_VMEM_BAMEM_ADDRESS_BITS          25
+  #define ISP_VMEM_BAMEM_RID_BITS              4
+  #define ISP_VMEM_BAMEM_TRANSPOSITION         1
+  #define ISP_VMEM_BAMEM_VEC_PLUS_SLICE        1
+  #define ISP_VMEM_BAMEM_ARB_SERVICE_CYCLE_BITS 1
+  #define ISP_VMEM_BAMEM_LUT_ELEMS             16
+  #define ISP_VMEM_BAMEM_LUT_ADDR_WIDTH        14
+  #define ISP_VMEM_BAMEM_HALF_BLOCK_WRITE      1
+  #define ISP_VMEM_BAMEM_SMART_FETCH           1
+  #define ISP_VMEM_BAMEM_BIG_ENDIANNESS        0
+#endif /* ISP_VMEM_IS_BAMEM */
+#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
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_ALIGN_ELEM                  2
+#endif /* ISP_VMEM_IS_BAMEM */
+#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
+
+/* HRT specific vector support */
+#define isp2400_mamoiada_vector_alignment         ISP_VEC_ALIGN
+#define isp2400_mamoiada_vector_elem_bits         ISP_VMEM_ELEMBITS
+#define isp2400_mamoiada_vector_elem_precision    ISP_VMEM_ELEM_PRECISION
+#define isp2400_mamoiada_vector_num_elems         ISP_VEC_NELEMS
+
+/* 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_VRF1_SIZE       24
+#define ISP_VRF2_SIZE       24
+#define ISP_VRF3_SIZE       24
+#define ISP_VRF4_SIZE       24
+#define ISP_VRF5_SIZE       24
+#define ISP_VRF6_SIZE       24
+#define ISP_VRF7_SIZE       24
+#define ISP_VRF8_SIZE       24
+#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/atomisp2/css2400/css_2400_system/hrt/isp2400_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp2400_support.h
new file mode 100644
index 000000000000..e00bc841d0f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp2400_support.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/isp_acquisition_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp_acquisition_defs.h
new file mode 100644
index 000000000000..593620721627
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp_acquisition_defs.h
@@ -0,0 +1,234 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2400_system/hrt/isp_capture_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp_capture_defs.h
new file mode 100644
index 000000000000..0a249ce3e589
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/isp_capture_defs.h
@@ -0,0 +1,310 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 CAPT_RCV_ACK                              1
+//#define CAPT_WRT_ACK                              2               
+//#define CAPT_IRQ_ACK                              3                        
+
+/* --------------------------------------------------*/
+
+#define NOF_IRQS                                  2
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+// Number of registers
+#define CAPT_NOF_REGS                             16
+
+// Register id's of MMIO slave accesible 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_ADDR_REG_WIDTH                 9
+#define CAPT_MEM_REGION_SIZE_REG_WIDTH            9
+#define CAPT_NUM_MEM_REGIONS_REG_WIDTH            9
+#define CAPT_INIT_REG_WIDTH                       (18 + 4)
+
+#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_NEXT_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))
+
+#define CAPT_INIT_RESTART_MEM_ADDR_WIDTH          9   
+#define CAPT_INIT_RESTART_MEM_REGION_WIDTH        9 
+
+/* 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_RESTART_MEM_ADDR_MSB            12
+#define CAPT_INIT_RESTART_MEM_REGION_LSB          13
+#define CAPT_INIT_RESTART_MEM_REGION_MSB          21
+
+
+#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
+#define CAPT_INIT_RESTART_MEM_ADDR_BITS           (CAPT_INIT_RESTART_MEM_ADDR_MSB - CAPT_INIT_RESTART_MEM_ADDR_LSB + 1)
+#define CAPT_INIT_RESTART_MEM_REGION_IDX          CAPT_INIT_RESTART_MEM_REGION_LSB
+#define CAPT_INIT_RESTART_MEM_REGION_BITS         (CAPT_INIT_RESTART_MEM_REGION_MSB - CAPT_INIT_RESTART_MEM_REGION_LSB + 1)
+
+
+
+/* --------------------------------------------------*/
+/* 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/atomisp2/css2400/css_2400_system/hrt/mmu_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/mmu_defs.h
new file mode 100644
index 000000000000..c038f39ffd25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/mmu_defs.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/scalar_processor_2400_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/scalar_processor_2400_params.h
new file mode 100644
index 000000000000..9b6c2893d950
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/scalar_processor_2400_params.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/sp_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/sp_hrt.h
new file mode 100644
index 000000000000..7ee4deba519a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/sp_hrt.h
@@ -0,0 +1,24 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_hrt_h_
+#define _sp_hrt_h_
+
+#define hrt_sp_dmem(cell) HRT_PROC_TYPE_PROP(cell, _dmem)
+
+#define hrt_sp_dmem_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_sp_dmem(cell))
+
+#endif /* _sp_hrt_h_ */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/str2mem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/str2mem_defs.h
new file mode 100644
index 000000000000..1cb62444cf68
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/str2mem_defs.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/streaming_to_mipi_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/streaming_to_mipi_defs.h
new file mode 100644
index 000000000000..60143b8743a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/streaming_to_mipi_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/timed_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/timed_controller_defs.h
new file mode 100644
index 000000000000..d2b8972b0d9e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/timed_controller_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2400_system/hrt/var.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/var.h
new file mode 100644
index 000000000000..5bc0ad34616e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/var.h
@@ -0,0 +1,74 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_VAR_H
+#define _HRT_VAR_H
+
+#include "version.h"
+#include "system_api.h"
+#include "hive_types.h"
+
+#define hrt_int_type_of_char   char
+#define hrt_int_type_of_uchar  unsigned char
+#define hrt_int_type_of_short  short
+#define hrt_int_type_of_ushort unsigned short
+#define hrt_int_type_of_int    int
+#define hrt_int_type_of_uint   unsigned int
+#define hrt_int_type_of_long   long
+#define hrt_int_type_of_ulong  unsigned long
+#define hrt_int_type_of_ptr    unsigned int
+
+#define hrt_host_type_of_char   char
+#define hrt_host_type_of_uchar  unsigned char
+#define hrt_host_type_of_short  short
+#define hrt_host_type_of_ushort unsigned short
+#define hrt_host_type_of_int    int
+#define hrt_host_type_of_uint   unsigned int
+#define hrt_host_type_of_long   long
+#define hrt_host_type_of_ulong  unsigned long
+#define hrt_host_type_of_ptr    void*
+
+#define HRT_TYPE_BYTES(cell, type) (HRT_TYPE_BITS(cell, type)/8)
+#define HRT_HOST_TYPE(cell_type)   HRTCAT(hrt_host_type_of_, cell_type)
+#define HRT_INT_TYPE(type)         HRTCAT(hrt_int_type_of_, type)
+
+#define hrt_scalar_store(cell, type, var, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_scalar_load(cell, type, var) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var)))
+
+#define hrt_indexed_store(cell, type, array, index, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type)), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_indexed_load(cell, type, array, index) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+         cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type))))
+
+#endif /* _HRT_VAR_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/version.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/version.h
new file mode 100644
index 000000000000..bbc4948baea9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/hrt/version.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp/pci/atomisp2/css2400/css_2400_system/spmem_dump.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/spmem_dump.c
new file mode 100644
index 000000000000..ddc7a8f05153
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2400_system/spmem_dump.c
@@ -0,0 +1,3634 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_map_h_
+#define _sp_map_h_
+
+
+#ifndef _hrt_dummy_use_blob_sp
+#define _hrt_dummy_use_blob_sp()
+#endif
+
+#define _hrt_cell_load_program_sp(proc) _hrt_cell_load_program_embedded(proc, sp)
+
+#ifndef ISP2401
+/* function input_system_acquisition_stop: ADE */
+#else
+/* function input_system_acquisition_stop: AD8 */
+#endif
+
+#ifndef ISP2401
+/* function longjmp: 684E */
+#else
+/* function longjmp: 69C1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SRST_MASK
+#define HIVE_MEM_HIVE_IF_SRST_MASK scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SRST_MASK 0x1C8
+#define HIVE_SIZE_HIVE_IF_SRST_MASK 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SRST_MASK scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SRST_MASK 0x1C8
+#define HIVE_SIZE_sp_HIVE_IF_SRST_MASK 16
+
+#ifndef ISP2401
+/* function tmpmem_init_dmem: 6580 */
+#else
+/* function tmpmem_init_dmem: 66BB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_receive_ack: 5EC4 */
+#else
+/* function ia_css_isys_sp_token_map_receive_ack: 5FFF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_addr_B: 332C */
+#else
+/* function ia_css_dmaproxy_sp_set_addr_B: 3520 */
+
+/* function ia_css_pipe_data_init_tagger_resources: A4F */
+#endif
+
+/* function debug_buffer_set_ddr_addr: DD */
+
+#ifndef ISP2401
+/* function receiver_port_reg_load: AC2 */
+#else
+/* function receiver_port_reg_load: ABC */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_mipi
+#define HIVE_MEM_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_mipi 0x631C
+#else
+#define HIVE_ADDR_vbuf_mipi 0x6378
+#endif
+#define HIVE_SIZE_vbuf_mipi 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_mipi 0x631C
+#else
+#define HIVE_ADDR_sp_vbuf_mipi 0x6378
+#endif
+#define HIVE_SIZE_sp_vbuf_mipi 12
+
+#ifndef ISP2401
+/* function ia_css_event_sp_decode: 351D */
+#else
+/* function ia_css_event_sp_decode: 3711 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_get_size: 48A5 */
+#else
+/* function ia_css_queue_get_size: 4B2D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_load: 4EE6 */
+#else
+/* function ia_css_queue_load: 5144 */
+#endif
+
+#ifndef ISP2401
+/* function setjmp: 6857 */
+#else
+/* function setjmp: 69CA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_isys_event_queue
+#define HIVE_MEM_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x4684
+#else
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x46CC
+#endif
+#define HIVE_SIZE_sem_for_sp2host_isys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x4684
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x46CC
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_isys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6E07 */
+#else
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6F4B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_func: 510B */
+#else
+/* function ia_css_sp_rawcopy_func: 5369 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_marked: 29F7 */
+#else
+/* function ia_css_tagger_buf_sp_pop_marked: 2B99 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stage
+#define HIVE_MEM_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stage 0x5C00
+#else
+#define HIVE_ADDR_isp_stage 0x5C60
+#endif
+#define HIVE_SIZE_isp_stage 832
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stage 0x5C00
+#else
+#define HIVE_ADDR_sp_isp_stage 0x5C60
+#endif
+#define HIVE_SIZE_sp_isp_stage 832
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_raw
+#define HIVE_MEM_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_raw 0x2F4
+#else
+#define HIVE_ADDR_vbuf_raw 0x30C
+#endif
+#define HIVE_SIZE_vbuf_raw 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_raw 0x2F4
+#else
+#define HIVE_ADDR_sp_vbuf_raw 0x30C
+#endif
+#define HIVE_SIZE_sp_vbuf_raw 4
+
+#ifndef ISP2401
+/* function ia_css_sp_bin_copy_func: 5032 */
+#else
+/* function ia_css_sp_bin_copy_func: 5290 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_item_store: 4C34 */
+#else
+/* function ia_css_queue_item_store: 4E92 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AA0
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AFC
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AA0
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AFC
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4AB4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4B10
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4AB4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4B10
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+
+/* function sp_start_isp: 45D */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_binary_group
+#define HIVE_MEM_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_binary_group 0x5FF0
+#else
+#define HIVE_ADDR_sp_binary_group 0x6050
+#endif
+#define HIVE_SIZE_sp_binary_group 32
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_binary_group 0x5FF0
+#else
+#define HIVE_ADDR_sp_sp_binary_group 0x6050
+#endif
+#define HIVE_SIZE_sp_sp_binary_group 32
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sw_state
+#define HIVE_MEM_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sw_state 0x62AC
+#else
+#define HIVE_ADDR_sp_sw_state 0x6308
+#endif
+#define HIVE_SIZE_sp_sw_state 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sw_state 0x62AC
+#else
+#define HIVE_ADDR_sp_sp_sw_state 0x6308
+#endif
+#define HIVE_SIZE_sp_sp_sw_state 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_main: D5B */
+#else
+/* function ia_css_thread_sp_main: D50 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_internal_buffers: 3723 */
+#else
+/* function ia_css_ispctrl_sp_init_internal_buffers: 3952 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_psys_event_queue_handle
+#define HIVE_MEM_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x4B54
+#else
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x4BB0
+#endif
+#define HIVE_SIZE_sp2host_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x4B54
+#else
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x4BB0
+#endif
+#define HIVE_SIZE_sp_sp2host_psys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_psys_event_queue
+#define HIVE_MEM_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x4698
+#else
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x46E0
+#endif
+#define HIVE_SIZE_sem_for_sp2host_psys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x4698
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x46E0
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_psys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_propagate_frame: 2410 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_stop_copy_preview
+#define HIVE_MEM_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_stop_copy_preview 0x6290
+#define HIVE_SIZE_sp_stop_copy_preview 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_stop_copy_preview 0x6290
+#define HIVE_SIZE_sp_sp_stop_copy_preview 4
+#else
+/* function ia_css_tagger_sp_propagate_frame: 2460 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_reg_load: B17 */
+#else
+/* function input_system_reg_load: B11 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_handles
+#define HIVE_MEM_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_handles 0x6328
+#else
+#define HIVE_ADDR_vbuf_handles 0x6384
+#endif
+#define HIVE_SIZE_vbuf_handles 960
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_handles 0x6328
+#else
+#define HIVE_ADDR_sp_vbuf_handles 0x6384
+#endif
+#define HIVE_SIZE_sp_vbuf_handles 960
+
+#ifndef ISP2401
+/* function ia_css_queue_store: 4D9A */
+
+/* function ia_css_sp_flash_register: 2C2C */
+#else
+/* function ia_css_queue_store: 4FF8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_dummy_function: 5652 */
+#else
+/* function ia_css_sp_flash_register: 2DCE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_create: 5B37 */
+#else
+/* function ia_css_isys_sp_backend_create: 5C72 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_init: 1833 */
+#else
+/* function ia_css_pipeline_sp_init: 186D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_configure: 2300 */
+#else
+/* function ia_css_tagger_sp_configure: 2350 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_end_binary: 3566 */
+#else
+/* function ia_css_ispctrl_sp_end_binary: 375A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4B60
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4BBC
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4B60
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4BBC
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function receiver_port_reg_store: AC9 */
+#else
+/* function receiver_port_reg_store: AC3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_is_pending_mask
+#define HIVE_MEM_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_is_pending_mask 0x5C
+#define HIVE_SIZE_event_is_pending_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_is_pending_mask 0x5C
+#define HIVE_SIZE_sp_event_is_pending_mask 44
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_frame
+#define HIVE_MEM_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x46AC
+#else
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x46F4
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x46AC
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x46F4
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_frame 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_isys_event_queue_handle
+#define HIVE_MEM_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x4B74
+#else
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x4BD0
+#endif
+#define HIVE_SIZE_sp2host_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x4B74
+#else
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x4BD0
+#endif
+#define HIVE_SIZE_sp_sp2host_isys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_com
+#define HIVE_MEM_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_com 0x4114
+#else
+#define HIVE_ADDR_host_sp_com 0x4134
+#endif
+#define HIVE_SIZE_host_sp_com 220
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_com 0x4114
+#else
+#define HIVE_ADDR_sp_host_sp_com 0x4134
+#endif
+#define HIVE_SIZE_sp_host_sp_com 220
+
+#ifndef ISP2401
+/* function ia_css_queue_get_free_space: 49F9 */
+#else
+/* function ia_css_queue_get_free_space: 4C57 */
+#endif
+
+#ifndef ISP2401
+/* function exec_image_pipe: 6C4 */
+#else
+/* function exec_image_pipe: 658 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_init_dmem_data
+#define HIVE_MEM_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_init_dmem_data 0x62B0
+#else
+#define HIVE_ADDR_sp_init_dmem_data 0x630C
+#endif
+#define HIVE_SIZE_sp_init_dmem_data 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x62B0
+#else
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x630C
+#endif
+#define HIVE_SIZE_sp_sp_init_dmem_data 24
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_start: 5914 */
+#else
+/* function ia_css_sp_metadata_start: 5A4F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_init_buffer_queues: 2C9B */
+#else
+/* function ia_css_bufq_sp_init_buffer_queues: 2E3D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_stop: 1816 */
+#else
+/* function ia_css_pipeline_sp_stop: 1850 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_connect_pipes: 27EA */
+#else
+/* function ia_css_tagger_sp_connect_pipes: 283A */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_wait: 70D */
+#else
+/* function sp_isys_copy_wait: 6A1 */
+#endif
+
+/* function is_isp_debug_buffer_full: 337 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 32AF */
+#else
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 3490 */
+#endif
+
+#ifndef ISP2401
+/* function encode_and_post_timer_event: A30 */
+#else
+/* function encode_and_post_timer_event: 9C4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_per_frame_data
+#define HIVE_MEM_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_per_frame_data 0x41F0
+#else
+#define HIVE_ADDR_sp_per_frame_data 0x4210
+#endif
+#define HIVE_SIZE_sp_per_frame_data 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_per_frame_data 0x41F0
+#else
+#define HIVE_ADDR_sp_sp_per_frame_data 0x4210
+#endif
+#define HIVE_SIZE_sp_sp_per_frame_data 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_dequeue: 62D4 */
+#else
+/* function ia_css_rmgr_sp_vbuf_dequeue: 640F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_psys_event_queue_handle
+#define HIVE_MEM_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x4B80
+#else
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x4BDC
+#endif
+#define HIVE_SIZE_host2sp_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x4B80
+#else
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x4BDC
+#endif
+#define HIVE_SIZE_sp_host2sp_psys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_xmem_bin_addr
+#define HIVE_MEM_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_xmem_bin_addr 0x41F4
+#else
+#define HIVE_ADDR_xmem_bin_addr 0x4214
+#endif
+#define HIVE_SIZE_xmem_bin_addr 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_xmem_bin_addr 0x41F4
+#else
+#define HIVE_ADDR_sp_xmem_bin_addr 0x4214
+#endif
+#define HIVE_SIZE_sp_xmem_bin_addr 4
+
+#ifndef ISP2401
+/* function tmr_clock_init: 65A0 */
+#else
+/* function tmr_clock_init: 66DB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_run: 1403 */
+#else
+/* function ia_css_pipeline_sp_run: 1424 */
+#endif
+
+#ifndef ISP2401
+/* function memcpy: 68F7 */
+#else
+/* function memcpy: 6A6A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GP_DEVICE_BASE
+#define HIVE_MEM_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_GP_DEVICE_BASE 0x2FC
+#else
+#define HIVE_ADDR_GP_DEVICE_BASE 0x314
+#endif
+#define HIVE_SIZE_GP_DEVICE_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x2FC
+#else
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x314
+#endif
+#define HIVE_SIZE_sp_GP_DEVICE_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_ready_queue
+#define HIVE_MEM_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x1E0
+#else
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x1E4
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_ready_queue 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x1E0
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x1E4
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_ready_queue 12
+
+#ifndef ISP2401
+/* function input_system_reg_store: B1E */
+#else
+/* function input_system_reg_store: B18 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_start: 5D4D */
+#else
+/* function ia_css_isys_sp_frontend_start: 5E88 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_uds_sp_scale_params: 6600 */
+#else
+/* function ia_css_uds_sp_scale_params: 6773 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_increase_size: E40 */
+#else
+/* function ia_css_circbuf_increase_size: E35 */
+#endif
+
+#ifndef ISP2401
+/* function __divu: 6875 */
+#else
+/* function __divu: 69E8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_get_state: C83 */
+#else
+/* function ia_css_thread_sp_get_state: C78 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_stop
+#define HIVE_MEM_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x46BC
+#else
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x4704
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_stop 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x46BC
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x4704
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_stop 20
+
+#ifndef ISP2401
+/* function thread_fiber_sp_main: E39 */
+#else
+/* function thread_fiber_sp_main: E2E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_pipe_thread
+#define HIVE_MEM_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pipe_thread 0x4800
+#define HIVE_SIZE_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_isp_pipe_thread 0x4848
+#define HIVE_SIZE_sp_isp_pipe_thread 360
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x4800
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x4848
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 360
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_handle_parameter_sets: 128A */
+#else
+/* function ia_css_parambuf_sp_handle_parameter_sets: 127F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_set_state: 5943 */
+#else
+/* function ia_css_spctrl_sp_set_state: 5A7E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_signal: 6AF7 */
+#else
+/* function ia_css_thread_sem_sp_signal: 6C6C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_IRQ_BASE
+#define HIVE_MEM_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_IRQ_BASE 0x2C
+#define HIVE_SIZE_IRQ_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_IRQ_BASE 0x2C
+#define HIVE_SIZE_sp_IRQ_BASE 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_TIMED_CTRL_BASE
+#define HIVE_MEM_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_TIMED_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_sp_TIMED_CTRL_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_isr: 6FDC */
+
+/* function ia_css_isys_sp_generate_exp_id: 60E5 */
+#else
+/* function ia_css_isys_sp_isr: 7139 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_init: 61CF */
+#else
+/* function ia_css_isys_sp_generate_exp_id: 6220 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_init: 6BC8 */
+#else
+/* function ia_css_rmgr_sp_init: 630A */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x308
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x308
+#define HIVE_SIZE_sp_is_isp_requested 4
+#else
+/* function ia_css_thread_sem_sp_init: 6D3B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_frame
+#define HIVE_MEM_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x46D0
+#else
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x4718
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_frame 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x46D0
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x4718
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_frame 40
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_execute: 3217 */
+#else
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x320
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x320
+#define HIVE_SIZE_sp_is_isp_requested 4
+
+/* function ia_css_dmaproxy_sp_execute: 33F6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_is_empty: 48E0 */
+#else
+/* function ia_css_queue_is_empty: 7098 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_has_stopped: 180C */
+#else
+/* function ia_css_pipeline_sp_has_stopped: 1846 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_extract: F44 */
+#else
+/* function ia_css_circbuf_extract: F39 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 2B0D */
+#else
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 2CAF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_sp_thread
+#define HIVE_MEM_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_current_sp_thread 0x1DC
+#define HIVE_SIZE_current_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_current_sp_thread 0x1DC
+#define HIVE_SIZE_sp_current_sp_thread 4
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_spid: 594A */
+#else
+/* function ia_css_spctrl_sp_get_spid: 5A85 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_reset_buffers: 2D22 */
+#else
+/* function ia_css_bufq_sp_reset_buffers: 2EC4 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr: 6E35 */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr: 6F79 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_uninit: 61C8 */
+#else
+/* function ia_css_rmgr_sp_uninit: 6303 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack
+#define HIVE_MEM_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_threads_stack 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_sp_threads_stack 28
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek: F26 */
+#else
+/* function ia_css_circbuf_peek: F1B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_wait_for_in_param: 1053 */
+#else
+/* function ia_css_parambuf_sp_wait_for_in_param: 1048 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_get_exp_id: 5FAD */
+#else
+/* function ia_css_isys_sp_token_map_get_exp_id: 60E8 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_param
+#define HIVE_MEM_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_param 0x46F8
+#else
+#define HIVE_ADDR_sp_all_cb_elems_param 0x4740
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x46F8
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x4740
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_param 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_pipeline_sp_curr_binary_id
+#define HIVE_MEM_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x1EC
+#else
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x1F0
+#endif
+#define HIVE_SIZE_pipeline_sp_curr_binary_id 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x1EC
+#else
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x1F0
+#endif
+#define HIVE_SIZE_sp_pipeline_sp_curr_binary_id 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame_desc
+#define HIVE_MEM_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x4708
+#else
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x4750
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x4708
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x4750
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame_desc 8
+
+#ifndef ISP2401
+/* function sp_isys_copy_func_v2: 706 */
+#else
+/* function sp_isys_copy_func_v2: 69A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_param
+#define HIVE_MEM_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_param 0x4710
+#else
+#define HIVE_ADDR_sem_for_reading_cb_param 0x4758
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_param 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x4710
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x4758
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_param 40
+
+#ifndef ISP2401
+/* function ia_css_queue_get_used_space: 49AD */
+#else
+/* function ia_css_queue_get_used_space: 4C0B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_start
+#define HIVE_MEM_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_start 0x4738
+#else
+#define HIVE_ADDR_sem_for_cont_capt_start 0x4780
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x4738
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x4780
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_start 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tmp_heap
+#define HIVE_MEM_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tmp_heap 0x6010
+#else
+#define HIVE_ADDR_tmp_heap 0x6070
+#endif
+#define HIVE_SIZE_tmp_heap 640
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tmp_heap 0x6010
+#else
+#define HIVE_ADDR_sp_tmp_heap 0x6070
+#endif
+#define HIVE_SIZE_sp_tmp_heap 640
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_get_num_vbuf: 64D8 */
+#else
+/* function ia_css_rmgr_sp_get_num_vbuf: 6613 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 3F49 */
+#else
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 418C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_lock_exp_id: 20CD */
+#else
+/* function ia_css_tagger_sp_lock_exp_id: 211D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4B8C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4BE8
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4B8C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4BE8
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+
+#ifndef ISP2401
+/* function ia_css_queue_is_full: 4A44 */
+#else
+/* function ia_css_queue_is_full: 4CA2 */
+#endif
+
+/* function debug_buffer_init_isp: E4 */
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_uninit: 5D07 */
+#else
+/* function ia_css_isys_sp_frontend_uninit: 5E42 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_exp_id_is_locked: 2003 */
+#else
+/* function ia_css_tagger_sp_exp_id_is_locked: 2053 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem
+#define HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x66E8
+#else
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x6744
+#endif
+#define HIVE_SIZE_ia_css_rmgr_sp_mipi_frame_sem 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x66E8
+#else
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x6744
+#endif
+#define HIVE_SIZE_sp_ia_css_rmgr_sp_mipi_frame_sem 60
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_dump: 62AF */
+#else
+/* function ia_css_rmgr_sp_refcount_dump: 63EA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4BC8
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4C24
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4BC8
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4C24
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_pipe_threads
+#define HIVE_MEM_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_pipe_threads 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_sp_pipe_threads 20
+
+#ifndef ISP2401
+/* function sp_event_proxy_func: 71B */
+#else
+/* function sp_event_proxy_func: 6AF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_isys_event_queue_handle
+#define HIVE_MEM_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x4BDC
+#else
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x4C38
+#endif
+#define HIVE_SIZE_host2sp_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x4BDC
+#else
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x4C38
+#endif
+#define HIVE_SIZE_sp_host2sp_isys_event_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_yield: 6A70 */
+#else
+/* function ia_css_thread_sp_yield: 6BEA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param_desc
+#define HIVE_MEM_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x474C
+#else
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x4794
+#endif
+#define HIVE_SIZE_sp_all_cbs_param_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x474C
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x4794
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param_desc 8
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb
+#define HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x5BF4
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x5C50
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_sp_invalidate_tlb 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x5BF4
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x5C50
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_sp_invalidate_tlb 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_fork: D10 */
+#else
+/* function ia_css_thread_sp_fork: D05 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_destroy: 27F4 */
+#else
+/* function ia_css_tagger_sp_destroy: 2844 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_read: 31B7 */
+#else
+/* function ia_css_dmaproxy_sp_vmem_read: 3396 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ifmtr_sp_init: 6136 */
+#else
+/* function ia_css_ifmtr_sp_init: 6271 */
+#endif
+
+#ifndef ISP2401
+/* function initialize_sp_group: 6D4 */
+#else
+/* function initialize_sp_group: 668 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_peek: 2919 */
+#else
+/* function ia_css_tagger_buf_sp_peek: 2ABB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_init: D3C */
+#else
+/* function ia_css_thread_sp_init: D31 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_reset_exp_id: 60DD */
+#else
+/* function ia_css_isys_sp_reset_exp_id: 6218 */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_update_fps: 65F0 */
+#else
+/* function qos_scheduler_update_fps: 6763 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 461E */
+#else
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 4879 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_DMEM_BASE
+#define HIVE_MEM_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_ISP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_sp_ISP_DMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_DMEM_BASE
+#define HIVE_MEM_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_SP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_sp_SP_DMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read: 322D */
+#else
+/* function __ia_css_queue_is_empty_text: 4B68 */
+
+/* function ia_css_dmaproxy_sp_read: 340C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_raw_copy_line_count
+#define HIVE_MEM_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_raw_copy_line_count 0x2C8
+#else
+#define HIVE_ADDR_raw_copy_line_count 0x2E0
+#endif
+#define HIVE_SIZE_raw_copy_line_count 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_raw_copy_line_count 0x2C8
+#else
+#define HIVE_ADDR_sp_raw_copy_line_count 0x2E0
+#endif
+#define HIVE_SIZE_sp_raw_copy_line_count 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_tag_cmd_queue_handle
+#define HIVE_MEM_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x4BE8
+#else
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x4C44
+#endif
+#define HIVE_SIZE_host2sp_tag_cmd_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x4BE8
+#else
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x4C44
+#endif
+#define HIVE_SIZE_sp_host2sp_tag_cmd_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_queue_peek: 4923 */
+#else
+/* function ia_css_queue_peek: 4B81 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_frame_cnt
+#define HIVE_MEM_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x4A94
+#else
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x4AF0
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_frame_cnt 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x4A94
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x4AF0
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_frame_cnt 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_can_send_token_mask
+#define HIVE_MEM_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_can_send_token_mask 0x88
+#define HIVE_SIZE_event_can_send_token_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_can_send_token_mask 0x88
+#define HIVE_SIZE_sp_event_can_send_token_mask 44
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_thread
+#define HIVE_MEM_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_thread 0x5F40
+#else
+#define HIVE_ADDR_isp_thread 0x5FA0
+#endif
+#define HIVE_SIZE_isp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_thread 0x5F40
+#else
+#define HIVE_ADDR_sp_isp_thread 0x5FA0
+#endif
+#define HIVE_SIZE_sp_isp_thread 4
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event_non_blocking: A78 */
+#else
+/* function encode_and_post_sp_event_non_blocking: A0C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_destroy: 5DDF */
+#else
+/* function ia_css_isys_sp_frontend_destroy: 5F1A */
+#endif
+
+/* function is_ddr_debug_buffer_full: 2CC */
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_stop: 5D1F */
+#else
+/* function ia_css_isys_sp_frontend_stop: 5E5A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_init: 607B */
+#else
+/* function ia_css_isys_sp_token_map_init: 61B6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 2969 */
+#else
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 2B0B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_fiber
+#define HIVE_MEM_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_fiber 0x19C
+#define HIVE_SIZE_sp_threads_fiber 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_fiber 0x19C
+#define HIVE_SIZE_sp_sp_threads_fiber 28
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event: A01 */
+#else
+/* function encode_and_post_sp_event: 995 */
+#endif
+
+/* function debug_enqueue_ddr: EE */
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 626A */
+#else
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 63A5 */
+#endif
+
+#ifndef ISP2401
+/* function dmaproxy_sp_read_write: 6EE4 */
+#else
+/* function dmaproxy_sp_read_write: 7017 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer
+#define HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5BF8
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5C54
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5BF8
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5C54
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_buffer_queue_handle
+#define HIVE_MEM_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x4BF4
+#else
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x4C50
+#endif
+#define HIVE_SIZE_host2sp_buffer_queue_handle 480
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x4BF4
+#else
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x4C50
+#endif
+#define HIVE_SIZE_sp_host2sp_buffer_queue_handle 480
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_service
+#define HIVE_MEM_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3178
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3198
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_service 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3178
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3198
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_service 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_process: 6BF0 */
+#else
+/* function ia_css_dmaproxy_sp_process: 6D63 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_mark_from_end: 2BF1 */
+#else
+/* function ia_css_tagger_buf_sp_mark_from_end: 2D93 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_rcv_acquire_ack: 59EC */
+#else
+/* function ia_css_isys_sp_backend_rcv_acquire_ack: 5B27 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_pre_acquire_request: 5A02 */
+#else
+/* function ia_css_isys_sp_backend_pre_acquire_request: 5B3D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_cs: 3653 */
+#else
+/* function ia_css_ispctrl_sp_init_cs: 3855 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_init: 5958 */
+#else
+/* function ia_css_spctrl_sp_init: 5A93 */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_init: 730 */
+#else
+/* function sp_event_proxy_init: 6C4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4DD4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4E30
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4DD4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4E30
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_output
+#define HIVE_MEM_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_output 0x41F8
+#else
+#define HIVE_ADDR_sp_output 0x4218
+#endif
+#define HIVE_SIZE_sp_output 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_output 0x41F8
+#else
+#define HIVE_ADDR_sp_sp_output 0x4218
+#endif
+#define HIVE_SIZE_sp_sp_output 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4DFC
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4E58
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4DFC
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4E58
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_CTRL_BASE
+#define HIVE_MEM_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_ISP_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_sp_ISP_CTRL_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_INPUT_FORMATTER_BASE
+#define HIVE_MEM_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_INPUT_FORMATTER_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_sp_INPUT_FORMATTER_BASE 16
+
+#ifndef ISP2401
+/* function sp_dma_proxy_reset_channels: 3487 */
+#else
+/* function sp_dma_proxy_reset_channels: 367B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_acquire: 5B0D */
+#else
+/* function ia_css_isys_sp_backend_acquire: 5C48 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_update_size: 28E8 */
+#else
+/* function ia_css_tagger_sp_update_size: 2A8A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_host_sp_queue
+#define HIVE_MEM_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x511C
+#else
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x5178
+#endif
+#define HIVE_SIZE_ia_css_bufq_host_sp_queue 2008
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x511C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x5178
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_host_sp_queue 2008
+
+#ifndef ISP2401
+/* function thread_fiber_sp_create: DA8 */
+#else
+/* function thread_fiber_sp_create: D9D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_increments: 3319 */
+#else
+/* function ia_css_dmaproxy_sp_set_increments: 350D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_frame
+#define HIVE_MEM_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x4754
+#else
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x479C
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_frame 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x4754
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x479C
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_frame 20
+
+#ifndef ISP2401
+/* function receiver_reg_store: AD7 */
+#else
+/* function receiver_reg_store: AD1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_param
+#define HIVE_MEM_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_param 0x4768
+#else
+#define HIVE_ADDR_sem_for_writing_cb_param 0x47B0
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_param 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x4768
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x47B0
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_param 20
+
+/* function sp_start_isp_entry: 453 */
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifdef HIVE_ADDR_sp_start_isp_entry
+#endif
+#define HIVE_ADDR_sp_start_isp_entry 0x453
+#endif
+#define HIVE_ADDR_sp_sp_start_isp_entry 0x453
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_all: 2B75 */
+#else
+/* function ia_css_tagger_buf_sp_unmark_all: 2D17 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_from_start: 2BB6 */
+#else
+/* function ia_css_tagger_buf_sp_unmark_from_start: 2D58 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_acquire: 34B3 */
+#else
+/* function ia_css_dmaproxy_sp_channel_acquire: 36A7 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_add_num_vbuf: 64B4 */
+#else
+/* function ia_css_rmgr_sp_add_num_vbuf: 65EF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_create: 60C4 */
+#else
+/* function ia_css_isys_sp_token_map_create: 61FF */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 3183 */
+#else
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 3362 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_acquire_buf_elem: 1FDB */
+#else
+/* function ia_css_tagger_sp_acquire_buf_elem: 202B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_is_dynamic_buffer: 306C */
+#else
+/* function ia_css_bufq_sp_is_dynamic_buffer: 320E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_group
+#define HIVE_MEM_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_group 0x4208
+#define HIVE_SIZE_sp_group 1144
+#else
+#define HIVE_ADDR_sp_group 0x4228
+#define HIVE_SIZE_sp_group 1184
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_group 0x4208
+#define HIVE_SIZE_sp_sp_group 1144
+#else
+#define HIVE_ADDR_sp_sp_group 0x4228
+#define HIVE_SIZE_sp_sp_group 1184
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_event_proxy_thread
+#define HIVE_MEM_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_proxy_thread 0x4954
+#define HIVE_SIZE_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_event_proxy_thread 0x49B0
+#define HIVE_SIZE_sp_event_proxy_thread 72
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x4954
+#define HIVE_SIZE_sp_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x49B0
+#define HIVE_SIZE_sp_sp_event_proxy_thread 72
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_kill: CD6 */
+#else
+/* function ia_css_thread_sp_kill: CCB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_create: 28A2 */
+#else
+/* function ia_css_tagger_sp_create: 2A38 */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_acquire_dmem: 6561 */
+#else
+/* function tmpmem_acquire_dmem: 669C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_MMU_BASE
+#define HIVE_MEM_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_MMU_BASE 0x24
+#define HIVE_SIZE_MMU_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_MMU_BASE 0x24
+#define HIVE_SIZE_sp_MMU_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_release: 349F */
+#else
+/* function ia_css_dmaproxy_sp_channel_release: 3693 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_is_idle: 347F */
+#else
+/* function ia_css_dmaproxy_sp_is_idle: 3673 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_qos_start
+#define HIVE_MEM_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_qos_start 0x477C
+#else
+#define HIVE_ADDR_sem_for_qos_start 0x47C4
+#endif
+#define HIVE_SIZE_sem_for_qos_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_qos_start 0x477C
+#else
+#define HIVE_ADDR_sp_sem_for_qos_start 0x47C4
+#endif
+#define HIVE_SIZE_sp_sem_for_qos_start 20
+
+#ifndef ISP2401
+/* function isp_hmem_load: B55 */
+#else
+/* function isp_hmem_load: B4F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_release_buf_elem: 1FB7 */
+#else
+/* function ia_css_tagger_sp_release_buf_elem: 2007 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_send: 34F5 */
+#else
+/* function ia_css_eventq_sp_send: 36E9 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_isys_sp_error_cnt
+#define HIVE_MEM_ia_css_isys_sp_error_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_isys_sp_error_cnt 0x62D4
+#else
+#define HIVE_ADDR_ia_css_isys_sp_error_cnt 0x6330
+#endif
+#define HIVE_SIZE_ia_css_isys_sp_error_cnt 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_isys_sp_error_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_isys_sp_error_cnt 0x62D4
+#else
+#define HIVE_ADDR_sp_ia_css_isys_sp_error_cnt 0x6330
+#endif
+#define HIVE_SIZE_sp_ia_css_isys_sp_error_cnt 16
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unlock_from_start: 2AA5 */
+#else
+/* function ia_css_tagger_buf_sp_unlock_from_start: 2C47 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_debug_buffer_ddr_address
+#define HIVE_MEM_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_debug_buffer_ddr_address 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_sp_debug_buffer_ddr_address 4
+
+#ifndef ISP2401
+/* function sp_isys_copy_request: 714 */
+#else
+/* function sp_isys_copy_request: 6A8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 6344 */
+#else
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 647F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_set_priority: CCE */
+#else
+/* function ia_css_thread_sp_set_priority: CC3 */
+#endif
+
+#ifndef ISP2401
+/* function sizeof_hmem: BFC */
+#else
+/* function sizeof_hmem: BF6 */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_release_dmem: 6550 */
+#else
+/* function tmpmem_release_dmem: 668B */
+#endif
+
+/* function cnd_input_system_cfg: 392 */
+
+#ifndef ISP2401
+/* function __ia_css_sp_rawcopy_func_critical: 6F65 */
+#else
+/* function __ia_css_sp_rawcopy_func_critical: 70C2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_width_exception: 3304 */
+#else
+/* function __ia_css_dmaproxy_sp_process_text: 3306 */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_assert: 8B1 */
+#else
+/* function ia_css_dmaproxy_sp_set_width_exception: 34F8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_flash_sp_init_internal_params: 2C90 */
+#else
+/* function sp_event_assert: 845 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 29AB */
+#else
+/* function ia_css_flash_sp_init_internal_params: 2E32 */
+#endif
+
+#ifndef ISP2401
+/* function __modu: 68BB */
+#else
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 2B4D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_isp_vector: 3189 */
+#else
+/* function __modu: 6A2E */
+
+/* function ia_css_dmaproxy_sp_init_isp_vector: 3368 */
+#endif
+
+/* function isp_vamem_store: 0 */
+
+#ifdef ISP2401
+/* function ia_css_tagger_sp_set_copy_pipe: 2A2F */
+
+#endif
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GDC_BASE
+#define HIVE_MEM_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GDC_BASE 0x44
+#define HIVE_SIZE_GDC_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GDC_BASE 0x44
+#define HIVE_SIZE_sp_GDC_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_queue_local_init: 4C0E */
+#else
+/* function ia_css_queue_local_init: 4E6C */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_callout_func: 6988 */
+#else
+/* function sp_event_proxy_callout_func: 6AFB */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_schedule_stage: 65C1 */
+#else
+/* function qos_scheduler_schedule_stage: 670F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_num_ready_threads
+#define HIVE_MEM_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x49E0
+#else
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x4A40
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x49E0
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x4A40
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_num_ready_threads 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack_size
+#define HIVE_MEM_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack_size 0x180
+#define HIVE_SIZE_sp_threads_stack_size 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack_size 0x180
+#define HIVE_SIZE_sp_sp_threads_stack_size 28
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 3F2F */
+#else
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 4172 */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_isys_sp_isr_text: 5E09 */
+#else
+/* function __ia_css_isys_sp_isr_text: 5F44 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_dequeue: 4A8C */
+#else
+/* function ia_css_queue_dequeue: 4CEA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel: 6E4C */
+#else
+/* function is_qos_standalone_mode: 66EA */
+
+/* function ia_css_dmaproxy_sp_configure_channel: 6F90 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_thread_fiber_sp
+#define HIVE_MEM_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_current_thread_fiber_sp 0x49E8
+#else
+#define HIVE_ADDR_current_thread_fiber_sp 0x4A44
+#endif
+#define HIVE_SIZE_current_thread_fiber_sp 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x49E8
+#else
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x4A44
+#endif
+#define HIVE_SIZE_sp_current_thread_fiber_sp 4
+
+#ifndef ISP2401
+/* function ia_css_circbuf_pop: FD8 */
+#else
+/* function ia_css_circbuf_pop: FCD */
+#endif
+
+#ifndef ISP2401
+/* function memset: 693A */
+#else
+/* function memset: 6AAD */
+#endif
+
+/* function irq_raise_set_token: B6 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GPIO_BASE
+#define HIVE_MEM_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GPIO_BASE 0x3C
+#define HIVE_SIZE_GPIO_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GPIO_BASE 0x3C
+#define HIVE_SIZE_sp_GPIO_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_pipeline_acc_stage_enable: 17D7 */
+#else
+/* function ia_css_pipeline_acc_stage_enable: 17FF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_unlock_exp_id: 2028 */
+#else
+/* function ia_css_tagger_sp_unlock_exp_id: 2078 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_ph
+#define HIVE_MEM_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_ph 0x62E4
+#else
+#define HIVE_ADDR_isp_ph 0x6340
+#endif
+#define HIVE_SIZE_isp_ph 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_ph 0x62E4
+#else
+#define HIVE_ADDR_sp_isp_ph 0x6340
+#endif
+#define HIVE_SIZE_sp_isp_ph 28
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_flush: 6009 */
+#else
+/* function ia_css_isys_sp_token_map_flush: 6144 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_ds: 37B2 */
+#else
+/* function ia_css_ispctrl_sp_init_ds: 39E1 */
+#endif
+
+#ifndef ISP2401
+/* function get_xmem_base_addr_raw: 3B5F */
+#else
+/* function get_xmem_base_addr_raw: 3D9A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param
+#define HIVE_MEM_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param 0x4790
+#else
+#define HIVE_ADDR_sp_all_cbs_param 0x47D8
+#endif
+#define HIVE_SIZE_sp_all_cbs_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x4790
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x47D8
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param 16
+
+#ifndef ISP2401
+/* function ia_css_circbuf_create: 1026 */
+#else
+/* function ia_css_circbuf_create: 101B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp_group
+#define HIVE_MEM_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp_group 0x47A0
+#else
+#define HIVE_ADDR_sem_for_sp_group 0x47E8
+#endif
+#define HIVE_SIZE_sem_for_sp_group 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp_group 0x47A0
+#else
+#define HIVE_ADDR_sp_sem_for_sp_group 0x47E8
+#endif
+#define HIVE_SIZE_sp_sem_for_sp_group 20
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_wait_for_in_frame: 64DF */
+#else
+/* function __ia_css_dmaproxy_sp_configure_channel_text: 34D7 */
+
+/* function ia_css_framebuf_sp_wait_for_in_frame: 661A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_tag_frame: 556F */
+#else
+/* function ia_css_sp_rawcopy_tag_frame: 57B0 */
+#endif
+
+#ifndef ISP2401
+/* function isp_hmem_clear: B25 */
+#else
+/* function isp_hmem_clear: B1F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_release_in_frame: 6522 */
+#else
+/* function ia_css_framebuf_sp_release_in_frame: 665D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_snd_acquire_request: 5A5F */
+#else
+/* function ia_css_isys_sp_backend_snd_acquire_request: 5B9A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_is_full: 5E90 */
+#else
+/* function ia_css_isys_sp_token_map_is_full: 5FCB */
+#endif
+
+#ifndef ISP2401
+/* function input_system_acquisition_run: AF9 */
+#else
+/* function input_system_acquisition_run: AF3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_start_binary: 3631 */
+#else
+/* function ia_css_ispctrl_sp_start_binary: 3833 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x58F4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x5950
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x58F4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x5950
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_recv: 34C7 */
+#else
+/* function ia_css_eventq_sp_recv: 36BB */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_pool
+#define HIVE_MEM_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_pool 0x2E8
+#else
+#define HIVE_ADDR_isp_pool 0x300
+#endif
+#define HIVE_SIZE_isp_pool 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pool 0x2E8
+#else
+#define HIVE_ADDR_sp_isp_pool 0x300
+#endif
+#define HIVE_SIZE_sp_isp_pool 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_rel_gen: 6211 */
+#else
+/* function ia_css_rmgr_sp_rel_gen: 634C */
+
+/* function ia_css_tagger_sp_unblock_clients: 2900 */
+#endif
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_end: 1FA7 */
+#else
+/* function css_get_frame_processing_time_end: 1FF7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_any_pending_mask
+#define HIVE_MEM_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_event_any_pending_mask 0x300
+#else
+#define HIVE_ADDR_event_any_pending_mask 0x318
+#endif
+#define HIVE_SIZE_event_any_pending_mask 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_any_pending_mask 0x300
+#else
+#define HIVE_ADDR_sp_event_any_pending_mask 0x318
+#endif
+#define HIVE_SIZE_sp_event_any_pending_mask 8
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_push: 5A16 */
+#else
+/* function ia_css_isys_sp_backend_push: 5B51 */
+#endif
+
+/* function sh_css_decode_tag_descr: 352 */
+
+/* function debug_enqueue_isp: 27B */
+
+#ifndef ISP2401
+/* function qos_scheduler_update_stage_budget: 65AF */
+#else
+/* function qos_scheduler_update_stage_budget: 66F2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_uninit: 5951 */
+#else
+/* function ia_css_spctrl_sp_uninit: 5A8C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SWITCH_CODE
+#define HIVE_MEM_HIVE_IF_SWITCH_CODE scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SWITCH_CODE 0x1D8
+#define HIVE_SIZE_HIVE_IF_SWITCH_CODE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SWITCH_CODE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SWITCH_CODE 0x1D8
+#define HIVE_SIZE_sp_HIVE_IF_SWITCH_CODE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x5908
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x5964
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_dis_bufs 140
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x5908
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x5964
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_dis_bufs 140
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_lock_from_start: 2AD9 */
+#else
+/* function ia_css_tagger_buf_sp_lock_from_start: 2C7B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_isp_idle
+#define HIVE_MEM_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_isp_idle 0x47B4
+#else
+#define HIVE_ADDR_sem_for_isp_idle 0x47FC
+#endif
+#define HIVE_SIZE_sem_for_isp_idle 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x47B4
+#else
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x47FC
+#endif
+#define HIVE_SIZE_sp_sem_for_isp_idle 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write_byte_addr: 31E6 */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr: 33C5 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init: 315D */
+#else
+/* function ia_css_dmaproxy_sp_init: 333C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 2D62 */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 2F04 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_VAMEM_BASE
+#define HIVE_MEM_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_ISP_VAMEM_BASE 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_sp_ISP_VAMEM_BASE 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rawcopy_sp_tagger
+#define HIVE_MEM_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x6294
+#else
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x62F0
+#endif
+#define HIVE_SIZE_ia_css_rawcopy_sp_tagger 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x6294
+#else
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x62F0
+#endif
+#define HIVE_SIZE_sp_ia_css_rawcopy_sp_tagger 24
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x5994
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x59F0
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_exp_ids 70
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x5994
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x59F0
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_exp_ids 70
+
+#ifndef ISP2401
+/* function ia_css_queue_item_load: 4D00 */
+#else
+/* function ia_css_queue_item_load: 4F5E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_state: 593C */
+#else
+/* function ia_css_spctrl_sp_get_state: 5A77 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_uninit: 6026 */
+#else
+/* function ia_css_isys_sp_token_map_uninit: 6161 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_callout_sp_thread
+#define HIVE_MEM_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_callout_sp_thread 0x49DC
+#else
+#define HIVE_ADDR_callout_sp_thread 0x1E0
+#endif
+#define HIVE_SIZE_callout_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_callout_sp_thread 0x49DC
+#else
+#define HIVE_ADDR_sp_callout_sp_thread 0x1E0
+#endif
+#define HIVE_SIZE_sp_callout_sp_thread 4
+
+#ifndef ISP2401
+/* function thread_fiber_sp_init: E2F */
+#else
+/* function thread_fiber_sp_init: E24 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_PMEM_BASE
+#define HIVE_MEM_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_SP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_sp_SP_PMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_snd_acquire_req: 5F96 */
+#else
+/* function ia_css_isys_sp_token_map_snd_acquire_req: 60D1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_input_stream_format
+#define HIVE_MEM_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_input_stream_format 0x40F8
+#else
+#define HIVE_ADDR_sp_isp_input_stream_format 0x4118
+#endif
+#define HIVE_SIZE_sp_isp_input_stream_format 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x40F8
+#else
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x4118
+#endif
+#define HIVE_SIZE_sp_sp_isp_input_stream_format 20
+
+#ifndef ISP2401
+/* function __mod: 68A7 */
+#else
+/* function __mod: 6A1A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 3247 */
+#else
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 3426 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_join: CFF */
+#else
+/* function ia_css_thread_sp_join: CF4 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_add_command: 6F4F */
+#else
+/* function ia_css_dmaproxy_sp_add_command: 7082 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_thread_func: 57F0 */
+#else
+/* function ia_css_sp_metadata_thread_func: 594F */
+#endif
+
+#ifndef ISP2401
+/* function __sp_event_proxy_func_critical: 6975 */
+#else
+/* function __sp_event_proxy_func_critical: 6AE8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_wait: 5903 */
+#else
+/* function ia_css_sp_metadata_wait: 5A3E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek_from_start: F08 */
+#else
+/* function ia_css_circbuf_peek_from_start: EFD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_event_sp_encode: 3552 */
+#else
+/* function ia_css_event_sp_encode: 3746 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_run: D72 */
+#else
+/* function ia_css_thread_sp_run: D67 */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_func: 6F6 */
+#else
+/* function sp_isys_copy_func: 68A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_flush: 5A7F */
+#else
+/* function ia_css_isys_sp_backend_flush: 5BBA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_frame_exists: 599B */
+#else
+/* function ia_css_isys_sp_backend_frame_exists: 5AD6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_init_isp_memories: 4789 */
+#else
+/* function ia_css_sp_isp_param_init_isp_memories: 4A11 */
+#endif
+
+#ifndef ISP2401
+/* function register_isr: 8A9 */
+#else
+/* function register_isr: 83D */
+#endif
+
+/* function irq_raise: C8 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 3124 */
+#else
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 32CC */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SRST_ADDRESS
+#define HIVE_MEM_HIVE_IF_SRST_ADDRESS scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SRST_ADDRESS 0x1B8
+#define HIVE_SIZE_HIVE_IF_SRST_ADDRESS 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SRST_ADDRESS scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SRST_ADDRESS 0x1B8
+#define HIVE_SIZE_sp_HIVE_IF_SRST_ADDRESS 16
+
+#ifndef ISP2401
+/* function pipeline_sp_initialize_stage: 190B */
+#else
+/* function pipeline_sp_initialize_stage: 1945 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_isys_sp_frontend_states
+#define HIVE_MEM_ia_css_isys_sp_frontend_states scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_isys_sp_frontend_states 0x62C8
+#else
+#define HIVE_ADDR_ia_css_isys_sp_frontend_states 0x6324
+#endif
+#define HIVE_SIZE_ia_css_isys_sp_frontend_states 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_isys_sp_frontend_states scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_isys_sp_frontend_states 0x62C8
+#else
+#define HIVE_ADDR_sp_ia_css_isys_sp_frontend_states 0x6324
+#endif
+#define HIVE_SIZE_sp_ia_css_isys_sp_frontend_states 12
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 6E1E */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 6F62 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_done_ds: 3799 */
+#else
+/* function ia_css_ispctrl_sp_done_ds: 39C8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_get_mem_inits: 4764 */
+#else
+/* function ia_css_sp_isp_param_get_mem_inits: 49EC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_init_buffer_queues: 13D0 */
+#else
+/* function ia_css_parambuf_sp_init_buffer_queues: 13F1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_pfp_spref
+#define HIVE_MEM_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_pfp_spref 0x2F0
+#else
+#define HIVE_ADDR_vbuf_pfp_spref 0x308
+#endif
+#define HIVE_SIZE_vbuf_pfp_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x2F0
+#else
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x308
+#endif
+#define HIVE_SIZE_sp_vbuf_pfp_spref 4
+
+#ifndef ISP2401
+/* function input_system_cfg: ABB */
+#else
+/* function input_system_cfg: AB5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_HMEM_BASE
+#define HIVE_MEM_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_ISP_HMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_sp_ISP_HMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_frames
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x59DC
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x5A38
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_frames 280
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x59DC
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x5A38
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_frames 280
+
+#ifndef ISP2401
+/* function qos_scheduler_init_stage_budget: 65E8 */
+#else
+/* function qos_scheduler_init_stage_budget: 6750 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_release: 5AF4 */
+#else
+/* function ia_css_isys_sp_backend_release: 5C2F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_destroy: 5B1E */
+#else
+/* function ia_css_isys_sp_backend_destroy: 5C59 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_buffer_queue_handle
+#define HIVE_MEM_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x5AF4
+#else
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x5B50
+#endif
+#define HIVE_SIZE_sp2host_buffer_queue_handle 96
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x5AF4
+#else
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x5B50
+#endif
+#define HIVE_SIZE_sp_sp2host_buffer_queue_handle 96
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_check_mipi_frame_size: 5F5A */
+#else
+/* function ia_css_isys_sp_token_map_check_mipi_frame_size: 6095 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_isp_vars: 4483 */
+#else
+/* function ia_css_ispctrl_sp_init_isp_vars: 46DE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_has_empty_mipi_buffer_cb: 5B70 */
+#else
+/* function ia_css_isys_sp_frontend_has_empty_mipi_buffer_cb: 5CAB */
+#endif
+
+#ifndef ISP2401
+/* function sp_warning: 8DC */
+#else
+/* function sp_warning: 870 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_enqueue: 6304 */
+#else
+/* function ia_css_rmgr_sp_vbuf_enqueue: 643F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_tag_exp_id: 2142 */
+#else
+/* function ia_css_tagger_sp_tag_exp_id: 2192 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write: 31FD */
+#else
+/* function ia_css_dmaproxy_sp_write: 33DC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_release_in_param: 1250 */
+#else
+/* function ia_css_parambuf_sp_release_in_param: 1245 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_irq_sw_interrupt_token
+#define HIVE_MEM_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_irq_sw_interrupt_token 0x40F4
+#else
+#define HIVE_ADDR_irq_sw_interrupt_token 0x4114
+#endif
+#define HIVE_SIZE_irq_sw_interrupt_token 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x40F4
+#else
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x4114
+#endif
+#define HIVE_SIZE_sp_irq_sw_interrupt_token 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_addresses
+#define HIVE_MEM_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_addresses 0x5F44
+#else
+#define HIVE_ADDR_sp_isp_addresses 0x5FA4
+#endif
+#define HIVE_SIZE_sp_isp_addresses 172
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_addresses 0x5F44
+#else
+#define HIVE_ADDR_sp_sp_isp_addresses 0x5FA4
+#endif
+#define HIVE_SIZE_sp_sp_isp_addresses 172
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_acq_gen: 6229 */
+#else
+/* function ia_css_rmgr_sp_acq_gen: 6364 */
+#endif
+
+#ifndef ISP2401
+/* function receiver_reg_load: AD0 */
+#else
+/* function receiver_reg_load: ACA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isps
+#define HIVE_MEM_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isps 0x6300
+#else
+#define HIVE_ADDR_isps 0x635C
+#endif
+#define HIVE_SIZE_isps 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isps 0x6300
+#else
+#define HIVE_ADDR_sp_isps 0x635C
+#endif
+#define HIVE_SIZE_sp_isps 28
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_queues_initialized
+#define HIVE_MEM_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_queues_initialized 0x410C
+#else
+#define HIVE_ADDR_host_sp_queues_initialized 0x412C
+#endif
+#define HIVE_SIZE_host_sp_queues_initialized 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x410C
+#else
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x412C
+#endif
+#define HIVE_SIZE_sp_host_sp_queues_initialized 4
+
+#ifndef ISP2401
+/* function ia_css_queue_uninit: 4BCC */
+#else
+/* function ia_css_queue_uninit: 4E2A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_ispctrl_sp_isp_started
+#define HIVE_MEM_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x5BFC
+#else
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x5C58
+#endif
+#define HIVE_SIZE_ia_css_ispctrl_sp_isp_started 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x5BFC
+#else
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x5C58
+#endif
+#define HIVE_SIZE_sp_ia_css_ispctrl_sp_isp_started 4
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf: 2DCE */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf: 2F70 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_height_exception: 32F5 */
+#else
+/* function ia_css_dmaproxy_sp_set_height_exception: 34E9 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 327A */
+#else
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 345A */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_num_ready_threads
+#define HIVE_MEM_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_num_ready_threads 0x49E4
+#define HIVE_SIZE_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_num_ready_threads 0x49E4
+#define HIVE_SIZE_sp_num_ready_threads 4
+
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 31CF */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 33AE */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_spref
+#define HIVE_MEM_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_spref 0x2EC
+#else
+#define HIVE_ADDR_vbuf_spref 0x304
+#endif
+#define HIVE_SIZE_vbuf_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_spref 0x2EC
+#else
+#define HIVE_ADDR_sp_vbuf_spref 0x304
+#endif
+#define HIVE_SIZE_sp_vbuf_spref 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_metadata_thread
+#define HIVE_MEM_sp_metadata_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_metadata_thread 0x4998
+#define HIVE_SIZE_sp_metadata_thread 68
+#else
+#define HIVE_ADDR_sp_metadata_thread 0x49F8
+#define HIVE_SIZE_sp_metadata_thread 72
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_metadata_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_metadata_thread 0x4998
+#define HIVE_SIZE_sp_sp_metadata_thread 68
+#else
+#define HIVE_ADDR_sp_sp_metadata_thread 0x49F8
+#define HIVE_SIZE_sp_sp_metadata_thread 72
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_enqueue: 4B16 */
+#else
+/* function ia_css_queue_enqueue: 4D74 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_request
+#define HIVE_MEM_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_request 0x4A98
+#else
+#define HIVE_ADDR_ia_css_flash_sp_request 0x4AF4
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_request 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x4A98
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x4AF4
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_request 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_write: 31A0 */
+#else
+/* function ia_css_dmaproxy_sp_vmem_write: 337F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tagger_frames
+#define HIVE_MEM_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tagger_frames 0x49EC
+#else
+#define HIVE_ADDR_tagger_frames 0x4A48
+#endif
+#define HIVE_SIZE_tagger_frames 168
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tagger_frames 0x49EC
+#else
+#define HIVE_ADDR_sp_tagger_frames 0x4A48
+#endif
+#define HIVE_SIZE_sp_tagger_frames 168
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_snd_capture_req: 5FB8 */
+#else
+/* function ia_css_isys_sp_token_map_snd_capture_req: 60F3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_if
+#define HIVE_MEM_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_if 0x47C8
+#else
+#define HIVE_ADDR_sem_for_reading_if 0x4810
+#endif
+#define HIVE_SIZE_sem_for_reading_if 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_if 0x47C8
+#else
+#define HIVE_ADDR_sp_sem_for_reading_if 0x4810
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_if 20
+
+#ifndef ISP2401
+/* function sp_generate_interrupts: 95B */
+#else
+/* function sp_generate_interrupts: 8EF */
+
+/* function ia_css_pipeline_sp_start: 1858 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_start: 181E */
+#else
+/* function ia_css_thread_default_callout: 6BE3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_init: 50F3 */
+#else
+/* function ia_css_sp_rawcopy_init: 5351 */
+#endif
+
+#ifndef ISP2401
+/* function tmr_clock_read: 6596 */
+#else
+/* function tmr_clock_read: 66D1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_BAMEM_BASE
+#define HIVE_MEM_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x2F8
+#else
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x310
+#endif
+#define HIVE_SIZE_ISP_BAMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x2F8
+#else
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x310
+#endif
+#define HIVE_SIZE_sp_ISP_BAMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_rcv_capture_ack: 5C1F */
+#else
+/* function ia_css_isys_sp_frontend_rcv_capture_ack: 5D5A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5B54
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5BB0
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5B54
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5BB0
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_start: 1FAF */
+#else
+/* function css_get_frame_processing_time_start: 1FFF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame
+#define HIVE_MEM_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame 0x47DC
+#else
+#define HIVE_ADDR_sp_all_cbs_frame 0x4824
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x47DC
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x4824
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame 16
+
+#ifndef ISP2401
+/* function thread_sp_queue_print: D8F */
+#else
+/* function thread_sp_queue_print: D84 */
+#endif
+
+#ifndef ISP2401
+/* function sp_notify_eof: 907 */
+#else
+/* function sp_notify_eof: 89B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_str2mem
+#define HIVE_MEM_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_str2mem 0x47EC
+#else
+#define HIVE_ADDR_sem_for_str2mem 0x4834
+#endif
+#define HIVE_SIZE_sem_for_str2mem 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_str2mem 0x47EC
+#else
+#define HIVE_ADDR_sp_sem_for_str2mem 0x4834
+#endif
+#define HIVE_SIZE_sp_sem_for_str2mem 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 2B41 */
+#else
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 2CE3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 2F86 */
+#else
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 3128 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_destroy: 101D */
+#else
+/* function ia_css_circbuf_destroy: 1012 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_PMEM_BASE
+#define HIVE_MEM_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_ISP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_sp_ISP_PMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_mem_load: 46F7 */
+#else
+/* function ia_css_sp_isp_param_mem_load: 497F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_from_start: 292D */
+#else
+/* function ia_css_tagger_buf_sp_pop_from_start: 2ACF */
+#endif
+
+#ifndef ISP2401
+/* function __div: 685F */
+#else
+/* function __div: 69D2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_create: 5DF0 */
+#else
+/* function ia_css_isys_sp_frontend_create: 5F2B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 6323 */
+#else
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 645E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_use
+#define HIVE_MEM_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x4A9C
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x4AF8
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_use 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x4A9C
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x4AF8
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_use 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_wait: 6B42 */
+#else
+/* function ia_css_thread_sem_sp_wait: 6CB7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sleep_mode
+#define HIVE_MEM_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sleep_mode 0x4110
+#else
+#define HIVE_ADDR_sp_sleep_mode 0x4130
+#endif
+#define HIVE_SIZE_sp_sleep_mode 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sleep_mode 0x4110
+#else
+#define HIVE_ADDR_sp_sp_sleep_mode 0x4130
+#endif
+#define HIVE_SIZE_sp_sp_sleep_mode 4
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_push: 2A3C */
+#else
+/* function ia_css_tagger_buf_sp_push: 2BDE */
+#endif
+
+/* function mmu_invalidate_cache: D3 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_max_cb_elems
+#define HIVE_MEM_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_max_cb_elems 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_sp_max_cb_elems 8
+
+#ifndef ISP2401
+/* function ia_css_queue_remote_init: 4BEE */
+#else
+/* function ia_css_queue_remote_init: 4E4C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stop_req
+#define HIVE_MEM_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stop_req 0x4680
+#else
+#define HIVE_ADDR_isp_stop_req 0x46C8
+#endif
+#define HIVE_SIZE_isp_stop_req 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stop_req 0x4680
+#else
+#define HIVE_ADDR_sp_isp_stop_req 0x46C8
+#endif
+#define HIVE_SIZE_sp_isp_stop_req 4
+
+#ifndef ISP2401
+#define HIVE_ICACHE_sp_critical_SEGMENT_START 0
+#define HIVE_ICACHE_sp_critical_NUM_SEGMENTS  1
+#endif
+
+#endif /* _sp_map_h_ */
+#ifndef ISP2401
+extern void sh_css_dump_sp_dmem(void);
+void sh_css_dump_sp_dmem(void)
+{
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/csi_rx_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/csi_rx_global.h
new file mode 100644
index 000000000000..146a578b7c74
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/csi_rx_global.h
@@ -0,0 +1,63 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t	long_packet_entry;
+	uint32_t	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;
+		uint32_t virtual_channel;
+		uint32_t data_type;
+		uint32_t comp_scheme;
+		uint32_t comp_predictor;
+		uint32_t comp_bit_idx;
+	} csi_mipi_cfg;
+};
+
+typedef struct csi_rx_frontend_cfg_s csi_rx_frontend_cfg_t;
+struct csi_rx_frontend_cfg_s {
+	uint32_t active_lanes;
+};
+
+extern const uint32_t N_SHORT_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID];
+extern const uint32_t N_LONG_PACKET_LUT_ENTRIES[N_CSI_RX_BACKEND_ID];
+extern const uint32_t N_CSI_RX_FE_CTRL_DLANES[N_CSI_RX_FRONTEND_ID];
+/* sid_width for CSI_RX_BACKEND<N>_ID */
+extern const uint32_t 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/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.c
new file mode 100644
index 000000000000..325b821f276c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.c
@@ -0,0 +1,360 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.iterator.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.iterator.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.copy_output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.copy_output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.crop.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.crop.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.fpn.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.fpn.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.dvs.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.dvs.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.qplane.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.qplane.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output0.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output0.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output1.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output1.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#ifdef ISP2401
+
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.sc.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.sc.offset;
+		}
+		if (size) {
+			ia_css_sc_config((struct sh_css_isp_sc_isp_config *)
+					&binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+					config_dmem, size);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#endif
+
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.raw.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.raw.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.tnr.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.tnr.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.ref.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.ref.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_vf(
+	const struct ia_css_binary *binary,
+	const struct ia_css_vf_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.vf.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.vf.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() leave:\n");
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.h
new file mode 100644
index 000000000000..8aacd3dbc05a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_configs.h
@@ -0,0 +1,189 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "isp/kernels/sc/sc_1.0/ia_css_sc.host.h"
+#endif
+#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 /* IA_CSS_INCLUDE_CONFIGURATIONS */
+/* Generated code: do not edit or commmit. */
+
+#ifndef _IA_CSS_ISP_CONFIG_H
+#define _IA_CSS_ISP_CONFIG_H
+
+/* Code generated by genparam/gencode.c:gen_param_enum() */
+
+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,
+#ifdef ISP2401
+	IA_CSS_SC_CONFIG_ID,
+#endif
+	IA_CSS_RAW_CONFIG_ID,
+	IA_CSS_TNR_CONFIG_ID,
+	IA_CSS_REF_CONFIG_ID,
+	IA_CSS_VF_CONFIG_ID,
+	IA_CSS_NUM_CONFIGURATION_IDS
+};
+
+/* Code generated by genparam/gencode.c:gen_param_offsets() */
+
+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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter sc;
+#endif
+		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 */
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#ifdef ISP2401
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#endif
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+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/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.c
new file mode 100644
index 000000000000..11e4463ebb50
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.c
@@ -0,0 +1,3220 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.aa.size;
+	unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.anr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.anr2.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bh.size;
+
+		unsigned 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 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.cnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.crop.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.csc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.dp.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.de.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ecd.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.formats.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fpn.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.gc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem1.gc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ce.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yuv2rgb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.rgb2yuv.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem0.r_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.g_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem2.b_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.uds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.raa.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.s3a.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ob.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vmem.ob.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.output.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.tnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.macc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.wb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.nr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yee.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ynr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ctc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem0.ctc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.xnr_table.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr3.size;
+
+		unsigned 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");
+		}
+
+	}
+#ifdef ISP2401
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.size;
+
+		unsigned offset = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_xnr3() enter:\n");
+
+			ia_css_xnr3_vmem_encode((struct sh_css_isp_xnr3_vmem_params *)
+					&stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+					&params->xnr3_config,
+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_xnr3() leave:\n");
+		}
+
+	}
+#endif
+}
+
+/* Code generated by genparam/gencode.c:gen_param_process_table() */
+
+void (* ia_css_kernel_process_param[IA_CSS_NUM_PARAMETER_IDS])(
+			unsigned 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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DP_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_WB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_TNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR2_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ECD_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_MACC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CTC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_AA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YUV2RGB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_RGB2YUV_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CSC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_NR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_GC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_R_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_G_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_B_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_TABLE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FORMATS_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR3_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_S3A_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OUTPUT_ID] = true;
+
+#endif
+	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/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.h
new file mode 100644
index 000000000000..5b3deb7f74ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_params.h
@@ -0,0 +1,399 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter xnr3;
+#endif
+	} 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 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)
+;
+#ifdef ISP2401
+
+#endif
+/* 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)
+;
+#ifdef ISP2401
+
+#endif
+#endif /* IA_CSS_INCLUDE_PARAMETER */
+
+#endif /* _IA_CSS_ISP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.c
new file mode 100644
index 000000000000..e87d05bc73ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.c
@@ -0,0 +1,214 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size   = binary->info->mem_offsets.offsets.state->vmem.aa.size;
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr2.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.dp.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.de.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.tnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.ref.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.ynr.size;
+
+		unsigned 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/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.h
new file mode 100644
index 000000000000..732adafb0a63
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hive_isp_css_2401_system_csi2p_generated/ia_css_isp_states.h
@@ -0,0 +1,72 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx.c
new file mode 100644
index 000000000000..505e2b600beb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx.c
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+const uint32_t 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 uint32_t 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 uint32_t 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 uint32_t N_CSI_RX_BE_SID_WIDTH[N_CSI_RX_BACKEND_ID] = {
+	3,
+	2,
+	2
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_local.h
new file mode 100644
index 000000000000..a2e9d54a4a37
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_local.h
@@ -0,0 +1,61 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_private.h
new file mode 100644
index 000000000000..5819bcff5e55
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/csi_rx_private.h
@@ -0,0 +1,282 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_public.h"
+
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+
+#include "assert_support.h" /* assert */
+#include "print_support.h" /* print */
+
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the csi rx fe state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+STORAGE_CLASS_CSI_RX_C void csi_rx_fe_ctrl_get_state(
+		const csi_rx_frontend_ID_t ID,
+		csi_rx_fe_ctrl_state_t *state)
+{
+	uint32_t 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 Get the state of the csi rx fe dlane process.
+ * Refer to "csi_rx_public.h" for details.
+ */
+STORAGE_CLASS_CSI_RX_C void csi_rx_fe_ctrl_get_dlane_state(
+		const csi_rx_frontend_ID_t ID,
+		const uint32_t 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 dump the csi rx fe state.
+ * Refer to "csi_rx_public.h" for details.
+ */
+STORAGE_CLASS_CSI_RX_C void csi_rx_fe_ctrl_dump_state(
+		const csi_rx_frontend_ID_t ID,
+		csi_rx_fe_ctrl_state_t *state)
+{
+	uint32_t 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.
+ */
+STORAGE_CLASS_CSI_RX_C void csi_rx_be_ctrl_get_state(
+		const csi_rx_backend_ID_t ID,
+		csi_rx_be_ctrl_state_t *state)
+{
+	uint32_t 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.
+ */
+STORAGE_CLASS_CSI_RX_C void csi_rx_be_ctrl_dump_state(
+		const csi_rx_backend_ID_t ID,
+		csi_rx_be_ctrl_state_t *state)
+{
+	uint32_t 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 diregard 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 packat entry %d shart 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 packat entry %d Long packet lut id 0x%x \n", ID, i, state->long_packet_lut_entry[i]);
+	}
+}
+/* end of NCI */
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Refer to "csi_rx_public.h" for details.
+ */
+STORAGE_CLASS_CSI_RX_C 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.
+ */
+STORAGE_CLASS_CSI_RX_C 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.
+ */
+STORAGE_CLASS_CSI_RX_C 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.
+ */
+STORAGE_CLASS_CSI_RX_C 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 */
+
+#endif /* __CSI_RX_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl.c
new file mode 100644
index 000000000000..14973d1c2756
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl.c
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+const uint32_t 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/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_local.h
new file mode 100644
index 000000000000..ea40284623d1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_local.h
@@ -0,0 +1,58 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+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/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_private.h
new file mode 100644
index 000000000000..470c92d287fe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/ibuf_ctrl_private.h
@@ -0,0 +1,233 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PRIVATE_H_INCLUDED__
+#define __IBUF_CTRL_PRIVATE_H_INCLUDED__
+
+#include "ibuf_ctrl_public.h"
+
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+
+#include "assert_support.h" /* assert */
+#include "print_support.h" /* print */
+
+
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the ibuf-controller state.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_IBUF_CTRL_C void ibuf_ctrl_get_state(
+		const ibuf_ctrl_ID_t ID,
+		ibuf_ctrl_state_t *state)
+{
+	uint32_t 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]));
+	}
+}
+
+/**
+ * @brief Get the state of the ibuf-controller process.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_IBUF_CTRL_C void ibuf_ctrl_get_proc_state(
+		const ibuf_ctrl_ID_t ID,
+		const uint32_t 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);
+}
+/**
+ * @brief Dump the ibuf-controller state.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_IBUF_CTRL_C void ibuf_ctrl_dump_state(
+		const ibuf_ctrl_ID_t ID,
+		ibuf_ctrl_state_t *state)
+{
+	uint32_t 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);
+	}
+}
+/** end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_IBUF_CTRL_C 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));
+}
+
+
+/**
+ * @brief Store a value to the register.
+ * Refer to "ibuf_ctrl_public.h" for details.
+ */
+STORAGE_CLASS_IBUF_CTRL_C 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);
+}
+/** end of DLI */
+
+
+#endif /* __IBUF_CTRL_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/input_system_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/input_system_local.h
new file mode 100644
index 000000000000..f199423e28da
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/input_system_local.h
@@ -0,0 +1,106 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "type_support.h"
+#include "input_system_global.h"
+
+#include "ibuf_ctrl.h"
+#include "csi_rx.h"
+#include "pixelgen.h"
+#include "isys_stream2mmio.h"
+#include "isys_irq.h"
+
+typedef input_system_err_t input_system_error_t;
+
+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)
+#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_PREDICTOR_NONE = 0,
+	MIPI_PREDICTOR_TYPE1,
+	MIPI_PREDICTOR_TYPE2,
+	N_MIPI_PREDICTOR_TYPES
+} mipi_predictor_t;
+
+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/atomisp2/css2400/css_2401_csi2p_system/host/input_system_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/input_system_private.h
new file mode 100644
index 000000000000..97505e436047
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/input_system_private.h
@@ -0,0 +1,128 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+STORAGE_CLASS_INPUT_SYSTEM_C input_system_err_t input_system_get_state(
+	const input_system_ID_t	ID,
+	input_system_state_t *state)
+{
+	uint32_t 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;
+}
+STORAGE_CLASS_INPUT_SYSTEM_C void input_system_dump_state(
+	const input_system_ID_t	ID,
+	input_system_state_t *state)
+{
+	uint32_t 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/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma.c
new file mode 100644
index 000000000000..77767228985e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma.c
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_dma.h"
+#include "assert_support.h"
+
+#ifndef __INLINE_ISYS2401_DMA__
+/*
+ * Include definitions for isys dma register access functions. isys_dma.h
+ * includes declarations of these functions by including isys_dma_public.h.
+ */
+#include "isys_dma_private.h"
+#endif
+
+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/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_local.h
new file mode 100644
index 000000000000..5c694a26386e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_local.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_LOCAL_H_INCLUDED__
+#define __ISYS_DMA_LOCAL_H_INCLUDED__
+
+#include "isys_dma_global.h"
+
+#endif /* __ISYS_DMA_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_private.h
new file mode 100644
index 000000000000..2cd1aeecf617
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_dma_private.h
@@ -0,0 +1,60 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+
+STORAGE_CLASS_ISYS2401_DMA_C 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);
+}
+
+STORAGE_CLASS_ISYS2401_DMA_C 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/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq.c
new file mode 100644
index 000000000000..14d1d3b627a9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq.c
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 */
+STORAGE_CLASS_ISYS2401_IRQ_C 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/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_local.h
new file mode 100644
index 000000000000..0bffb5680e25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_local.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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(USE_INPUT_SYSTEM_VERSION_2401)
+
+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(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#endif	/* __ISYS_IRQ_LOCAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_private.h
new file mode 100644
index 000000000000..c17ce131c9e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_irq_private.h
@@ -0,0 +1,108 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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(USE_INPUT_SYSTEM_VERSION_2401)
+
+/* -------------------------------------------------------+
+ |             Native command interface (NCI)             |
+ + -------------------------------------------------------*/
+
+/**
+* @brief Get the isys irq status.
+* Refer to "isys_irq.h" for details.
+*/
+STORAGE_CLASS_ISYS2401_IRQ_C 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.
+*/
+STORAGE_CLASS_ISYS2401_IRQ_C 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 */
+STORAGE_CLASS_ISYS2401_IRQ_C 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);
+}
+
+STORAGE_CLASS_ISYS2401_IRQ_C 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(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#endif	/* __ISYS_IRQ_PRIVATE_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio.c
new file mode 100644
index 000000000000..67570138ba24
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio.c
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_local.h
new file mode 100644
index 000000000000..801523977e1d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_local.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_private.h
new file mode 100644
index 000000000000..1603a09b621a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/isys_stream2mmio_private.h
@@ -0,0 +1,168 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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)
+{
+	uint32_t 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/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_local.h
new file mode 100644
index 000000000000..24f4da9aef40
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_local.h
@@ -0,0 +1,50 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_private.h
new file mode 100644
index 000000000000..3f34b508f0bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/pixelgen_private.h
@@ -0,0 +1,164 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "hive_isp_css_host_ids_hrt.h"
+#include "PixelGen_SysBlock_defs.h"
+#include "device_access.h"	/* ia_css_device_load_uint32 */
+#include "assert_support.h" /* assert */
+
+
+/*****************************************************
+ *
+ * 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 vlue 0 0x%x \n", ID, state->prbs_rstval0);
+	ia_css_print("Pixel Generator ID %d PRBS reset vlue 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 */
+/*****************************************************
+ *
+ * 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 */
+#endif /* __PIXELGEN_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/system_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/system_local.h
new file mode 100644
index 000000000000..c16670989702
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/host/system_local.h
@@ -0,0 +1,381 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+/* This interface is deprecated */
+/*#include "hive_isp_css_custom_host_hrt.h"*/
+#endif
+
+#include "system_global.h"
+
+#ifdef __FIST__
+#define HRT_ADDRESS_WIDTH	32		/* Surprise, this is a local property and even differs per platform */
+#else
+#define HRT_ADDRESS_WIDTH	64		/* Surprise, this is a local property */
+#endif
+
+#if !defined(__KERNEL__) || (1 == 1)
+/* This interface is deprecated */
+#include "hrt/hive_types.h"
+#else  /* __KERNEL__ */
+#include <type_support.h>
+
+#if HRT_ADDRESS_WIDTH == 64
+typedef uint64_t			hrt_address;
+#elif HRT_ADDRESS_WIDTH == 32
+typedef uint32_t			hrt_address;
+#else
+#error "system_local.h: HRT_ADDRESS_WIDTH must be one of {32,64}"
+#endif
+
+typedef uint32_t			hrt_vaddress;
+typedef uint32_t			hrt_data;
+#endif /* __KERNEL__ */
+
+/*
+ * Cell specific address maps
+ */
+#if HRT_ADDRESS_WIDTH == 64
+
+#define GP_FIFO_BASE   ((hrt_address)0x0000000000090104)		/* This is NOT a base address */
+
+/* DDR */
+static const hrt_address DDR_BASE[N_DDR_ID] = {
+	0x0000000120000000ULL};
+
+/* ISP */
+static const hrt_address ISP_CTRL_BASE[N_ISP_ID] = {
+	0x0000000000020000ULL};
+
+static const hrt_address ISP_DMEM_BASE[N_ISP_ID] = {
+	0x0000000000200000ULL};
+
+static const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID] = {
+	0x0000000000100000ULL};
+
+static const hrt_address ISP_VAMEM_BASE[N_VAMEM_ID] = {
+	0x00000000001C0000ULL,
+	0x00000000001D0000ULL,
+	0x00000000001E0000ULL};
+
+static const hrt_address ISP_HMEM_BASE[N_HMEM_ID] = {
+	0x00000000001F0000ULL};
+
+/* SP */
+static const hrt_address SP_CTRL_BASE[N_SP_ID] = {
+	0x0000000000010000ULL};
+
+static const hrt_address SP_DMEM_BASE[N_SP_ID] = {
+	0x0000000000300000ULL};
+
+/* MMU */
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM) || defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+/*
+ * MMU0_ID: The data MMU
+ * MMU1_ID: The icache MMU
+ */
+static const hrt_address MMU_BASE[N_MMU_ID] = {
+	0x0000000000070000ULL,
+	0x00000000000A0000ULL};
+#else
+#error "system_local.h: SYSTEM must be one of {2400, 2401 }"
+#endif
+
+/* DMA */
+static const hrt_address DMA_BASE[N_DMA_ID] = {
+	0x0000000000040000ULL};
+
+static const hrt_address ISYS2401_DMA_BASE[N_ISYS2401_DMA_ID] = {
+	0x00000000000CA000ULL};
+
+/* IRQ */
+static const hrt_address IRQ_BASE[N_IRQ_ID] = {
+	0x0000000000000500ULL,
+	0x0000000000030A00ULL,
+	0x000000000008C000ULL,
+	0x0000000000090200ULL};
+/*
+	0x0000000000000500ULL};
+ */
+
+/* GDC */
+static const hrt_address GDC_BASE[N_GDC_ID] = {
+	0x0000000000050000ULL,
+	0x0000000000060000ULL};
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+static const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID] = {
+	0x0000000000000000ULL};
+
+/*
+static const hrt_address GP_REGS_BASE[N_GP_REGS_ID] = {
+	0x0000000000000000ULL};
+
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	0x0000000000090000ULL};
+*/
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+static 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*/
+static const hrt_address GP_TIMER_BASE =
+	(hrt_address)0x0000000000000600ULL;
+
+/* GPIO */
+static const hrt_address GPIO_BASE[N_GPIO_ID] = {
+	0x0000000000000400ULL};
+
+/* TIMED_CTRL */
+static const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID] = {
+	0x0000000000000100ULL};
+
+
+/* INPUT_FORMATTER */
+static const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID] = {
+	0x0000000000030000ULL,
+	0x0000000000030200ULL,
+	0x0000000000030400ULL,
+	0x0000000000030600ULL}; /* memcpy() */
+
+/* INPUT_SYSTEM */
+static 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 */
+static const hrt_address RX_BASE[N_RX_ID] = {
+	0x0000000000080100ULL};
+
+/* IBUF_CTRL, part of the Input System 2401 */
+static 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 */
+static 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 */
+static 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 */
+static 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 */
+static 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 */
+static const hrt_address STREAM2MMIO_CTRL_BASE[N_STREAM2MMIO_ID] = {
+	0x00000000000C0C00ULL,	/* stream2mmio controller A */
+	0x00000000000C2C00ULL,	/* stream2mmio controller B */
+	0x00000000000C4C00ULL	/* stream2mmio controller C */
+};
+#elif HRT_ADDRESS_WIDTH == 32
+
+#define GP_FIFO_BASE   ((hrt_address)0x00090104)		/* This is NOT a base address */
+
+/* DDR : Attention, this value not defined in 32-bit */
+static const hrt_address DDR_BASE[N_DDR_ID] = {
+	0x00000000UL};
+
+/* ISP */
+static const hrt_address ISP_CTRL_BASE[N_ISP_ID] = {
+	0x00020000UL};
+
+static const hrt_address ISP_DMEM_BASE[N_ISP_ID] = {
+	0xffffffffUL};
+
+static const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID] = {
+	0xffffffffUL};
+
+static const hrt_address ISP_VAMEM_BASE[N_VAMEM_ID] = {
+	0xffffffffUL,
+	0xffffffffUL,
+	0xffffffffUL};
+
+static const hrt_address ISP_HMEM_BASE[N_HMEM_ID] = {
+	0xffffffffUL};
+
+/* SP */
+static const hrt_address SP_CTRL_BASE[N_SP_ID] = {
+	0x00010000UL};
+
+static const hrt_address SP_DMEM_BASE[N_SP_ID] = {
+	0x00300000UL};
+
+/* MMU */
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM) || defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+/*
+ * MMU0_ID: The data MMU
+ * MMU1_ID: The icache MMU
+ */
+static const hrt_address MMU_BASE[N_MMU_ID] = {
+	0x00070000UL,
+	0x000A0000UL};
+#else
+#error "system_local.h: SYSTEM must be one of {2400, 2401 }"
+#endif
+
+/* DMA */
+static const hrt_address DMA_BASE[N_DMA_ID] = {
+	0x00040000UL};
+
+static const hrt_address ISYS2401_DMA_BASE[N_ISYS2401_DMA_ID] = {
+	0x000CA000UL};
+
+/* IRQ */
+static const hrt_address IRQ_BASE[N_IRQ_ID] = {
+	0x00000500UL,
+	0x00030A00UL,
+	0x0008C000UL,
+	0x00090200UL};
+/*
+	0x00000500UL};
+ */
+
+/* GDC */
+static const hrt_address GDC_BASE[N_GDC_ID] = {
+	0x00050000UL,
+	0x00060000UL};
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+static const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID] = {
+	0x00000000UL};
+
+/*
+static const hrt_address GP_REGS_BASE[N_GP_REGS_ID] = {
+	0x00000000UL};
+
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	0x00090000UL};
+*/
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	0x00000000UL};
+
+/*GP TIMER , all timer registers are inter-twined,
+ * so, having multiple base addresses for
+ * different timers does not help*/
+static const hrt_address GP_TIMER_BASE =
+	(hrt_address)0x00000600UL;
+/* GPIO */
+static const hrt_address GPIO_BASE[N_GPIO_ID] = {
+	0x00000400UL};
+
+/* TIMED_CTRL */
+static const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID] = {
+	0x00000100UL};
+
+
+/* INPUT_FORMATTER */
+static const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID] = {
+	0x00030000UL,
+	0x00030200UL,
+	0x00030400UL};
+/*	0x00030600UL, */ /* memcpy() */
+
+/* INPUT_SYSTEM */
+static const hrt_address INPUT_SYSTEM_BASE[N_INPUT_SYSTEM_ID] = {
+	0x00080000UL};
+/*	0x00081000UL, */ /* capture A */
+/*	0x00082000UL, */ /* capture B */
+/*	0x00083000UL, */ /* capture C */
+/*	0x00084000UL, */ /* Acquisition */
+/*	0x00085000UL, */ /* DMA */
+/*	0x00089000UL, */ /* ctrl */
+/*	0x0008A000UL, */ /* GP regs */
+/*	0x0008B000UL, */ /* FIFO */
+/*	0x0008C000UL, */ /* IRQ */
+
+/* RX, the MIPI lane control regs start at offset 0 */
+static const hrt_address RX_BASE[N_RX_ID] = {
+	0x00080100UL};
+
+/* IBUF_CTRL, part of the Input System 2401 */
+static const hrt_address IBUF_CTRL_BASE[N_IBUF_CTRL_ID] = {
+	0x000C1800UL,	/* ibuf controller A */
+	0x000C3800UL,	/* ibuf controller B */
+	0x000C5800UL	/* ibuf controller C */
+};
+
+/* ISYS IRQ Controllers, part of the Input System 2401 */
+static const hrt_address ISYS_IRQ_BASE[N_ISYS_IRQ_ID] = {
+	0x000C1400ULL,	/* port a */
+	0x000C3400ULL,	/* port b */
+	0x000C5400ULL	/* port c */
+};
+
+/* CSI FE, part of the Input System 2401 */
+static const hrt_address CSI_RX_FE_CTRL_BASE[N_CSI_RX_FRONTEND_ID] = {
+	0x000C0400UL,	/* csi fe controller A */
+	0x000C2400UL,	/* csi fe controller B */
+	0x000C4400UL	/* csi fe controller C */
+};
+/* CSI BE, part of the Input System 2401 */
+static const hrt_address CSI_RX_FE_CTRL_BASE[N_CSI_RX_BACKEND_ID] = {
+	0x000C0800UL,	/* csi be controller A */
+	0x000C2800UL,	/* csi be controller B */
+	0x000C4800UL	/* csi be controller C */
+};
+/* PIXEL Generator, part of the Input System 2401 */
+static const hrt_address PIXELGEN_CTRL_BASE[N_PIXELGEN_ID] = {
+	0x000C1000UL,	/* pixel gen controller A */
+	0x000C3000UL,	/* pixel gen controller B */
+	0x000C5000UL	/* pixel gen controller C */
+};
+/* Stream2MMIO, part of the Input System 2401 */
+static const hrt_address STREAM2MMIO_CTRL_BASE[N_STREAM2MMIO_ID] = {
+	0x000C0C00UL,	/* stream2mmio controller A */
+	0x000C2C00UL,	/* stream2mmio controller B */
+	0x000C4C00UL	/* stream2mmio controller C */
+};
+
+#else
+#error "system_local.h: HRT_ADDRESS_WIDTH must be one of {32,64}"
+#endif
+
+#endif /* __SYSTEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/PixelGen_SysBlock_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/PixelGen_SysBlock_defs.h
new file mode 100644
index 000000000000..1b3391c242a3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/PixelGen_SysBlock_defs.h
@@ -0,0 +1,126 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+
+#ifdef ISYS2401_PXG_A
+#else
+#ifdef ISYS2401_PXG_B
+#else
+#ifdef ISYS2401_PXG_C
+#else
+#include <mipi_backend/hrt/include/mipi_backend_defs.h>
+#endif
+#endif
+#endif
+
+/* 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_DISBALE_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/atomisp2/css2400/css_2401_csi2p_system/hrt/bits.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/bits.h
new file mode 100644
index 000000000000..e71e33d9d143
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/bits.h
@@ -0,0 +1,104 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/cell_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/cell_params.h
new file mode 100644
index 000000000000..b5756bfe8eb6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/cell_params.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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        (1<<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/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_common_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_common_defs.h
new file mode 100644
index 000000000000..f3054fe04d03
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_common_defs.h
@@ -0,0 +1,200 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 reseved 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/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_defs.h
new file mode 100644
index 000000000000..6f5b7d3d3715
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/css_receiver_2400_defs.h
@@ -0,0 +1,258 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 (1<<16)
+#define _HRT_CSS_RECEIVER_2400_VSYNC_VAL (1<<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/atomisp2/css2400/css_2401_csi2p_system/hrt/defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/defs.h
new file mode 100644
index 000000000000..47505f41790c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/dma_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/dma_v2_defs.h
new file mode 100644
index 000000000000..d184a8b313c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/dma_v2_defs.h
@@ -0,0 +1,199 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/gdc_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gdc_v2_defs.h
new file mode 100644
index 000000000000..77722d205701
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gdc_v2_defs.h
@@ -0,0 +1,170 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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             (1 << 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/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_regs_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_regs_defs.h
new file mode 100644
index 000000000000..34e734f6648e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_regs_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_regs_defs_h
+#define _gp_regs_defs_h
+
+#define _HRT_GP_REGS_IS_FWD_REG_IDX 0
+
+#define _HRT_GP_REGS_REG_ALIGN 4
+
+#endif /* _gp_regs_defs_h */   
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_timer_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_timer_defs.h
new file mode 100644
index 000000000000..3082e2f5e014
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gp_timer_defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/gpio_block_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gpio_block_defs.h
new file mode 100644
index 000000000000..a807d4c99041
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/gpio_block_defs.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_2401_irq_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_2401_irq_types_hrt.h
new file mode 100644
index 000000000000..2f7cb2dff0e9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_2401_irq_types_hrt.h
@@ -0,0 +1,68 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_2401_IRQ_TYPES_HRT_H_
+#define _HIVE_ISP_CSS_2401_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_is2401               = HIVE_GP_DEV_IRQ_IS2401_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_2401_IRQ_TYPES_HRT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_defs.h
new file mode 100644
index 000000000000..5a2ce9108ae4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_defs.h
@@ -0,0 +1,435 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CSS_2401_SYSTEM     1
+#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)
+#define HIVE_ISP_DDR_BYTES_RTL   (127 * 1024 * 1024)
+#define HIVE_ISP_DDR_SMALL_BYTES (128 * 256 / 8)
+#define HIVE_ISP_PAGE_SHIFT    12
+#define HIVE_ISP_PAGE_SIZE     (1<<HIVE_ISP_PAGE_SHIFT)
+
+#define CSS_DDR_WORD_BITS        HIVE_ISP_DDR_WORD_BITS
+#define CSS_DDR_WORD_BYTES       HIVE_ISP_DDR_WORD_BYTES
+
+/* settings used in applications */
+#define HIVE_XMEM_WIDTH            HIVE_ISP_DDR_WORD_BITS
+#define HIVE_VMEM_VECTOR_ELEMENTS  64
+#define HIVE_VMEM_ELEMENT_BITS     14
+#define HIVE_XMEM_ELEMENT_BITS     16
+#define HIVE_VMEM_VECTOR_BYTES (HIVE_VMEM_VECTOR_ELEMENTS*HIVE_XMEM_ELEMENT_BITS/8) /* used for # addr bytes for one vector */
+#define HIVE_XMEM_PACKED_WORD_VMEM_ELEMENTS (HIVE_XMEM_WIDTH/HIVE_VMEM_ELEMENT_BITS)
+#define HIVE_XMEM_WORD_VMEM_ELEMENTS        (HIVE_XMEM_WIDTH/HIVE_XMEM_ELEMENT_BITS)
+#define XMEM_INT_SIZE              4
+
+
+
+#define HIVE_ISYS_INP_BUFFER_BYTES (64*1024)  /* 64 kByte = 2k words (of 256 bits) */
+
+/* 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
+#define HIVE_GP_REGS_SWITCH_ISYS_IDX                           21
+
+/* 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
+#define HIVE_GP_REGS_SRST_ISYS_INP_BUF_BUS                     31
+
+/* 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_IS2401_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_IS2401_IRQ                               19
+#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
+#define HIVE_GP_TIMER_ISP_PMEM_ERROR_IRQ                       60
+
+/* 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
+
+#define HIVE_TESTBENCH_MIPI_PARPATHEN_REG_IDX                  14
+#define HIVE_TESTBENCH_FB_HPLL_FREQ_REG_IDX                    15
+#define HIVE_TESTBENCH_ISCLK_RATIO_REG_IDX                     16
+
+/* 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/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_host_ids_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_host_ids_hrt.h
new file mode 100644
index 000000000000..8d4c9d67c0e2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_host_ids_hrt.h
@@ -0,0 +1,119 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_host_ids_hrt_h_
+#define _hive_isp_css_host_ids_hrt_h_
+
+/* ISP_CSS identifiers */
+#define INP_SYS       testbench_isp_isp_css_part_is_2400_inp_sys
+#define ISYS_GP_REGS  testbench_isp_isp_css_part_is_2400_inp_sys_gpreg
+#define ISYS_IRQ_CTRL testbench_isp_isp_css_part_is_2400_inp_sys_irq_ctrl
+#define ISYS_CAP_A    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_a
+#define ISYS_CAP_B    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_b
+#define ISYS_CAP_C    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_c
+#define ISYS_INP_BUF  testbench_isp_isp_css_part_input_buffer
+#define ISYS_INP_CTRL testbench_isp_isp_css_part_is_2400_inp_sys_inp_ctrl
+#define ISYS_ACQ      testbench_isp_isp_css_part_is_2400_inp_sys_acq_unit
+
+#define ISP           testbench_isp_isp_css_sec_part_isp
+#define SP            testbench_isp_isp_css_sec_part_scp
+
+#define IF_PRIM       testbench_isp_isp_css_part_is_2400_ifmt_ift_prim  
+#define IF_PRIM_B     testbench_isp_isp_css_part_is_2400_ifmt_ift_prim_b
+#define IF_SEC        testbench_isp_isp_css_part_is_2400_ifmt_ift_sec
+#define IF_SEC_MASTER testbench_isp_isp_css_part_is_2400_ifmt_ift_sec_mt_out
+#define STR_TO_MEM    testbench_isp_isp_css_part_is_2400_ifmt_mem_cpy
+#define IFMT_GP_REGS  testbench_isp_isp_css_part_is_2400_ifmt_gp_reg
+#define IFMT_IRQ_CTRL testbench_isp_isp_css_part_is_2400_ifmt_irq_ctrl
+
+#define CSS_RECEIVER  testbench_isp_isp_css_part_is_2400_inp_sys_csi_receiver
+
+#define TC            testbench_isp_isp_css_sec_part_gpd_tc
+#define GPTIMER       testbench_isp_isp_css_sec_part_gpd_gptimer
+#define DMA           testbench_isp_isp_css_sec_part_isp_dma
+#define GDC           testbench_isp_isp_css_sec_part_gdc1
+#define GDC2          testbench_isp_isp_css_sec_part_gdc2
+#define IRQ_CTRL      testbench_isp_isp_css_sec_part_gpd_irq_ctrl
+#define GPIO          testbench_isp_isp_css_sec_part_gpd_c_gpio
+#define GP_REGS       testbench_isp_isp_css_sec_part_gpd_gp_reg
+#define ISEL_GP_REGS  testbench_isp_isp_css_part_is_2400_isel_gpr
+#define ISEL_IRQ_CTRL testbench_isp_isp_css_part_is_2400_isel_irq_ctrl
+#define DATA_MMU      testbench_isp_isp_css_sec_part_data_out_sys_c_mmu
+#define ICACHE_MMU    testbench_isp_isp_css_sec_part_icache_out_sys_c_mmu
+
+/* next is actually not FIFO but FIFO adapter, or slave to streaming adapter */
+#define ISP_SP_FIFO   testbench_isp_isp_css_sec_part_fa_sp_isp
+#define ISEL_FIFO     testbench_isp_isp_css_part_is_2400_isel_sf_fa_in
+
+#define FIFO_GPF_SP   testbench_isp_isp_css_sec_part_sf_fa2sp_in
+#define FIFO_GPF_ISP  testbench_isp_isp_css_sec_part_sf_fa2isp_in
+#define FIFO_SP_GPF   testbench_isp_isp_css_sec_part_sf_sp2fa_in
+#define FIFO_ISP_GPF  testbench_isp_isp_css_sec_part_sf_isp2fa_in
+
+#define DATA_OCP_MASTER    testbench_isp_isp_css_sec_part_data_out_sys_cio2ocp_wide_data_out_mt
+#define ICACHE_OCP_MASTER  testbench_isp_isp_css_sec_part_icache_out_sys_cio2ocp_wide_data_out_mt
+
+#define SP_IN_FIFO    testbench_isp_isp_css_sec_part_sf_fa2sp_in
+#define SP_OUT_FIFO   testbench_isp_isp_css_sec_part_sf_sp2fa_out
+#define ISP_IN_FIFO   testbench_isp_isp_css_sec_part_sf_fa2isp_in
+#define ISP_OUT_FIFO  testbench_isp_isp_css_sec_part_sf_isp2fa_out
+#define GEN_SHORT_PACK_PORT testbench_isp_isp_css_part_is_2400_inp_sys_csi_str_mon_fa_gensh_out
+
+/* input_system_2401 identifiers */
+#define ISYS2401_GP_REGS    testbench_isp_isp_css_part_is_2401_gpreg
+#define ISYS2401_DMA        testbench_isp_isp_css_part_is_2401_dma
+#define ISYS2401_IRQ_CTRL   testbench_isp_isp_css_part_is_2401_isys_irq_ctrl
+
+#define ISYS2401_CSI_RX_A     testbench_isp_isp_css_part_is_2401_is_pipe_a_csi_rx
+#define ISYS2401_MIPI_BE_A    testbench_isp_isp_css_part_is_2401_is_pipe_a_mipi_be
+#define ISYS2401_S2M_A        testbench_isp_isp_css_part_is_2401_is_pipe_a_s2m
+#define ISYS2401_PXG_A        testbench_isp_isp_css_part_is_2401_is_pipe_a_pxlgen
+#define ISYS2401_IBUF_CNTRL_A testbench_isp_isp_css_part_is_2401_is_pipe_a_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_A   testbench_isp_isp_css_part_is_2401_is_pipe_a_irq_ctrl_pipe
+
+#define ISYS2401_CSI_RX_B     testbench_isp_isp_css_part_is_2401_is_pipe_b_csi_rx
+#define ISYS2401_MIPI_BE_B    testbench_isp_isp_css_part_is_2401_is_pipe_b_mipi_be
+#define ISYS2401_S2M_B        testbench_isp_isp_css_part_is_2401_is_pipe_b_s2m
+#define ISYS2401_PXG_B        testbench_isp_isp_css_part_is_2401_is_pipe_b_pxlgen
+#define ISYS2401_IBUF_CNTRL_B testbench_isp_isp_css_part_is_2401_is_pipe_b_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_B   testbench_isp_isp_css_part_is_2401_is_pipe_b_irq_ctrl_pipe
+
+#define ISYS2401_CSI_RX_C     testbench_isp_isp_css_part_is_2401_is_pipe_c_csi_rx
+#define ISYS2401_MIPI_BE_C    testbench_isp_isp_css_part_is_2401_is_pipe_c_mipi_be
+#define ISYS2401_S2M_C        testbench_isp_isp_css_part_is_2401_is_pipe_c_s2m
+#define ISYS2401_PXG_C        testbench_isp_isp_css_part_is_2401_is_pipe_c_pxlgen
+#define ISYS2401_IBUF_CNTRL_C testbench_isp_isp_css_part_is_2401_is_pipe_c_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_C   testbench_isp_isp_css_part_is_2401_is_pipe_c_irq_ctrl_pipe
+
+
+/* Testbench identifiers */
+#define DDR             testbench_ddram
+#define DDR_SMALL       testbench_ddram_small
+#define XMEM            DDR
+#define GPIO_ADAPTER    testbench_gp_adapter
+#define SIG_MONITOR     testbench_sig_mon
+#define DDR_SLAVE       testbench_ddram_ip0
+#define DDR_SMALL_SLAVE testbench_ddram_small_ip0
+#define HOST_MASTER     host_op0
+
+#define CSI_SENSOR         testbench_vied_sensor
+#define CSI_SENSOR_GP_REGS testbench_vied_sensor_gpreg
+#define CSI_STR_IN_A       testbench_vied_sensor_tx_a_csi_tx_data_in
+#define CSI_STR_IN_B       testbench_vied_sensor_tx_b_csi_tx_data_in
+#define CSI_STR_IN_C       testbench_vied_sensor_tx_c_csi_tx_data_in
+#define CSI_SENSOR_TX_A    testbench_vied_sensor_tx_a
+#define CSI_SENSOR_TX_B    testbench_vied_sensor_tx_b
+#define CSI_SENSOR_TX_C    testbench_vied_sensor_tx_c
+
+#endif /* _hive_isp_css_host_ids_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
new file mode 100644
index 000000000000..b4211a0c631a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_types.h
new file mode 100644
index 000000000000..58b0e6effbd0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/hive_types.h
@@ -0,0 +1,128 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+
+/* by default assume 32 bit master port (both data and address) */
+#ifndef HRT_DATA_WIDTH
+#define HRT_DATA_WIDTH 32
+#endif
+#ifndef HRT_ADDRESS_WIDTH
+#define HRT_ADDRESS_WIDTH 32
+#endif
+
+#define HRT_DATA_BYTES    (HRT_DATA_WIDTH/8)
+#define HRT_ADDRESS_BYTES (HRT_ADDRESS_WIDTH/8)
+
+#if HRT_DATA_WIDTH == 64
+typedef hive_uint64 hrt_data;
+#elif HRT_DATA_WIDTH == 32
+typedef hive_uint32 hrt_data;
+#else
+#error data width not supported
+#endif
+
+#if HRT_ADDRESS_WIDTH == 64
+typedef hive_uint64 hrt_address; 
+#elif HRT_ADDRESS_WIDTH == 32
+typedef hive_uint32 hrt_address;
+#else
+#error adddres width not supported
+#endif
+
+/* The SP side representation of an HMM virtual address */
+typedef hive_uint32 hrt_vaddress;
+
+/* 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/atomisp2/css2400/css_2401_csi2p_system/hrt/ibuf_cntrl_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/ibuf_cntrl_defs.h
new file mode 100644
index 000000000000..f82bb79785cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/ibuf_cntrl_defs.h
@@ -0,0 +1,138 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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  (1<<_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/atomisp2/css2400/css_2401_csi2p_system/hrt/if_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/if_defs.h
new file mode 100644
index 000000000000..7d39e45796ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/if_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/input_formatter_subsystem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_formatter_subsystem_defs.h
new file mode 100644
index 000000000000..16bfe1d80bc9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_formatter_subsystem_defs.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/input_selector_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_selector_defs.h
new file mode 100644
index 000000000000..87fbf82edb5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_selector_defs.h
@@ -0,0 +1,89 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/input_switch_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_switch_2400_defs.h
new file mode 100644
index 000000000000..20a13c4cdb56
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_switch_2400_defs.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_ctrl_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_ctrl_defs.h
new file mode 100644
index 000000000000..a7f0ca80bc9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_ctrl_defs.h
@@ -0,0 +1,254 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_defs.h
new file mode 100644
index 000000000000..ae62163034a6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/input_system_defs.h
@@ -0,0 +1,126 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/irq_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/irq_controller_defs.h
new file mode 100644
index 000000000000..ec6dd4487158
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/irq_controller_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2400_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2400_support.h
new file mode 100644
index 000000000000..e00bc841d0f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2400_support.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2401_mamoiada_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2401_mamoiada_params.h
new file mode 100644
index 000000000000..033e23bcf672
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp2401_mamoiada_params.h
@@ -0,0 +1,258 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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
+
+/* Cell name  */
+#define ISP_CELL_TYPE                          isp2401_mamoiada
+#define ISP_VMEM                               simd_vmem
+#define _HRT_ISP_VMEM                          isp2401_mamoiada_simd_vmem
+
+/* 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_VMEM_IS_BAMEM                      1
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_BAMEM_MAX_BOI_HEIGHT        8
+  #define ISP_VMEM_BAMEM_LATENCY               5
+  #define ISP_VMEM_BAMEM_BANK_NARROWING_FACTOR 2
+  #define ISP_VMEM_BAMEM_NR_DATA_PLANES        8
+  #define ISP_VMEM_BAMEM_NR_CFG_REGISTERS      16
+  #define ISP_VMEM_BAMEM_LININT                0
+  #define ISP_VMEM_BAMEM_DAP_BITS              3
+  #define ISP_VMEM_BAMEM_LININT_FRAC_BITS      0
+  #define ISP_VMEM_BAMEM_PID_BITS              3
+  #define ISP_VMEM_BAMEM_OFFSET_BITS           19
+  #define ISP_VMEM_BAMEM_ADDRESS_BITS          25
+  #define ISP_VMEM_BAMEM_RID_BITS              4
+  #define ISP_VMEM_BAMEM_TRANSPOSITION         1
+  #define ISP_VMEM_BAMEM_VEC_PLUS_SLICE        1
+  #define ISP_VMEM_BAMEM_ARB_SERVICE_CYCLE_BITS 1
+  #define ISP_VMEM_BAMEM_LUT_ELEMS             16
+  #define ISP_VMEM_BAMEM_LUT_ADDR_WIDTH        14
+  #define ISP_VMEM_BAMEM_HALF_BLOCK_WRITE      1
+  #define ISP_VMEM_BAMEM_SMART_FETCH           1
+  #define ISP_VMEM_BAMEM_BIG_ENDIANNESS        0
+#endif /* ISP_VMEM_IS_BAMEM */
+#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
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_ALIGN_ELEM                  2
+#endif /* ISP_VMEM_IS_BAMEM */
+#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))
+
+#ifdef C_RUN
+#define ISP_VEC_ALIGN                          (_isp_ceil_div(ISP_VEC_WIDTH, 64)*8)
+#else
+#define ISP_VEC_ALIGN                          ISP_VMEM_ALIGN
+#endif
+
+/* HRT specific vector support */
+#define isp2401_mamoiada_vector_alignment         ISP_VEC_ALIGN
+#define isp2401_mamoiada_vector_elem_bits         ISP_VMEM_ELEMBITS
+#define isp2401_mamoiada_vector_elem_precision    ISP_VMEM_ELEM_PRECISION
+#define isp2401_mamoiada_vector_num_elems         ISP_VEC_NELEMS
+
+/* 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_VRF1_SIZE       32
+#define ISP_VRF2_SIZE       32
+#define ISP_VRF3_SIZE       32
+#define ISP_VRF4_SIZE       32
+#define ISP_VRF5_SIZE       32
+#define ISP_VRF6_SIZE       32
+#define ISP_VRF7_SIZE       32
+#define ISP_VRF8_SIZE       32
+#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/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_acquisition_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_acquisition_defs.h
new file mode 100644
index 000000000000..593620721627
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_acquisition_defs.h
@@ -0,0 +1,234 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_capture_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_capture_defs.h
new file mode 100644
index 000000000000..aa413df022f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/isp_capture_defs.h
@@ -0,0 +1,310 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 CAPT_RCV_ACK                              1
+//#define CAPT_WRT_ACK                              2               
+//#define CAPT_IRQ_ACK                              3                        
+
+/* --------------------------------------------------*/
+
+#define NOF_IRQS                                  2
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+// Number of registers
+#define CAPT_NOF_REGS                             16
+
+// Register id's of MMIO slave accesible 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_ADDR_REG_WIDTH                 9
+//#define CAPT_MEM_REGION_SIZE_REG_WIDTH            9
+//#define CAPT_NUM_MEM_REGIONS_REG_WIDTH            9
+#define CAPT_INIT_REG_WIDTH                       (22 + 4)
+
+#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_NEXT_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))
+
+//#define CAPT_INIT_RESTART_MEM_ADDR_WIDTH          9   
+//#define CAPT_INIT_RESTART_MEM_REGION_WIDTH        9 
+
+/* 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_RESTART_MEM_ADDR_MSB            14
+#define CAPT_INIT_RESTART_MEM_REGION_LSB          15
+#define CAPT_INIT_RESTART_MEM_REGION_MSB          25
+
+
+#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
+#define CAPT_INIT_RESTART_MEM_ADDR_BITS           (CAPT_INIT_RESTART_MEM_ADDR_MSB - CAPT_INIT_RESTART_MEM_ADDR_LSB + 1)
+#define CAPT_INIT_RESTART_MEM_REGION_IDX          CAPT_INIT_RESTART_MEM_REGION_LSB
+#define CAPT_INIT_RESTART_MEM_REGION_BITS         (CAPT_INIT_RESTART_MEM_REGION_MSB - CAPT_INIT_RESTART_MEM_REGION_LSB + 1)
+
+
+
+/* --------------------------------------------------*/
+/* 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/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_common_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_common_defs.h
new file mode 100644
index 000000000000..76705d7a2b44
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_common_defs.h
@@ -0,0 +1,210 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 reseved 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/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_defs.h
new file mode 100644
index 000000000000..db5a1d2caba0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mipi_backend_defs.h
@@ -0,0 +1,215 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/mmu_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mmu_defs.h
new file mode 100644
index 000000000000..c038f39ffd25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/mmu_defs.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/rx_csi_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/rx_csi_defs.h
new file mode 100644
index 000000000000..0aad86e2e914
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/rx_csi_defs.h
@@ -0,0 +1,175 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/scalar_processor_2400_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/scalar_processor_2400_params.h
new file mode 100644
index 000000000000..9b6c2893d950
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/scalar_processor_2400_params.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/sp_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/sp_hrt.h
new file mode 100644
index 000000000000..7ee4deba519a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/sp_hrt.h
@@ -0,0 +1,24 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_hrt_h_
+#define _sp_hrt_h_
+
+#define hrt_sp_dmem(cell) HRT_PROC_TYPE_PROP(cell, _dmem)
+
+#define hrt_sp_dmem_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_sp_dmem(cell))
+
+#endif /* _sp_hrt_h_ */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/str2mem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/str2mem_defs.h
new file mode 100644
index 000000000000..1cb62444cf68
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/str2mem_defs.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/stream2mmio_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/stream2mmio_defs.h
new file mode 100644
index 000000000000..46b52fe5ae99
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/stream2mmio_defs.h
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/streaming_to_mipi_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/streaming_to_mipi_defs.h
new file mode 100644
index 000000000000..60143b8743a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/streaming_to_mipi_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/timed_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/timed_controller_defs.h
new file mode 100644
index 000000000000..d2b8972b0d9e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/timed_controller_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_csi2p_system/hrt/var.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/var.h
new file mode 100644
index 000000000000..19b19ef484f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/var.h
@@ -0,0 +1,99 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_VAR_H
+#define _HRT_VAR_H
+
+#include "version.h"
+#include "system_api.h"
+#include "hive_types.h"
+
+#define hrt_int_type_of_char   char
+#define hrt_int_type_of_uchar  unsigned char
+#define hrt_int_type_of_short  short
+#define hrt_int_type_of_ushort unsigned short
+#define hrt_int_type_of_int    int
+#define hrt_int_type_of_uint   unsigned int
+#define hrt_int_type_of_long   long
+#define hrt_int_type_of_ulong  unsigned long
+#define hrt_int_type_of_ptr    unsigned int
+
+#define hrt_host_type_of_char   char
+#define hrt_host_type_of_uchar  unsigned char
+#define hrt_host_type_of_short  short
+#define hrt_host_type_of_ushort unsigned short
+#define hrt_host_type_of_int    int
+#define hrt_host_type_of_uint   unsigned int
+#define hrt_host_type_of_long   long
+#define hrt_host_type_of_ulong  unsigned long
+#define hrt_host_type_of_ptr    void*
+
+#define HRT_TYPE_BYTES(cell, type) (HRT_TYPE_BITS(cell, type)/8)
+#define HRT_HOST_TYPE(cell_type)   HRTCAT(hrt_host_type_of_, cell_type)
+#define HRT_INT_TYPE(type)         HRTCAT(hrt_int_type_of_, type)
+
+#ifdef C_RUN
+
+#ifdef C_RUN_DYNAMIC_LINK_PROGRAMS
+extern void *csim_processor_get_crun_symbol(hive_proc_id p, const char *sym);
+#define _hrt_cell_get_crun_symbol(cell,sym)          csim_processor_get_crun_symbol(cell,HRTSTR(sym))
+#define _hrt_cell_get_crun_indexed_symbol(cell,sym)  csim_processor_get_crun_symbol(cell,HRTSTR(sym))
+#else
+#define _hrt_cell_get_crun_symbol(cell,sym)         (&sym)
+#define _hrt_cell_get_crun_indexed_symbol(cell,sym) (sym)
+#endif //  C_RUN_DYNAMIC_LINK_PROGRAMS
+
+#define hrt_scalar_store(cell, type, var, data) \
+	((*(HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_symbol(cell,var)) = (data))
+#define hrt_scalar_load(cell, type, var) \
+	((*(HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_symbol(cell,var)))
+
+#define hrt_indexed_store(cell, type, array, index, data) \
+	((((HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_indexed_symbol(cell,array))[index]) = (data))
+#define hrt_indexed_load(cell, type, array, index) \
+	(((HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_indexed_symbol(cell,array))[index])
+
+#else /* C_RUN */
+
+#define hrt_scalar_store(cell, type, var, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_scalar_load(cell, type, var) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var)))
+
+#define hrt_indexed_store(cell, type, array, index, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type)), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_indexed_load(cell, type, array, index) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+         cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type))))
+
+#endif /* C_RUN */
+
+#endif /* _HRT_VAR_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/version.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/version.h
new file mode 100644
index 000000000000..bbc4948baea9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/hrt/version.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/ibuf_ctrl_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/ibuf_ctrl_global.h
new file mode 100644
index 000000000000..edb23252c48e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/ibuf_ctrl_global.h
@@ -0,0 +1,80 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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	(1 << 8)
+#define _IBUF_CNTRL_DMA_SYNC_WAIT_FOR_SYNC		1
+#define _IBUF_CNTRL_DMA_SYNC_FSM_WAIT_FOR_ACK		(0x3 << 1)
+
+typedef struct ib_buffer_s	ib_buffer_t;
+struct	ib_buffer_s {
+	uint32_t	start_addr;	/* start address of the buffer in the
+					 * "input-buffer hardware block"
+					 */
+
+	uint32_t	stride;		/* stride per buffer line (in bytes) */
+	uint32_t	lines;		/* lines in the buffer */
+};
+
+typedef struct ibuf_ctrl_cfg_s ibuf_ctrl_cfg_t;
+struct ibuf_ctrl_cfg_s {
+
+	bool online;
+
+	struct {
+		/* DMA configuration */
+		uint32_t channel;
+		uint32_t cmd; /* must be _DMA_V2_MOVE_A2B_NO_SYNC_CHK_COMMAND */
+
+		/* DMA reconfiguration */
+		uint32_t shift_returned_items;
+		uint32_t elems_per_word_in_ibuf;
+		uint32_t elems_per_word_in_dest;
+	} dma_cfg;
+
+	ib_buffer_t ib_buffer;
+
+	struct {
+		uint32_t stride;
+		uint32_t start_addr;
+		uint32_t lines;
+	} dest_buf_cfg;
+
+	uint32_t items_per_store;
+	uint32_t stores_per_frame;
+
+	struct {
+		uint32_t sync_cmd;	/* must be _STREAM2MMIO_CMD_TOKEN_SYNC_FRAME */
+		uint32_t store_cmd;	/* must be _STREAM2MMIO_CMD_TOKEN_STORE_PACKETS */
+	} stream2mmio_cfg;
+};
+
+extern const uint32_t N_IBUF_CTRL_PROCS[N_IBUF_CTRL_ID];
+
+#endif /* __IBUF_CTRL_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/input_system_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/input_system_global.h
new file mode 100644
index 000000000000..25e3f04f374b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/input_system_global.h
@@ -0,0 +1,206 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_GLOBAL_H_INCLUDED__
+#define __INPUT_SYSTEM_GLOBAL_H_INCLUDED__
+
+#define IS_INPUT_SYSTEM_VERSION_VERSION_2401
+
+/* CSI reveiver has 3 ports. */
+#define		N_CSI_PORTS (3)
+
+#include "isys_dma.h"		/*	isys2401_dma_channel,
+				 *	isys2401_dma_cfg_t
+				 */
+
+#include "ibuf_ctrl.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_ERR_NO_ERROR = 0,
+	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,
+	N_INPUT_SYSTEM_ERR
+} input_system_err_t;
+
+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 enum {
+	INPUT_SYSTEM_POLL_ON_WAIT_FOR_FRAME,
+	INPUT_SYSTEM_POLL_ON_CAPTURE_REQUEST,
+} input_system_polling_mode_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;
+	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 input_system_cfg_s input_system_cfg_t;
+struct input_system_cfg_s {
+	input_system_input_port_ID_t	input_port_id;
+
+	input_system_source_type_t	mode;
+#ifdef ISP2401
+	input_system_polling_mode_t	polling_mode;
+#endif
+
+	bool online;
+	bool raw_packed;
+	int8_t linked_isys_stream_id;
+
+	struct {
+		bool	comp_enable;
+		int32_t	active_lanes;
+		int32_t	fmt_type;
+		int32_t	ch_id;
+		int32_t comp_predictor;
+		int32_t comp_scheme;
+	} csi_port_attr;
+
+	pixelgen_tpg_cfg_t	tpg_port_attr;
+
+	pixelgen_prbs_cfg_t prbs_port_attr;
+
+	struct {
+		int32_t align_req_in_bytes;
+		int32_t bits_per_pixel;
+		int32_t pixels_per_line;
+		int32_t lines_per_frame;
+	} input_port_resolution;
+
+	struct {
+		int32_t left_padding;
+		int32_t max_isp_input_width;
+	} output_port_attr;
+
+	struct {
+		bool    enable;
+		int32_t fmt_type;
+		int32_t align_req_in_bytes;
+		int32_t bits_per_pixel;
+		int32_t pixels_per_line;
+		int32_t lines_per_frame;
+	} metadata;
+};
+
+typedef struct virtual_input_system_stream_s virtual_input_system_stream_t;
+struct virtual_input_system_stream_s {
+	uint32_t 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 */
+	uint8_t enable_metadata;
+	input_system_input_port_t	input_port;
+	input_system_channel_t		channel;
+	input_system_channel_t		md_channel; /* metadata channel */
+	uint8_t online;
+	int8_t linked_isys_stream_id;
+	uint8_t valid;
+#ifdef ISP2401
+	input_system_polling_mode_t	polling_mode;
+	int32_t subscr_index;
+#endif
+};
+
+typedef struct virtual_input_system_stream_cfg_s virtual_input_system_stream_cfg_t;
+struct virtual_input_system_stream_cfg_s {
+	uint8_t 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;
+	uint8_t 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
+
+
+#endif /* __INPUT_SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_dma_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_dma_global.h
new file mode 100644
index 000000000000..e7a734a9fc43
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_dma_global.h
@@ -0,0 +1,87 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 libary
+ * is available. The device libary 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 {
+	uint32_t stride;
+	uint32_t elements;
+	uint32_t cropping;
+	uint32_t 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 libary
+ * is available. The device libary 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;
+	uint32_t		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/atomisp2/css2400/css_2401_csi2p_system/isys_irq_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_irq_global.h
new file mode 100644
index 000000000000..41d051db3987
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_irq_global.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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(USE_INPUT_SYSTEM_VERSION_2401)
+
+/* 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(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#endif	/* __ISYS_IRQ_GLOBAL_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_stream2mmio_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_stream2mmio_global.h
new file mode 100644
index 000000000000..649f44fd2408
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/isys_stream2mmio_global.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t				bits_per_pixel;
+	uint32_t				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/atomisp2/css2400/css_2401_csi2p_system/pixelgen_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/pixelgen_global.h
new file mode 100644
index 000000000000..216813e42a0a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/pixelgen_global.h
@@ -0,0 +1,91 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 sync_generator_cfg_s sync_generator_cfg_t;
+struct sync_generator_cfg_s {
+	uint32_t	hblank_cycles;
+	uint32_t	vblank_cycles;
+	uint32_t	pixels_per_clock;
+	uint32_t	nr_of_frames;
+	uint32_t	pixels_per_line;
+	uint32_t	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 */
+		uint32_t R1;
+		uint32_t G1;
+		uint32_t B1;
+
+		/* be used by CHBO only */
+		uint32_t R2;
+		uint32_t G2;
+		uint32_t B2;
+	} color_cfg;
+
+	struct {
+		uint32_t	h_mask;		/* horizontal mask */
+		uint32_t	v_mask;		/* vertical mask */
+		uint32_t	hv_mask;	/* horizontal+vertical mask? */
+	} mask_cfg;
+
+	struct {
+		int32_t	h_delta;	/* horizontal delta? */
+		int32_t	v_delta;	/* vertical delta? */
+	} delta_cfg;
+
+	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 {
+	int32_t	seed0;
+	int32_t	seed1;
+
+	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/atomisp2/css2400/css_2401_csi2p_system/spmem_dump.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/spmem_dump.c
new file mode 100644
index 000000000000..d733a3503a20
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/spmem_dump.c
@@ -0,0 +1,3686 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_map_h_
+#define _sp_map_h_
+
+
+#ifndef _hrt_dummy_use_blob_sp
+#define _hrt_dummy_use_blob_sp()
+#endif
+
+#define _hrt_cell_load_program_sp(proc) _hrt_cell_load_program_embedded(proc, sp)
+
+#ifndef ISP2401
+/* function longjmp: 680D */
+#else
+/* function longjmp: 6A0B */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_init_dmem: 6558 */
+#else
+/* function tmpmem_init_dmem: 671E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_addr_B: 3C50 */
+#else
+/* function ia_css_dmaproxy_sp_set_addr_B: 3DC5 */
+
+/* function ia_css_pipe_data_init_tagger_resources: AC7 */
+#endif
+
+/* function debug_buffer_set_ddr_addr: DD */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_mipi
+#define HIVE_MEM_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_mipi 0x7398
+#else
+#define HIVE_ADDR_vbuf_mipi 0x7444
+#endif
+#define HIVE_SIZE_vbuf_mipi 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_mipi 0x7398
+#else
+#define HIVE_ADDR_sp_vbuf_mipi 0x7444
+#endif
+#define HIVE_SIZE_sp_vbuf_mipi 12
+
+#ifndef ISP2401
+/* function ia_css_event_sp_decode: 3E41 */
+#else
+/* function ia_css_event_sp_decode: 3FB6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_get_size: 51BF */
+#else
+/* function ia_css_queue_get_size: 53C8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_load: 5800 */
+#else
+/* function ia_css_queue_load: 59DF */
+#endif
+
+#ifndef ISP2401
+/* function setjmp: 6816 */
+#else
+/* function setjmp: 6A14 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_sfi_get_current_frame: 27BF */
+#else
+/* function ia_css_pipeline_sp_sfi_get_current_frame: 2790 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_isys_event_queue
+#define HIVE_MEM_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x5760
+#else
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x57FC
+#endif
+#define HIVE_SIZE_sem_for_sp2host_isys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x5760
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x57FC
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_isys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6DA9 */
+#else
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6FF7 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_func: 596B */
+#else
+/* function ia_css_sp_rawcopy_func: 5B4A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_marked: 3339 */
+#else
+/* function ia_css_tagger_buf_sp_pop_marked: 345C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_CSI_RX_BE_SID_WIDTH
+#define HIVE_MEM_N_CSI_RX_BE_SID_WIDTH scalar_processor_2400_dmem
+#define HIVE_ADDR_N_CSI_RX_BE_SID_WIDTH 0x1D0
+#define HIVE_SIZE_N_CSI_RX_BE_SID_WIDTH 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_CSI_RX_BE_SID_WIDTH scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_CSI_RX_BE_SID_WIDTH 0x1D0
+#define HIVE_SIZE_sp_N_CSI_RX_BE_SID_WIDTH 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stage
+#define HIVE_MEM_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stage 0x6C98
+#else
+#define HIVE_ADDR_isp_stage 0x6D48
+#endif
+#define HIVE_SIZE_isp_stage 832
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stage 0x6C98
+#else
+#define HIVE_ADDR_sp_isp_stage 0x6D48
+#endif
+#define HIVE_SIZE_sp_isp_stage 832
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_raw
+#define HIVE_MEM_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_raw 0x37C
+#else
+#define HIVE_ADDR_vbuf_raw 0x394
+#endif
+#define HIVE_SIZE_vbuf_raw 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_raw 0x37C
+#else
+#define HIVE_ADDR_sp_vbuf_raw 0x394
+#endif
+#define HIVE_SIZE_sp_vbuf_raw 4
+
+#ifndef ISP2401
+/* function ia_css_sp_bin_copy_func: 594C */
+#else
+/* function ia_css_sp_bin_copy_func: 5B2B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_item_store: 554E */
+#else
+/* function ia_css_queue_item_store: 572D */
+#endif
+
+#ifndef ISP2401
+/* function input_system_reset: 1286 */
+#else
+/* function input_system_reset: 1201 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x5B38
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x5BE4
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x5B38
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x5BE4
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x5B4C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x5BF8
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x5B4C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x5BF8
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+
+/* function sp_start_isp: 39C */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_binary_group
+#define HIVE_MEM_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_binary_group 0x7088
+#else
+#define HIVE_ADDR_sp_binary_group 0x7138
+#endif
+#define HIVE_SIZE_sp_binary_group 32
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_binary_group 0x7088
+#else
+#define HIVE_ADDR_sp_sp_binary_group 0x7138
+#endif
+#define HIVE_SIZE_sp_sp_binary_group 32
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sw_state
+#define HIVE_MEM_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sw_state 0x7344
+#else
+#define HIVE_ADDR_sp_sw_state 0x73F0
+#endif
+#define HIVE_SIZE_sp_sw_state 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sw_state 0x7344
+#else
+#define HIVE_ADDR_sp_sp_sw_state 0x73F0
+#endif
+#define HIVE_SIZE_sp_sp_sw_state 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_main: 13F7 */
+#else
+/* function ia_css_thread_sp_main: 136D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_internal_buffers: 4047 */
+#else
+/* function ia_css_ispctrl_sp_init_internal_buffers: 41F7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_psys_event_queue_handle
+#define HIVE_MEM_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x5BEC
+#else
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x5C98
+#endif
+#define HIVE_SIZE_sp2host_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x5BEC
+#else
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x5C98
+#endif
+#define HIVE_SIZE_sp_sp2host_psys_event_queue_handle 12
+
+#ifndef ISP2401
+/* function pixelgen_unit_test: E68 */
+#else
+/* function pixelgen_unit_test: E62 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_psys_event_queue
+#define HIVE_MEM_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x5774
+#else
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x5810
+#endif
+#define HIVE_SIZE_sem_for_sp2host_psys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x5774
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x5810
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_psys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_propagate_frame: 2D52 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_stop_copy_preview
+#define HIVE_MEM_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_stop_copy_preview 0x7328
+#define HIVE_SIZE_sp_stop_copy_preview 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_stop_copy_preview 0x7328
+#define HIVE_SIZE_sp_sp_stop_copy_preview 4
+#else
+/* function ia_css_tagger_sp_propagate_frame: 2D23 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_handles
+#define HIVE_MEM_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_handles 0x73A4
+#else
+#define HIVE_ADDR_vbuf_handles 0x7450
+#endif
+#define HIVE_SIZE_vbuf_handles 960
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_handles 0x73A4
+#else
+#define HIVE_ADDR_sp_vbuf_handles 0x7450
+#endif
+#define HIVE_SIZE_sp_vbuf_handles 960
+
+#ifndef ISP2401
+/* function ia_css_queue_store: 56B4 */
+
+/* function ia_css_sp_flash_register: 356E */
+#else
+/* function ia_css_queue_store: 5893 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_dummy_function: 5CF7 */
+#else
+/* function ia_css_sp_flash_register: 3691 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_init: 201C */
+#else
+/* function ia_css_pipeline_sp_init: 1FD7 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_configure: 2C42 */
+#else
+/* function ia_css_tagger_sp_configure: 2C13 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_end_binary: 3E8A */
+#else
+/* function ia_css_ispctrl_sp_end_binary: 3FFF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x5BF8
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x5CA4
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x5BF8
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x5CA4
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function pixelgen_tpg_run: F1E */
+#else
+/* function pixelgen_tpg_run: F18 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_is_pending_mask
+#define HIVE_MEM_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_is_pending_mask 0x5C
+#define HIVE_SIZE_event_is_pending_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_is_pending_mask 0x5C
+#define HIVE_SIZE_sp_event_is_pending_mask 44
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_frame
+#define HIVE_MEM_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x5788
+#else
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x5824
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x5788
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x5824
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_frame 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_isys_event_queue_handle
+#define HIVE_MEM_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x5C0C
+#else
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x5CB8
+#endif
+#define HIVE_SIZE_sp2host_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x5C0C
+#else
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x5CB8
+#endif
+#define HIVE_SIZE_sp_sp2host_isys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_com
+#define HIVE_MEM_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_com 0x3E48
+#else
+#define HIVE_ADDR_host_sp_com 0x3E6C
+#endif
+#define HIVE_SIZE_host_sp_com 220
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_com 0x3E48
+#else
+#define HIVE_ADDR_sp_host_sp_com 0x3E6C
+#endif
+#define HIVE_SIZE_sp_host_sp_com 220
+
+#ifndef ISP2401
+/* function ia_css_queue_get_free_space: 5313 */
+#else
+/* function ia_css_queue_get_free_space: 54F2 */
+#endif
+
+#ifndef ISP2401
+/* function exec_image_pipe: 5E6 */
+#else
+/* function exec_image_pipe: 57A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_init_dmem_data
+#define HIVE_MEM_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_init_dmem_data 0x7348
+#else
+#define HIVE_ADDR_sp_init_dmem_data 0x73F4
+#endif
+#define HIVE_SIZE_sp_init_dmem_data 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x7348
+#else
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x73F4
+#endif
+#define HIVE_SIZE_sp_sp_init_dmem_data 24
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_start: 5DD1 */
+#else
+/* function ia_css_sp_metadata_start: 5EB3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_init_buffer_queues: 35BF */
+#else
+/* function ia_css_bufq_sp_init_buffer_queues: 36E2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_stop: 1FFF */
+#else
+/* function ia_css_pipeline_sp_stop: 1FBA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_connect_pipes: 312C */
+#else
+/* function ia_css_tagger_sp_connect_pipes: 30FD */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_wait: 644 */
+#else
+/* function sp_isys_copy_wait: 5D8 */
+#endif
+
+/* function is_isp_debug_buffer_full: 337 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 3BD3 */
+#else
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 3D35 */
+#endif
+
+#ifndef ISP2401
+/* function encode_and_post_timer_event: AA8 */
+#else
+/* function encode_and_post_timer_event: A3C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_input_system_bz2788_active
+#define HIVE_MEM_input_system_bz2788_active scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_input_system_bz2788_active 0x250C
+#else
+#define HIVE_ADDR_input_system_bz2788_active 0x2524
+#endif
+#define HIVE_SIZE_input_system_bz2788_active 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_input_system_bz2788_active scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_input_system_bz2788_active 0x250C
+#else
+#define HIVE_ADDR_sp_input_system_bz2788_active 0x2524
+#endif
+#define HIVE_SIZE_sp_input_system_bz2788_active 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_IBUF_CTRL_PROCS
+#define HIVE_MEM_N_IBUF_CTRL_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_N_IBUF_CTRL_PROCS 0x1FC
+#define HIVE_SIZE_N_IBUF_CTRL_PROCS 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_IBUF_CTRL_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_IBUF_CTRL_PROCS 0x1FC
+#define HIVE_SIZE_sp_N_IBUF_CTRL_PROCS 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_per_frame_data
+#define HIVE_MEM_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_per_frame_data 0x3F24
+#else
+#define HIVE_ADDR_sp_per_frame_data 0x3F48
+#endif
+#define HIVE_SIZE_sp_per_frame_data 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_per_frame_data 0x3F24
+#else
+#define HIVE_ADDR_sp_sp_per_frame_data 0x3F48
+#endif
+#define HIVE_SIZE_sp_sp_per_frame_data 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_dequeue: 62AC */
+#else
+/* function ia_css_rmgr_sp_vbuf_dequeue: 6472 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_psys_event_queue_handle
+#define HIVE_MEM_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x5C18
+#else
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x5CC4
+#endif
+#define HIVE_SIZE_host2sp_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x5C18
+#else
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x5CC4
+#endif
+#define HIVE_SIZE_sp_host2sp_psys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_xmem_bin_addr
+#define HIVE_MEM_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_xmem_bin_addr 0x3F28
+#else
+#define HIVE_ADDR_xmem_bin_addr 0x3F4C
+#endif
+#define HIVE_SIZE_xmem_bin_addr 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_xmem_bin_addr 0x3F28
+#else
+#define HIVE_ADDR_sp_xmem_bin_addr 0x3F4C
+#endif
+#define HIVE_SIZE_sp_xmem_bin_addr 4
+
+#ifndef ISP2401
+/* function tmr_clock_init: 16F9 */
+#else
+/* function tmr_clock_init: 166F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_run: 1ABF */
+#else
+/* function ia_css_pipeline_sp_run: 1A61 */
+#endif
+
+#ifndef ISP2401
+/* function memcpy: 68B6 */
+#else
+/* function memcpy: 6AB4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_ISYS2401_DMA_CHANNEL_PROCS
+#define HIVE_MEM_N_ISYS2401_DMA_CHANNEL_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_N_ISYS2401_DMA_CHANNEL_PROCS 0x214
+#define HIVE_SIZE_N_ISYS2401_DMA_CHANNEL_PROCS 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_ISYS2401_DMA_CHANNEL_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_ISYS2401_DMA_CHANNEL_PROCS 0x214
+#define HIVE_SIZE_sp_N_ISYS2401_DMA_CHANNEL_PROCS 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GP_DEVICE_BASE
+#define HIVE_MEM_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_GP_DEVICE_BASE 0x384
+#else
+#define HIVE_ADDR_GP_DEVICE_BASE 0x39C
+#endif
+#define HIVE_SIZE_GP_DEVICE_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x384
+#else
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x39C
+#endif
+#define HIVE_SIZE_sp_GP_DEVICE_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_ready_queue
+#define HIVE_MEM_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x278
+#else
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x27C
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_ready_queue 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x278
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x27C
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_ready_queue 12
+
+#ifndef ISP2401
+/* function stream2mmio_send_command: E0A */
+#else
+/* function stream2mmio_send_command: E04 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_uds_sp_scale_params: 65BF */
+#else
+/* function ia_css_uds_sp_scale_params: 67BD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_increase_size: 14DC */
+#else
+/* function ia_css_circbuf_increase_size: 1452 */
+#endif
+
+#ifndef ISP2401
+/* function __divu: 6834 */
+#else
+/* function __divu: 6A32 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_get_state: 131F */
+#else
+/* function ia_css_thread_sp_get_state: 1295 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_stop
+#define HIVE_MEM_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x5798
+#else
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x5834
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_stop 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x5798
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x5834
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_stop 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_SHORT_PACKET_LUT_ENTRIES
+#define HIVE_MEM_N_SHORT_PACKET_LUT_ENTRIES scalar_processor_2400_dmem
+#define HIVE_ADDR_N_SHORT_PACKET_LUT_ENTRIES 0x1AC
+#define HIVE_SIZE_N_SHORT_PACKET_LUT_ENTRIES 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_SHORT_PACKET_LUT_ENTRIES scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_SHORT_PACKET_LUT_ENTRIES 0x1AC
+#define HIVE_SIZE_sp_N_SHORT_PACKET_LUT_ENTRIES 12
+
+#ifndef ISP2401
+/* function thread_fiber_sp_main: 14D5 */
+#else
+/* function thread_fiber_sp_main: 144B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_pipe_thread
+#define HIVE_MEM_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pipe_thread 0x58DC
+#define HIVE_SIZE_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_isp_pipe_thread 0x5978
+#define HIVE_SIZE_sp_isp_pipe_thread 360
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x58DC
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x5978
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 360
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_handle_parameter_sets: 193F */
+#else
+/* function ia_css_parambuf_sp_handle_parameter_sets: 18B5 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_set_state: 5DED */
+#else
+/* function ia_css_spctrl_sp_set_state: 5ECF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_signal: 6A99 */
+#else
+/* function ia_css_thread_sem_sp_signal: 6D18 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_IRQ_BASE
+#define HIVE_MEM_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_IRQ_BASE 0x2C
+#define HIVE_SIZE_IRQ_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_IRQ_BASE 0x2C
+#define HIVE_SIZE_sp_IRQ_BASE 16
+
+#ifndef ISP2401
+/* function ia_css_virtual_isys_sp_isr_init: 5E8C */
+#else
+/* function ia_css_virtual_isys_sp_isr_init: 5F70 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_TIMED_CTRL_BASE
+#define HIVE_MEM_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_TIMED_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_sp_TIMED_CTRL_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_generate_exp_id: 613C */
+
+/* function ia_css_rmgr_sp_init: 61A7 */
+#else
+/* function ia_css_isys_sp_generate_exp_id: 6302 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_init: 6B6A */
+#else
+/* function ia_css_rmgr_sp_init: 636D */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x390
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x390
+#define HIVE_SIZE_sp_is_isp_requested 4
+#else
+/* function ia_css_thread_sem_sp_init: 6DE7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_frame
+#define HIVE_MEM_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x57AC
+#else
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x5848
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_frame 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x57AC
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x5848
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_frame 40
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_execute: 3B3B */
+#else
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x3A8
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x3A8
+#define HIVE_SIZE_sp_is_isp_requested 4
+
+/* function ia_css_dmaproxy_sp_execute: 3C9B */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_rst: CE6 */
+#else
+/* function csi_rx_backend_rst: CE0 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_is_empty: 51FA */
+#else
+/* function ia_css_queue_is_empty: 7144 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_has_stopped: 1FF5 */
+#else
+/* function ia_css_pipeline_sp_has_stopped: 1FB0 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_extract: 15E0 */
+#else
+/* function ia_css_circbuf_extract: 1556 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 344F */
+#else
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 3572 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_sp_thread
+#define HIVE_MEM_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_current_sp_thread 0x274
+#define HIVE_SIZE_current_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_current_sp_thread 0x274
+#define HIVE_SIZE_sp_current_sp_thread 4
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_spid: 5DF4 */
+#else
+/* function ia_css_spctrl_sp_get_spid: 5ED6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_reset_buffers: 3646 */
+#else
+/* function ia_css_bufq_sp_reset_buffers: 3769 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr: 6DD7 */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr: 7025 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_uninit: 61A0 */
+#else
+/* function ia_css_rmgr_sp_uninit: 6366 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack
+#define HIVE_MEM_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_threads_stack 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_sp_threads_stack 24
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_STREAM2MMIO_SID_PROCS
+#define HIVE_MEM_N_STREAM2MMIO_SID_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_N_STREAM2MMIO_SID_PROCS 0x218
+#define HIVE_SIZE_N_STREAM2MMIO_SID_PROCS 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_STREAM2MMIO_SID_PROCS scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_STREAM2MMIO_SID_PROCS 0x218
+#define HIVE_SIZE_sp_N_STREAM2MMIO_SID_PROCS 12
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek: 15C2 */
+#else
+/* function ia_css_circbuf_peek: 1538 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_wait_for_in_param: 1708 */
+#else
+/* function ia_css_parambuf_sp_wait_for_in_param: 167E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_param
+#define HIVE_MEM_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_param 0x57D4
+#else
+#define HIVE_ADDR_sp_all_cb_elems_param 0x5870
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x57D4
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x5870
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_param 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_pipeline_sp_curr_binary_id
+#define HIVE_MEM_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x284
+#else
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x288
+#endif
+#define HIVE_SIZE_pipeline_sp_curr_binary_id 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x284
+#else
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x288
+#endif
+#define HIVE_SIZE_sp_pipeline_sp_curr_binary_id 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame_desc
+#define HIVE_MEM_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x57E4
+#else
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x5880
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x57E4
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x5880
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame_desc 8
+
+#ifndef ISP2401
+/* function sp_isys_copy_func_v2: 629 */
+#else
+/* function sp_isys_copy_func_v2: 5BD */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_param
+#define HIVE_MEM_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_param 0x57EC
+#else
+#define HIVE_ADDR_sem_for_reading_cb_param 0x5888
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_param 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x57EC
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x5888
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_param 40
+
+#ifndef ISP2401
+/* function ia_css_queue_get_used_space: 52C7 */
+#else
+/* function ia_css_queue_get_used_space: 54A6 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_start
+#define HIVE_MEM_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_start 0x5814
+#else
+#define HIVE_ADDR_sem_for_cont_capt_start 0x58B0
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x5814
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x58B0
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_start 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tmp_heap
+#define HIVE_MEM_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tmp_heap 0x70A8
+#else
+#define HIVE_ADDR_tmp_heap 0x7158
+#endif
+#define HIVE_SIZE_tmp_heap 640
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tmp_heap 0x70A8
+#else
+#define HIVE_ADDR_sp_tmp_heap 0x7158
+#endif
+#define HIVE_SIZE_sp_tmp_heap 640
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_get_num_vbuf: 64B0 */
+#else
+/* function ia_css_rmgr_sp_get_num_vbuf: 6676 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 4863 */
+#else
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 4A27 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_lock_exp_id: 2A0F */
+#else
+/* function ia_css_tagger_sp_lock_exp_id: 29E0 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x5C24
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x5CD0
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x5C24
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x5CD0
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+
+#ifndef ISP2401
+/* function ia_css_queue_is_full: 535E */
+#else
+/* function ia_css_queue_is_full: 553D */
+#endif
+
+/* function debug_buffer_init_isp: E4 */
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_exp_id_is_locked: 2945 */
+#else
+/* function ia_css_tagger_sp_exp_id_is_locked: 2916 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem
+#define HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x7764
+#else
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x7810
+#endif
+#define HIVE_SIZE_ia_css_rmgr_sp_mipi_frame_sem 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x7764
+#else
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x7810
+#endif
+#define HIVE_SIZE_sp_ia_css_rmgr_sp_mipi_frame_sem 60
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_dump: 6287 */
+#else
+/* function ia_css_rmgr_sp_refcount_dump: 644D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x5C60
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x5D0C
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x5C60
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x5D0C
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_pipe_threads
+#define HIVE_MEM_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_pipe_threads 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_sp_pipe_threads 20
+
+#ifndef ISP2401
+/* function sp_event_proxy_func: 78D */
+#else
+/* function sp_event_proxy_func: 721 */
+#endif
+
+#ifndef ISP2401
+/* function ibuf_ctrl_run: D7F */
+#else
+/* function ibuf_ctrl_run: D79 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_isys_event_queue_handle
+#define HIVE_MEM_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x5C74
+#else
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x5D20
+#endif
+#define HIVE_SIZE_host2sp_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x5C74
+#else
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x5D20
+#endif
+#define HIVE_SIZE_sp_host2sp_isys_event_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_yield: 6A12 */
+#else
+/* function ia_css_thread_sp_yield: 6C96 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param_desc
+#define HIVE_MEM_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x5828
+#else
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x58C4
+#endif
+#define HIVE_SIZE_sp_all_cbs_param_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x5828
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x58C4
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param_desc 8
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb
+#define HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x6C8C
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x6D38
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_sp_invalidate_tlb 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x6C8C
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x6D38
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_sp_invalidate_tlb 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_fork: 13AC */
+#else
+/* function ia_css_thread_sp_fork: 1322 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_destroy: 3136 */
+#else
+/* function ia_css_tagger_sp_destroy: 3107 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_read: 3ADB */
+#else
+/* function ia_css_dmaproxy_sp_vmem_read: 3C3B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_LONG_PACKET_LUT_ENTRIES
+#define HIVE_MEM_N_LONG_PACKET_LUT_ENTRIES scalar_processor_2400_dmem
+#define HIVE_ADDR_N_LONG_PACKET_LUT_ENTRIES 0x1B8
+#define HIVE_SIZE_N_LONG_PACKET_LUT_ENTRIES 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_LONG_PACKET_LUT_ENTRIES scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_LONG_PACKET_LUT_ENTRIES 0x1B8
+#define HIVE_SIZE_sp_N_LONG_PACKET_LUT_ENTRIES 12
+
+#ifndef ISP2401
+/* function initialize_sp_group: 5F6 */
+#else
+/* function initialize_sp_group: 58A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_peek: 325B */
+#else
+/* function ia_css_tagger_buf_sp_peek: 337E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_init: 13D8 */
+#else
+/* function ia_css_thread_sp_init: 134E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_reset_exp_id: 6133 */
+#else
+/* function qos_scheduler_update_fps: 67AD */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_update_fps: 65AF */
+#else
+/* function ia_css_isys_sp_reset_exp_id: 62F9 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 4F38 */
+#else
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 5114 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_DMEM_BASE
+#define HIVE_MEM_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_ISP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_sp_ISP_DMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_DMEM_BASE
+#define HIVE_MEM_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_SP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_sp_SP_DMEM_BASE 4
+
+#ifndef ISP2401
+/* function ibuf_ctrl_transfer: D67 */
+#else
+/* function ibuf_ctrl_transfer: D61 */
+
+/* function __ia_css_queue_is_empty_text: 5403 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read: 3B51 */
+#else
+/* function ia_css_dmaproxy_sp_read: 3CB1 */
+#endif
+
+#ifndef ISP2401
+/* function virtual_isys_stream_is_capture_done: 5EB0 */
+#else
+/* function virtual_isys_stream_is_capture_done: 5F94 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_raw_copy_line_count
+#define HIVE_MEM_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_raw_copy_line_count 0x360
+#else
+#define HIVE_ADDR_raw_copy_line_count 0x378
+#endif
+#define HIVE_SIZE_raw_copy_line_count 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_raw_copy_line_count 0x360
+#else
+#define HIVE_ADDR_sp_raw_copy_line_count 0x378
+#endif
+#define HIVE_SIZE_sp_raw_copy_line_count 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_tag_cmd_queue_handle
+#define HIVE_MEM_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x5C80
+#else
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x5D2C
+#endif
+#define HIVE_SIZE_host2sp_tag_cmd_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x5C80
+#else
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x5D2C
+#endif
+#define HIVE_SIZE_sp_host2sp_tag_cmd_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_queue_peek: 523D */
+#else
+/* function ia_css_queue_peek: 541C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_frame_cnt
+#define HIVE_MEM_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x5B2C
+#else
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x5BD8
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_frame_cnt 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x5B2C
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x5BD8
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_frame_cnt 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_can_send_token_mask
+#define HIVE_MEM_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_can_send_token_mask 0x88
+#define HIVE_SIZE_event_can_send_token_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_can_send_token_mask 0x88
+#define HIVE_SIZE_sp_event_can_send_token_mask 44
+
+#ifndef ISP2401
+/* function csi_rx_frontend_stop: C11 */
+#else
+/* function csi_rx_frontend_stop: C0B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_thread
+#define HIVE_MEM_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_thread 0x6FD8
+#else
+#define HIVE_ADDR_isp_thread 0x7088
+#endif
+#define HIVE_SIZE_isp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_thread 0x6FD8
+#else
+#define HIVE_ADDR_sp_isp_thread 0x7088
+#endif
+#define HIVE_SIZE_sp_isp_thread 4
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event_non_blocking: AF0 */
+#else
+/* function encode_and_post_sp_event_non_blocking: A84 */
+#endif
+
+/* function is_ddr_debug_buffer_full: 2CC */
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 32AB */
+#else
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 33CE */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_fiber
+#define HIVE_MEM_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_fiber 0x194
+#define HIVE_SIZE_sp_threads_fiber 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_fiber 0x194
+#define HIVE_SIZE_sp_sp_threads_fiber 24
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event: A79 */
+#else
+/* function encode_and_post_sp_event: A0D */
+#endif
+
+/* function debug_enqueue_ddr: EE */
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 6242 */
+#else
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 6408 */
+#endif
+
+#ifndef ISP2401
+/* function dmaproxy_sp_read_write: 6E86 */
+#else
+/* function dmaproxy_sp_read_write: 70C3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer
+#define HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x6C90
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x6D3C
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x6C90
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x6D3C
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_buffer_queue_handle
+#define HIVE_MEM_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x5C8C
+#else
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x5D38
+#endif
+#define HIVE_SIZE_host2sp_buffer_queue_handle 480
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x5C8C
+#else
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x5D38
+#endif
+#define HIVE_SIZE_sp_host2sp_buffer_queue_handle 480
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_service
+#define HIVE_MEM_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3054
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3074
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_service 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3054
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3074
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_service 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_process: 6B92 */
+#else
+/* function ia_css_dmaproxy_sp_process: 6E0F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_mark_from_end: 3533 */
+#else
+/* function ia_css_tagger_buf_sp_mark_from_end: 3656 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_cs: 3F77 */
+#else
+/* function ia_css_ispctrl_sp_init_cs: 40FA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_init: 5E02 */
+#else
+/* function ia_css_spctrl_sp_init: 5EE4 */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_init: 7A2 */
+#else
+/* function sp_event_proxy_init: 736 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_input_port_close: 109B */
+#else
+/* function input_system_input_port_close: 1095 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x5E6C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x5F18
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x5E6C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x5F18
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_output
+#define HIVE_MEM_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_output 0x3F2C
+#else
+#define HIVE_ADDR_sp_output 0x3F50
+#endif
+#define HIVE_SIZE_sp_output 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_output 0x3F2C
+#else
+#define HIVE_ADDR_sp_sp_output 0x3F50
+#endif
+#define HIVE_SIZE_sp_sp_output 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x5E94
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x5F40
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x5E94
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x5F40
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+
+#ifndef ISP2401
+/* function pixelgen_prbs_config: E93 */
+#else
+/* function pixelgen_prbs_config: E8D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_CTRL_BASE
+#define HIVE_MEM_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_ISP_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_sp_ISP_CTRL_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_INPUT_FORMATTER_BASE
+#define HIVE_MEM_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_INPUT_FORMATTER_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_sp_INPUT_FORMATTER_BASE 16
+
+#ifndef ISP2401
+/* function sp_dma_proxy_reset_channels: 3DAB */
+#else
+/* function sp_dma_proxy_reset_channels: 3F20 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_update_size: 322A */
+#else
+/* function ia_css_tagger_sp_update_size: 334D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_host_sp_queue
+#define HIVE_MEM_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x61B4
+#else
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x6260
+#endif
+#define HIVE_SIZE_ia_css_bufq_host_sp_queue 2008
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x61B4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x6260
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_host_sp_queue 2008
+
+#ifndef ISP2401
+/* function thread_fiber_sp_create: 1444 */
+#else
+/* function thread_fiber_sp_create: 13BA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_increments: 3C3D */
+#else
+/* function ia_css_dmaproxy_sp_set_increments: 3DB2 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_frame
+#define HIVE_MEM_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x5830
+#else
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x58CC
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_frame 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x5830
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x58CC
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_frame 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_param
+#define HIVE_MEM_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_param 0x5844
+#else
+#define HIVE_ADDR_sem_for_writing_cb_param 0x58E0
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_param 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x5844
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x58E0
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_param 20
+
+#ifndef ISP2401
+/* function pixelgen_tpg_is_done: F0D */
+#else
+/* function pixelgen_tpg_is_done: F07 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_stream_capture_indication: 5FB6 */
+#else
+/* function ia_css_isys_stream_capture_indication: 60D7 */
+#endif
+
+/* function sp_start_isp_entry: 392 */
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifdef HIVE_ADDR_sp_start_isp_entry
+#endif
+#define HIVE_ADDR_sp_start_isp_entry 0x392
+#endif
+#define HIVE_ADDR_sp_sp_start_isp_entry 0x392
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_all: 34B7 */
+#else
+/* function ia_css_tagger_buf_sp_unmark_all: 35DA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_from_start: 34F8 */
+#else
+/* function ia_css_tagger_buf_sp_unmark_from_start: 361B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_acquire: 3DD7 */
+#else
+/* function ia_css_dmaproxy_sp_channel_acquire: 3F4C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_add_num_vbuf: 648C */
+#else
+/* function ia_css_rmgr_sp_add_num_vbuf: 6652 */
+#endif
+
+#ifndef ISP2401
+/* function ibuf_ctrl_config: D8B */
+#else
+/* function ibuf_ctrl_config: D85 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_stream_stop: 602E */
+#else
+/* function ia_css_isys_stream_stop: 61F4 */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 3AA7 */
+#else
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 3C07 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_acquire_buf_elem: 291D */
+#else
+/* function ia_css_tagger_sp_acquire_buf_elem: 28EE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_is_dynamic_buffer: 3990 */
+#else
+/* function ia_css_bufq_sp_is_dynamic_buffer: 3AB3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_group
+#define HIVE_MEM_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_group 0x3F3C
+#define HIVE_SIZE_sp_group 6176
+#else
+#define HIVE_ADDR_sp_group 0x3F60
+#define HIVE_SIZE_sp_group 6296
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_group 0x3F3C
+#define HIVE_SIZE_sp_sp_group 6176
+#else
+#define HIVE_ADDR_sp_sp_group 0x3F60
+#define HIVE_SIZE_sp_sp_group 6296
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_event_proxy_thread
+#define HIVE_MEM_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_proxy_thread 0x5A30
+#define HIVE_SIZE_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_event_proxy_thread 0x5AE0
+#define HIVE_SIZE_sp_event_proxy_thread 72
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x5A30
+#define HIVE_SIZE_sp_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x5AE0
+#define HIVE_SIZE_sp_sp_event_proxy_thread 72
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_kill: 1372 */
+#else
+/* function ia_css_thread_sp_kill: 12E8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_create: 31E4 */
+#else
+/* function ia_css_tagger_sp_create: 32FB */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_acquire_dmem: 6539 */
+#else
+/* function tmpmem_acquire_dmem: 66FF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_MMU_BASE
+#define HIVE_MEM_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_MMU_BASE 0x24
+#define HIVE_SIZE_MMU_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_MMU_BASE 0x24
+#define HIVE_SIZE_sp_MMU_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_release: 3DC3 */
+#else
+/* function ia_css_dmaproxy_sp_channel_release: 3F38 */
+#endif
+
+#ifndef ISP2401
+/* function pixelgen_prbs_run: E81 */
+#else
+/* function pixelgen_prbs_run: E7B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_is_idle: 3DA3 */
+#else
+/* function ia_css_dmaproxy_sp_is_idle: 3F18 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_qos_start
+#define HIVE_MEM_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_qos_start 0x5858
+#else
+#define HIVE_ADDR_sem_for_qos_start 0x58F4
+#endif
+#define HIVE_SIZE_sem_for_qos_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_qos_start 0x5858
+#else
+#define HIVE_ADDR_sp_sem_for_qos_start 0x58F4
+#endif
+#define HIVE_SIZE_sp_sem_for_qos_start 20
+
+#ifndef ISP2401
+/* function isp_hmem_load: B63 */
+#else
+/* function isp_hmem_load: B5D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_release_buf_elem: 28F9 */
+#else
+/* function ia_css_tagger_sp_release_buf_elem: 28CA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_send: 3E19 */
+#else
+/* function ia_css_eventq_sp_send: 3F8E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unlock_from_start: 33E7 */
+#else
+/* function ia_css_tagger_buf_sp_unlock_from_start: 350A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_debug_buffer_ddr_address
+#define HIVE_MEM_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_debug_buffer_ddr_address 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_sp_debug_buffer_ddr_address 4
+
+#ifndef ISP2401
+/* function sp_isys_copy_request: 6ED */
+#else
+/* function sp_isys_copy_request: 681 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 631C */
+#else
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 64E2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_set_priority: 136A */
+#else
+/* function ia_css_thread_sp_set_priority: 12E0 */
+#endif
+
+#ifndef ISP2401
+/* function sizeof_hmem: C0A */
+#else
+/* function sizeof_hmem: C04 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_channel_open: 1241 */
+#else
+/* function input_system_channel_open: 11BC */
+#endif
+
+#ifndef ISP2401
+/* function pixelgen_tpg_stop: EFB */
+#else
+/* function pixelgen_tpg_stop: EF5 */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_release_dmem: 6528 */
+#else
+/* function tmpmem_release_dmem: 66EE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_width_exception: 3C28 */
+#else
+/* function __ia_css_dmaproxy_sp_process_text: 3BAB */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_assert: 929 */
+#else
+/* function ia_css_dmaproxy_sp_set_width_exception: 3D9D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_flash_sp_init_internal_params: 35B4 */
+#else
+/* function sp_event_assert: 8BD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 32ED */
+#else
+/* function ia_css_flash_sp_init_internal_params: 36D7 */
+#endif
+
+#ifndef ISP2401
+/* function __modu: 687A */
+#else
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 3410 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_isp_vector: 3AAD */
+#else
+/* function __modu: 6A78 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_channel_transfer: 122A */
+#else
+/* function ia_css_dmaproxy_sp_init_isp_vector: 3C0D */
+
+/* function input_system_channel_transfer: 11A5 */
+#endif
+
+/* function isp_vamem_store: 0 */
+
+#ifdef ISP2401
+/* function ia_css_tagger_sp_set_copy_pipe: 32F2 */
+
+#endif
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GDC_BASE
+#define HIVE_MEM_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GDC_BASE 0x44
+#define HIVE_SIZE_GDC_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GDC_BASE 0x44
+#define HIVE_SIZE_sp_GDC_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_queue_local_init: 5528 */
+#else
+/* function ia_css_queue_local_init: 5707 */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_callout_func: 6947 */
+#else
+/* function sp_event_proxy_callout_func: 6B45 */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_schedule_stage: 6580 */
+#else
+/* function qos_scheduler_schedule_stage: 6759 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_num_ready_threads
+#define HIVE_MEM_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x5A78
+#else
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x5B28
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x5A78
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x5B28
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_num_ready_threads 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack_size
+#define HIVE_MEM_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack_size 0x17C
+#define HIVE_SIZE_sp_threads_stack_size 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack_size 0x17C
+#define HIVE_SIZE_sp_sp_threads_stack_size 24
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 4849 */
+#else
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 4A0D */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_virtual_isys_sp_isr_text: 5E45 */
+#else
+/* function __ia_css_virtual_isys_sp_isr_text: 5F4E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_dequeue: 53A6 */
+#else
+/* function ia_css_queue_dequeue: 5585 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel: 6DEE */
+#else
+/* function is_qos_standalone_mode: 6734 */
+
+/* function ia_css_dmaproxy_sp_configure_channel: 703C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_thread_fiber_sp
+#define HIVE_MEM_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_current_thread_fiber_sp 0x5A80
+#else
+#define HIVE_ADDR_current_thread_fiber_sp 0x5B2C
+#endif
+#define HIVE_SIZE_current_thread_fiber_sp 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x5A80
+#else
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x5B2C
+#endif
+#define HIVE_SIZE_sp_current_thread_fiber_sp 4
+
+#ifndef ISP2401
+/* function ia_css_circbuf_pop: 1674 */
+#else
+/* function ia_css_circbuf_pop: 15EA */
+#endif
+
+#ifndef ISP2401
+/* function memset: 68F9 */
+#else
+/* function memset: 6AF7 */
+#endif
+
+/* function irq_raise_set_token: B6 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GPIO_BASE
+#define HIVE_MEM_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GPIO_BASE 0x3C
+#define HIVE_SIZE_GPIO_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GPIO_BASE 0x3C
+#define HIVE_SIZE_sp_GPIO_BASE 4
+
+#ifndef ISP2401
+/* function pixelgen_prbs_stop: E6F */
+#else
+/* function pixelgen_prbs_stop: E69 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_acc_stage_enable: 1FC0 */
+#else
+/* function ia_css_pipeline_acc_stage_enable: 1F69 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_unlock_exp_id: 296A */
+#else
+/* function ia_css_tagger_sp_unlock_exp_id: 293B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_ph
+#define HIVE_MEM_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_ph 0x7360
+#else
+#define HIVE_ADDR_isp_ph 0x740C
+#endif
+#define HIVE_SIZE_isp_ph 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_ph 0x7360
+#else
+#define HIVE_ADDR_sp_isp_ph 0x740C
+#endif
+#define HIVE_SIZE_sp_isp_ph 28
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_ds: 40D6 */
+#else
+/* function ia_css_ispctrl_sp_init_ds: 4286 */
+#endif
+
+#ifndef ISP2401
+/* function get_xmem_base_addr_raw: 4479 */
+#else
+/* function get_xmem_base_addr_raw: 4635 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param
+#define HIVE_MEM_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param 0x586C
+#else
+#define HIVE_ADDR_sp_all_cbs_param 0x5908
+#endif
+#define HIVE_SIZE_sp_all_cbs_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x586C
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x5908
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param 16
+
+#ifndef ISP2401
+/* function pixelgen_tpg_config: F30 */
+#else
+/* function pixelgen_tpg_config: F2A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_create: 16C2 */
+#else
+/* function ia_css_circbuf_create: 1638 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp_group
+#define HIVE_MEM_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp_group 0x587C
+#else
+#define HIVE_ADDR_sem_for_sp_group 0x5918
+#endif
+#define HIVE_SIZE_sem_for_sp_group 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp_group 0x587C
+#else
+#define HIVE_ADDR_sp_sem_for_sp_group 0x5918
+#endif
+#define HIVE_SIZE_sp_sem_for_sp_group 20
+
+#ifndef ISP2401
+/* function csi_rx_frontend_run: C22 */
+#else
+/* function csi_rx_frontend_run: C1C */
+
+/* function __ia_css_dmaproxy_sp_configure_channel_text: 3D7C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_wait_for_in_frame: 64B7 */
+#else
+/* function ia_css_framebuf_sp_wait_for_in_frame: 667D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_stream_open: 60E3 */
+#else
+/* function ia_css_isys_stream_open: 62A9 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_tag_frame: 5C71 */
+#else
+/* function ia_css_sp_rawcopy_tag_frame: 5E35 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_channel_configure: 125D */
+#else
+/* function input_system_channel_configure: 11D8 */
+#endif
+
+#ifndef ISP2401
+/* function isp_hmem_clear: B33 */
+#else
+/* function isp_hmem_clear: B2D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_release_in_frame: 64FA */
+#else
+/* function ia_css_framebuf_sp_release_in_frame: 66C0 */
+#endif
+
+#ifndef ISP2401
+/* function stream2mmio_config: E1B */
+#else
+/* function stream2mmio_config: E15 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_start_binary: 3F55 */
+#else
+/* function ia_css_ispctrl_sp_start_binary: 40D8 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x698C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x6A38
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x698C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x6A38
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_recv: 3DEB */
+#else
+/* function ia_css_eventq_sp_recv: 3F60 */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_frontend_config: C7A */
+#else
+/* function csi_rx_frontend_config: C74 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_pool
+#define HIVE_MEM_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_pool 0x370
+#else
+#define HIVE_ADDR_isp_pool 0x388
+#endif
+#define HIVE_SIZE_isp_pool 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pool 0x370
+#else
+#define HIVE_ADDR_sp_isp_pool 0x388
+#endif
+#define HIVE_SIZE_sp_isp_pool 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_rel_gen: 61E9 */
+#else
+/* function ia_css_rmgr_sp_rel_gen: 63AF */
+
+/* function ia_css_tagger_sp_unblock_clients: 31C3 */
+#endif
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_end: 28E9 */
+#else
+/* function css_get_frame_processing_time_end: 28BA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_any_pending_mask
+#define HIVE_MEM_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_event_any_pending_mask 0x388
+#else
+#define HIVE_ADDR_event_any_pending_mask 0x3A0
+#endif
+#define HIVE_SIZE_event_any_pending_mask 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_any_pending_mask 0x388
+#else
+#define HIVE_ADDR_sp_event_any_pending_mask 0x3A0
+#endif
+#define HIVE_SIZE_sp_event_any_pending_mask 8
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_get_pipe_io_status: 1AB8 */
+#else
+/* function ia_css_pipeline_sp_get_pipe_io_status: 1A5A */
+#endif
+
+/* function sh_css_decode_tag_descr: 352 */
+
+/* function debug_enqueue_isp: 27B */
+
+#ifndef ISP2401
+/* function qos_scheduler_update_stage_budget: 656E */
+#else
+/* function qos_scheduler_update_stage_budget: 673C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_uninit: 5DFB */
+#else
+/* function ia_css_spctrl_sp_uninit: 5EDD */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_run: C68 */
+#else
+/* function csi_rx_backend_run: C62 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x69A0
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x6A4C
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_dis_bufs 140
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x69A0
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x6A4C
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_dis_bufs 140
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_lock_from_start: 341B */
+#else
+/* function ia_css_tagger_buf_sp_lock_from_start: 353E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_isp_idle
+#define HIVE_MEM_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_isp_idle 0x5890
+#else
+#define HIVE_ADDR_sem_for_isp_idle 0x592C
+#endif
+#define HIVE_SIZE_sem_for_isp_idle 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x5890
+#else
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x592C
+#endif
+#define HIVE_SIZE_sp_sem_for_isp_idle 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write_byte_addr: 3B0A */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr: 3C6A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init: 3A81 */
+#else
+/* function ia_css_dmaproxy_sp_init: 3BE1 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 3686 */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 37A9 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_VAMEM_BASE
+#define HIVE_MEM_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_ISP_VAMEM_BASE 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_sp_ISP_VAMEM_BASE 12
+
+#ifndef ISP2401
+/* function input_system_channel_sync: 11A4 */
+#else
+/* function input_system_channel_sync: 6C10 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rawcopy_sp_tagger
+#define HIVE_MEM_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x732C
+#else
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x73D8
+#endif
+#define HIVE_SIZE_ia_css_rawcopy_sp_tagger 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x732C
+#else
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x73D8
+#endif
+#define HIVE_SIZE_sp_ia_css_rawcopy_sp_tagger 24
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x6A2C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x6AD8
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_exp_ids 70
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x6A2C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x6AD8
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_exp_ids 70
+
+#ifndef ISP2401
+/* function ia_css_queue_item_load: 561A */
+#else
+/* function ia_css_queue_item_load: 57F9 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_state: 5DE6 */
+#else
+/* function ia_css_spctrl_sp_get_state: 5EC8 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_callout_sp_thread
+#define HIVE_MEM_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_callout_sp_thread 0x5A74
+#else
+#define HIVE_ADDR_callout_sp_thread 0x278
+#endif
+#define HIVE_SIZE_callout_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_callout_sp_thread 0x5A74
+#else
+#define HIVE_ADDR_sp_callout_sp_thread 0x278
+#endif
+#define HIVE_SIZE_sp_callout_sp_thread 4
+
+#ifndef ISP2401
+/* function thread_fiber_sp_init: 14CB */
+#else
+/* function thread_fiber_sp_init: 1441 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_PMEM_BASE
+#define HIVE_MEM_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_SP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_sp_SP_PMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_input_stream_format
+#define HIVE_MEM_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_input_stream_format 0x3E2C
+#else
+#define HIVE_ADDR_sp_isp_input_stream_format 0x3E50
+#endif
+#define HIVE_SIZE_sp_isp_input_stream_format 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x3E2C
+#else
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x3E50
+#endif
+#define HIVE_SIZE_sp_sp_isp_input_stream_format 20
+
+#ifndef ISP2401
+/* function __mod: 6866 */
+#else
+/* function __mod: 6A64 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 3B6B */
+#else
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 3CCB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_join: 139B */
+#else
+/* function ia_css_thread_sp_join: 1311 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_add_command: 6EF1 */
+#else
+/* function ia_css_dmaproxy_sp_add_command: 712E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_thread_func: 5DDF */
+#else
+/* function ia_css_sp_metadata_thread_func: 5EC1 */
+#endif
+
+#ifndef ISP2401
+/* function __sp_event_proxy_func_critical: 6934 */
+#else
+/* function __sp_event_proxy_func_critical: 6B32 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_wait_for_isys_stream_N: 5F53 */
+#else
+/* function ia_css_pipeline_sp_wait_for_isys_stream_N: 6074 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_wait: 5DD8 */
+#else
+/* function ia_css_sp_metadata_wait: 5EBA */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek_from_start: 15A4 */
+#else
+/* function ia_css_circbuf_peek_from_start: 151A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_event_sp_encode: 3E76 */
+#else
+/* function ia_css_event_sp_encode: 3FEB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_run: 140E */
+#else
+/* function ia_css_thread_sp_run: 1384 */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_func: 618 */
+#else
+/* function sp_isys_copy_func: 5AC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_init_isp_memories: 50A3 */
+#else
+/* function ia_css_sp_isp_param_init_isp_memories: 52AC */
+#endif
+
+#ifndef ISP2401
+/* function register_isr: 921 */
+#else
+/* function register_isr: 8B5 */
+#endif
+
+/* function irq_raise: C8 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 3A48 */
+#else
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 3B71 */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_disable: C34 */
+#else
+/* function csi_rx_backend_disable: C2E */
+#endif
+
+#ifndef ISP2401
+/* function pipeline_sp_initialize_stage: 2104 */
+#else
+/* function pipeline_sp_initialize_stage: 20BF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_N_CSI_RX_FE_CTRL_DLANES
+#define HIVE_MEM_N_CSI_RX_FE_CTRL_DLANES scalar_processor_2400_dmem
+#define HIVE_ADDR_N_CSI_RX_FE_CTRL_DLANES 0x1C4
+#define HIVE_SIZE_N_CSI_RX_FE_CTRL_DLANES 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_N_CSI_RX_FE_CTRL_DLANES scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_N_CSI_RX_FE_CTRL_DLANES 0x1C4
+#define HIVE_SIZE_sp_N_CSI_RX_FE_CTRL_DLANES 12
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 6DC0 */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 700E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_done_ds: 40BD */
+#else
+/* function ia_css_ispctrl_sp_done_ds: 426D */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_config: C8B */
+#else
+/* function csi_rx_backend_config: C85 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_get_mem_inits: 507E */
+#else
+/* function ia_css_sp_isp_param_get_mem_inits: 5287 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_init_buffer_queues: 1A85 */
+#else
+/* function ia_css_parambuf_sp_init_buffer_queues: 1A27 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_pfp_spref
+#define HIVE_MEM_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_pfp_spref 0x378
+#else
+#define HIVE_ADDR_vbuf_pfp_spref 0x390
+#endif
+#define HIVE_SIZE_vbuf_pfp_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x378
+#else
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x390
+#endif
+#define HIVE_SIZE_sp_vbuf_pfp_spref 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_HMEM_BASE
+#define HIVE_MEM_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_ISP_HMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_sp_ISP_HMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_frames
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x6A74
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x6B20
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_frames 280
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x6A74
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x6B20
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_frames 280
+
+#ifndef ISP2401
+/* function qos_scheduler_init_stage_budget: 65A7 */
+#else
+/* function qos_scheduler_init_stage_budget: 679A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_buffer_queue_handle
+#define HIVE_MEM_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x6B8C
+#else
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x6C38
+#endif
+#define HIVE_SIZE_sp2host_buffer_queue_handle 96
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x6B8C
+#else
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x6C38
+#endif
+#define HIVE_SIZE_sp_sp2host_buffer_queue_handle 96
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_isp_vars: 4D9D */
+#else
+/* function ia_css_ispctrl_sp_init_isp_vars: 4F79 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_stream_start: 6010 */
+#else
+/* function ia_css_isys_stream_start: 6187 */
+#endif
+
+#ifndef ISP2401
+/* function sp_warning: 954 */
+#else
+/* function sp_warning: 8E8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_enqueue: 62DC */
+#else
+/* function ia_css_rmgr_sp_vbuf_enqueue: 64A2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_tag_exp_id: 2A84 */
+#else
+/* function ia_css_tagger_sp_tag_exp_id: 2A55 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_sfi_release_current_frame: 276B */
+#else
+/* function ia_css_pipeline_sp_sfi_release_current_frame: 273C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write: 3B21 */
+#else
+/* function ia_css_dmaproxy_sp_write: 3C81 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_stream_start_async: 608A */
+#else
+/* function ia_css_isys_stream_start_async: 6250 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_release_in_param: 1905 */
+#else
+/* function ia_css_parambuf_sp_release_in_param: 187B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_irq_sw_interrupt_token
+#define HIVE_MEM_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_irq_sw_interrupt_token 0x3E28
+#else
+#define HIVE_ADDR_irq_sw_interrupt_token 0x3E4C
+#endif
+#define HIVE_SIZE_irq_sw_interrupt_token 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x3E28
+#else
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x3E4C
+#endif
+#define HIVE_SIZE_sp_irq_sw_interrupt_token 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_addresses
+#define HIVE_MEM_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_addresses 0x6FDC
+#else
+#define HIVE_ADDR_sp_isp_addresses 0x708C
+#endif
+#define HIVE_SIZE_sp_isp_addresses 172
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_addresses 0x6FDC
+#else
+#define HIVE_ADDR_sp_sp_isp_addresses 0x708C
+#endif
+#define HIVE_SIZE_sp_sp_isp_addresses 172
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_acq_gen: 6201 */
+#else
+/* function ia_css_rmgr_sp_acq_gen: 63C7 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_input_port_open: 10ED */
+#else
+/* function input_system_input_port_open: 10E7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isps
+#define HIVE_MEM_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isps 0x737C
+#else
+#define HIVE_ADDR_isps 0x7428
+#endif
+#define HIVE_SIZE_isps 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isps 0x737C
+#else
+#define HIVE_ADDR_sp_isps 0x7428
+#endif
+#define HIVE_SIZE_sp_isps 28
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_queues_initialized
+#define HIVE_MEM_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_queues_initialized 0x3E40
+#else
+#define HIVE_ADDR_host_sp_queues_initialized 0x3E64
+#endif
+#define HIVE_SIZE_host_sp_queues_initialized 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x3E40
+#else
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x3E64
+#endif
+#define HIVE_SIZE_sp_host_sp_queues_initialized 4
+
+#ifndef ISP2401
+/* function ia_css_queue_uninit: 54E6 */
+#else
+/* function ia_css_queue_uninit: 56C5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_ispctrl_sp_isp_started
+#define HIVE_MEM_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x6C94
+#else
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x6D40
+#endif
+#define HIVE_SIZE_ia_css_ispctrl_sp_isp_started 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x6C94
+#else
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x6D40
+#endif
+#define HIVE_SIZE_sp_ia_css_ispctrl_sp_isp_started 4
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf: 36F2 */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf: 3815 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_height_exception: 3C19 */
+#else
+/* function ia_css_dmaproxy_sp_set_height_exception: 3D8E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 3B9E */
+#else
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 3CFF */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_stop: C57 */
+#else
+/* function csi_rx_backend_stop: C51 */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_num_ready_threads
+#define HIVE_MEM_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_num_ready_threads 0x5A7C
+#define HIVE_SIZE_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_num_ready_threads 0x5A7C
+#define HIVE_SIZE_sp_num_ready_threads 4
+
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 3AF3 */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 3C53 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_spref
+#define HIVE_MEM_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_spref 0x374
+#else
+#define HIVE_ADDR_vbuf_spref 0x38C
+#endif
+#define HIVE_SIZE_vbuf_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_spref 0x374
+#else
+#define HIVE_ADDR_sp_vbuf_spref 0x38C
+#endif
+#define HIVE_SIZE_sp_vbuf_spref 4
+
+#ifndef ISP2401
+/* function ia_css_queue_enqueue: 5430 */
+#else
+/* function ia_css_queue_enqueue: 560F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_request
+#define HIVE_MEM_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_request 0x5B30
+#else
+#define HIVE_ADDR_ia_css_flash_sp_request 0x5BDC
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_request 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x5B30
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x5BDC
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_request 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_write: 3AC4 */
+#else
+/* function ia_css_dmaproxy_sp_vmem_write: 3C24 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tagger_frames
+#define HIVE_MEM_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tagger_frames 0x5A84
+#else
+#define HIVE_ADDR_tagger_frames 0x5B30
+#endif
+#define HIVE_SIZE_tagger_frames 168
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tagger_frames 0x5A84
+#else
+#define HIVE_ADDR_sp_tagger_frames 0x5B30
+#endif
+#define HIVE_SIZE_sp_tagger_frames 168
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_if
+#define HIVE_MEM_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_if 0x58A4
+#else
+#define HIVE_ADDR_sem_for_reading_if 0x5940
+#endif
+#define HIVE_SIZE_sem_for_reading_if 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_if 0x58A4
+#else
+#define HIVE_ADDR_sp_sem_for_reading_if 0x5940
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_if 20
+
+#ifndef ISP2401
+/* function sp_generate_interrupts: 9D3 */
+#else
+/* function sp_generate_interrupts: 967 */
+
+/* function ia_css_pipeline_sp_start: 1FC2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_start: 2007 */
+#else
+/* function ia_css_thread_default_callout: 6C8F */
+#endif
+
+#ifndef ISP2401
+/* function csi_rx_backend_enable: C45 */
+#else
+/* function csi_rx_backend_enable: C3F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_init: 5953 */
+#else
+/* function ia_css_sp_rawcopy_init: 5B32 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_input_port_configure: 113F */
+#else
+/* function input_system_input_port_configure: 1139 */
+#endif
+
+#ifndef ISP2401
+/* function tmr_clock_read: 16EF */
+#else
+/* function tmr_clock_read: 1665 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_BAMEM_BASE
+#define HIVE_MEM_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x380
+#else
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x398
+#endif
+#define HIVE_SIZE_ISP_BAMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x380
+#else
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x398
+#endif
+#define HIVE_SIZE_sp_ISP_BAMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x6BEC
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x6C98
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x6BEC
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x6C98
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+
+#ifndef ISP2401
+/* function isys2401_dma_config_legacy: DE0 */
+#else
+/* function isys2401_dma_config_legacy: DDA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ibuf_ctrl_master_ports
+#define HIVE_MEM_ibuf_ctrl_master_ports scalar_processor_2400_dmem
+#define HIVE_ADDR_ibuf_ctrl_master_ports 0x208
+#define HIVE_SIZE_ibuf_ctrl_master_ports 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ibuf_ctrl_master_ports scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ibuf_ctrl_master_ports 0x208
+#define HIVE_SIZE_sp_ibuf_ctrl_master_ports 12
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_start: 28F1 */
+#else
+/* function css_get_frame_processing_time_start: 28C2 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame
+#define HIVE_MEM_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame 0x58B8
+#else
+#define HIVE_ADDR_sp_all_cbs_frame 0x5954
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x58B8
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x5954
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame 16
+
+#ifndef ISP2401
+/* function ia_css_virtual_isys_sp_isr: 6F07 */
+#else
+/* function ia_css_virtual_isys_sp_isr: 716E */
+#endif
+
+#ifndef ISP2401
+/* function thread_sp_queue_print: 142B */
+#else
+/* function thread_sp_queue_print: 13A1 */
+#endif
+
+#ifndef ISP2401
+/* function sp_notify_eof: 97F */
+#else
+/* function sp_notify_eof: 913 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_str2mem
+#define HIVE_MEM_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_str2mem 0x58C8
+#else
+#define HIVE_ADDR_sem_for_str2mem 0x5964
+#endif
+#define HIVE_SIZE_sem_for_str2mem 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_str2mem 0x58C8
+#else
+#define HIVE_ADDR_sp_sem_for_str2mem 0x5964
+#endif
+#define HIVE_SIZE_sp_sem_for_str2mem 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 3483 */
+#else
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 35A6 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 38AA */
+#else
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 39CD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_sfi_mode_is_enabled: 28BF */
+#else
+/* function ia_css_pipeline_sp_sfi_mode_is_enabled: 2890 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_destroy: 16B9 */
+#else
+/* function ia_css_circbuf_destroy: 162F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_PMEM_BASE
+#define HIVE_MEM_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_ISP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_sp_ISP_PMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_mem_load: 5011 */
+#else
+/* function ia_css_sp_isp_param_mem_load: 521A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_from_start: 326F */
+#else
+/* function ia_css_tagger_buf_sp_pop_from_start: 3392 */
+#endif
+
+#ifndef ISP2401
+/* function __div: 681E */
+#else
+/* function __div: 6A1C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 62FB */
+#else
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 64C1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_use
+#define HIVE_MEM_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x5B34
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x5BE0
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_use 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x5B34
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x5BE0
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_use 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_wait: 6AE4 */
+#else
+/* function ia_css_thread_sem_sp_wait: 6D63 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sleep_mode
+#define HIVE_MEM_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sleep_mode 0x3E44
+#else
+#define HIVE_ADDR_sp_sleep_mode 0x3E68
+#endif
+#define HIVE_SIZE_sp_sleep_mode 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sleep_mode 0x3E44
+#else
+#define HIVE_ADDR_sp_sp_sleep_mode 0x3E68
+#endif
+#define HIVE_SIZE_sp_sp_sleep_mode 4
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_push: 337E */
+#else
+/* function ia_css_tagger_buf_sp_push: 34A1 */
+#endif
+
+/* function mmu_invalidate_cache: D3 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_max_cb_elems
+#define HIVE_MEM_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_max_cb_elems 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_sp_max_cb_elems 8
+
+#ifndef ISP2401
+/* function ia_css_queue_remote_init: 5508 */
+#else
+/* function ia_css_queue_remote_init: 56E7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stop_req
+#define HIVE_MEM_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stop_req 0x575C
+#else
+#define HIVE_ADDR_isp_stop_req 0x57F8
+#endif
+#define HIVE_SIZE_isp_stop_req 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stop_req 0x575C
+#else
+#define HIVE_ADDR_sp_isp_stop_req 0x57F8
+#endif
+#define HIVE_SIZE_sp_isp_stop_req 4
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_sfi_request_next_frame: 2781 */
+#else
+/* function ia_css_pipeline_sp_sfi_request_next_frame: 2752 */
+#endif
+
+#ifndef ISP2401
+#define HIVE_ICACHE_sp_critical_SEGMENT_START 0
+#define HIVE_ICACHE_sp_critical_NUM_SEGMENTS  1
+#endif
+
+#endif /* _sp_map_h_ */
+#ifndef ISP2401
+extern void sh_css_dump_sp_dmem(void);
+void sh_css_dump_sp_dmem(void)
+{
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/system_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/system_global.h
new file mode 100644
index 000000000000..9f7ecac46273
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_csi2p_system/system_global.h
@@ -0,0 +1,458 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_GLOBAL_H_INCLUDED__
+#define __SYSTEM_GLOBAL_H_INCLUDED__
+
+#include <hive_isp_css_defs.h>
+#include <type_support.h>
+
+/*
+ * 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 ISP_DMA_MAX_BURST_LENGTH	2
+
+/*
+ * Create a list of HAS and IS properties that defines the system
+ *
+ * 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 USE_INPUT_SYSTEM_VERSION_2401
+
+#define IS_ISP_2400_SYSTEM
+/*
+ * Since this file is visible everywhere and the system definition
+ * macros are not, detect the separate definitions for {host, SP, ISP}
+ *
+ * The 2401 system has the nice property that it uses a vanilla 2400 SP
+ * so the SP will believe it is a 2400 system rather than 2401...
+ */
+/* #if defined(SYSTEM_hive_isp_css_2401_system) || defined(__isp2401_mamoiada) || defined(__scalar_processor_2401) */
+#if defined(SYSTEM_hive_isp_css_2401_system) || defined(__isp2401_mamoiada)
+#define IS_ISP_2401_MAMOIADA_SYSTEM
+#define HAS_ISP_2401_MAMOIADA
+#define HAS_SP_2400
+/* #elif defined(SYSTEM_hive_isp_css_2400_system) || defined(__isp2400_mamoiada) || defined(__scalar_processor_2400)*/
+#elif defined(SYSTEM_hive_isp_css_2400_system) || defined(__isp2400_mamoiada)
+#define IS_ISP_2400_MAMOIADA_SYSTEM
+#define HAS_ISP_2400_MAMOIADA
+#define HAS_SP_2400
+#else
+#error "system_global.h: 2400_SYSTEM must be one of {2400, 2401 }"
+#endif
+
+#define HAS_MMU_VERSION_2
+#define HAS_DMA_VERSION_2
+#define HAS_GDC_VERSION_2
+#define HAS_VAMEM_VERSION_2
+#define HAS_HMEM_VERSION_1
+#define HAS_BAMEM_VERSION_2
+#define HAS_IRQ_VERSION_2
+#define HAS_IRQ_MAP_VERSION_2
+#define HAS_INPUT_FORMATTER_VERSION_2
+/* 2401: HAS_INPUT_SYSTEM_VERSION_3 */
+/* 2400: HAS_INPUT_SYSTEM_VERSION_2 */
+#define HAS_INPUT_SYSTEM_VERSION_2
+#define HAS_INPUT_SYSTEM_VERSION_2401
+#define HAS_BUFFERED_SENSOR
+#define HAS_FIFO_MONITORS_VERSION_2
+/* #define HAS_GP_REGS_VERSION_2 */
+#define HAS_GP_DEVICE_VERSION_2
+#define HAS_GPIO_VERSION_1
+#define HAS_TIMED_CTRL_VERSION_1
+#define HAS_RX_VERSION_2
+#define HAS_NO_INPUT_FORMATTER
+/*#define HAS_NO_PACKED_RAW_PIXELS*/
+/*#define HAS_NO_DVS_6AXIS_CONFIG_UPDATE*/
+
+#define DMA_DDR_TO_VAMEM_WORKAROUND
+#define DMA_DDR_TO_HMEM_WORKAROUND
+
+
+/*
+ * Semi global. "HRT" is accessible from SP, but
+ * the HRT types do not fully apply
+ */
+#define HRT_VADDRESS_WIDTH	32
+/* Surprise, this is a local property*/
+/*#define HRT_ADDRESS_WIDTH	64 */
+#define HRT_DATA_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
+
+#define CSI2P_DISABLE_ISYS2401_ONLINE_MODE
+
+/* per-frame parameter handling support */
+#define SH_CSS_ENABLE_PER_FRAME_PARAMS
+
+typedef uint32_t			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;
+
+#if defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+typedef enum {
+	MMU0_ID = 0,
+	MMU1_ID,
+	N_MMU_ID
+} mmu_ID_t;
+#elif defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+typedef enum {
+	MMU0_ID = 0,
+	MMU1_ID,
+	N_MMU_ID
+} mmu_ID_t;
+#else
+#error "system_global.h: SYSTEM must be one of {2400, 2401}"
+#endif
+
+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 {
+	ISYS_IRQ0_ID = 0,	/* port a */
+	ISYS_IRQ1_ID,	/* port b */
+	ISYS_IRQ2_ID,	/* port c */
+	N_ISYS_IRQ_ID
+} isys_irq_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;
+
+/*
+ * Deprecated: Since all gp_reg instances are different
+ * and put in the address maps of other devices we cannot
+ * enumerate them as that assumes the instrances are the
+ * same.
+ *
+ * We define a single GP_DEVICE containing all gp_regs
+ * w.r.t. a single base address
+ *
+typedef enum {
+	GP_REGS0_ID = 0,
+	N_GP_REGS_ID
+} gp_regs_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;
+
+typedef enum {
+	MIPI_PORT0_ID = 0,
+	MIPI_PORT1_ID,
+	MIPI_PORT2_ID,
+	N_MIPI_PORT_ID
+} mipi_port_ID_t;
+
+#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
+
+/*
+ * 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;
+
+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 compatability */
+#define N_IA_CSS_ISP_MEMORIES   IA_CSS_NUM_MEMORIES
+#define IA_CSS_NUM_ISP_MEMORIES IA_CSS_NUM_MEMORIES
+
+#endif /* __SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.c
new file mode 100644
index 000000000000..325b821f276c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.c
@@ -0,0 +1,360 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.iterator.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.iterator.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_iterator() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.copy_output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.copy_output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_copy_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.crop.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.crop.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_crop() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.fpn.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.fpn.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_fpn() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.dvs.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.dvs.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_dvs() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.qplane.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.qplane.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_qplane() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output0.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output0.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output0() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output1.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output1.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output1() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.output.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.output.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_output() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#ifdef ISP2401
+
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.sc.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.sc.offset;
+		}
+		if (size) {
+			ia_css_sc_config((struct sh_css_isp_sc_isp_config *)
+					&binary->mem_params.params[IA_CSS_PARAM_CLASS_CONFIG][IA_CSS_ISP_DMEM].address[offset],
+					config_dmem, size);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_sc() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+#endif
+
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.raw.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.raw.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_raw() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.tnr.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.tnr.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_tnr() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.ref.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.ref.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_ref() leave:\n");
+}
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_vf(
+	const struct ia_css_binary *binary,
+	const struct ia_css_vf_configuration *config_dmem)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() enter:\n");
+
+	{
+		unsigned offset = 0;
+		unsigned size   = 0;
+		if (binary->info->mem_offsets.offsets.config) {
+			size   = binary->info->mem_offsets.offsets.config->dmem.vf.size;
+			offset = binary->info->mem_offsets.offsets.config->dmem.vf.offset;
+		}
+		if (size) {
+			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);		}
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_configure_vf() leave:\n");
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.h
new file mode 100644
index 000000000000..8aacd3dbc05a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_configs.h
@@ -0,0 +1,189 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "isp/kernels/sc/sc_1.0/ia_css_sc.host.h"
+#endif
+#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 /* IA_CSS_INCLUDE_CONFIGURATIONS */
+/* Generated code: do not edit or commmit. */
+
+#ifndef _IA_CSS_ISP_CONFIG_H
+#define _IA_CSS_ISP_CONFIG_H
+
+/* Code generated by genparam/gencode.c:gen_param_enum() */
+
+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,
+#ifdef ISP2401
+	IA_CSS_SC_CONFIG_ID,
+#endif
+	IA_CSS_RAW_CONFIG_ID,
+	IA_CSS_TNR_CONFIG_ID,
+	IA_CSS_REF_CONFIG_ID,
+	IA_CSS_VF_CONFIG_ID,
+	IA_CSS_NUM_CONFIGURATION_IDS
+};
+
+/* Code generated by genparam/gencode.c:gen_param_offsets() */
+
+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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter sc;
+#endif
+		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 */
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_iterator(
+	const struct ia_css_binary *binary,
+	const struct ia_css_iterator_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_copy_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_copy_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_crop(
+	const struct ia_css_binary *binary,
+	const struct ia_css_crop_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_fpn(
+	const struct ia_css_binary *binary,
+	const struct ia_css_fpn_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_dvs(
+	const struct ia_css_binary *binary,
+	const struct ia_css_dvs_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_qplane(
+	const struct ia_css_binary *binary,
+	const struct ia_css_qplane_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output0(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output0_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output1(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output1_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_output(
+	const struct ia_css_binary *binary,
+	const struct ia_css_output_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#ifdef ISP2401
+void
+ia_css_configure_sc(
+	const struct ia_css_binary *binary,
+	const struct ia_css_sc_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+#endif
+void
+ia_css_configure_raw(
+	const struct ia_css_binary *binary,
+	const struct ia_css_raw_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_tnr(
+	const struct ia_css_binary *binary,
+	const struct ia_css_tnr_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+ia_css_configure_ref(
+	const struct ia_css_binary *binary,
+	const struct ia_css_ref_configuration *config_dmem);
+
+/* Code generated by genparam/genconfig.c:gen_configure_function() */
+
+void
+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/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.c
new file mode 100644
index 000000000000..11e4463ebb50
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.c
@@ -0,0 +1,3220 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.aa.size;
+	unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.anr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.anr2.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bh.size;
+
+		unsigned 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 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.cnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.crop.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.csc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.dp.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.de.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ecd.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.formats.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fpn.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.gc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem1.gc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ce.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yuv2rgb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.rgb2yuv.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem0.r_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.g_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem2.b_gamma.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.uds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.raa.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.s3a.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ob.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vmem.ob.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.output.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.bds.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.tnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.macc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_horicoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.sdis2_vertcoef.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_horiproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.sdis2_vertproj.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.wb.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.nr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.yee.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ynr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.fc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.ctc.size;
+
+		unsigned 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 size   = stage->binary->info->mem_offsets.offsets.param->vamem0.ctc.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vamem1.xnr_table.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr.size;
+
+		unsigned 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 pipe_id,
+	const struct ia_css_pipeline_stage *stage,
+	struct ia_css_isp_parameters *params)
+{
+	assert(params != NULL);
+
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.xnr3.size;
+
+		unsigned 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");
+		}
+
+	}
+#ifdef ISP2401
+	{
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.size;
+
+		unsigned offset = stage->binary->info->mem_offsets.offsets.param->vmem.xnr3.offset;
+
+		if (size) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "ia_css_process_xnr3() enter:\n");
+
+			ia_css_xnr3_vmem_encode((struct sh_css_isp_xnr3_vmem_params *)
+					&stage->binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_VMEM].address[offset],
+					&params->xnr3_config,
+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_xnr3() leave:\n");
+		}
+
+	}
+#endif
+}
+
+/* Code generated by genparam/gencode.c:gen_param_process_table() */
+
+void (* ia_css_kernel_process_param[IA_CSS_NUM_PARAMETER_IDS])(
+			unsigned 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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DP_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_WB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_TNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_DE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ANR2_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_ECD_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_MACC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CTC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_AA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_YUV2RGB_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_RGB2YUV_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_CSC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_NR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_GC_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORICOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTCOEF_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_HORIPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_SDIS2_VERTPROJ_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_R_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_G_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_B_GAMMA_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_TABLE_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_FORMATS_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_XNR3_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_S3A_ID] = true;
+
+#endif
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	assert(params != NULL);
+	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;
+#ifndef ISP2401
+	params->config_changed[IA_CSS_OUTPUT_ID] = true;
+
+#endif
+	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/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.h
new file mode 100644
index 000000000000..5b3deb7f74ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_params.h
@@ -0,0 +1,399 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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;
+#ifdef ISP2401
+		struct ia_css_isp_parameter xnr3;
+#endif
+	} 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 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)
+;
+#ifdef ISP2401
+
+#endif
+/* 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)
+;
+#ifdef ISP2401
+
+#endif
+#endif /* IA_CSS_INCLUDE_PARAMETER */
+
+#endif /* _IA_CSS_ISP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.c
new file mode 100644
index 000000000000..e87d05bc73ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.c
@@ -0,0 +1,214 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size   = binary->info->mem_offsets.offsets.state->vmem.aa.size;
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.cnr2.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.dp.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.de.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.tnr.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->dmem.ref.size;
+
+		unsigned 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 size   = binary->info->mem_offsets.offsets.state->vmem.ynr.size;
+
+		unsigned 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/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.h
new file mode 100644
index 000000000000..732adafb0a63
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hive_isp_css_2401_system_generated/ia_css_isp_states.h
@@ -0,0 +1,72 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/css_2401_system/hrt/bits.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/bits.h
new file mode 100644
index 000000000000..e71e33d9d143
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/bits.h
@@ -0,0 +1,104 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/cell_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/cell_params.h
new file mode 100644
index 000000000000..b5756bfe8eb6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/cell_params.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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        (1<<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/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_common_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_common_defs.h
new file mode 100644
index 000000000000..f3054fe04d03
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_common_defs.h
@@ -0,0 +1,200 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 reseved 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/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_defs.h
new file mode 100644
index 000000000000..6f5b7d3d3715
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/css_receiver_2400_defs.h
@@ -0,0 +1,258 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 (1<<16)
+#define _HRT_CSS_RECEIVER_2400_VSYNC_VAL (1<<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/atomisp2/css2400/css_2401_system/hrt/defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/defs.h
new file mode 100644
index 000000000000..47505f41790c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/dma_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/dma_v2_defs.h
new file mode 100644
index 000000000000..d184a8b313c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/dma_v2_defs.h
@@ -0,0 +1,199 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/gdc_v2_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gdc_v2_defs.h
new file mode 100644
index 000000000000..77722d205701
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gdc_v2_defs.h
@@ -0,0 +1,170 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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             (1 << 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/atomisp2/css2400/css_2401_system/hrt/gp_regs_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gp_regs_defs.h
new file mode 100644
index 000000000000..34e734f6648e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gp_regs_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_regs_defs_h
+#define _gp_regs_defs_h
+
+#define _HRT_GP_REGS_IS_FWD_REG_IDX 0
+
+#define _HRT_GP_REGS_REG_ALIGN 4
+
+#endif /* _gp_regs_defs_h */   
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gp_timer_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gp_timer_defs.h
new file mode 100644
index 000000000000..3082e2f5e014
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gp_timer_defs.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/gpio_block_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gpio_block_defs.h
new file mode 100644
index 000000000000..a807d4c99041
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/gpio_block_defs.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_2401_irq_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_2401_irq_types_hrt.h
new file mode 100644
index 000000000000..7a94c1d85b08
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_2401_irq_types_hrt.h
@@ -0,0 +1,69 @@
+/*
+#ifndef ISP2401
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_2401_IRQ_TYPES_HRT_H_
+#define _HIVE_ISP_CSS_2401_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_is2401               = HIVE_GP_DEV_IRQ_IS2401_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_2401_IRQ_TYPES_HRT_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_defs.h
new file mode 100644
index 000000000000..5a2ce9108ae4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_defs.h
@@ -0,0 +1,435 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CSS_2401_SYSTEM     1
+#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)
+#define HIVE_ISP_DDR_BYTES_RTL   (127 * 1024 * 1024)
+#define HIVE_ISP_DDR_SMALL_BYTES (128 * 256 / 8)
+#define HIVE_ISP_PAGE_SHIFT    12
+#define HIVE_ISP_PAGE_SIZE     (1<<HIVE_ISP_PAGE_SHIFT)
+
+#define CSS_DDR_WORD_BITS        HIVE_ISP_DDR_WORD_BITS
+#define CSS_DDR_WORD_BYTES       HIVE_ISP_DDR_WORD_BYTES
+
+/* settings used in applications */
+#define HIVE_XMEM_WIDTH            HIVE_ISP_DDR_WORD_BITS
+#define HIVE_VMEM_VECTOR_ELEMENTS  64
+#define HIVE_VMEM_ELEMENT_BITS     14
+#define HIVE_XMEM_ELEMENT_BITS     16
+#define HIVE_VMEM_VECTOR_BYTES (HIVE_VMEM_VECTOR_ELEMENTS*HIVE_XMEM_ELEMENT_BITS/8) /* used for # addr bytes for one vector */
+#define HIVE_XMEM_PACKED_WORD_VMEM_ELEMENTS (HIVE_XMEM_WIDTH/HIVE_VMEM_ELEMENT_BITS)
+#define HIVE_XMEM_WORD_VMEM_ELEMENTS        (HIVE_XMEM_WIDTH/HIVE_XMEM_ELEMENT_BITS)
+#define XMEM_INT_SIZE              4
+
+
+
+#define HIVE_ISYS_INP_BUFFER_BYTES (64*1024)  /* 64 kByte = 2k words (of 256 bits) */
+
+/* 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
+#define HIVE_GP_REGS_SWITCH_ISYS_IDX                           21
+
+/* 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
+#define HIVE_GP_REGS_SRST_ISYS_INP_BUF_BUS                     31
+
+/* 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_IS2401_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_IS2401_IRQ                               19
+#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
+#define HIVE_GP_TIMER_ISP_PMEM_ERROR_IRQ                       60
+
+/* 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
+
+#define HIVE_TESTBENCH_MIPI_PARPATHEN_REG_IDX                  14
+#define HIVE_TESTBENCH_FB_HPLL_FREQ_REG_IDX                    15
+#define HIVE_TESTBENCH_ISCLK_RATIO_REG_IDX                     16
+
+/* 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/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_host_ids_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_host_ids_hrt.h
new file mode 100644
index 000000000000..8d4c9d67c0e2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_host_ids_hrt.h
@@ -0,0 +1,119 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_host_ids_hrt_h_
+#define _hive_isp_css_host_ids_hrt_h_
+
+/* ISP_CSS identifiers */
+#define INP_SYS       testbench_isp_isp_css_part_is_2400_inp_sys
+#define ISYS_GP_REGS  testbench_isp_isp_css_part_is_2400_inp_sys_gpreg
+#define ISYS_IRQ_CTRL testbench_isp_isp_css_part_is_2400_inp_sys_irq_ctrl
+#define ISYS_CAP_A    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_a
+#define ISYS_CAP_B    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_b
+#define ISYS_CAP_C    testbench_isp_isp_css_part_is_2400_inp_sys_capt_unit_c
+#define ISYS_INP_BUF  testbench_isp_isp_css_part_input_buffer
+#define ISYS_INP_CTRL testbench_isp_isp_css_part_is_2400_inp_sys_inp_ctrl
+#define ISYS_ACQ      testbench_isp_isp_css_part_is_2400_inp_sys_acq_unit
+
+#define ISP           testbench_isp_isp_css_sec_part_isp
+#define SP            testbench_isp_isp_css_sec_part_scp
+
+#define IF_PRIM       testbench_isp_isp_css_part_is_2400_ifmt_ift_prim  
+#define IF_PRIM_B     testbench_isp_isp_css_part_is_2400_ifmt_ift_prim_b
+#define IF_SEC        testbench_isp_isp_css_part_is_2400_ifmt_ift_sec
+#define IF_SEC_MASTER testbench_isp_isp_css_part_is_2400_ifmt_ift_sec_mt_out
+#define STR_TO_MEM    testbench_isp_isp_css_part_is_2400_ifmt_mem_cpy
+#define IFMT_GP_REGS  testbench_isp_isp_css_part_is_2400_ifmt_gp_reg
+#define IFMT_IRQ_CTRL testbench_isp_isp_css_part_is_2400_ifmt_irq_ctrl
+
+#define CSS_RECEIVER  testbench_isp_isp_css_part_is_2400_inp_sys_csi_receiver
+
+#define TC            testbench_isp_isp_css_sec_part_gpd_tc
+#define GPTIMER       testbench_isp_isp_css_sec_part_gpd_gptimer
+#define DMA           testbench_isp_isp_css_sec_part_isp_dma
+#define GDC           testbench_isp_isp_css_sec_part_gdc1
+#define GDC2          testbench_isp_isp_css_sec_part_gdc2
+#define IRQ_CTRL      testbench_isp_isp_css_sec_part_gpd_irq_ctrl
+#define GPIO          testbench_isp_isp_css_sec_part_gpd_c_gpio
+#define GP_REGS       testbench_isp_isp_css_sec_part_gpd_gp_reg
+#define ISEL_GP_REGS  testbench_isp_isp_css_part_is_2400_isel_gpr
+#define ISEL_IRQ_CTRL testbench_isp_isp_css_part_is_2400_isel_irq_ctrl
+#define DATA_MMU      testbench_isp_isp_css_sec_part_data_out_sys_c_mmu
+#define ICACHE_MMU    testbench_isp_isp_css_sec_part_icache_out_sys_c_mmu
+
+/* next is actually not FIFO but FIFO adapter, or slave to streaming adapter */
+#define ISP_SP_FIFO   testbench_isp_isp_css_sec_part_fa_sp_isp
+#define ISEL_FIFO     testbench_isp_isp_css_part_is_2400_isel_sf_fa_in
+
+#define FIFO_GPF_SP   testbench_isp_isp_css_sec_part_sf_fa2sp_in
+#define FIFO_GPF_ISP  testbench_isp_isp_css_sec_part_sf_fa2isp_in
+#define FIFO_SP_GPF   testbench_isp_isp_css_sec_part_sf_sp2fa_in
+#define FIFO_ISP_GPF  testbench_isp_isp_css_sec_part_sf_isp2fa_in
+
+#define DATA_OCP_MASTER    testbench_isp_isp_css_sec_part_data_out_sys_cio2ocp_wide_data_out_mt
+#define ICACHE_OCP_MASTER  testbench_isp_isp_css_sec_part_icache_out_sys_cio2ocp_wide_data_out_mt
+
+#define SP_IN_FIFO    testbench_isp_isp_css_sec_part_sf_fa2sp_in
+#define SP_OUT_FIFO   testbench_isp_isp_css_sec_part_sf_sp2fa_out
+#define ISP_IN_FIFO   testbench_isp_isp_css_sec_part_sf_fa2isp_in
+#define ISP_OUT_FIFO  testbench_isp_isp_css_sec_part_sf_isp2fa_out
+#define GEN_SHORT_PACK_PORT testbench_isp_isp_css_part_is_2400_inp_sys_csi_str_mon_fa_gensh_out
+
+/* input_system_2401 identifiers */
+#define ISYS2401_GP_REGS    testbench_isp_isp_css_part_is_2401_gpreg
+#define ISYS2401_DMA        testbench_isp_isp_css_part_is_2401_dma
+#define ISYS2401_IRQ_CTRL   testbench_isp_isp_css_part_is_2401_isys_irq_ctrl
+
+#define ISYS2401_CSI_RX_A     testbench_isp_isp_css_part_is_2401_is_pipe_a_csi_rx
+#define ISYS2401_MIPI_BE_A    testbench_isp_isp_css_part_is_2401_is_pipe_a_mipi_be
+#define ISYS2401_S2M_A        testbench_isp_isp_css_part_is_2401_is_pipe_a_s2m
+#define ISYS2401_PXG_A        testbench_isp_isp_css_part_is_2401_is_pipe_a_pxlgen
+#define ISYS2401_IBUF_CNTRL_A testbench_isp_isp_css_part_is_2401_is_pipe_a_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_A   testbench_isp_isp_css_part_is_2401_is_pipe_a_irq_ctrl_pipe
+
+#define ISYS2401_CSI_RX_B     testbench_isp_isp_css_part_is_2401_is_pipe_b_csi_rx
+#define ISYS2401_MIPI_BE_B    testbench_isp_isp_css_part_is_2401_is_pipe_b_mipi_be
+#define ISYS2401_S2M_B        testbench_isp_isp_css_part_is_2401_is_pipe_b_s2m
+#define ISYS2401_PXG_B        testbench_isp_isp_css_part_is_2401_is_pipe_b_pxlgen
+#define ISYS2401_IBUF_CNTRL_B testbench_isp_isp_css_part_is_2401_is_pipe_b_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_B   testbench_isp_isp_css_part_is_2401_is_pipe_b_irq_ctrl_pipe
+
+#define ISYS2401_CSI_RX_C     testbench_isp_isp_css_part_is_2401_is_pipe_c_csi_rx
+#define ISYS2401_MIPI_BE_C    testbench_isp_isp_css_part_is_2401_is_pipe_c_mipi_be
+#define ISYS2401_S2M_C        testbench_isp_isp_css_part_is_2401_is_pipe_c_s2m
+#define ISYS2401_PXG_C        testbench_isp_isp_css_part_is_2401_is_pipe_c_pxlgen
+#define ISYS2401_IBUF_CNTRL_C testbench_isp_isp_css_part_is_2401_is_pipe_c_ibuf_ctrl
+#define ISYS2401_IRQ_CTRL_C   testbench_isp_isp_css_part_is_2401_is_pipe_c_irq_ctrl_pipe
+
+
+/* Testbench identifiers */
+#define DDR             testbench_ddram
+#define DDR_SMALL       testbench_ddram_small
+#define XMEM            DDR
+#define GPIO_ADAPTER    testbench_gp_adapter
+#define SIG_MONITOR     testbench_sig_mon
+#define DDR_SLAVE       testbench_ddram_ip0
+#define DDR_SMALL_SLAVE testbench_ddram_small_ip0
+#define HOST_MASTER     host_op0
+
+#define CSI_SENSOR         testbench_vied_sensor
+#define CSI_SENSOR_GP_REGS testbench_vied_sensor_gpreg
+#define CSI_STR_IN_A       testbench_vied_sensor_tx_a_csi_tx_data_in
+#define CSI_STR_IN_B       testbench_vied_sensor_tx_b_csi_tx_data_in
+#define CSI_STR_IN_C       testbench_vied_sensor_tx_c_csi_tx_data_in
+#define CSI_SENSOR_TX_A    testbench_vied_sensor_tx_a
+#define CSI_SENSOR_TX_B    testbench_vied_sensor_tx_b
+#define CSI_SENSOR_TX_C    testbench_vied_sensor_tx_c
+
+#endif /* _hive_isp_css_host_ids_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
new file mode 100644
index 000000000000..b4211a0c631a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_isp_css_streaming_to_mipi_types_hrt.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/hive_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_types.h
new file mode 100644
index 000000000000..58b0e6effbd0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/hive_types.h
@@ -0,0 +1,128 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+
+/* by default assume 32 bit master port (both data and address) */
+#ifndef HRT_DATA_WIDTH
+#define HRT_DATA_WIDTH 32
+#endif
+#ifndef HRT_ADDRESS_WIDTH
+#define HRT_ADDRESS_WIDTH 32
+#endif
+
+#define HRT_DATA_BYTES    (HRT_DATA_WIDTH/8)
+#define HRT_ADDRESS_BYTES (HRT_ADDRESS_WIDTH/8)
+
+#if HRT_DATA_WIDTH == 64
+typedef hive_uint64 hrt_data;
+#elif HRT_DATA_WIDTH == 32
+typedef hive_uint32 hrt_data;
+#else
+#error data width not supported
+#endif
+
+#if HRT_ADDRESS_WIDTH == 64
+typedef hive_uint64 hrt_address; 
+#elif HRT_ADDRESS_WIDTH == 32
+typedef hive_uint32 hrt_address;
+#else
+#error adddres width not supported
+#endif
+
+/* The SP side representation of an HMM virtual address */
+typedef hive_uint32 hrt_vaddress;
+
+/* 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/atomisp2/css2400/css_2401_system/hrt/if_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/if_defs.h
new file mode 100644
index 000000000000..7d39e45796ae
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/if_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/input_formatter_subsystem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_formatter_subsystem_defs.h
new file mode 100644
index 000000000000..16bfe1d80bc9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_formatter_subsystem_defs.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/input_selector_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_selector_defs.h
new file mode 100644
index 000000000000..87fbf82edb5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_selector_defs.h
@@ -0,0 +1,89 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/input_switch_2400_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_switch_2400_defs.h
new file mode 100644
index 000000000000..20a13c4cdb56
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_switch_2400_defs.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/input_system_ctrl_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_system_ctrl_defs.h
new file mode 100644
index 000000000000..a7f0ca80bc9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_system_ctrl_defs.h
@@ -0,0 +1,254 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2401_system/hrt/input_system_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_system_defs.h
new file mode 100644
index 000000000000..ae62163034a6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/input_system_defs.h
@@ -0,0 +1,126 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/irq_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/irq_controller_defs.h
new file mode 100644
index 000000000000..ec6dd4487158
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/irq_controller_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/isp2400_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp2400_support.h
new file mode 100644
index 000000000000..e00bc841d0f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp2400_support.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/isp2401_mamoiada_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp2401_mamoiada_params.h
new file mode 100644
index 000000000000..033e23bcf672
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp2401_mamoiada_params.h
@@ -0,0 +1,258 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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
+
+/* Cell name  */
+#define ISP_CELL_TYPE                          isp2401_mamoiada
+#define ISP_VMEM                               simd_vmem
+#define _HRT_ISP_VMEM                          isp2401_mamoiada_simd_vmem
+
+/* 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_VMEM_IS_BAMEM                      1
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_BAMEM_MAX_BOI_HEIGHT        8
+  #define ISP_VMEM_BAMEM_LATENCY               5
+  #define ISP_VMEM_BAMEM_BANK_NARROWING_FACTOR 2
+  #define ISP_VMEM_BAMEM_NR_DATA_PLANES        8
+  #define ISP_VMEM_BAMEM_NR_CFG_REGISTERS      16
+  #define ISP_VMEM_BAMEM_LININT                0
+  #define ISP_VMEM_BAMEM_DAP_BITS              3
+  #define ISP_VMEM_BAMEM_LININT_FRAC_BITS      0
+  #define ISP_VMEM_BAMEM_PID_BITS              3
+  #define ISP_VMEM_BAMEM_OFFSET_BITS           19
+  #define ISP_VMEM_BAMEM_ADDRESS_BITS          25
+  #define ISP_VMEM_BAMEM_RID_BITS              4
+  #define ISP_VMEM_BAMEM_TRANSPOSITION         1
+  #define ISP_VMEM_BAMEM_VEC_PLUS_SLICE        1
+  #define ISP_VMEM_BAMEM_ARB_SERVICE_CYCLE_BITS 1
+  #define ISP_VMEM_BAMEM_LUT_ELEMS             16
+  #define ISP_VMEM_BAMEM_LUT_ADDR_WIDTH        14
+  #define ISP_VMEM_BAMEM_HALF_BLOCK_WRITE      1
+  #define ISP_VMEM_BAMEM_SMART_FETCH           1
+  #define ISP_VMEM_BAMEM_BIG_ENDIANNESS        0
+#endif /* ISP_VMEM_IS_BAMEM */
+#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
+#if ISP_VMEM_IS_BAMEM
+  #define ISP_VMEM_ALIGN_ELEM                  2
+#endif /* ISP_VMEM_IS_BAMEM */
+#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))
+
+#ifdef C_RUN
+#define ISP_VEC_ALIGN                          (_isp_ceil_div(ISP_VEC_WIDTH, 64)*8)
+#else
+#define ISP_VEC_ALIGN                          ISP_VMEM_ALIGN
+#endif
+
+/* HRT specific vector support */
+#define isp2401_mamoiada_vector_alignment         ISP_VEC_ALIGN
+#define isp2401_mamoiada_vector_elem_bits         ISP_VMEM_ELEMBITS
+#define isp2401_mamoiada_vector_elem_precision    ISP_VMEM_ELEM_PRECISION
+#define isp2401_mamoiada_vector_num_elems         ISP_VEC_NELEMS
+
+/* 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_VRF1_SIZE       32
+#define ISP_VRF2_SIZE       32
+#define ISP_VRF3_SIZE       32
+#define ISP_VRF4_SIZE       32
+#define ISP_VRF5_SIZE       32
+#define ISP_VRF6_SIZE       32
+#define ISP_VRF7_SIZE       32
+#define ISP_VRF8_SIZE       32
+#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/atomisp2/css2400/css_2401_system/hrt/isp_acquisition_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp_acquisition_defs.h
new file mode 100644
index 000000000000..593620721627
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp_acquisition_defs.h
@@ -0,0 +1,234 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 accesible 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/atomisp2/css2400/css_2401_system/hrt/isp_capture_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp_capture_defs.h
new file mode 100644
index 000000000000..aa413df022f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/isp_capture_defs.h
@@ -0,0 +1,310 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 CAPT_RCV_ACK                              1
+//#define CAPT_WRT_ACK                              2               
+//#define CAPT_IRQ_ACK                              3                        
+
+/* --------------------------------------------------*/
+
+#define NOF_IRQS                                  2
+
+/* --------------------------------------------------*/
+/* REGISTER INFO */
+/* --------------------------------------------------*/
+
+// Number of registers
+#define CAPT_NOF_REGS                             16
+
+// Register id's of MMIO slave accesible 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_ADDR_REG_WIDTH                 9
+//#define CAPT_MEM_REGION_SIZE_REG_WIDTH            9
+//#define CAPT_NUM_MEM_REGIONS_REG_WIDTH            9
+#define CAPT_INIT_REG_WIDTH                       (22 + 4)
+
+#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_NEXT_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))
+
+//#define CAPT_INIT_RESTART_MEM_ADDR_WIDTH          9   
+//#define CAPT_INIT_RESTART_MEM_REGION_WIDTH        9 
+
+/* 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_RESTART_MEM_ADDR_MSB            14
+#define CAPT_INIT_RESTART_MEM_REGION_LSB          15
+#define CAPT_INIT_RESTART_MEM_REGION_MSB          25
+
+
+#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
+#define CAPT_INIT_RESTART_MEM_ADDR_BITS           (CAPT_INIT_RESTART_MEM_ADDR_MSB - CAPT_INIT_RESTART_MEM_ADDR_LSB + 1)
+#define CAPT_INIT_RESTART_MEM_REGION_IDX          CAPT_INIT_RESTART_MEM_REGION_LSB
+#define CAPT_INIT_RESTART_MEM_REGION_BITS         (CAPT_INIT_RESTART_MEM_REGION_MSB - CAPT_INIT_RESTART_MEM_REGION_LSB + 1)
+
+
+
+/* --------------------------------------------------*/
+/* 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/atomisp2/css2400/css_2401_system/hrt/mmu_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/mmu_defs.h
new file mode 100644
index 000000000000..c038f39ffd25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/mmu_defs.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/scalar_processor_2400_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/scalar_processor_2400_params.h
new file mode 100644
index 000000000000..9b6c2893d950
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/scalar_processor_2400_params.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/sp_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/sp_hrt.h
new file mode 100644
index 000000000000..7ee4deba519a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/sp_hrt.h
@@ -0,0 +1,24 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_hrt_h_
+#define _sp_hrt_h_
+
+#define hrt_sp_dmem(cell) HRT_PROC_TYPE_PROP(cell, _dmem)
+
+#define hrt_sp_dmem_master_port_address(cell) hrt_mem_master_port_address(cell, hrt_sp_dmem(cell))
+
+#endif /* _sp_hrt_h_ */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/str2mem_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/str2mem_defs.h
new file mode 100644
index 000000000000..1cb62444cf68
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/str2mem_defs.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/streaming_to_mipi_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/streaming_to_mipi_defs.h
new file mode 100644
index 000000000000..60143b8743a2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/streaming_to_mipi_defs.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/timed_controller_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/timed_controller_defs.h
new file mode 100644
index 000000000000..d2b8972b0d9e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/timed_controller_defs.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/css_2401_system/hrt/var.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/var.h
new file mode 100644
index 000000000000..19b19ef484f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/var.h
@@ -0,0 +1,99 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_VAR_H
+#define _HRT_VAR_H
+
+#include "version.h"
+#include "system_api.h"
+#include "hive_types.h"
+
+#define hrt_int_type_of_char   char
+#define hrt_int_type_of_uchar  unsigned char
+#define hrt_int_type_of_short  short
+#define hrt_int_type_of_ushort unsigned short
+#define hrt_int_type_of_int    int
+#define hrt_int_type_of_uint   unsigned int
+#define hrt_int_type_of_long   long
+#define hrt_int_type_of_ulong  unsigned long
+#define hrt_int_type_of_ptr    unsigned int
+
+#define hrt_host_type_of_char   char
+#define hrt_host_type_of_uchar  unsigned char
+#define hrt_host_type_of_short  short
+#define hrt_host_type_of_ushort unsigned short
+#define hrt_host_type_of_int    int
+#define hrt_host_type_of_uint   unsigned int
+#define hrt_host_type_of_long   long
+#define hrt_host_type_of_ulong  unsigned long
+#define hrt_host_type_of_ptr    void*
+
+#define HRT_TYPE_BYTES(cell, type) (HRT_TYPE_BITS(cell, type)/8)
+#define HRT_HOST_TYPE(cell_type)   HRTCAT(hrt_host_type_of_, cell_type)
+#define HRT_INT_TYPE(type)         HRTCAT(hrt_int_type_of_, type)
+
+#ifdef C_RUN
+
+#ifdef C_RUN_DYNAMIC_LINK_PROGRAMS
+extern void *csim_processor_get_crun_symbol(hive_proc_id p, const char *sym);
+#define _hrt_cell_get_crun_symbol(cell,sym)          csim_processor_get_crun_symbol(cell,HRTSTR(sym))
+#define _hrt_cell_get_crun_indexed_symbol(cell,sym)  csim_processor_get_crun_symbol(cell,HRTSTR(sym))
+#else
+#define _hrt_cell_get_crun_symbol(cell,sym)         (&sym)
+#define _hrt_cell_get_crun_indexed_symbol(cell,sym) (sym)
+#endif //  C_RUN_DYNAMIC_LINK_PROGRAMS
+
+#define hrt_scalar_store(cell, type, var, data) \
+	((*(HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_symbol(cell,var)) = (data))
+#define hrt_scalar_load(cell, type, var) \
+	((*(HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_symbol(cell,var)))
+
+#define hrt_indexed_store(cell, type, array, index, data) \
+	((((HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_indexed_symbol(cell,array))[index]) = (data))
+#define hrt_indexed_load(cell, type, array, index) \
+	(((HRT_HOST_TYPE(type)*)_hrt_cell_get_crun_indexed_symbol(cell,array))[index])
+
+#else /* C_RUN */
+
+#define hrt_scalar_store(cell, type, var, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_scalar_load(cell, type, var) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+	       cell, \
+	       HRTCAT(HIVE_MEM_,var), \
+	       HRTCAT(HIVE_ADDR_,var)))
+
+#define hrt_indexed_store(cell, type, array, index, data) \
+  HRTCAT(hrt_mem_store_,HRT_TYPE_BITS(cell, type))(\
+	       cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type)), \
+	       (HRT_INT_TYPE(type))(data))
+
+#define hrt_indexed_load(cell, type, array, index) \
+  (HRT_HOST_TYPE(type))(HRTCAT4(_hrt_mem_load_,HRT_PROC_TYPE(cell),_,type) ( \
+         cell, \
+	       HRTCAT(HIVE_MEM_,array), \
+	       (HRTCAT(HIVE_ADDR_,array))+((index)*HRT_TYPE_BYTES(cell, type))))
+
+#endif /* C_RUN */
+
+#endif /* _HRT_VAR_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/version.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/version.h
new file mode 100644
index 000000000000..bbc4948baea9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/hrt/version.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp/pci/atomisp2/css2400/css_2401_system/spmem_dump.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/spmem_dump.c
new file mode 100644
index 000000000000..09f0780f0c80
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_2401_system/spmem_dump.c
@@ -0,0 +1,3634 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_map_h_
+#define _sp_map_h_
+
+
+#ifndef _hrt_dummy_use_blob_sp
+#define _hrt_dummy_use_blob_sp()
+#endif
+
+#define _hrt_cell_load_program_sp(proc) _hrt_cell_load_program_embedded(proc, sp)
+
+#ifndef ISP2401
+/* function input_system_acquisition_stop: ADE */
+#else
+/* function input_system_acquisition_stop: AD8 */
+#endif
+
+#ifndef ISP2401
+/* function longjmp: 684E */
+#else
+/* function longjmp: 69C1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SRST_MASK
+#define HIVE_MEM_HIVE_IF_SRST_MASK scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SRST_MASK 0x1C8
+#define HIVE_SIZE_HIVE_IF_SRST_MASK 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SRST_MASK scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SRST_MASK 0x1C8
+#define HIVE_SIZE_sp_HIVE_IF_SRST_MASK 16
+
+#ifndef ISP2401
+/* function tmpmem_init_dmem: 6599 */
+#else
+/* function tmpmem_init_dmem: 66D4 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_receive_ack: 5EDD */
+#else
+/* function ia_css_isys_sp_token_map_receive_ack: 6018 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_addr_B: 3345 */
+#else
+/* function ia_css_dmaproxy_sp_set_addr_B: 3539 */
+
+/* function ia_css_pipe_data_init_tagger_resources: A4F */
+#endif
+
+/* function debug_buffer_set_ddr_addr: DD */
+
+#ifndef ISP2401
+/* function receiver_port_reg_load: AC2 */
+#else
+/* function receiver_port_reg_load: ABC */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_mipi
+#define HIVE_MEM_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_mipi 0x631C
+#else
+#define HIVE_ADDR_vbuf_mipi 0x6378
+#endif
+#define HIVE_SIZE_vbuf_mipi 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_mipi scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_mipi 0x631C
+#else
+#define HIVE_ADDR_sp_vbuf_mipi 0x6378
+#endif
+#define HIVE_SIZE_sp_vbuf_mipi 12
+
+#ifndef ISP2401
+/* function ia_css_event_sp_decode: 3536 */
+#else
+/* function ia_css_event_sp_decode: 372A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_get_size: 48BE */
+#else
+/* function ia_css_queue_get_size: 4B46 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_load: 4EFF */
+#else
+/* function ia_css_queue_load: 515D */
+#endif
+
+#ifndef ISP2401
+/* function setjmp: 6857 */
+#else
+/* function setjmp: 69CA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_isys_event_queue
+#define HIVE_MEM_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x4684
+#else
+#define HIVE_ADDR_sem_for_sp2host_isys_event_queue 0x46CC
+#endif
+#define HIVE_SIZE_sem_for_sp2host_isys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_isys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x4684
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_isys_event_queue 0x46CC
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_isys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6E07 */
+#else
+/* function ia_css_dmaproxy_sp_wait_for_ack: 6F4B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_func: 5124 */
+#else
+/* function ia_css_sp_rawcopy_func: 5382 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_marked: 2A10 */
+#else
+/* function ia_css_tagger_buf_sp_pop_marked: 2BB2 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stage
+#define HIVE_MEM_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stage 0x5C00
+#else
+#define HIVE_ADDR_isp_stage 0x5C60
+#endif
+#define HIVE_SIZE_isp_stage 832
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stage scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stage 0x5C00
+#else
+#define HIVE_ADDR_sp_isp_stage 0x5C60
+#endif
+#define HIVE_SIZE_sp_isp_stage 832
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_raw
+#define HIVE_MEM_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_raw 0x2F4
+#else
+#define HIVE_ADDR_vbuf_raw 0x30C
+#endif
+#define HIVE_SIZE_vbuf_raw 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_raw scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_raw 0x2F4
+#else
+#define HIVE_ADDR_sp_vbuf_raw 0x30C
+#endif
+#define HIVE_SIZE_sp_vbuf_raw 4
+
+#ifndef ISP2401
+/* function ia_css_sp_bin_copy_func: 504B */
+#else
+/* function ia_css_sp_bin_copy_func: 52A9 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_item_store: 4C4D */
+#else
+/* function ia_css_queue_item_store: 4EAB */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AA0
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AFC
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_metadata_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AA0
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 0x4AFC
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_metadata_bufs 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4AB4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4B10
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_buffer_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4AB4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 0x4B10
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_buffer_bufs 160
+
+/* function sp_start_isp: 45D */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_binary_group
+#define HIVE_MEM_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_binary_group 0x5FF0
+#else
+#define HIVE_ADDR_sp_binary_group 0x6050
+#endif
+#define HIVE_SIZE_sp_binary_group 32
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_binary_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_binary_group 0x5FF0
+#else
+#define HIVE_ADDR_sp_sp_binary_group 0x6050
+#endif
+#define HIVE_SIZE_sp_sp_binary_group 32
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sw_state
+#define HIVE_MEM_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sw_state 0x62AC
+#else
+#define HIVE_ADDR_sp_sw_state 0x6308
+#endif
+#define HIVE_SIZE_sp_sw_state 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sw_state scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sw_state 0x62AC
+#else
+#define HIVE_ADDR_sp_sp_sw_state 0x6308
+#endif
+#define HIVE_SIZE_sp_sp_sw_state 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_main: D5B */
+#else
+/* function ia_css_thread_sp_main: D50 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_internal_buffers: 373C */
+#else
+/* function ia_css_ispctrl_sp_init_internal_buffers: 396B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_psys_event_queue_handle
+#define HIVE_MEM_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x4B54
+#else
+#define HIVE_ADDR_sp2host_psys_event_queue_handle 0x4BB0
+#endif
+#define HIVE_SIZE_sp2host_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x4B54
+#else
+#define HIVE_ADDR_sp_sp2host_psys_event_queue_handle 0x4BB0
+#endif
+#define HIVE_SIZE_sp_sp2host_psys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp2host_psys_event_queue
+#define HIVE_MEM_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x4698
+#else
+#define HIVE_ADDR_sem_for_sp2host_psys_event_queue 0x46E0
+#endif
+#define HIVE_SIZE_sem_for_sp2host_psys_event_queue 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp2host_psys_event_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x4698
+#else
+#define HIVE_ADDR_sp_sem_for_sp2host_psys_event_queue 0x46E0
+#endif
+#define HIVE_SIZE_sp_sem_for_sp2host_psys_event_queue 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_propagate_frame: 2429 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_stop_copy_preview
+#define HIVE_MEM_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_stop_copy_preview 0x6290
+#define HIVE_SIZE_sp_stop_copy_preview 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_stop_copy_preview scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_stop_copy_preview 0x6290
+#define HIVE_SIZE_sp_sp_stop_copy_preview 4
+#else
+/* function ia_css_tagger_sp_propagate_frame: 2479 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_reg_load: B17 */
+#else
+/* function input_system_reg_load: B11 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_handles
+#define HIVE_MEM_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_handles 0x6328
+#else
+#define HIVE_ADDR_vbuf_handles 0x6384
+#endif
+#define HIVE_SIZE_vbuf_handles 960
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_handles scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_handles 0x6328
+#else
+#define HIVE_ADDR_sp_vbuf_handles 0x6384
+#endif
+#define HIVE_SIZE_sp_vbuf_handles 960
+
+#ifndef ISP2401
+/* function ia_css_queue_store: 4DB3 */
+
+/* function ia_css_sp_flash_register: 2C45 */
+#else
+/* function ia_css_queue_store: 5011 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_dummy_function: 566B */
+#else
+/* function ia_css_sp_flash_register: 2DE7 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_create: 5B50 */
+#else
+/* function ia_css_isys_sp_backend_create: 5C8B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_init: 184C */
+#else
+/* function ia_css_pipeline_sp_init: 1886 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_configure: 2319 */
+#else
+/* function ia_css_tagger_sp_configure: 2369 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_end_binary: 357F */
+#else
+/* function ia_css_ispctrl_sp_end_binary: 3773 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4B60
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4BBC
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4B60
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 0x4BBC
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_per_frame_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function receiver_port_reg_store: AC9 */
+#else
+/* function receiver_port_reg_store: AC3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_is_pending_mask
+#define HIVE_MEM_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_is_pending_mask 0x5C
+#define HIVE_SIZE_event_is_pending_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_is_pending_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_is_pending_mask 0x5C
+#define HIVE_SIZE_sp_event_is_pending_mask 44
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_frame
+#define HIVE_MEM_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x46AC
+#else
+#define HIVE_ADDR_sp_all_cb_elems_frame 0x46F4
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x46AC
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_frame 0x46F4
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_frame 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_isys_event_queue_handle
+#define HIVE_MEM_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x4B74
+#else
+#define HIVE_ADDR_sp2host_isys_event_queue_handle 0x4BD0
+#endif
+#define HIVE_SIZE_sp2host_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x4B74
+#else
+#define HIVE_ADDR_sp_sp2host_isys_event_queue_handle 0x4BD0
+#endif
+#define HIVE_SIZE_sp_sp2host_isys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_com
+#define HIVE_MEM_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_com 0x4114
+#else
+#define HIVE_ADDR_host_sp_com 0x4134
+#endif
+#define HIVE_SIZE_host_sp_com 220
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_com scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_com 0x4114
+#else
+#define HIVE_ADDR_sp_host_sp_com 0x4134
+#endif
+#define HIVE_SIZE_sp_host_sp_com 220
+
+#ifndef ISP2401
+/* function ia_css_queue_get_free_space: 4A12 */
+#else
+/* function ia_css_queue_get_free_space: 4C70 */
+#endif
+
+#ifndef ISP2401
+/* function exec_image_pipe: 6C4 */
+#else
+/* function exec_image_pipe: 658 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_init_dmem_data
+#define HIVE_MEM_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_init_dmem_data 0x62B0
+#else
+#define HIVE_ADDR_sp_init_dmem_data 0x630C
+#endif
+#define HIVE_SIZE_sp_init_dmem_data 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_init_dmem_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x62B0
+#else
+#define HIVE_ADDR_sp_sp_init_dmem_data 0x630C
+#endif
+#define HIVE_SIZE_sp_sp_init_dmem_data 24
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_start: 592D */
+#else
+/* function ia_css_sp_metadata_start: 5A68 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_init_buffer_queues: 2CB4 */
+#else
+/* function ia_css_bufq_sp_init_buffer_queues: 2E56 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_stop: 182F */
+#else
+/* function ia_css_pipeline_sp_stop: 1869 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_connect_pipes: 2803 */
+#else
+/* function ia_css_tagger_sp_connect_pipes: 2853 */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_wait: 70D */
+#else
+/* function sp_isys_copy_wait: 6A1 */
+#endif
+
+/* function is_isp_debug_buffer_full: 337 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 32C8 */
+#else
+/* function ia_css_dmaproxy_sp_configure_channel_from_info: 34A9 */
+#endif
+
+#ifndef ISP2401
+/* function encode_and_post_timer_event: A30 */
+#else
+/* function encode_and_post_timer_event: 9C4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_per_frame_data
+#define HIVE_MEM_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_per_frame_data 0x41F0
+#else
+#define HIVE_ADDR_sp_per_frame_data 0x4210
+#endif
+#define HIVE_SIZE_sp_per_frame_data 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_per_frame_data scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_per_frame_data 0x41F0
+#else
+#define HIVE_ADDR_sp_sp_per_frame_data 0x4210
+#endif
+#define HIVE_SIZE_sp_sp_per_frame_data 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_dequeue: 62ED */
+#else
+/* function ia_css_rmgr_sp_vbuf_dequeue: 6428 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_psys_event_queue_handle
+#define HIVE_MEM_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x4B80
+#else
+#define HIVE_ADDR_host2sp_psys_event_queue_handle 0x4BDC
+#endif
+#define HIVE_SIZE_host2sp_psys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_psys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x4B80
+#else
+#define HIVE_ADDR_sp_host2sp_psys_event_queue_handle 0x4BDC
+#endif
+#define HIVE_SIZE_sp_host2sp_psys_event_queue_handle 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_xmem_bin_addr
+#define HIVE_MEM_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_xmem_bin_addr 0x41F4
+#else
+#define HIVE_ADDR_xmem_bin_addr 0x4214
+#endif
+#define HIVE_SIZE_xmem_bin_addr 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_xmem_bin_addr scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_xmem_bin_addr 0x41F4
+#else
+#define HIVE_ADDR_sp_xmem_bin_addr 0x4214
+#endif
+#define HIVE_SIZE_sp_xmem_bin_addr 4
+
+#ifndef ISP2401
+/* function tmr_clock_init: 13FB */
+#else
+/* function tmr_clock_init: 141C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_run: 141C */
+#else
+/* function ia_css_pipeline_sp_run: 143D */
+#endif
+
+#ifndef ISP2401
+/* function memcpy: 68F7 */
+#else
+/* function memcpy: 6A6A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GP_DEVICE_BASE
+#define HIVE_MEM_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_GP_DEVICE_BASE 0x2FC
+#else
+#define HIVE_ADDR_GP_DEVICE_BASE 0x314
+#endif
+#define HIVE_SIZE_GP_DEVICE_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GP_DEVICE_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x2FC
+#else
+#define HIVE_ADDR_sp_GP_DEVICE_BASE 0x314
+#endif
+#define HIVE_SIZE_sp_GP_DEVICE_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_ready_queue
+#define HIVE_MEM_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x1E0
+#else
+#define HIVE_ADDR_ia_css_thread_sp_ready_queue 0x1E4
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_ready_queue 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_ready_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x1E0
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_ready_queue 0x1E4
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_ready_queue 12
+
+#ifndef ISP2401
+/* function input_system_reg_store: B1E */
+#else
+/* function input_system_reg_store: B18 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_start: 5D66 */
+#else
+/* function ia_css_isys_sp_frontend_start: 5EA1 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_uds_sp_scale_params: 6600 */
+#else
+/* function ia_css_uds_sp_scale_params: 6773 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_increase_size: E40 */
+#else
+/* function ia_css_circbuf_increase_size: E35 */
+#endif
+
+#ifndef ISP2401
+/* function __divu: 6875 */
+#else
+/* function __divu: 69E8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_get_state: C83 */
+#else
+/* function ia_css_thread_sp_get_state: C78 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_stop
+#define HIVE_MEM_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x46BC
+#else
+#define HIVE_ADDR_sem_for_cont_capt_stop 0x4704
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_stop 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_stop scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x46BC
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_stop 0x4704
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_stop 20
+
+#ifndef ISP2401
+/* function thread_fiber_sp_main: E39 */
+#else
+/* function thread_fiber_sp_main: E2E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_pipe_thread
+#define HIVE_MEM_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pipe_thread 0x4800
+#define HIVE_SIZE_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_isp_pipe_thread 0x4848
+#define HIVE_SIZE_sp_isp_pipe_thread 360
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_pipe_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x4800
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 340
+#else
+#define HIVE_ADDR_sp_sp_isp_pipe_thread 0x4848
+#define HIVE_SIZE_sp_sp_isp_pipe_thread 360
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_handle_parameter_sets: 128A */
+#else
+/* function ia_css_parambuf_sp_handle_parameter_sets: 127F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_set_state: 595C */
+#else
+/* function ia_css_spctrl_sp_set_state: 5A97 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_signal: 6AF7 */
+#else
+/* function ia_css_thread_sem_sp_signal: 6C6C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_IRQ_BASE
+#define HIVE_MEM_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_IRQ_BASE 0x2C
+#define HIVE_SIZE_IRQ_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_IRQ_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_IRQ_BASE 0x2C
+#define HIVE_SIZE_sp_IRQ_BASE 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_TIMED_CTRL_BASE
+#define HIVE_MEM_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_TIMED_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_TIMED_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_TIMED_CTRL_BASE 0x40
+#define HIVE_SIZE_sp_TIMED_CTRL_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_isr: 6FDC */
+
+/* function ia_css_isys_sp_generate_exp_id: 60FE */
+#else
+/* function ia_css_isys_sp_isr: 7139 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_init: 61E8 */
+#else
+/* function ia_css_isys_sp_generate_exp_id: 6239 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_init: 6BC8 */
+#else
+/* function ia_css_rmgr_sp_init: 6323 */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x308
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x308
+#define HIVE_SIZE_sp_is_isp_requested 4
+#else
+/* function ia_css_thread_sem_sp_init: 6D3B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_frame
+#define HIVE_MEM_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x46D0
+#else
+#define HIVE_ADDR_sem_for_reading_cb_frame 0x4718
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_frame 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x46D0
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_frame 0x4718
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_frame 40
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_execute: 3230 */
+#else
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_is_isp_requested
+#define HIVE_MEM_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_is_isp_requested 0x320
+#define HIVE_SIZE_is_isp_requested 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_is_isp_requested scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_is_isp_requested 0x320
+#define HIVE_SIZE_sp_is_isp_requested 4
+
+/* function ia_css_dmaproxy_sp_execute: 340F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_is_empty: 48F9 */
+#else
+/* function ia_css_queue_is_empty: 7098 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_has_stopped: 1825 */
+#else
+/* function ia_css_pipeline_sp_has_stopped: 185F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_extract: F44 */
+#else
+/* function ia_css_circbuf_extract: F39 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 2B26 */
+#else
+/* function ia_css_tagger_buf_sp_is_locked_from_start: 2CC8 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_sp_thread
+#define HIVE_MEM_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_current_sp_thread 0x1DC
+#define HIVE_SIZE_current_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_sp_thread scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_current_sp_thread 0x1DC
+#define HIVE_SIZE_sp_current_sp_thread 4
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_spid: 5963 */
+#else
+/* function ia_css_spctrl_sp_get_spid: 5A9E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_reset_buffers: 2D3B */
+#else
+/* function ia_css_bufq_sp_reset_buffers: 2EDD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr: 6E35 */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr: 6F79 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_uninit: 61E1 */
+#else
+/* function ia_css_rmgr_sp_uninit: 631C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack
+#define HIVE_MEM_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_threads_stack 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack 0x164
+#define HIVE_SIZE_sp_sp_threads_stack 28
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek: F26 */
+#else
+/* function ia_css_circbuf_peek: F1B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_wait_for_in_param: 1053 */
+#else
+/* function ia_css_parambuf_sp_wait_for_in_param: 1048 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_get_exp_id: 5FC6 */
+#else
+/* function ia_css_isys_sp_token_map_get_exp_id: 6101 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cb_elems_param
+#define HIVE_MEM_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cb_elems_param 0x46F8
+#else
+#define HIVE_ADDR_sp_all_cb_elems_param 0x4740
+#endif
+#define HIVE_SIZE_sp_all_cb_elems_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cb_elems_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x46F8
+#else
+#define HIVE_ADDR_sp_sp_all_cb_elems_param 0x4740
+#endif
+#define HIVE_SIZE_sp_sp_all_cb_elems_param 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_pipeline_sp_curr_binary_id
+#define HIVE_MEM_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x1EC
+#else
+#define HIVE_ADDR_pipeline_sp_curr_binary_id 0x1F0
+#endif
+#define HIVE_SIZE_pipeline_sp_curr_binary_id 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_pipeline_sp_curr_binary_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x1EC
+#else
+#define HIVE_ADDR_sp_pipeline_sp_curr_binary_id 0x1F0
+#endif
+#define HIVE_SIZE_sp_pipeline_sp_curr_binary_id 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame_desc
+#define HIVE_MEM_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x4708
+#else
+#define HIVE_ADDR_sp_all_cbs_frame_desc 0x4750
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x4708
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame_desc 0x4750
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame_desc 8
+
+#ifndef ISP2401
+/* function sp_isys_copy_func_v2: 706 */
+#else
+/* function sp_isys_copy_func_v2: 69A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_cb_param
+#define HIVE_MEM_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_cb_param 0x4710
+#else
+#define HIVE_ADDR_sem_for_reading_cb_param 0x4758
+#endif
+#define HIVE_SIZE_sem_for_reading_cb_param 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x4710
+#else
+#define HIVE_ADDR_sp_sem_for_reading_cb_param 0x4758
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_cb_param 40
+
+#ifndef ISP2401
+/* function ia_css_queue_get_used_space: 49C6 */
+#else
+/* function ia_css_queue_get_used_space: 4C24 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_cont_capt_start
+#define HIVE_MEM_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_cont_capt_start 0x4738
+#else
+#define HIVE_ADDR_sem_for_cont_capt_start 0x4780
+#endif
+#define HIVE_SIZE_sem_for_cont_capt_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_cont_capt_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x4738
+#else
+#define HIVE_ADDR_sp_sem_for_cont_capt_start 0x4780
+#endif
+#define HIVE_SIZE_sp_sem_for_cont_capt_start 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tmp_heap
+#define HIVE_MEM_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tmp_heap 0x6010
+#else
+#define HIVE_ADDR_tmp_heap 0x6070
+#endif
+#define HIVE_SIZE_tmp_heap 640
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tmp_heap scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tmp_heap 0x6010
+#else
+#define HIVE_ADDR_sp_tmp_heap 0x6070
+#endif
+#define HIVE_SIZE_sp_tmp_heap 640
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_get_num_vbuf: 64F1 */
+#else
+/* function ia_css_rmgr_sp_get_num_vbuf: 662C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 3F62 */
+#else
+/* function ia_css_ispctrl_sp_output_compute_dma_info: 41A5 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_lock_exp_id: 20E6 */
+#else
+/* function ia_css_tagger_sp_lock_exp_id: 2136 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4B8C
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4BE8
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_s3a_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4B8C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 0x4BE8
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_s3a_bufs 60
+
+#ifndef ISP2401
+/* function ia_css_queue_is_full: 4A5D */
+#else
+/* function ia_css_queue_is_full: 4CBB */
+#endif
+
+/* function debug_buffer_init_isp: E4 */
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_uninit: 5D20 */
+#else
+/* function ia_css_isys_sp_frontend_uninit: 5E5B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_exp_id_is_locked: 201C */
+#else
+/* function ia_css_tagger_sp_exp_id_is_locked: 206C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem
+#define HIVE_MEM_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x66E8
+#else
+#define HIVE_ADDR_ia_css_rmgr_sp_mipi_frame_sem 0x6744
+#endif
+#define HIVE_SIZE_ia_css_rmgr_sp_mipi_frame_sem 60
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rmgr_sp_mipi_frame_sem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x66E8
+#else
+#define HIVE_ADDR_sp_ia_css_rmgr_sp_mipi_frame_sem 0x6744
+#endif
+#define HIVE_SIZE_sp_ia_css_rmgr_sp_mipi_frame_sem 60
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_dump: 62C8 */
+#else
+/* function ia_css_rmgr_sp_refcount_dump: 6403 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4BC8
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4C24
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4BC8
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 0x4C24
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_isp_parameters_id 20
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_pipe_threads
+#define HIVE_MEM_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_pipe_threads 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_pipe_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_pipe_threads 0x150
+#define HIVE_SIZE_sp_sp_pipe_threads 20
+
+#ifndef ISP2401
+/* function sp_event_proxy_func: 71B */
+#else
+/* function sp_event_proxy_func: 6AF */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_isys_event_queue_handle
+#define HIVE_MEM_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x4BDC
+#else
+#define HIVE_ADDR_host2sp_isys_event_queue_handle 0x4C38
+#endif
+#define HIVE_SIZE_host2sp_isys_event_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_isys_event_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x4BDC
+#else
+#define HIVE_ADDR_sp_host2sp_isys_event_queue_handle 0x4C38
+#endif
+#define HIVE_SIZE_sp_host2sp_isys_event_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_yield: 6A70 */
+#else
+/* function ia_css_thread_sp_yield: 6BEA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param_desc
+#define HIVE_MEM_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x474C
+#else
+#define HIVE_ADDR_sp_all_cbs_param_desc 0x4794
+#endif
+#define HIVE_SIZE_sp_all_cbs_param_desc 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param_desc scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x474C
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param_desc 0x4794
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param_desc 8
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb
+#define HIVE_MEM_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x5BF4
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb 0x5C50
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_sp_invalidate_tlb 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_sp_invalidate_tlb scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x5BF4
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_sp_invalidate_tlb 0x5C50
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_sp_invalidate_tlb 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_fork: D10 */
+#else
+/* function ia_css_thread_sp_fork: D05 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_destroy: 280D */
+#else
+/* function ia_css_tagger_sp_destroy: 285D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_read: 31D0 */
+#else
+/* function ia_css_dmaproxy_sp_vmem_read: 33AF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ifmtr_sp_init: 614F */
+#else
+/* function ia_css_ifmtr_sp_init: 628A */
+#endif
+
+#ifndef ISP2401
+/* function initialize_sp_group: 6D4 */
+#else
+/* function initialize_sp_group: 668 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_peek: 2932 */
+#else
+/* function ia_css_tagger_buf_sp_peek: 2AD4 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_init: D3C */
+#else
+/* function ia_css_thread_sp_init: D31 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_reset_exp_id: 60F6 */
+#else
+/* function ia_css_isys_sp_reset_exp_id: 6231 */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_update_fps: 65F0 */
+#else
+/* function qos_scheduler_update_fps: 6763 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 4637 */
+#else
+/* function ia_css_ispctrl_sp_set_stream_base_addr: 4892 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_DMEM_BASE
+#define HIVE_MEM_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_ISP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_DMEM_BASE 0x10
+#define HIVE_SIZE_sp_ISP_DMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_DMEM_BASE
+#define HIVE_MEM_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_SP_DMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_DMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_DMEM_BASE 0x4
+#define HIVE_SIZE_sp_SP_DMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read: 3246 */
+#else
+/* function __ia_css_queue_is_empty_text: 4B81 */
+
+/* function ia_css_dmaproxy_sp_read: 3425 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_raw_copy_line_count
+#define HIVE_MEM_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_raw_copy_line_count 0x2C8
+#else
+#define HIVE_ADDR_raw_copy_line_count 0x2E0
+#endif
+#define HIVE_SIZE_raw_copy_line_count 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_raw_copy_line_count scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_raw_copy_line_count 0x2C8
+#else
+#define HIVE_ADDR_sp_raw_copy_line_count 0x2E0
+#endif
+#define HIVE_SIZE_sp_raw_copy_line_count 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_tag_cmd_queue_handle
+#define HIVE_MEM_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x4BE8
+#else
+#define HIVE_ADDR_host2sp_tag_cmd_queue_handle 0x4C44
+#endif
+#define HIVE_SIZE_host2sp_tag_cmd_queue_handle 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_tag_cmd_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x4BE8
+#else
+#define HIVE_ADDR_sp_host2sp_tag_cmd_queue_handle 0x4C44
+#endif
+#define HIVE_SIZE_sp_host2sp_tag_cmd_queue_handle 12
+
+#ifndef ISP2401
+/* function ia_css_queue_peek: 493C */
+#else
+/* function ia_css_queue_peek: 4B9A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_frame_cnt
+#define HIVE_MEM_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x4A94
+#else
+#define HIVE_ADDR_ia_css_flash_sp_frame_cnt 0x4AF0
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_frame_cnt 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_frame_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x4A94
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_frame_cnt 0x4AF0
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_frame_cnt 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_can_send_token_mask
+#define HIVE_MEM_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_event_can_send_token_mask 0x88
+#define HIVE_SIZE_event_can_send_token_mask 44
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_can_send_token_mask scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_event_can_send_token_mask 0x88
+#define HIVE_SIZE_sp_event_can_send_token_mask 44
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_thread
+#define HIVE_MEM_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_thread 0x5F40
+#else
+#define HIVE_ADDR_isp_thread 0x5FA0
+#endif
+#define HIVE_SIZE_isp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_thread 0x5F40
+#else
+#define HIVE_ADDR_sp_isp_thread 0x5FA0
+#endif
+#define HIVE_SIZE_sp_isp_thread 4
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event_non_blocking: A78 */
+#else
+/* function encode_and_post_sp_event_non_blocking: A0C */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_destroy: 5DF8 */
+#else
+/* function ia_css_isys_sp_frontend_destroy: 5F33 */
+#endif
+
+/* function is_ddr_debug_buffer_full: 2CC */
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_stop: 5D38 */
+#else
+/* function ia_css_isys_sp_frontend_stop: 5E73 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_init: 6094 */
+#else
+/* function ia_css_isys_sp_token_map_init: 61CF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 2982 */
+#else
+/* function ia_css_tagger_buf_sp_get_oldest_marked_offset: 2B24 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_fiber
+#define HIVE_MEM_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_fiber 0x19C
+#define HIVE_SIZE_sp_threads_fiber 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_fiber scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_fiber 0x19C
+#define HIVE_SIZE_sp_sp_threads_fiber 28
+
+#ifndef ISP2401
+/* function encode_and_post_sp_event: A01 */
+#else
+/* function encode_and_post_sp_event: 995 */
+#endif
+
+/* function debug_enqueue_ddr: EE */
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 6283 */
+#else
+/* function ia_css_rmgr_sp_refcount_init_vbuf: 63BE */
+#endif
+
+#ifndef ISP2401
+/* function dmaproxy_sp_read_write: 6EE4 */
+#else
+/* function dmaproxy_sp_read_write: 7017 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer
+#define HIVE_MEM_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5BF8
+#else
+#define HIVE_ADDR_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5C54
+#endif
+#define HIVE_SIZE_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_dmaproxy_isp_dma_cmd_buffer scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5BF8
+#else
+#define HIVE_ADDR_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 0x5C54
+#endif
+#define HIVE_SIZE_sp_ia_css_dmaproxy_isp_dma_cmd_buffer 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host2sp_buffer_queue_handle
+#define HIVE_MEM_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x4BF4
+#else
+#define HIVE_ADDR_host2sp_buffer_queue_handle 0x4C50
+#endif
+#define HIVE_SIZE_host2sp_buffer_queue_handle 480
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host2sp_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x4BF4
+#else
+#define HIVE_ADDR_sp_host2sp_buffer_queue_handle 0x4C50
+#endif
+#define HIVE_SIZE_sp_host2sp_buffer_queue_handle 480
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_service
+#define HIVE_MEM_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3178
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_service 0x3198
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_service 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_service scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3178
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_service 0x3198
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_service 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_process: 6BF0 */
+#else
+/* function ia_css_dmaproxy_sp_process: 6D63 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_mark_from_end: 2C0A */
+#else
+/* function ia_css_tagger_buf_sp_mark_from_end: 2DAC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_rcv_acquire_ack: 5A05 */
+#else
+/* function ia_css_isys_sp_backend_rcv_acquire_ack: 5B40 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_pre_acquire_request: 5A1B */
+#else
+/* function ia_css_isys_sp_backend_pre_acquire_request: 5B56 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_cs: 366C */
+#else
+/* function ia_css_ispctrl_sp_init_cs: 386E */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_init: 5971 */
+#else
+/* function ia_css_spctrl_sp_init: 5AAC */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_init: 730 */
+#else
+/* function sp_event_proxy_init: 6C4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4DD4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4E30
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4DD4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 0x4E30
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_previous_clock_tick 40
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_output
+#define HIVE_MEM_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_output 0x41F8
+#else
+#define HIVE_ADDR_sp_output 0x4218
+#endif
+#define HIVE_SIZE_sp_output 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_output scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_output 0x41F8
+#else
+#define HIVE_ADDR_sp_sp_output 0x4218
+#endif
+#define HIVE_SIZE_sp_sp_output 16
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4DFC
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4E58
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4DFC
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 0x4E58
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_host2sp_buf_queues 800
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_CTRL_BASE
+#define HIVE_MEM_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_ISP_CTRL_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_CTRL_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_CTRL_BASE 0x8
+#define HIVE_SIZE_sp_ISP_CTRL_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_INPUT_FORMATTER_BASE
+#define HIVE_MEM_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_INPUT_FORMATTER_BASE 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_INPUT_FORMATTER_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_INPUT_FORMATTER_BASE 0x4C
+#define HIVE_SIZE_sp_INPUT_FORMATTER_BASE 16
+
+#ifndef ISP2401
+/* function sp_dma_proxy_reset_channels: 34A0 */
+#else
+/* function sp_dma_proxy_reset_channels: 3694 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_acquire: 5B26 */
+#else
+/* function ia_css_isys_sp_backend_acquire: 5C61 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_update_size: 2901 */
+#else
+/* function ia_css_tagger_sp_update_size: 2AA3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_host_sp_queue
+#define HIVE_MEM_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x511C
+#else
+#define HIVE_ADDR_ia_css_bufq_host_sp_queue 0x5178
+#endif
+#define HIVE_SIZE_ia_css_bufq_host_sp_queue 2008
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_host_sp_queue scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x511C
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_host_sp_queue 0x5178
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_host_sp_queue 2008
+
+#ifndef ISP2401
+/* function thread_fiber_sp_create: DA8 */
+#else
+/* function thread_fiber_sp_create: D9D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_increments: 3332 */
+#else
+/* function ia_css_dmaproxy_sp_set_increments: 3526 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_frame
+#define HIVE_MEM_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x4754
+#else
+#define HIVE_ADDR_sem_for_writing_cb_frame 0x479C
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_frame 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x4754
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_frame 0x479C
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_frame 20
+
+#ifndef ISP2401
+/* function receiver_reg_store: AD7 */
+#else
+/* function receiver_reg_store: AD1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_writing_cb_param
+#define HIVE_MEM_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_writing_cb_param 0x4768
+#else
+#define HIVE_ADDR_sem_for_writing_cb_param 0x47B0
+#endif
+#define HIVE_SIZE_sem_for_writing_cb_param 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_writing_cb_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x4768
+#else
+#define HIVE_ADDR_sp_sem_for_writing_cb_param 0x47B0
+#endif
+#define HIVE_SIZE_sp_sem_for_writing_cb_param 20
+
+/* function sp_start_isp_entry: 453 */
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifdef HIVE_ADDR_sp_start_isp_entry
+#endif
+#define HIVE_ADDR_sp_start_isp_entry 0x453
+#endif
+#define HIVE_ADDR_sp_sp_start_isp_entry 0x453
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_all: 2B8E */
+#else
+/* function ia_css_tagger_buf_sp_unmark_all: 2D30 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unmark_from_start: 2BCF */
+#else
+/* function ia_css_tagger_buf_sp_unmark_from_start: 2D71 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_acquire: 34CC */
+#else
+/* function ia_css_dmaproxy_sp_channel_acquire: 36C0 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_add_num_vbuf: 64CD */
+#else
+/* function ia_css_rmgr_sp_add_num_vbuf: 6608 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_create: 60DD */
+#else
+/* function ia_css_isys_sp_token_map_create: 6218 */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 319C */
+#else
+/* function __ia_css_dmaproxy_sp_wait_for_ack_text: 337B */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_acquire_buf_elem: 1FF4 */
+#else
+/* function ia_css_tagger_sp_acquire_buf_elem: 2044 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_is_dynamic_buffer: 3085 */
+#else
+/* function ia_css_bufq_sp_is_dynamic_buffer: 3227 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_group
+#define HIVE_MEM_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_group 0x4208
+#define HIVE_SIZE_sp_group 1144
+#else
+#define HIVE_ADDR_sp_group 0x4228
+#define HIVE_SIZE_sp_group 1184
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_group 0x4208
+#define HIVE_SIZE_sp_sp_group 1144
+#else
+#define HIVE_ADDR_sp_sp_group 0x4228
+#define HIVE_SIZE_sp_sp_group 1184
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_event_proxy_thread
+#define HIVE_MEM_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_proxy_thread 0x4954
+#define HIVE_SIZE_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_event_proxy_thread 0x49B0
+#define HIVE_SIZE_sp_event_proxy_thread 72
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_event_proxy_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x4954
+#define HIVE_SIZE_sp_sp_event_proxy_thread 68
+#else
+#define HIVE_ADDR_sp_sp_event_proxy_thread 0x49B0
+#define HIVE_SIZE_sp_sp_event_proxy_thread 72
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_kill: CD6 */
+#else
+/* function ia_css_thread_sp_kill: CCB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_create: 28BB */
+#else
+/* function ia_css_tagger_sp_create: 2A51 */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_acquire_dmem: 657A */
+#else
+/* function tmpmem_acquire_dmem: 66B5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_MMU_BASE
+#define HIVE_MEM_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_MMU_BASE 0x24
+#define HIVE_SIZE_MMU_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_MMU_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_MMU_BASE 0x24
+#define HIVE_SIZE_sp_MMU_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_channel_release: 34B8 */
+#else
+/* function ia_css_dmaproxy_sp_channel_release: 36AC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_is_idle: 3498 */
+#else
+/* function ia_css_dmaproxy_sp_is_idle: 368C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_qos_start
+#define HIVE_MEM_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_qos_start 0x477C
+#else
+#define HIVE_ADDR_sem_for_qos_start 0x47C4
+#endif
+#define HIVE_SIZE_sem_for_qos_start 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_qos_start scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_qos_start 0x477C
+#else
+#define HIVE_ADDR_sp_sem_for_qos_start 0x47C4
+#endif
+#define HIVE_SIZE_sp_sem_for_qos_start 20
+
+#ifndef ISP2401
+/* function isp_hmem_load: B55 */
+#else
+/* function isp_hmem_load: B4F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_release_buf_elem: 1FD0 */
+#else
+/* function ia_css_tagger_sp_release_buf_elem: 2020 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_send: 350E */
+#else
+/* function ia_css_eventq_sp_send: 3702 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_isys_sp_error_cnt
+#define HIVE_MEM_ia_css_isys_sp_error_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_isys_sp_error_cnt 0x62D4
+#else
+#define HIVE_ADDR_ia_css_isys_sp_error_cnt 0x6330
+#endif
+#define HIVE_SIZE_ia_css_isys_sp_error_cnt 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_isys_sp_error_cnt scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_isys_sp_error_cnt 0x62D4
+#else
+#define HIVE_ADDR_sp_ia_css_isys_sp_error_cnt 0x6330
+#endif
+#define HIVE_SIZE_sp_ia_css_isys_sp_error_cnt 16
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_unlock_from_start: 2ABE */
+#else
+/* function ia_css_tagger_buf_sp_unlock_from_start: 2C60 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_debug_buffer_ddr_address
+#define HIVE_MEM_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_debug_buffer_ddr_address 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_debug_buffer_ddr_address scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_debug_buffer_ddr_address 0xBC
+#define HIVE_SIZE_sp_debug_buffer_ddr_address 4
+
+#ifndef ISP2401
+/* function sp_isys_copy_request: 714 */
+#else
+/* function sp_isys_copy_request: 6A8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 635D */
+#else
+/* function ia_css_rmgr_sp_refcount_retain_vbuf: 6498 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_set_priority: CCE */
+#else
+/* function ia_css_thread_sp_set_priority: CC3 */
+#endif
+
+#ifndef ISP2401
+/* function sizeof_hmem: BFC */
+#else
+/* function sizeof_hmem: BF6 */
+#endif
+
+#ifndef ISP2401
+/* function tmpmem_release_dmem: 6569 */
+#else
+/* function tmpmem_release_dmem: 66A4 */
+#endif
+
+/* function cnd_input_system_cfg: 392 */
+
+#ifndef ISP2401
+/* function __ia_css_sp_rawcopy_func_critical: 6F65 */
+#else
+/* function __ia_css_sp_rawcopy_func_critical: 70C2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_width_exception: 331D */
+#else
+/* function __ia_css_dmaproxy_sp_process_text: 331F */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_assert: 8B1 */
+#else
+/* function ia_css_dmaproxy_sp_set_width_exception: 3511 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_flash_sp_init_internal_params: 2CA9 */
+#else
+/* function sp_event_assert: 845 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 29C4 */
+#else
+/* function ia_css_flash_sp_init_internal_params: 2E4B */
+#endif
+
+#ifndef ISP2401
+/* function __modu: 68BB */
+#else
+/* function ia_css_tagger_buf_sp_pop_unmarked_and_unlocked: 2B66 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_isp_vector: 31A2 */
+#else
+/* function __modu: 6A2E */
+
+/* function ia_css_dmaproxy_sp_init_isp_vector: 3381 */
+#endif
+
+/* function isp_vamem_store: 0 */
+
+#ifdef ISP2401
+/* function ia_css_tagger_sp_set_copy_pipe: 2A48 */
+
+#endif
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GDC_BASE
+#define HIVE_MEM_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GDC_BASE 0x44
+#define HIVE_SIZE_GDC_BASE 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GDC_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GDC_BASE 0x44
+#define HIVE_SIZE_sp_GDC_BASE 8
+
+#ifndef ISP2401
+/* function ia_css_queue_local_init: 4C27 */
+#else
+/* function ia_css_queue_local_init: 4E85 */
+#endif
+
+#ifndef ISP2401
+/* function sp_event_proxy_callout_func: 6988 */
+#else
+/* function sp_event_proxy_callout_func: 6AFB */
+#endif
+
+#ifndef ISP2401
+/* function qos_scheduler_schedule_stage: 65C1 */
+#else
+/* function qos_scheduler_schedule_stage: 670F */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_thread_sp_num_ready_threads
+#define HIVE_MEM_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x49E0
+#else
+#define HIVE_ADDR_ia_css_thread_sp_num_ready_threads 0x4A40
+#endif
+#define HIVE_SIZE_ia_css_thread_sp_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_thread_sp_num_ready_threads scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x49E0
+#else
+#define HIVE_ADDR_sp_ia_css_thread_sp_num_ready_threads 0x4A40
+#endif
+#define HIVE_SIZE_sp_ia_css_thread_sp_num_ready_threads 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_threads_stack_size
+#define HIVE_MEM_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_threads_stack_size 0x180
+#define HIVE_SIZE_sp_threads_stack_size 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_threads_stack_size scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_threads_stack_size 0x180
+#define HIVE_SIZE_sp_sp_threads_stack_size 28
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 3F48 */
+#else
+/* function ia_css_ispctrl_sp_isp_done_row_striping: 418B */
+#endif
+
+#ifndef ISP2401
+/* function __ia_css_isys_sp_isr_text: 5E22 */
+#else
+/* function __ia_css_isys_sp_isr_text: 5F5D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_dequeue: 4AA5 */
+#else
+/* function ia_css_queue_dequeue: 4D03 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_configure_channel: 6E4C */
+#else
+/* function is_qos_standalone_mode: 66EA */
+
+/* function ia_css_dmaproxy_sp_configure_channel: 6F90 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_current_thread_fiber_sp
+#define HIVE_MEM_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_current_thread_fiber_sp 0x49E8
+#else
+#define HIVE_ADDR_current_thread_fiber_sp 0x4A44
+#endif
+#define HIVE_SIZE_current_thread_fiber_sp 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_current_thread_fiber_sp scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x49E8
+#else
+#define HIVE_ADDR_sp_current_thread_fiber_sp 0x4A44
+#endif
+#define HIVE_SIZE_sp_current_thread_fiber_sp 4
+
+#ifndef ISP2401
+/* function ia_css_circbuf_pop: FD8 */
+#else
+/* function ia_css_circbuf_pop: FCD */
+#endif
+
+#ifndef ISP2401
+/* function memset: 693A */
+#else
+/* function memset: 6AAD */
+#endif
+
+/* function irq_raise_set_token: B6 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_GPIO_BASE
+#define HIVE_MEM_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_GPIO_BASE 0x3C
+#define HIVE_SIZE_GPIO_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_GPIO_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_GPIO_BASE 0x3C
+#define HIVE_SIZE_sp_GPIO_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_pipeline_acc_stage_enable: 17F0 */
+#else
+/* function ia_css_pipeline_acc_stage_enable: 1818 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_unlock_exp_id: 2041 */
+#else
+/* function ia_css_tagger_sp_unlock_exp_id: 2091 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_ph
+#define HIVE_MEM_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_ph 0x62E4
+#else
+#define HIVE_ADDR_isp_ph 0x6340
+#endif
+#define HIVE_SIZE_isp_ph 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_ph scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_ph 0x62E4
+#else
+#define HIVE_ADDR_sp_isp_ph 0x6340
+#endif
+#define HIVE_SIZE_sp_isp_ph 28
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_flush: 6022 */
+#else
+/* function ia_css_isys_sp_token_map_flush: 615D */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_ds: 37CB */
+#else
+/* function ia_css_ispctrl_sp_init_ds: 39FA */
+#endif
+
+#ifndef ISP2401
+/* function get_xmem_base_addr_raw: 3B78 */
+#else
+/* function get_xmem_base_addr_raw: 3DB3 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_param
+#define HIVE_MEM_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_param 0x4790
+#else
+#define HIVE_ADDR_sp_all_cbs_param 0x47D8
+#endif
+#define HIVE_SIZE_sp_all_cbs_param 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_param scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x4790
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_param 0x47D8
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_param 16
+
+#ifndef ISP2401
+/* function ia_css_circbuf_create: 1026 */
+#else
+/* function ia_css_circbuf_create: 101B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_sp_group
+#define HIVE_MEM_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_sp_group 0x47A0
+#else
+#define HIVE_ADDR_sem_for_sp_group 0x47E8
+#endif
+#define HIVE_SIZE_sem_for_sp_group 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_sp_group scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_sp_group 0x47A0
+#else
+#define HIVE_ADDR_sp_sem_for_sp_group 0x47E8
+#endif
+#define HIVE_SIZE_sp_sem_for_sp_group 20
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_wait_for_in_frame: 64F8 */
+#else
+/* function __ia_css_dmaproxy_sp_configure_channel_text: 34F0 */
+
+/* function ia_css_framebuf_sp_wait_for_in_frame: 6633 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_tag_frame: 5588 */
+#else
+/* function ia_css_sp_rawcopy_tag_frame: 57C9 */
+#endif
+
+#ifndef ISP2401
+/* function isp_hmem_clear: B25 */
+#else
+/* function isp_hmem_clear: B1F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_framebuf_sp_release_in_frame: 653B */
+#else
+/* function ia_css_framebuf_sp_release_in_frame: 6676 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_snd_acquire_request: 5A78 */
+#else
+/* function ia_css_isys_sp_backend_snd_acquire_request: 5BB3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_is_full: 5EA9 */
+#else
+/* function ia_css_isys_sp_token_map_is_full: 5FE4 */
+#endif
+
+#ifndef ISP2401
+/* function input_system_acquisition_run: AF9 */
+#else
+/* function input_system_acquisition_run: AF3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_start_binary: 364A */
+#else
+/* function ia_css_ispctrl_sp_start_binary: 384C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs
+#define HIVE_MEM_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x58F4
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x5950
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x58F4
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 0x5950
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_h_pipe_private_ddr_ptrs 20
+
+#ifndef ISP2401
+/* function ia_css_eventq_sp_recv: 34E0 */
+#else
+/* function ia_css_eventq_sp_recv: 36D4 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_pool
+#define HIVE_MEM_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_pool 0x2E8
+#else
+#define HIVE_ADDR_isp_pool 0x300
+#endif
+#define HIVE_SIZE_isp_pool 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_pool scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_pool 0x2E8
+#else
+#define HIVE_ADDR_sp_isp_pool 0x300
+#endif
+#define HIVE_SIZE_sp_isp_pool 4
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_rel_gen: 622A */
+#else
+/* function ia_css_rmgr_sp_rel_gen: 6365 */
+
+/* function ia_css_tagger_sp_unblock_clients: 2919 */
+#endif
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_end: 1FC0 */
+#else
+/* function css_get_frame_processing_time_end: 2010 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_event_any_pending_mask
+#define HIVE_MEM_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_event_any_pending_mask 0x300
+#else
+#define HIVE_ADDR_event_any_pending_mask 0x318
+#endif
+#define HIVE_SIZE_event_any_pending_mask 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_event_any_pending_mask scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_event_any_pending_mask 0x300
+#else
+#define HIVE_ADDR_sp_event_any_pending_mask 0x318
+#endif
+#define HIVE_SIZE_sp_event_any_pending_mask 8
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_push: 5A2F */
+#else
+/* function ia_css_isys_sp_backend_push: 5B6A */
+#endif
+
+/* function sh_css_decode_tag_descr: 352 */
+
+/* function debug_enqueue_isp: 27B */
+
+#ifndef ISP2401
+/* function qos_scheduler_update_stage_budget: 65AF */
+#else
+/* function qos_scheduler_update_stage_budget: 66F2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_uninit: 596A */
+#else
+/* function ia_css_spctrl_sp_uninit: 5AA5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SWITCH_CODE
+#define HIVE_MEM_HIVE_IF_SWITCH_CODE scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SWITCH_CODE 0x1D8
+#define HIVE_SIZE_HIVE_IF_SWITCH_CODE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SWITCH_CODE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SWITCH_CODE 0x1D8
+#define HIVE_SIZE_sp_HIVE_IF_SWITCH_CODE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x5908
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_dis_bufs 0x5964
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_dis_bufs 140
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_dis_bufs scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x5908
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_dis_bufs 0x5964
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_dis_bufs 140
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_lock_from_start: 2AF2 */
+#else
+/* function ia_css_tagger_buf_sp_lock_from_start: 2C94 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_isp_idle
+#define HIVE_MEM_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_isp_idle 0x47B4
+#else
+#define HIVE_ADDR_sem_for_isp_idle 0x47FC
+#endif
+#define HIVE_SIZE_sem_for_isp_idle 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_isp_idle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x47B4
+#else
+#define HIVE_ADDR_sp_sem_for_isp_idle 0x47FC
+#endif
+#define HIVE_SIZE_sp_sem_for_isp_idle 20
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write_byte_addr: 31FF */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr: 33DE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init: 3176 */
+#else
+/* function ia_css_dmaproxy_sp_init: 3355 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 2D7B */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf_clock_tick: 2F1D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_VAMEM_BASE
+#define HIVE_MEM_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_ISP_VAMEM_BASE 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_VAMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_VAMEM_BASE 0x14
+#define HIVE_SIZE_sp_ISP_VAMEM_BASE 12
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_rawcopy_sp_tagger
+#define HIVE_MEM_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x6294
+#else
+#define HIVE_ADDR_ia_css_rawcopy_sp_tagger 0x62F0
+#endif
+#define HIVE_SIZE_ia_css_rawcopy_sp_tagger 24
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_rawcopy_sp_tagger scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x6294
+#else
+#define HIVE_ADDR_sp_ia_css_rawcopy_sp_tagger 0x62F0
+#endif
+#define HIVE_SIZE_sp_ia_css_rawcopy_sp_tagger 24
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x5994
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_exp_ids 0x59F0
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_exp_ids 70
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_exp_ids scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x5994
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_exp_ids 0x59F0
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_exp_ids 70
+
+#ifndef ISP2401
+/* function ia_css_queue_item_load: 4D19 */
+#else
+/* function ia_css_queue_item_load: 4F77 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_spctrl_sp_get_state: 5955 */
+#else
+/* function ia_css_spctrl_sp_get_state: 5A90 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_uninit: 603F */
+#else
+/* function ia_css_isys_sp_token_map_uninit: 617A */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_callout_sp_thread
+#define HIVE_MEM_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_callout_sp_thread 0x49DC
+#else
+#define HIVE_ADDR_callout_sp_thread 0x1E0
+#endif
+#define HIVE_SIZE_callout_sp_thread 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_callout_sp_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_callout_sp_thread 0x49DC
+#else
+#define HIVE_ADDR_sp_callout_sp_thread 0x1E0
+#endif
+#define HIVE_SIZE_sp_callout_sp_thread 4
+
+#ifndef ISP2401
+/* function thread_fiber_sp_init: E2F */
+#else
+/* function thread_fiber_sp_init: E24 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_SP_PMEM_BASE
+#define HIVE_MEM_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_SP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_SP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_SP_PMEM_BASE 0x0
+#define HIVE_SIZE_sp_SP_PMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_snd_acquire_req: 5FAF */
+#else
+/* function ia_css_isys_sp_token_map_snd_acquire_req: 60EA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_input_stream_format
+#define HIVE_MEM_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_input_stream_format 0x40F8
+#else
+#define HIVE_ADDR_sp_isp_input_stream_format 0x4118
+#endif
+#define HIVE_SIZE_sp_isp_input_stream_format 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_input_stream_format scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x40F8
+#else
+#define HIVE_ADDR_sp_sp_isp_input_stream_format 0x4118
+#endif
+#define HIVE_SIZE_sp_sp_isp_input_stream_format 20
+
+#ifndef ISP2401
+/* function __mod: 68A7 */
+#else
+/* function __mod: 6A1A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 3260 */
+#else
+/* function ia_css_dmaproxy_sp_init_dmem_channel: 343F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_join: CFF */
+#else
+/* function ia_css_thread_sp_join: CF4 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_add_command: 6F4F */
+#else
+/* function ia_css_dmaproxy_sp_add_command: 7082 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_thread_func: 5809 */
+#else
+/* function ia_css_sp_metadata_thread_func: 5968 */
+#endif
+
+#ifndef ISP2401
+/* function __sp_event_proxy_func_critical: 6975 */
+#else
+/* function __sp_event_proxy_func_critical: 6AE8 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_metadata_wait: 591C */
+#else
+/* function ia_css_sp_metadata_wait: 5A57 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_peek_from_start: F08 */
+#else
+/* function ia_css_circbuf_peek_from_start: EFD */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_event_sp_encode: 356B */
+#else
+/* function ia_css_event_sp_encode: 375F */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_thread_sp_run: D72 */
+#else
+/* function ia_css_thread_sp_run: D67 */
+#endif
+
+#ifndef ISP2401
+/* function sp_isys_copy_func: 6F6 */
+#else
+/* function sp_isys_copy_func: 68A */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_flush: 5A98 */
+#else
+/* function ia_css_isys_sp_backend_flush: 5BD3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_frame_exists: 59B4 */
+#else
+/* function ia_css_isys_sp_backend_frame_exists: 5AEF */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_init_isp_memories: 47A2 */
+#else
+/* function ia_css_sp_isp_param_init_isp_memories: 4A2A */
+#endif
+
+#ifndef ISP2401
+/* function register_isr: 8A9 */
+#else
+/* function register_isr: 83D */
+#endif
+
+/* function irq_raise: C8 */
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 313D */
+#else
+/* function ia_css_dmaproxy_sp_mmu_invalidate: 32E5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_HIVE_IF_SRST_ADDRESS
+#define HIVE_MEM_HIVE_IF_SRST_ADDRESS scalar_processor_2400_dmem
+#define HIVE_ADDR_HIVE_IF_SRST_ADDRESS 0x1B8
+#define HIVE_SIZE_HIVE_IF_SRST_ADDRESS 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_HIVE_IF_SRST_ADDRESS scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_HIVE_IF_SRST_ADDRESS 0x1B8
+#define HIVE_SIZE_sp_HIVE_IF_SRST_ADDRESS 16
+
+#ifndef ISP2401
+/* function pipeline_sp_initialize_stage: 1924 */
+#else
+/* function pipeline_sp_initialize_stage: 195E */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_isys_sp_frontend_states
+#define HIVE_MEM_ia_css_isys_sp_frontend_states scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_isys_sp_frontend_states 0x62C8
+#else
+#define HIVE_ADDR_ia_css_isys_sp_frontend_states 0x6324
+#endif
+#define HIVE_SIZE_ia_css_isys_sp_frontend_states 12
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_isys_sp_frontend_states scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_isys_sp_frontend_states 0x62C8
+#else
+#define HIVE_ADDR_sp_ia_css_isys_sp_frontend_states 0x6324
+#endif
+#define HIVE_SIZE_sp_ia_css_isys_sp_frontend_states 12
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 6E1E */
+#else
+/* function ia_css_dmaproxy_sp_read_byte_addr_mmio: 6F62 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_done_ds: 37B2 */
+#else
+/* function ia_css_ispctrl_sp_done_ds: 39E1 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_get_mem_inits: 477D */
+#else
+/* function ia_css_sp_isp_param_get_mem_inits: 4A05 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_init_buffer_queues: 13D0 */
+#else
+/* function ia_css_parambuf_sp_init_buffer_queues: 13F1 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_pfp_spref
+#define HIVE_MEM_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_pfp_spref 0x2F0
+#else
+#define HIVE_ADDR_vbuf_pfp_spref 0x308
+#endif
+#define HIVE_SIZE_vbuf_pfp_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_pfp_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x2F0
+#else
+#define HIVE_ADDR_sp_vbuf_pfp_spref 0x308
+#endif
+#define HIVE_SIZE_sp_vbuf_pfp_spref 4
+
+#ifndef ISP2401
+/* function input_system_cfg: ABB */
+#else
+/* function input_system_cfg: AB5 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_HMEM_BASE
+#define HIVE_MEM_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_ISP_HMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_HMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_HMEM_BASE 0x20
+#define HIVE_SIZE_sp_ISP_HMEM_BASE 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_pipe_private_frames
+#define HIVE_MEM_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x59DC
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_pipe_private_frames 0x5A38
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_pipe_private_frames 280
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_pipe_private_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x59DC
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_pipe_private_frames 0x5A38
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_pipe_private_frames 280
+
+#ifndef ISP2401
+/* function qos_scheduler_init_stage_budget: 65E8 */
+#else
+/* function qos_scheduler_init_stage_budget: 6750 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_release: 5B0D */
+#else
+/* function ia_css_isys_sp_backend_release: 5C48 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_backend_destroy: 5B37 */
+#else
+/* function ia_css_isys_sp_backend_destroy: 5C72 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp2host_buffer_queue_handle
+#define HIVE_MEM_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x5AF4
+#else
+#define HIVE_ADDR_sp2host_buffer_queue_handle 0x5B50
+#endif
+#define HIVE_SIZE_sp2host_buffer_queue_handle 96
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp2host_buffer_queue_handle scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x5AF4
+#else
+#define HIVE_ADDR_sp_sp2host_buffer_queue_handle 0x5B50
+#endif
+#define HIVE_SIZE_sp_sp2host_buffer_queue_handle 96
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_check_mipi_frame_size: 5F73 */
+#else
+/* function ia_css_isys_sp_token_map_check_mipi_frame_size: 60AE */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_ispctrl_sp_init_isp_vars: 449C */
+#else
+/* function ia_css_ispctrl_sp_init_isp_vars: 46F7 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_has_empty_mipi_buffer_cb: 5B89 */
+#else
+/* function ia_css_isys_sp_frontend_has_empty_mipi_buffer_cb: 5CC4 */
+#endif
+
+#ifndef ISP2401
+/* function sp_warning: 8DC */
+#else
+/* function sp_warning: 870 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_vbuf_enqueue: 631D */
+#else
+/* function ia_css_rmgr_sp_vbuf_enqueue: 6458 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_sp_tag_exp_id: 215B */
+#else
+/* function ia_css_tagger_sp_tag_exp_id: 21AB */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_write: 3216 */
+#else
+/* function ia_css_dmaproxy_sp_write: 33F5 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_parambuf_sp_release_in_param: 1250 */
+#else
+/* function ia_css_parambuf_sp_release_in_param: 1245 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_irq_sw_interrupt_token
+#define HIVE_MEM_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_irq_sw_interrupt_token 0x40F4
+#else
+#define HIVE_ADDR_irq_sw_interrupt_token 0x4114
+#endif
+#define HIVE_SIZE_irq_sw_interrupt_token 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_irq_sw_interrupt_token scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x40F4
+#else
+#define HIVE_ADDR_sp_irq_sw_interrupt_token 0x4114
+#endif
+#define HIVE_SIZE_sp_irq_sw_interrupt_token 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_isp_addresses
+#define HIVE_MEM_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_addresses 0x5F44
+#else
+#define HIVE_ADDR_sp_isp_addresses 0x5FA4
+#endif
+#define HIVE_SIZE_sp_isp_addresses 172
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_isp_addresses scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_isp_addresses 0x5F44
+#else
+#define HIVE_ADDR_sp_sp_isp_addresses 0x5FA4
+#endif
+#define HIVE_SIZE_sp_sp_isp_addresses 172
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_acq_gen: 6242 */
+#else
+/* function ia_css_rmgr_sp_acq_gen: 637D */
+#endif
+
+#ifndef ISP2401
+/* function receiver_reg_load: AD0 */
+#else
+/* function receiver_reg_load: ACA */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isps
+#define HIVE_MEM_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isps 0x6300
+#else
+#define HIVE_ADDR_isps 0x635C
+#endif
+#define HIVE_SIZE_isps 28
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isps scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isps 0x6300
+#else
+#define HIVE_ADDR_sp_isps 0x635C
+#endif
+#define HIVE_SIZE_sp_isps 28
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_host_sp_queues_initialized
+#define HIVE_MEM_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_host_sp_queues_initialized 0x410C
+#else
+#define HIVE_ADDR_host_sp_queues_initialized 0x412C
+#endif
+#define HIVE_SIZE_host_sp_queues_initialized 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_host_sp_queues_initialized scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x410C
+#else
+#define HIVE_ADDR_sp_host_sp_queues_initialized 0x412C
+#endif
+#define HIVE_SIZE_sp_host_sp_queues_initialized 4
+
+#ifndef ISP2401
+/* function ia_css_queue_uninit: 4BE5 */
+#else
+/* function ia_css_queue_uninit: 4E43 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_ispctrl_sp_isp_started
+#define HIVE_MEM_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x5BFC
+#else
+#define HIVE_ADDR_ia_css_ispctrl_sp_isp_started 0x5C58
+#endif
+#define HIVE_SIZE_ia_css_ispctrl_sp_isp_started 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_ispctrl_sp_isp_started scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x5BFC
+#else
+#define HIVE_ADDR_sp_ia_css_ispctrl_sp_isp_started 0x5C58
+#endif
+#define HIVE_SIZE_sp_ia_css_ispctrl_sp_isp_started 4
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_release_dynamic_buf: 2DE7 */
+#else
+/* function ia_css_bufq_sp_release_dynamic_buf: 2F89 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_set_height_exception: 330E */
+#else
+/* function ia_css_dmaproxy_sp_set_height_exception: 3502 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 3293 */
+#else
+/* function ia_css_dmaproxy_sp_init_vmem_channel: 3473 */
+#endif
+
+#ifndef ISP2401
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_num_ready_threads
+#define HIVE_MEM_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_num_ready_threads 0x49E4
+#define HIVE_SIZE_num_ready_threads 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_num_ready_threads scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_num_ready_threads 0x49E4
+#define HIVE_SIZE_sp_num_ready_threads 4
+
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 31E8 */
+#else
+/* function ia_css_dmaproxy_sp_write_byte_addr_mmio: 33C7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_vbuf_spref
+#define HIVE_MEM_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_vbuf_spref 0x2EC
+#else
+#define HIVE_ADDR_vbuf_spref 0x304
+#endif
+#define HIVE_SIZE_vbuf_spref 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_vbuf_spref scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_vbuf_spref 0x2EC
+#else
+#define HIVE_ADDR_sp_vbuf_spref 0x304
+#endif
+#define HIVE_SIZE_sp_vbuf_spref 4
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_metadata_thread
+#define HIVE_MEM_sp_metadata_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_metadata_thread 0x4998
+#define HIVE_SIZE_sp_metadata_thread 68
+#else
+#define HIVE_ADDR_sp_metadata_thread 0x49F8
+#define HIVE_SIZE_sp_metadata_thread 72
+#endif
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_metadata_thread scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_metadata_thread 0x4998
+#define HIVE_SIZE_sp_sp_metadata_thread 68
+#else
+#define HIVE_ADDR_sp_sp_metadata_thread 0x49F8
+#define HIVE_SIZE_sp_sp_metadata_thread 72
+#endif
+
+#ifndef ISP2401
+/* function ia_css_queue_enqueue: 4B2F */
+#else
+/* function ia_css_queue_enqueue: 4D8D */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_request
+#define HIVE_MEM_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_request 0x4A98
+#else
+#define HIVE_ADDR_ia_css_flash_sp_request 0x4AF4
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_request 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_request scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x4A98
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_request 0x4AF4
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_request 4
+
+#ifndef ISP2401
+/* function ia_css_dmaproxy_sp_vmem_write: 31B9 */
+#else
+/* function ia_css_dmaproxy_sp_vmem_write: 3398 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_tagger_frames
+#define HIVE_MEM_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_tagger_frames 0x49EC
+#else
+#define HIVE_ADDR_tagger_frames 0x4A48
+#endif
+#define HIVE_SIZE_tagger_frames 168
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_tagger_frames scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_tagger_frames 0x49EC
+#else
+#define HIVE_ADDR_sp_tagger_frames 0x4A48
+#endif
+#define HIVE_SIZE_sp_tagger_frames 168
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_token_map_snd_capture_req: 5FD1 */
+#else
+/* function ia_css_isys_sp_token_map_snd_capture_req: 610C */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_reading_if
+#define HIVE_MEM_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_reading_if 0x47C8
+#else
+#define HIVE_ADDR_sem_for_reading_if 0x4810
+#endif
+#define HIVE_SIZE_sem_for_reading_if 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_reading_if scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_reading_if 0x47C8
+#else
+#define HIVE_ADDR_sp_sem_for_reading_if 0x4810
+#endif
+#define HIVE_SIZE_sp_sem_for_reading_if 20
+
+#ifndef ISP2401
+/* function sp_generate_interrupts: 95B */
+#else
+/* function sp_generate_interrupts: 8EF */
+
+/* function ia_css_pipeline_sp_start: 1871 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_pipeline_sp_start: 1837 */
+#else
+/* function ia_css_thread_default_callout: 6BE3 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_sp_rawcopy_init: 510C */
+#else
+/* function ia_css_sp_rawcopy_init: 536A */
+#endif
+
+#ifndef ISP2401
+/* function tmr_clock_read: 13F1 */
+#else
+/* function tmr_clock_read: 1412 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_BAMEM_BASE
+#define HIVE_MEM_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x2F8
+#else
+#define HIVE_ADDR_ISP_BAMEM_BASE 0x310
+#endif
+#define HIVE_SIZE_ISP_BAMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_BAMEM_BASE scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x2F8
+#else
+#define HIVE_ADDR_sp_ISP_BAMEM_BASE 0x310
+#endif
+#define HIVE_SIZE_sp_ISP_BAMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_rcv_capture_ack: 5C38 */
+#else
+/* function ia_css_isys_sp_frontend_rcv_capture_ack: 5D73 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues
+#define HIVE_MEM_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5B54
+#else
+#define HIVE_ADDR_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5BB0
+#endif
+#define HIVE_SIZE_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5B54
+#else
+#define HIVE_ADDR_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 0x5BB0
+#endif
+#define HIVE_SIZE_sp_ia_css_bufq_sp_sems_for_sp2host_buf_queues 160
+
+#ifndef ISP2401
+/* function css_get_frame_processing_time_start: 1FC8 */
+#else
+/* function css_get_frame_processing_time_start: 2018 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_all_cbs_frame
+#define HIVE_MEM_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_all_cbs_frame 0x47DC
+#else
+#define HIVE_ADDR_sp_all_cbs_frame 0x4824
+#endif
+#define HIVE_SIZE_sp_all_cbs_frame 16
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_all_cbs_frame scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x47DC
+#else
+#define HIVE_ADDR_sp_sp_all_cbs_frame 0x4824
+#endif
+#define HIVE_SIZE_sp_sp_all_cbs_frame 16
+
+#ifndef ISP2401
+/* function thread_sp_queue_print: D8F */
+#else
+/* function thread_sp_queue_print: D84 */
+#endif
+
+#ifndef ISP2401
+/* function sp_notify_eof: 907 */
+#else
+/* function sp_notify_eof: 89B */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sem_for_str2mem
+#define HIVE_MEM_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sem_for_str2mem 0x47EC
+#else
+#define HIVE_ADDR_sem_for_str2mem 0x4834
+#endif
+#define HIVE_SIZE_sem_for_str2mem 20
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sem_for_str2mem scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sem_for_str2mem 0x47EC
+#else
+#define HIVE_ADDR_sp_sem_for_str2mem 0x4834
+#endif
+#define HIVE_SIZE_sp_sem_for_str2mem 20
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 2B5A */
+#else
+/* function ia_css_tagger_buf_sp_is_marked_from_start: 2CFC */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 2F9F */
+#else
+/* function ia_css_bufq_sp_acquire_dynamic_buf: 3141 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_circbuf_destroy: 101D */
+#else
+/* function ia_css_circbuf_destroy: 1012 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ISP_PMEM_BASE
+#define HIVE_MEM_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_ISP_PMEM_BASE 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ISP_PMEM_BASE scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_ISP_PMEM_BASE 0xC
+#define HIVE_SIZE_sp_ISP_PMEM_BASE 4
+
+#ifndef ISP2401
+/* function ia_css_sp_isp_param_mem_load: 4710 */
+#else
+/* function ia_css_sp_isp_param_mem_load: 4998 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_pop_from_start: 2946 */
+#else
+/* function ia_css_tagger_buf_sp_pop_from_start: 2AE8 */
+#endif
+
+#ifndef ISP2401
+/* function __div: 685F */
+#else
+/* function __div: 69D2 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_isys_sp_frontend_create: 5E09 */
+#else
+/* function ia_css_isys_sp_frontend_create: 5F44 */
+#endif
+
+#ifndef ISP2401
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 633C */
+#else
+/* function ia_css_rmgr_sp_refcount_release_vbuf: 6477 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_ia_css_flash_sp_in_use
+#define HIVE_MEM_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x4A9C
+#else
+#define HIVE_ADDR_ia_css_flash_sp_in_use 0x4AF8
+#endif
+#define HIVE_SIZE_ia_css_flash_sp_in_use 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_ia_css_flash_sp_in_use scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x4A9C
+#else
+#define HIVE_ADDR_sp_ia_css_flash_sp_in_use 0x4AF8
+#endif
+#define HIVE_SIZE_sp_ia_css_flash_sp_in_use 4
+
+#ifndef ISP2401
+/* function ia_css_thread_sem_sp_wait: 6B42 */
+#else
+/* function ia_css_thread_sem_sp_wait: 6CB7 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_sleep_mode
+#define HIVE_MEM_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sleep_mode 0x4110
+#else
+#define HIVE_ADDR_sp_sleep_mode 0x4130
+#endif
+#define HIVE_SIZE_sp_sleep_mode 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_sleep_mode scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_sp_sleep_mode 0x4110
+#else
+#define HIVE_ADDR_sp_sp_sleep_mode 0x4130
+#endif
+#define HIVE_SIZE_sp_sp_sleep_mode 4
+
+#ifndef ISP2401
+/* function ia_css_tagger_buf_sp_push: 2A55 */
+#else
+/* function ia_css_tagger_buf_sp_push: 2BF7 */
+#endif
+
+/* function mmu_invalidate_cache: D3 */
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_sp_max_cb_elems
+#define HIVE_MEM_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_max_cb_elems 8
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_sp_max_cb_elems scalar_processor_2400_dmem
+#define HIVE_ADDR_sp_sp_max_cb_elems 0x148
+#define HIVE_SIZE_sp_sp_max_cb_elems 8
+
+#ifndef ISP2401
+/* function ia_css_queue_remote_init: 4C07 */
+#else
+/* function ia_css_queue_remote_init: 4E65 */
+#endif
+
+#ifndef HIVE_MULTIPLE_PROGRAMS
+#ifndef HIVE_MEM_isp_stop_req
+#define HIVE_MEM_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_isp_stop_req 0x4680
+#else
+#define HIVE_ADDR_isp_stop_req 0x46C8
+#endif
+#define HIVE_SIZE_isp_stop_req 4
+#else
+#endif
+#endif
+#define HIVE_MEM_sp_isp_stop_req scalar_processor_2400_dmem
+#ifndef ISP2401
+#define HIVE_ADDR_sp_isp_stop_req 0x4680
+#else
+#define HIVE_ADDR_sp_isp_stop_req 0x46C8
+#endif
+#define HIVE_SIZE_sp_isp_stop_req 4
+
+#ifndef ISP2401
+#define HIVE_ICACHE_sp_critical_SEGMENT_START 0
+#define HIVE_ICACHE_sp_critical_NUM_SEGMENTS  1
+#endif
+
+#endif /* _sp_map_h_ */
+#ifndef ISP2401
+extern void sh_css_dump_sp_dmem(void);
+void sh_css_dump_sp_dmem(void)
+{
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_api_version.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_api_version.h
new file mode 100644
index 000000000000..1f6a55ff5db8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_api_version.h
@@ -0,0 +1,673 @@
+/*
+#ifndef ISP2401
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+Support for Intel Camera Imaging ISP subsystem.
+Copyright (c) 2010 - 2015, Intel Corporation.
+#endif
+
+#ifdef ISP2401
+This program is free software; you can redistribute it and/or modify it
+under the terms and conditions of the GNU General Public License,
+version 2, as published by the Free Software Foundation.
+
+This program is distributed in the hope it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+more details.
+*/
+#endif
+#ifndef __CSS_API_VERSION_H
+#define __CSS_API_VERSION_H
+
+/** @file
+ * CSS API version file. This file contains the version number of the CSS-API.
+ *
+ * This file is generated from a set of input files describing the CSS-API
+ * changes. Don't edit this file directly.
+ */
+
+
+/**
+
+The version string has four dot-separated numbers, read left to right:
+  The first two are the API version, and should not be changed.
+  The third number is incremented by a CSS firmware developer when the
+    API change is not backwards compatible.
+  The fourth number is incremented by the a CSS firmware developer for
+    every API change.
+    It should be zeroed when the third number changes.
+
+*/
+
+#ifndef ISP2401
+#define CSS_API_VERSION_STRING	"2.1.15.3"
+#else
+#define CSS_API_VERSION_STRING	"2.1.20.9"
+#endif
+
+/*
+Change log
+
+v2.0.1.0, initial version:
+- added API versioning
+
+v2.0.1.1, activate CSS-API versioning:
+- added description of major and minor version numbers
+
+v2.0.1.2, modified struct ia_css_frame_info:
+- added new member ia_css_crop_info
+
+v2.0.1.3, added IA_CSS_ERR_NOT_SUPPORTED
+
+v2.1.0.0
+- moved version number to 2.1.0.0
+- created new files for refactoring the code
+
+v2.1.1.0, modified struct ia_css_pipe_config and struct ia_css_pipe_info and struct ia_css_pipe:
+- use array to handle multiple output ports
+
+v2.1.1.1
+- added api to lock/unlock of RAW Buffers to Support HALv3 Feature
+
+v2.1.1.2, modified struct ia_css_stream_config:
+- to support multiple isys streams in one virtual channel, keep the old one for backward compatibility
+
+v2.1.2.0, modify ia_css_stream_config:
+- add isys_config and input_config to support multiple isys stream within one virtual channel
+
+v2.1.2.1, add IA_CSS_STREAM_FORMAT_NUM
+- add IA_CSS_STREAM_FORMAT_NUM definition to reflect the number of ia_css_stream_format enums
+
+v2.1.2.2, modified enum ia_css_stream_format
+- Add 16bit YUV formats to ia_css_stream_format enum:
+- IA_CSS_STREAM_FORMAT_YUV420_16 (directly after IA_CSS_STREAM_FORMAT_YUV420_10)
+- IA_CSS_STREAM_FORMAT_YUV422_16 (directly after IA_CSS_STREAM_FORMAT_YUV422_10)
+
+v2.1.2.3
+- added api to enable/disable digital zoom for capture pipe.
+
+v2.1.2.4, change CSS API to generate the shading table which should be directly sent to ISP:
+- keep the old CSS API (which uses the conversion of the shading table in CSS) for backward compatibility
+
+v2.1.2.5
+- Added SP frame time measurement (in ticks) and result is sent on a new member
+- in ia_css_buffer.h.
+
+v2.1.2.6, add function ia_css_check_firmware_version()
+- the function ia_css_check_firmware_version() returns true when the firmware version matches and returns false otherwise.
+
+v2.1.2.7
+- rename dynamic_data_index to dynamic_queue_id in struct ia_css_frame.
+- update IA_CSS_PIPE_MODE_NUM
+
+v2.1.2.8
+- added flag for video full range
+
+v2.1.2.9
+- add public parameters for xnr3 kernel
+
+v2.1.2.10
+- add new interface to enable output mirroring
+
+v2.1.2.11, MIPI buffers optimization
+- modified struct ia_css_mipi_buffer_config, added number of MIPI buffers needed for the stream
+- backwards compatible, need another patch to remove legacy function and code
+
+v2.1.2.12
+- create consolidated  firmware package for 2400, 2401, csi2p, bxtpoc
+
+v2.1.3.0
+- rename ia_css_output_config.enable_mirror
+- add new interface to enable vertical output flipping
+
+v2.1.3.1
+- deprecated ia_css_rx_get_irq_info and ia_css_rx_clear_irq_info because both are hardcoded to work on CSI port 1.
+- added new functions ia_css_rx_port_get_irq_info and ia_css_rx_port_clear_irq_info, both have a port ID as extra argument.
+
+v2.1.3.2
+- reverted v2.1.3.0 change
+
+v2.1.3.3
+- Added isys event queue.
+- Renamed ia_css_dequeue_event to ia_css_dequeue_psys_event
+- Made ia_css_dequeue_event deprecated
+
+v2.1.3.4
+- added new interface to support ACC extension QoS feature.
+- added IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE.
+
+v2.1.3.5
+- added tiled frame format IA_CSS_FRAME_FORMAT_NV12_TILEY
+
+v2.1.3.6
+- added functions ia_css_host_data_allocate and ia_css_host_data_free
+
+v2.1.4.0, default pipe config change
+- disable enable_dz param by default
+
+v2.1.5.0
+- removed mix_range field from yuvp1_y_ee_nr_frng_public_config
+
+v2.1.5.1, exposure IDs per stream
+- added MIN/MAX exposure ID macros
+- made exposure ID sequence per-stream instead of global (across all streams)
+
+#ifdef ISP2401
+v2.1.5.1, Add parameters to mmgr routines via a macro.
+- Replaced mmgr funtions with macros to add caller func name + line #.
+- This is done to help debug memory access issues, allocation issues, etc.
+
+#endif
+v2.1.6.0, Interface for vertical output flip
+- add new interface to enable vertical output flipping
+- rename ia_css_output_config.enable_mirror
+
+#ifndef ISP2401
+v2.1.6.1, Effective res on pipe
+#else
+v2.1.6.2 (2 changes parallel), Effective res on pipe
+#endif
+- Added input_effective_res to struct ia_css_pipe_config in ia_css_pipe_public.h.
+
+#ifndef ISP2401
+v2.1.6.2, CSS-API version file generated from individual changes
+#else
+v2.1.6.3 (2 changes parallel), CSS-API version file generated from individual changes
+#endif
+- Avoid merge-conflicts by generating version file from individual CSS-API changes.
+- Parallel CSS-API changes can map to the same version number after this change.
+- Version numbers for a change could increase due to parallel changes being merged.
+- The version number would not decrease for a change.
+
+#ifndef ISP2401
+v2.1.6.5 (2 changes parallel), Add SP FW error event
+#else
+v2.1.6.6 (4 changes parallel), Add SP FW error event
+#endif
+- Added FW error event. This gets raised when the SP FW runs into an
+- error situation from which it cannot recover.
+
+#ifndef ISP2401
+v2.1.6.5 (2 changes parallel), expose bnr FF enable bits in bnr public API
+#else
+v2.1.6.6 (4 changes parallel), expose bnr FF enable bits in bnr public API
+#endif
+- Added ff enable bits to bnr_public_config_dn_detect_ctrl_config_t struct
+
+#ifndef ISP2401
+v2.1.6.5 (2 changes parallel), ISP configuration per pipe 
+#else
+v2.1.6.6 (4 changes parallel), ISP configuration per pipe 
+#endif
+- Added ISP configuration per pipe support: p_isp_config field in
+- struct ia_css_pipe_config and ia_css_pipe_set_isp_config_on_pipe
+- and ia_css_pipe_set_isp_config functions
+
+#ifndef ISP2401
+v2.1.7.0, removed css_version.h
+#else
+v2.1.7.0 (2 changes parallel), removed css_version.h
+#endif
+- Removed css_version.h that was used for versioning in manual (non-CI) releases.
+
+#ifndef ISP2401
+v2.1.7.1, Add helpers (get and set) for ISP cfg per pipe
+#else
+v2.1.7.2 (2 changes parallel), Add helpers (get and set) for ISP cfg per pipe
+#endif
+- Add helpers (get and set) for ISP configuration per pipe
+
+#ifndef ISP2401
+v2.1.7.2, Add feature to lock all RAW buffers
+#else
+v2.1.7.3 (2 changes parallel), Add feature to lock all RAW buffers
+#endif
+- This API change adds a boolean flag (lock_all) in the stream_config struct.
+- If this flag is set to true, then all frames will be locked if locking is
+- enabled. By default this flag is set to false.
+- When this flag is false, then only buffers that are sent to the preview pipe
+- will be locked. If continuous viewfinder is disabled, the flag should be set
+- to true.
+
+#ifndef ISP2401
+v2.1.8.0 (2 changes parallel), Various changes to support ACC configuration per pipe
+#else
+v2.1.8.0 (4 changes parallel), Various changes to support ACC configuration per pipe
+#endif
+- Add ia_css_pipe_get_isp_config()
+- Remove ia_css_pipe_set_isp_config_on_pipe (duplicated
+- by ia_css_pipe_set_isp_config)
+- Add isp configuration as parameter for
+- ia_css_pipe_set_isp_config
+- Remove ia_css_pipe_isp_config_set()
+- Remove ia_css_pipe_isp_config_get()
+
+#ifndef ISP2401
+v2.1.8.2 (2 changes parallel), Added member num_invalid_frames to ia_css_pipe_info structure.
+#else
+v2.1.8.3 (4 changes parallel), Added member num_invalid_frames to ia_css_pipe_info structure.
+#endif
+- Added member num_invalid_frames to ia_css_pipe_info structure.
+- This helps the driver make sure that the first valid output
+- frame goes into the first user-supplied output buffer.
+
+#ifndef ISP2401
+v2.1.8.4 (2 changes parallel), ISYS EOF timestamp for output buffers
+#else
+v2.1.8.5 (4 changes parallel), ISYS EOF timestamp for output buffers
+#endif
+- driver gets EOF timer to every out frame . ia_css_buffer modified to accomodate same.
+
+#ifndef ISP2401
+v2.1.8.4 (4 changes parallel), display_config
+#else
+v2.1.8.5 (6 changes parallel), display_config
+#endif
+- Added formats- and output config parameters for configuration of the (optional) display output.
+
+#ifndef ISP2401
+v2.1.8.4 (2 changes parallel), Adding zoom region parameters to CSS API
+#else
+v2.1.8.5 (4 changes parallel), Adding zoom region parameters to CSS API
+#endif
+- Adding ia_css_point and ia_css_region structures to css-api.
+- Adding zoom_region(type ia_css_region) parameter to ia_css_dz_config structure.
+- By using this user can do the zoom based on zoom region and
+- the center of the zoom region is not restricted at the center of the input frame.
+
+#ifndef ISP2401
+v2.1.8.6 (1 changes parallel), Add new ia_css_fw_warning type
+#else
+v2.1.8.7 (3 changes parallel), Add new ia_css_fw_warning type
+#endif
+- Add IA_CSS_FW_WARNING_TAG_EXP_ID_FAILED enum to ia_css_fw_warning type
+- Extend sp_warning() with exp_id parameter
+
+#ifndef ISP2401
+v2.1.8.6 (1 changes parallel), Add includes in GC, GC2 kernel interface files
+#else
+v2.1.8.7 (3 changes parallel), Add includes in GC, GC2 kernel interface files
+#endif
+- add ia_css_ctc_types.h includes in ia_css_gc_types.h and ia_css_gc2_types.h. Needed to get ia_css_vamem_type.
+
+#ifndef ISP2401
+v2.1.9.0 (1 changes parallel), Introduce sp assert event.
+#else
+v2.1.9.0 (3 changes parallel), Introduce sp assert event.
+#endif
+- Add IA_CSS_EVENT_TYPE_FW_ASSERT. The FW sends the event in case an assert goes off.
+
+#ifndef ISP2401
+v2.1.9.1 (1 changes parallel), Exclude driver part from ia_css_buffer.h as it is also used by SP
+#else
+v2.1.9.2 (3 changes parallel), Exclude driver part from ia_css_buffer.h as it is also used by SP
+#endif
+- Excluded driver part of the interface from SP/ISP code
+- Driver I/F is not affected
+
+#ifndef ISP2401
+v2.1.9.2, added IA_CSS_EVENT_TYPE_TIMER
+#else
+v2.1.9.3 (2 changes parallel), added IA_CSS_EVENT_TYPE_TIMER
+#endif
+- Added a new event called IA_CSS_EVENT_TYPE_TIMER
+
+#ifndef ISP2401
+v2.1.10.0 (4 changes parallel), Add a flag "enable_dpc" to "struct ia_css_pipe_config"
+#else
+v2.1.10.0 (6 changes parallel), Add a flag "enable_dpc" to "struct ia_css_pipe_config"
+#endif
+- Add a flag "enable_dpc" to "struct ia_css_pipe_config"
+
+#ifndef ISP2401
+v2.1.10.6 (6 changes parallel), change the pipe version type from integer to enum
+#else
+v2.1.10.8 (9 changes parallel), change the pipe version type from integer to enum
+#endif
+- add new enum to enumerate ISP pipe version
+- change the pipe version type in pipe_config from integer to enum
+
+#ifndef ISP2401
+v2.1.13.0 (8 changes parallel), Stop Support for Skycam B0
+#else
+v2.1.14.0 (12 changes parallel), Stop Support for Skycam B0
+#endif
+- Remove a few pre-processor defines for Skycam B0/C0 as support
+
+#ifndef ISP2401
+v2.1.14.0 (24 changes parallel), change the pipe version type from integer to enum
+#else
+v2.1.15.0 (28 changes parallel), change the pipe version type from integer to enum
+#endif
+- remove the temporary workaround for backward compatability
+
+#ifndef ISP2401
+v2.1.14.0 (13 changes parallel), expose_gamma_enable_option
+#else
+v2.1.15.0 (17 changes parallel), expose_gamma_enable_option
+#endif
+- added enable param to gamma_corr_public_config
+- added documentation to rgbpp_public.h
+
+#ifndef ISP2401
+v2.1.14.0 (12 changes parallel), Remove deprecated FW_ERROR event.
+#else
+v2.1.15.0 (16 changes parallel), Remove deprecated FW_ERROR event.
+#endif
+- Remove code for deprecated FW_ERROR event.
+
+#ifndef ISP2401
+v2.1.14.3 (5 changes parallel), fix IEFD's puclic API types
+#else
+v2.1.15.5 (8 changes parallel), fix IEFD's puclic API types
+#endif
+- fix IEFD public API members types: rad_cu6_x1,rad_cu_unsharp_x1 & unsharp_amount
+
+#ifndef ISP2401
+v2.1.14.3 (5 changes parallel), Add IA_CSS_FW_WARNING_FRAME_PARAM_MISMATCH
+#else
+v2.1.15.5 (8 changes parallel), Add IA_CSS_FW_WARNING_FRAME_PARAM_MISMATCH
+#endif
+- Add IA_CSS_FW_WARNING_FRAME_PARAM_MISMATCH enum to ia_css_fw_warning type
+
+#ifndef ISP2401
+v2.1.14.4 (5 changes parallel), new API getter functions for gdc in buffer information
+#else
+v2.1.15.8 (11 changes parallel), add_flag_to_disable_continous_viewfinder
+- add a new flag in stream_config to disable continuous viewfinder
+- in ZSL use case.
+
+v2.1.16.0 (8 changes parallel), revert ia_css_skc_dvs_statistics field size change 
+- Reverted field size change, change was not ready for driver yet.
+
+v2.1.17.0 (7 changes parallel), change CSS API to fix the shading correction off-center issue
+- update the ia_css_shading_info structure in ia_css_types.h
+
+v2.1.17.0 (32 changes parallel), add_flag_to_disable_continous_viewfinder_part2
+- remove the old interfaces
+
+v2.1.17.4 (8 changes parallel), Added public interface for setting the scaler LUT.
+- Added the public struct to output system and modified the global config struct.
+
+v2.1.17.5 (7 changes parallel), Add parameters for new TNR3 component
+- Add new parameters for new TNR3 component
+
+v2.1.17.6 (9 changes parallel), Update skycam DPC_MAX_NUMBER_OF_DP
+- Automated tooling requires an API change request
+- This change changes the implementation of #define DPC_MAX_NUMBER_OF_DP
+- it now returns a different number
+
+v2.1.17.6 (8 changes parallel), Return an error when both DPC and BDS are enabled in a pipe config
+- Return an error when both DPC and BDS are enabled in a pipe config
+
+v2.1.17.6 (9 changes parallel), new API getter functions for gdc in buffer information
+#endif
+- ia_css_pipe_get_dvs_filter() added
+- ia_css_pipe_get_gdc_in_buffer_info() added
+
+#ifndef ISP2401
+v2.1.14.5 (8 changes parallel), Update CNR2 ineffective values
+#else
+v2.1.17.7 (12 changes parallel), Update CNR2 ineffective values
+#endif
+- Fixed Incorrect ineffective values listed in ia_css_cnr_config
+- Correct Ineffective value is 8191
+
+#ifndef ISP2401
+v2.1.14.5 (8 changes parallel), af_roi_api
+#else
+v2.1.17.7 (12 changes parallel), af_roi_api
+#endif
+- added a new function to set AF ROI ia_css_set_af_roi
+- added a new struct ia_css_s3a_roi_offset
+
+#ifndef ISP2401
+v2.1.14.5 (8 changes parallel), remove x_y_end_from_ae_and_awb
+#else
+v2.1.17.7 (12 changes parallel), Enlarge AF AWB_FR stats buffers
+- Enlarge AF and AWB_FR stats buffers to support max grid width per stripe as oppose to per frame
+
+v2.1.17.7 (12 changes parallel), remove x_y_end_from_ae_and_awb
+#endif
+- added a flag to prepare removal of x_end and y_end from ae grid public config
+- added a flag to prepare removal of x_end and y_end from awb grid public config
+
+#ifndef ISP2401
+v2.1.14.5 (4 changes parallel), Added public interface for setting the scaler LUT.
+- Added the public struct to output system and modified the global config struct.
+#else
+v2.1.17.8 (5 changes parallel)
+- added input_yuv , input_raw to ia_css_binary_info.enable 
+- struct, these attributes were always there but not saved
+- in the binary_info struct
+#endif
+
+#ifndef ISP2401
+v2.1.14.6 (8 changes parallel), add_flag_to_disable_continous_viewfinder
+- add a new flag in stream_config to disable continuous viewfinder
+- in ZSL use case.
+#else
+v2.1.17.9 (6 changes parallel), cleanup_awb_ae_rgb_integration_flags
+- this change only cleans up an approved api CR see wikis below
+#endif
+
+#ifndef ISP2401
+v2.1.14.6 (8 changes parallel), Enlarge AF AWB_FR stats buffers
+- Enlarge AF and AWB_FR stats buffers to support max grid width per stripe as oppose to per frame
+#else
+v2.1.17.10 (6 changes parallel), output_system_input_resolution
+- adedd gdc_output_system_in_resolution to pipe config struct
+#endif
+
+#ifndef ISP2401
+v2.1.14.8 (6 changes parallel), pipe config option for vf output bci mode downscaling
+#else
+v2.1.17.10 (5 changes parallel), Per pipe DPC configuration is added to ia_css_isp_parameters
+- Per pipe DPC configuration is added to ia_css_isp_parameters 
+
+v2.1.17.10 (10 changes parallel), pipe config option for vf output bci mode downscaling
+#endif
+- vf downscaling using yuv_scale binary.
+
+#ifndef ISP2401
+v2.1.14.10 (7 changes parallel), Add scale mode GDC V2 LUT to CSS API
+#else
+v2.1.17.12 (11 changes parallel), Add scale mode GDC V2 LUT to CSS API
+#endif
+- Allow client to set global LUT for gdc v2 (First step in this change. See wiki page for more details)
+
+#ifndef ISP2401
+v2.1.14.10 (8 changes parallel), Include added to type-support.h.
+#else
+v2.1.17.12 (12 changes parallel), Include added to type-support.h.
+#endif
+- Include of hive/cell_support.h was added to type-support.h, in order to
+- have access to define HAVE_STDINT.
+
+#ifndef ISP2401
+v2.1.14.11 (7 changes parallel), Pipe configuration to enable BLI mode downscaling for
+#else
+v2.1.17.13 (11 changes parallel), Pipe configuration to enable BLI mode downscaling for
+#endif
+- BLI mode downscaling for capture post-processing
+
+#ifndef ISP2401
+v2.1.14.14 (9 changes parallel), Fix copyright headers (no functional change)
+#else
+v2.1.17.15 (8 changes parallel), Add copyright headers to css files
+- Add copyright headers to css API files
+
+v2.1.17.15 (8 changes parallel), add copyright header to include files
+- add copyright header to include files
+
+v2.1.17.15 (8 changes parallel), add copyright header to isp files
+- add copyright header to isp files
+
+v2.1.17.15 (8 changes parallel), add copyright header to refactored code
+- add copyright header to refactored code
+- (base, camera, runtime directories)
+
+v2.1.17.16 (13 changes parallel), Fix copyright headers (no functional change)
+#endif
+- No functional change; only fixes copyright headers
+
+#ifndef ISP2401
+v2.1.14.14 (6 changes parallel), Remove continuous mode special case handling in ia_css_pipe_set_isp_config
+#else
+v2.1.17.16 (10 changes parallel), Remove continuous mode special case handling in ia_css_pipe_set_isp_config
+#endif
+- For continuous mode isp_config was being send to all pipes,
+- even though API ia_css_pipe_set_isp_config is for single pipe
+- Removed incorrect case
+
+#ifndef ISP2401
+v2.1.14.14 (6 changes parallel), DVS statistics grid produced by accelerator
+#else
+v2.1.17.16 (5 changes parallel), Added documentation to formats_config header file
+- Added description of ranges for full-range flag
+
+v2.1.17.16 (10 changes parallel), DVS statistics grid produced by accelerator
+#endif
+- Add DVS statistics produced by accelerator grid to pipe info
+- Add ia_css_pipe_has_dvs_stats function
+
+#ifndef ISP2401
+v2.1.14.15 (7 changes parallel), cont_remove_x_y_end_from_ae_and_awb
+#else
+v2.1.17.17 (5 changes parallel), Provide the CSS interface to select the luma only binaries
+- Add a flag "enable_luma_only" to "struct ia_css_pipe_config"
+
+v2.1.17.17 (11 changes parallel), cont_remove_x_y_end_from_ae_and_awb
+#endif
+- this patch doesn't introduce any new api change, it only fixes a recent
+- api merged change (#31938) , in order to have success CI i had to upload an api change request
+
+#ifndef ISP2401
+v2.1.14.17 (6 changes parallel), Add XNR3 blending strength to kernel interface
+- Added a blending strength field to the XNR3 kernel interface to add
+- support for blending.
+#else
+v2.1.17.17 (10 changes parallel), GAC state dump for debug
+- added ia_css_dump_gac_state function
+
+v2.1.17.18 (23 changes parallel), output_format_nv12_16
+- added new output fromat nv12_16
+#endif
+
+#ifndef ISP2401
+v2.1.14.18 (22 changes parallel), eliminate two_pixels_per_clock field
+#else
+v2.1.17.18 (4 changes parallel), Remove author details from SKC src code
+- remove author details from skc src code
+
+v2.1.17.19 (26 changes parallel), eliminate two_pixels_per_clock field
+#endif
+- remove obsolete field two_pixels_per_clock
+
+#ifndef ISP2401
+v2.1.14.19 (3 changes parallel), Fix copyright headers (no functional change)
+#else
+v2.1.17.20 (7 changes parallel), Fix copyright headers (no functional change)
+#endif
+- No functional change; only fixes copyright headers
+
+#ifndef ISP2401
+v2.1.14.21 (3 changes parallel), ia_css_skc_dvs_statistics field size change
+- ia_css_skc_dvs_statistics field size change
+#else
+v2.1.17.20 (11 changes parallel), Add XNR3 blending strength to kernel interface
+- Added a blending strength field to the XNR3 kernel interface to add
+- support for blending.
+#endif
+
+#ifndef ISP2401
+v2.1.15.0 (3 changes parallel), revert ia_css_skc_dvs_statistics field size change 
+- Reverted field size change, change was not ready for driver yet.
+#else
+v2.1.17.21 (24 changes parallel), Add N_CSS_PRBS_IDS and N_CSS_TPG_IDS
+- Add N_CSS_PRBS_IDS to reflect the number of ia_css_prbs_id enum
+- Add N_CSS_TPG_IDS to reflect the number of ia_css_tpg_id enum
+#endif
+
+#ifndef ISP2401
+v2.1.15.2 (3 changes parallel), Return an error when both DPC and BDS are enabled in a pipe config
+- Return an error when both DPC and BDS are enabled in a pipe config
+#else
+v2.1.17.23 (8 changes parallel), ia_css_skc_dvs_statistics field size change
+- ia_css_skc_dvs_statistics field size change
+#endif
+
+#ifndef ISP2401
+v2.1.15.3 (2 changes parallel), Update skycam DPC_MAX_NUMBER_OF_DP
+- Automated tooling requires an API change request
+- This change changes the implementation of #define DPC_MAX_NUMBER_OF_DP
+- it now returns a different number
+#else
+v2.1.19.0 (6 changes parallel)
+- Added code to calculate input_res using the Windows specification of binning
+#endif
+
+#ifndef ISP2401
+v2.1.15.3 (18 changes parallel), output_format_nv12_16
+- added new output fromat nv12_16
+#else
+v2.1.20.0 (7 changes parallel), Add interface to select TNR enabled binaries
+- Add a bool "enable_tnr" to "struct ia_css_pipe_config"
+
+v2.1.20.0 (6 changes parallel), OSYS & GDC Debug dump function addition
+- add GDC state dump function
+- add OSYS state dump function
+
+v2.1.20.4 (7 changes parallel), Add ref_buf_select parameter for TNR3 to kernel interface
+- Added a ref_buf_select parameter to the TNR3 kernel interface to add
+- support for multiple reference buffers.
+
+v2.1.20.4 (6 changes parallel), DVS MAX grid dimensions to cover maximal resolution
+- rename DVS_TABLE_HEIGHT/WIDTH to MAX_DVS_COORDS_TABLE_HEIGHT/WIDTH
+- modify value of the above macros to cover max resolution
+
+v2.1.20.5 (54 changes parallel), add input feeder calculations getter
+- add input_feeder_config public struct
+- add get_input_feeder_config getter
+
+v2.1.20.5 (4 changes parallel), Enable runtime updating mapped args for QoS extension pipe
+- added ia_css_pipe_update_qos_ext_mapped_arg()
+
+v2.1.20.7 (77 changes parallel), Add parameters to CPU routines via a macro.
+- Replaced CPU memory allocation functions with macros to add caller func name + line number.
+- This is done to help debug memory access issues, allocation issues, etc.
+- Changed API: only ia_css_env.h
+
+v2.1.20.7 (2 changes parallel), Frame format override
+- Added a function call to the pipe interface for overriding
+- the frame format as set in the pipe.
+- This is an optional interface that can be used under
+- some strict conditions.
+
+v2.1.20.7 (2 changes parallel), Output_system_in_res Information
+- Output_system_in_res_info field added to pipe_info struct 
+
+v2.1.20.8, Temprarily disable memory debug features for SVOS.
+- Temporary commented out the additions to allow SKC testing till root cause found
+- Changed files ia_css_env.h and sh_css.c.
+
+v2.1.20.9, Enable ISP 2.7 naming
+- Add IA_CSS_PIPE_VERSION_2_7 to enum ia_css_pipe_version
+- Add #define SH_CSS_ISP_PIPE_VERSION_2_7 4
+#endif
+
+*/
+
+#endif /*__CSS_API_VERSION_H*/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_trace.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_trace.h
new file mode 100644
index 000000000000..01f7c33b5b40
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/css_trace.h
@@ -0,0 +1,388 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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>
+#ifdef ISP2401
+#include "sh_css_internal.h"	/* for SH_CSS_MAX_SP_THREADS */
+#endif
+
+/*
+	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 {
+	uint8_t   major;
+	uint8_t   minor;
+	uint16_t  counter;
+};
+
+#ifdef ISP2401
+#define MAX_SCRATCH_DATA	4
+#define MAX_CMD_DATA		2
+
+#endif
+/* trace header: holds the version and the topology of the tracer. */
+struct trace_header_t {
+#ifndef ISP2401
+	/* 1st dword */
+#else
+	/* 1st dword: descriptor */
+#endif
+	uint8_t   version;
+	uint8_t   max_threads;
+	uint16_t  max_tracer_points;
+#ifdef ISP2401
+	/* 2nd field: command + data */
+#endif
+	/* 2nd dword */
+	uint32_t  command;
+	/* 3rd & 4th dword */
+#ifndef ISP2401
+	uint32_t  data[2];
+#else
+	uint32_t  data[MAX_CMD_DATA];
+	/* 3rd field: debug pointer */
+#endif
+	/* 5th & 6th dword: debug pointer mechanism */
+	uint32_t  debug_ptr_signature;
+	uint32_t  debug_ptr_value;
+#ifdef ISP2401
+	/* Rest of the header: status & scratch data */
+	uint8_t   thr_status_byte[SH_CSS_MAX_SP_THREADS];
+	uint16_t  thr_status_word[SH_CSS_MAX_SP_THREADS];
+	uint32_t  thr_status_dword[SH_CSS_MAX_SP_THREADS];
+	uint32_t  scratch_debug[MAX_SCRATCH_DATA];
+#endif
+};
+
+#ifndef ISP2401
+#define TRACER_VER			2
+#else
+/* 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
+
+#endif
+#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
+
+#ifndef ISP2401
+typedef enum {
+#else
+enum TRACE_CORE_ID {
+#endif
+	TRACE_SP0_ID,
+	TRACE_SP1_ID,
+	TRACE_ISP_ID
+#ifndef ISP2401
+} TRACE_CORE_ID;
+#else
+};
+#endif
+
+/* TODO: add timing format? */
+#ifndef ISP2401
+typedef enum {
+	TRACE_DUMP_FORMAT_POINT,
+	TRACE_DUMP_FORMAT_VALUE24_HEX,
+	TRACE_DUMP_FORMAT_VALUE24_DEC,
+#else
+enum TRACE_DUMP_FORMAT {
+	TRACE_DUMP_FORMAT_POINT_NO_TID,
+	TRACE_DUMP_FORMAT_VALUE24,
+#endif
+	TRACE_DUMP_FORMAT_VALUE24_TIMING,
+#ifndef ISP2401
+	TRACE_DUMP_FORMAT_VALUE24_TIMING_DELTA
+} TRACE_DUMP_FORMAT;
+#else
+	TRACE_DUMP_FORMAT_VALUE24_TIMING_DELTA,
+	TRACE_DUMP_FORMAT_POINT
+};
+#endif
+
+
+/* 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))
+#ifndef ISP2401
+#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)
+#else
+#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)
+#endif
+
+#define TRACE_SP1_HEADER_ADDR (TRACE_SP1_ADDR)
+#define TRACE_SP1_HEADER_SIZE (sizeof(struct trace_header_t))
+#ifndef ISP2401
+#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)
+#else
+#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)
+#endif
+
+#define TRACE_ISP_HEADER_ADDR (TRACE_ISP_ADDR)
+#define TRACE_ISP_HEADER_SIZE (sizeof(struct trace_header_t))
+#ifndef ISP2401
+#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)
+
+#else
+#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)
+#endif
+
+#ifndef ISP2401
+/* offsets for master_port read/write */
+#define HDR_HDR_OFFSET              0	/* offset of the header */
+#define HDR_COMMAND_OFFSET          4	/* offset of the command */
+#define HDR_DATA_OFFSET             8	/* offset of the command data */
+#define HDR_DEBUG_SIGNATURE_OFFSET  16	/* offset of the param debug signature in trace_header_t */
+#define HDR_DEBUG_POINTER_OFFSET    20	/* offset of the param debug pointer in trace_header_t */
+#endif
+
+/* common majors */
+#ifdef ISP2401
+/* SP0 */
+#endif
+#define MAJOR_MAIN              1
+#define MAJOR_ISP_STAGE_ENTRY   2
+#define MAJOR_DMA_PRXY          3
+#define MAJOR_START_ISP         4
+#ifdef ISP2401
+/* 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
+#endif
+
+#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)
+#ifndef ISP2401
+#define FIELD_VALUE_UNPACK(f)	FIELD_UNPACK(f,FIELD_VALUE_MASK,FIELD_VALUE_OFFSET)
+#else
+#define FIELD_VALUE_UNPACK(f)		FIELD_UNPACK(f,FIELD_VALUE_MASK,FIELD_VALUE_OFFSET)
+#endif
+
+#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)
+#ifndef ISP2401
+#define FIELD_MINOR_UNPACK(f)	FIELD_UNPACK(f,FIELD_MINOR_MASK,FIELD_MINOR_OFFSET)
+#else
+#define FIELD_MINOR_UNPACK(f)		FIELD_UNPACK(f,FIELD_MINOR_MASK,FIELD_MINOR_OFFSET)
+#endif
+
+#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)
+#ifndef ISP2401
+#define FIELD_MAJOR_UNPACK(f)	FIELD_UNPACK(f,FIELD_MAJOR_MASK,FIELD_MAJOR_OFFSET)
+#else
+#define FIELD_MAJOR_UNPACK(f)		FIELD_UNPACK(f,FIELD_MAJOR_MASK,FIELD_MAJOR_OFFSET)
+#endif
+
+#ifndef ISP2401
+#define FIELD_FORMAT_OFFSET		(FIELD_MAJOR_OFFSET + FIELD_MAJOR_WIDTH)
+#define FIELD_FORMAT_WIDTH 		(3)
+#define FIELD_FORMAT_MASK 		FIELD_MASK(FIELD_FORMAT_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)
+#else
+/* 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)
+#endif
+
+#define FIELD_VALUE_24_OFFSET		(0)
+#define FIELD_VALUE_24_WIDTH		(24)
+#ifndef ISP2401
+#define FIELD_VALUE_24_MASK			FIELD_MASK(FIELD_VALUE_24_WIDTH)
+#else
+#define FIELD_VALUE_24_MASK		FIELD_MASK(FIELD_VALUE_24_WIDTH)
+#endif
+#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)
+
+#ifndef ISP2401
+#define PACK_TRACEPOINT(format,major, minor, value)	\
+	(FIELD_FORMAT_PACK(format) | FIELD_MAJOR_PACK(major) | FIELD_MINOR_PACK(minor) | FIELD_VALUE_PACK(value))
+#else
+#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))
+#endif
+
+#ifndef ISP2401
+#define PACK_TRACE_VALUE24(format, major, value)	\
+	(FIELD_FORMAT_PACK(format) | FIELD_MAJOR_PACK(major) | FIELD_VALUE_24_PACK(value))
+#else
+#define PACK_TRACE_VALUE24(major, value)	\
+	(FIELD_TID_PACK(FIELD_TID_SEL_FORMAT_PAT) | FIELD_MAJOR_PACK(major) | FIELD_VALUE_24_PACK(value))
+#endif
+
+#endif /* __CSS_TRACE_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/debug_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/debug_global.h
new file mode 100644
index 000000000000..076c4ba76175
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/debug_global.h
@@ -0,0 +1,83 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t			enable;
+	uint32_t			bufmode;
+	uint32_t			head;
+	uint32_t			tail;
+	uint32_t			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 {
+	uint32_t			enable;
+	int8_t				padding1[HIVE_ISP_DDR_WORD_BYTES-sizeof(uint32_t)];
+	uint32_t			bufmode;
+	int8_t				padding2[HIVE_ISP_DDR_WORD_BYTES-sizeof(uint32_t)];
+	uint32_t			head;
+	int8_t				padding3[HIVE_ISP_DDR_WORD_BYTES-sizeof(uint32_t)];
+	uint32_t			tail;
+	int8_t				padding4[HIVE_ISP_DDR_WORD_BYTES-sizeof(uint32_t)];
+	uint32_t			buf[DEBUG_BUF_SIZE];
+};
+#endif
+
+#endif /* __DEBUG_GLOBAL_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/dma_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/dma_global.h
new file mode 100644
index 000000000000..60d6de1332cd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/dma_global.h
@@ -0,0 +1,255 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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)(addr_b):(unsigned)(addr_a)); \
+  hive_dma_snd(dma_id, read?(unsigned)(addr_a):(unsigned)(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)(addr_b):(unsigned)(addr_a)); \
+  hive_dma_snd(dma_id, read?(unsigned)(addr_a):(unsigned)(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 {
+	uint8_t  crop, elems;
+	uint16_t width;
+	uint32_t stride;
+};
+
+/* Descriptor for dma configuration */
+struct dma_channel_config {
+	uint8_t  connection;
+	uint8_t  extension;
+	uint8_t  height;
+	struct dma_port_config a, b;
+};
+
+#endif /* __DMA_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/event_fifo_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/event_fifo_global.h
new file mode 100644
index 000000000000..4df7a405cdcf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/event_fifo_global.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/fifo_monitor_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/fifo_monitor_global.h
new file mode 100644
index 000000000000..f43bf0ad2468
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/fifo_monitor_global.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/gdc_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gdc_global.h
new file mode 100644
index 000000000000..4505775b224c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gdc_global.h
@@ -0,0 +1,90 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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			(1 << 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 {
+	uint16_t  params[N_GDC_PARAM];
+	uint16_t  ipx_start_array[N_GDC_PARAM];
+	uint16_t  ibuf_offset[N_GDC_PARAM];
+	uint16_t  obuf_offset[N_GDC_PARAM];
+} gdc_scale_param_mem_t;
+
+typedef struct gdc_warp_param_mem_s {
+	uint32_t      origin_x;
+	uint32_t      origin_y;
+	uint32_t      in_addr_offset;
+	uint32_t      in_block_width;
+	uint32_t      in_block_height;
+	uint32_t      p0_x;
+	uint32_t      p0_y;
+	uint32_t      p1_x;
+	uint32_t      p1_y;
+	uint32_t      p2_x;
+	uint32_t      p2_y;
+	uint32_t      p3_x;
+	uint32_t      p3_y;
+	uint32_t      padding[3];
+} gdc_warp_param_mem_t;
+
+
+#endif /* __GDC_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gp_device_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gp_device_global.h
new file mode 100644
index 000000000000..30ad77059d93
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gp_device_global.h
@@ -0,0 +1,85 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/gp_timer_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gp_timer_global.h
new file mode 100644
index 000000000000..ee636ad6c5b3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gp_timer_global.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/gpio_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gpio_global.h
new file mode 100644
index 000000000000..a82ca2a8cada
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/gpio_global.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/hmem_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/hmem_global.h
new file mode 100644
index 000000000000..7e05d7d880d1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/hmem_global.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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			(1UL<<HMEM_RANGE_LOG2)
+
+typedef uint32_t			hmem_data_t;
+
+#endif /* __HMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug.c
new file mode 100644
index 000000000000..c412810887b3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug.c
@@ -0,0 +1,72 @@
+/*
+ * 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"
+
+#ifndef __INLINE_DEBUG__
+#include "debug_private.h"
+#endif /* __INLINE_DEBUG__ */
+
+#include "memory_access.h"
+
+#define __INLINE_SP__
+#include "sp.h"
+
+#include "assert_support.h"
+
+/* The address of the remote copy */
+hrt_address	debug_buffer_address = (hrt_address)-1;
+hrt_vaddress	debug_buffer_ddr_address = (hrt_vaddress)-1;
+/* The local copy */
+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 hrt_vaddress addr)
+{
+	debug_buf_mode_t mode = DEBUG_BUFFER_MODE_LINEAR;
+	uint32_t enable = 1;
+	uint32_t head = 0;
+	uint32_t tail = 0;
+	/* set the ddr queue */
+	debug_buffer_ddr_address = addr;
+	mmgr_store(addr + DEBUG_DATA_BUF_MODE_DDR_ADDR,
+				&mode, sizeof(debug_buf_mode_t));
+	mmgr_store(addr + DEBUG_DATA_HEAD_DDR_ADDR,
+				&head, sizeof(uint32_t));
+	mmgr_store(addr + DEBUG_DATA_TAIL_DDR_ADDR,
+				&tail, sizeof(uint32_t));
+	mmgr_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/atomisp2/css2400/hive_isp_css_common/host/debug_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug_local.h
new file mode 100644
index 000000000000..2b0c5f48c0e7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug_local.h
@@ -0,0 +1,21 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/debug_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug_private.h
new file mode 100644
index 000000000000..a047aadc6619
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/debug_private.h
@@ -0,0 +1,99 @@
+/*
+ * 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 "memory_access.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)
+{
+	uint32_t 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)
+{
+	uint32_t 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)
+{
+	uint32_t	remote_tail;
+
+	mmgr_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;
+		mmgr_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;
+		mmgr_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));
+		mmgr_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/atomisp2/css2400/hive_isp_css_common/host/dma.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma.c
new file mode 100644
index 000000000000..87a25d4289ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma.c
@@ -0,0 +1,299 @@
+/*
+ * 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 <stddef.h>		/* NULL */
+
+#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 != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/dma_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma_local.h
new file mode 100644
index 000000000000..ab631e6f64b5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma_local.h
@@ -0,0 +1,207 @@
+/*
+ * 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 <hrt/defs.h>				/* HRTCAT() */
+#include <hrt/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/atomisp2/css2400/hive_isp_css_common/host/dma_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma_private.h
new file mode 100644
index 000000000000..ba54b1f0467b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/dma_private.h
@@ -0,0 +1,41 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/event_fifo.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo.c
new file mode 100644
index 000000000000..777670948d6f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo.c
@@ -0,0 +1,19 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/event_fifo_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo_local.h
new file mode 100644
index 000000000000..c595692c6ea9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo_local.h
@@ -0,0 +1,57 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/event_fifo_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo_private.h
new file mode 100644
index 000000000000..9d3a29696094
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/event_fifo_private.h
@@ -0,0 +1,75 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <hrt/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/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor.c
new file mode 100644
index 000000000000..1087944d637f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor.c
@@ -0,0 +1,567 @@
+/*
+ * 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 <hrt/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__ */
+
+STORAGE_CLASS_INLINE bool fifo_monitor_status_valid (
+	const fifo_monitor_ID_t		ID,
+	const unsigned int			reg,
+	const unsigned int			port_id);
+
+STORAGE_CLASS_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 != NULL);
+
+	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 != NULL);
+
+	(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 != NULL);
+
+	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;
+}
+
+STORAGE_CLASS_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;
+}
+
+STORAGE_CLASS_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/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_local.h
new file mode 100644
index 000000000000..ed2f86181788
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_local.h
@@ -0,0 +1,99 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_private.h
new file mode 100644
index 000000000000..618b2f7e9c75
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/fifo_monitor_private.h
@@ -0,0 +1,79 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gdc.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc.c
new file mode 100644
index 000000000000..69fa616889b1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc.c
@@ -0,0 +1,127 @@
+/*
+ * 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
+ */
+STORAGE_CLASS_INLINE void gdc_reg_store(
+	const gdc_ID_t		ID,
+	const unsigned int	reg,
+	const hrt_data		value);
+
+STORAGE_CLASS_INLINE hrt_data gdc_reg_load(
+	const gdc_ID_t		ID,
+	const unsigned int	reg);
+
+
+#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
+ */
+STORAGE_CLASS_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;
+}
+
+STORAGE_CLASS_INLINE hrt_data gdc_reg_load(
+	const gdc_ID_t		ID,
+	const unsigned int	reg)
+{
+return ia_css_device_load_uint32(GDC_BASE[ID] + reg*sizeof(hrt_data));
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc_local.h
new file mode 100644
index 000000000000..0c6de867e012
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gdc_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc_private.h
new file mode 100644
index 000000000000..f7dec75adf78
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gdc_private.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gp_device.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device.c
new file mode 100644
index 000000000000..9a34ac052adf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device.c
@@ -0,0 +1,108 @@
+/*
+ * 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 != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/gp_device_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device_local.h
new file mode 100644
index 000000000000..113d5ed32d42
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device_local.h
@@ -0,0 +1,143 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gp_device_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device_private.h
new file mode 100644
index 000000000000..bce1fdf79114
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_device_private.h
@@ -0,0 +1,46 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gp_timer.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer.c
new file mode 100644
index 000000000000..5a4eabf79ee2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer.c
@@ -0,0 +1,70 @@
+/*
+ * 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(uint32_t reg, uint32_t value);
+
+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(uint32_t 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/atomisp2/css2400/hive_isp_css_common/host/gp_timer_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer_local.h
new file mode 100644
index 000000000000..19ce35d87291
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer_local.h
@@ -0,0 +1,45 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gp_timer_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer_private.h
new file mode 100644
index 000000000000..705be5e5cc70
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gp_timer_private.h
@@ -0,0 +1,22 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gpio_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gpio_local.h
new file mode 100644
index 000000000000..f4652b79734d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gpio_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/gpio_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gpio_private.h
new file mode 100644
index 000000000000..6ace2184b522
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/gpio_private.h
@@ -0,0 +1,44 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_ddr_hrt_modified.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_ddr_hrt_modified.h
new file mode 100644
index 000000000000..39785aa21459
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_ddr_hrt_modified.h
@@ -0,0 +1,148 @@
+/*
+ * 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 _hive_isp_css_ddr_hrt_modified_h_
+#define _hive_isp_css_ddr_hrt_modified_h_
+
+#include <hmm_64/hmm.h>
+
+/* This function reads an image from DDR and stores it in the img_buf array
+   that has been allocated by the caller.
+   The specifics of how the pixels are stored into DDR by the DMA are taken
+   into account (bits padded to a width of 256, depending on the number of
+   elements per ddr word).
+   The DMA specific parameters give to this function (elems_per_xword and sign_extend)
+   should correspond to those given to the DMA engine.
+   The address is a virtual address which will be translated to a physical address before
+   data is loaded from or stored to that address.
+
+   The return value is 0 in case of success and 1 in case of failure.
+ */
+unsigned int
+hrt_isp_css_read_image_from_ddr(
+    unsigned short *img_buf,
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword,
+    unsigned int sign_extend,
+    hmm_ptr virt_addr);
+
+/* This function writes an image to DDR, keeping the same aspects into account as the read_image function
+   above. */
+unsigned int
+hrt_isp_css_write_image_to_ddr(
+    const unsigned short *img_buf,
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword,
+    unsigned int sign_extend,
+    hmm_ptr virt_addr);
+
+/* return the size in bytes of an image (frame or plane). */
+unsigned int
+hrt_isp_css_sizeof_image_in_ddr(
+    unsigned int width,
+    unsigned int height,
+    unsigned int bits_per_element);
+
+unsigned int
+hrt_isp_css_stride_of_image_in_ddr(
+    unsigned int width,
+    unsigned int bits_per_element);
+
+hmm_ptr
+hrt_isp_css_alloc_image_in_ddr(
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword);
+
+hmm_ptr
+hrt_isp_css_calloc_image_in_ddr(
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword);
+
+#ifndef HIVE_ISP_NO_GDC
+#include "gdc_v2_defs.h"
+
+hmm_ptr
+hrt_isp_css_alloc_gdc_lut_in_ddr(void);
+
+void
+hrt_isp_css_write_gdc_lut_to_ddr(
+    short values[4][HRT_GDC_N],
+    hmm_ptr virt_addr);
+#endif
+
+#ifdef _HIVE_ISP_CSS_FPGA_SYSTEM
+hmm_ptr
+hrt_isp_css_alloc_image_for_display(
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword);
+
+hmm_ptr
+hrt_isp_css_calloc_image_for_display(
+    unsigned int width,
+    unsigned int height,
+    unsigned int elems_per_xword);
+#endif
+
+/* New set of functions, these do not require the elems_per_xword, but use bits_per_element instead,
+   this way the user does not need to know about the width of a DDR word. */
+unsigned int
+hrt_isp_css_read_unsigned(
+    unsigned short *target,
+    unsigned int width,
+    unsigned int height,
+    unsigned int source_bits_per_element,
+    hmm_ptr source);
+
+unsigned int
+hrt_isp_css_read_signed(
+    short *target,
+    unsigned int width,
+    unsigned int height,
+    unsigned int source_bits_per_element,
+    hmm_ptr source);
+
+unsigned int 
+hrt_isp_css_write_unsigned(
+    const unsigned short *source,
+    unsigned int width,
+    unsigned int height,
+    unsigned int target_bits_per_element,
+    hmm_ptr target);
+
+unsigned int 
+hrt_isp_css_write_signed(
+    const short *source,
+    unsigned int width,
+    unsigned int height,
+    unsigned int target_bits_per_element,
+    hmm_ptr target);
+
+hmm_ptr
+hrt_isp_css_alloc(
+    unsigned int width,
+    unsigned int height,
+    unsigned int bits_per_element);
+
+hmm_ptr
+hrt_isp_css_calloc(
+    unsigned int width,
+    unsigned int height,
+    unsigned int bits_per_element);
+
+#endif /* _hive_isp_css_ddr_hrt_modified_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_hrt_modified.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_hrt_modified.h
new file mode 100644
index 000000000000..342553d10e08
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hive_isp_css_hrt_modified.h
@@ -0,0 +1,79 @@
+/*
+ * 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 _hive_isp_css_hrt_h
+#define _hive_isp_css_hrt_h
+
+#include "system_types.h"
+
+#include "hive_isp_css_host_ids_hrt.h"
+#include "hive_isp_css_defs.h"
+
+#ifdef HRT_ISP_CSS_CUSTOM_HOST
+#ifndef HRT_USE_VIR_ADDRS
+#define HRT_USE_VIR_ADDRS
+#endif
+/*#include "hive_isp_css_custom_host_hrt.h"*/
+#endif
+
+#include <gpio_block.h>
+#include <gp_regs.h>
+#include <gp_timer_hrt.h>
+  #include <css_receiver_2400_hrt.h>
+//  #include <isp2400_mamoiada_params.h>
+//  #include <isp2400_support.h>
+  /* insert idle signal clearing and setting around hrt_main */
+  #if !defined(HRT_HW) || defined(HRT_ISP_CSS_INSERT_IDLE_SIGNAL)
+    #define hrt_main _hrt_isp_css_main
+  #endif
+  #ifdef _HIVE_ISP_CSS_SPECMAN_SYSTEM
+    #include "hive_isp_css_2400_specman_system.h"
+  #else
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+    #include "hive_isp_css_2400_system.h"
+#elif defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+    #include "hive_isp_css_2401_system.h"
+#else
+#error "hive_isp_css_hrt_modified.h: SYSTEM must be one of {2400_MAMOIADA_SYSTEM, 2401_MAMOIADA_SYSTEM}"
+#endif
+  #endif
+#include <sp_hrt.h>
+#include <input_system_hrt.h>
+#include <input_selector_hrt.h>
+#include <sig_monitor_hrt.h>
+
+#include "hive_isp_css_sdram_wakeup_hrt.h"
+#include "hive_isp_css_idle_signal_hrt.h"
+#include "hive_isp_css_sp_hrt.h"
+#include "hive_isp_css_isp_hrt.h"
+#include "hive_isp_css_streaming_to_mipi_hrt.h"
+#include "hive_isp_css_testbench_hrt.h"
+#include "hive_isp_css_streaming_monitors_hrt.h"
+#include "hive_isp_css_gp_regs_hrt.h"
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+#include "hive_isp_css_irq_hrt.h"
+#elif defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+#include "hive_isp_css_2401_irq_hrt.h"
+#else
+#error "hive_isp_css_hrt_modified.h: SYSTEM must be one of {2400_MAMOIADA_SYSTEM, 2401_MAMOIADA_SYSTEM}"
+#endif
+
+#include "hive_isp_css_stream_switch_hrt.h"
+
+#include "hive_isp_css_ddr_hrt_modified.h"
+#include "hive_isp_css_dma_set_hrt.h"
+
+#define HIVE_ISP_CSS_NO_STREAM_SWITCH 1
+
+#endif /* _hive_isp_css_hrt_h */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem.c
new file mode 100644
index 000000000000..e48f180c9507
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem.c
@@ -0,0 +1,19 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/hmem_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem_local.h
new file mode 100644
index 000000000000..499f55f07253
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/hmem_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem_private.h
new file mode 100644
index 000000000000..2b636e0e6482
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/hmem_private.h
@@ -0,0 +1,30 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/input_formatter.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter.c
new file mode 100644
index 000000000000..a8997e45738e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter.c
@@ -0,0 +1,227 @@
+/*
+ * 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 USE_INPUT_SYSTEM_VERSION_2
+
+#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__ */
+
+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 uint8_t 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 */
+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 != NULL);
+
+	/* 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 != NULL);
+/*
+	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 != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/input_formatter_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter_local.h
new file mode 100644
index 000000000000..3e00b5e6bad7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter_local.h
@@ -0,0 +1,120 @@
+/*
+ * 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 {
+	uint32_t	reset;
+	uint32_t	input_endianness;
+	uint32_t	output_endianness;
+	uint32_t	bitswap;
+	uint32_t	block_synch;
+	uint32_t	packet_synch;
+	uint32_t	readpostwrite_synch;
+	uint32_t	is_2ppc;
+	uint32_t	en_status_update;
+};
+
+static const unsigned int input_formatter_alignment[N_INPUT_FORMATTER_ID] = {
+	ISP_VEC_ALIGN, ISP_VEC_ALIGN, HIVE_ISP_CTRL_DATA_BYTES};
+
+#endif /* __INPUT_FORMATTER_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter_private.h
new file mode 100644
index 000000000000..d34933e44aa9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_formatter_private.h
@@ -0,0 +1,46 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/input_system.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system.c
new file mode 100644
index 000000000000..f35e18987b67
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system.c
@@ -0,0 +1,1823 @@
+/*
+ * 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"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2
+
+#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)
+
+const ib_buffer_t   IB_BUFFER_NULL = {0 ,0, 0 };
+
+static input_system_error_t input_system_configure_channel(
+	const channel_cfg_t		channel);
+
+static input_system_error_t input_system_configure_channel_sensor(
+	const channel_cfg_t		channel);
+
+static input_system_error_t input_buffer_configuration(void);
+
+static input_system_error_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 ib_buffer_t* const		cfg);
+
+static void acquisition_unit_configure(
+	const input_system_ID_t			ID,
+	const sub_system_ID_t			sub_id,
+	const 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_error_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_error_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_error_t input_system_multiplexer_cfg(
+	input_system_multiplex_t* const			lhs,
+	const input_system_multiplex_t			rhs,
+	input_system_config_flags_t* const		flags);
+
+
+
+STORAGE_CLASS_INLINE void capture_unit_get_state(
+	const input_system_ID_t			ID,
+	const sub_system_ID_t			sub_id,
+	capture_unit_state_t			*state);
+
+STORAGE_CLASS_INLINE void acquisition_unit_get_state(
+	const input_system_ID_t			ID,
+	const sub_system_ID_t			sub_id,
+	acquisition_unit_state_t		*state);
+
+STORAGE_CLASS_INLINE void ctrl_unit_get_state(
+	const input_system_ID_t			ID,
+	const sub_system_ID_t			sub_id,
+	ctrl_unit_state_t				*state);
+
+STORAGE_CLASS_INLINE void mipi_port_get_state(
+	const rx_ID_t					ID,
+	const mipi_port_ID_t			port_ID,
+	mipi_port_state_t				*state);
+
+STORAGE_CLASS_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 != NULL);
+
+	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]));
+	}
+
+return;
+}
+
+void receiver_get_state(
+	const rx_ID_t				ID,
+	receiver_state_t			*state)
+{
+	mipi_port_ID_t	port_id;
+	unsigned int	ch_id;
+
+	assert(ID < N_RX_ID);
+	assert(state != NULL);
+
+	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 = (mipi_port_ID_t)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 = (unsigned int)0; 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);
+
+return;
+}
+
+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);
+
+return;
+}
+
+void receiver_port_enable(
+	const rx_ID_t			ID,
+	const mipi_port_ID_t		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);
+return;
+}
+
+bool is_receiver_port_enabled(
+	const rx_ID_t			ID,
+	const mipi_port_ID_t		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 mipi_port_ID_t		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);
+return;
+}
+
+rx_irq_info_t receiver_get_irq_info(
+	const rx_ID_t			ID,
+	const mipi_port_ID_t		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 mipi_port_ID_t		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);
+return;
+}
+
+STORAGE_CLASS_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 != NULL);
+
+	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);
+
+return;
+}
+
+STORAGE_CLASS_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 != NULL);
+
+	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);
+
+return;
+}
+
+STORAGE_CLASS_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 != NULL);
+
+	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);
+
+return;
+}
+
+STORAGE_CLASS_INLINE void mipi_port_get_state(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t			port_ID,
+	mipi_port_state_t			*state)
+{
+	int	i;
+
+	assert(ID < N_RX_ID);
+	assert(port_ID < N_MIPI_PORT_ID);
+	assert(state != NULL);
+
+	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));
+	}
+
+return;
+}
+
+STORAGE_CLASS_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 != NULL);
+
+	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++) {
+		uint8_t	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++) {
+		uint8_t	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);
+	}
+
+return;
+}
+
+// MW: "2400" in the name is not good, but this is to avoid a naming conflict
+input_system_cfg2400_t config;
+
+static void receiver_rst(
+	const rx_ID_t				ID)
+{
+	mipi_port_ID_t		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?
+
+	return;
+}
+
+//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.
+
+	return;
+}
+
+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);
+
+	return;
+}
+
+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);
+
+	return;
+}
+
+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 != NULL);
+
+	// 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);
+
+	return;
+}
+
+
+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?
+	}
+
+	return;
+}
+
+// Function that resets current configuration.
+input_system_error_t input_system_configuration_reset(void)
+{
+	unsigned int i;
+
+	receiver_rst(RX0_ID);
+
+	input_system_network_rst(INPUT_SYSTEM0_ID);
+
+	gp_device_rst(INPUT_SYSTEM0_ID);
+
+	input_switch_rst(INPUT_SYSTEM0_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_CFG_FLAG_RESET;
+	}
+
+	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_error_t input_system_configure_channel(
+	const channel_cfg_t		channel)
+{
+	input_system_error_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 :
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+				break;
+			case INPUT_SYSTEM_SOURCE_PRBS :
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+				break;
+			case INPUT_SYSTEM_SOURCE_FIFO :
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+				break;
+			default :
+				return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+				break;
+		}
+
+		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_error_t input_buffer_configuration(void)
+{
+	uint32_t current_address    = 0;
+	uint32_t unallocated_memory = IB_CAPACITY_IN_WORDS;
+
+	ib_buffer_t 	candidate_buffer_acq  = IB_BUFFER_NULL;
+	uint32_t 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;
+					break;
+			}
+
+			// 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;
+					break;
+			}
+		} 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 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 != NULL);
+
+	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);
+
+	return;
+}
+
+
+static void acquisition_unit_configure(
+	const input_system_ID_t			ID,
+	const sub_system_ID_t			sub_id,
+	const ib_buffer_t* const		cfg)
+{
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(sub_id == ACQUISITION_UNIT0_ID);
+	assert(cfg != NULL);
+
+	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);
+
+	return;
+}
+
+
+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 != NULL);
+
+	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);
+	return;
+}
+
+static void input_system_network_configure(
+	const input_system_ID_t				ID,
+	const input_system_network_cfg_t * const 	cfg)
+{
+	uint32_t sub_id;
+
+	assert(ID < N_INPUT_SYSTEM_ID);
+	assert(cfg != NULL);
+
+	// 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]));
+	}
+
+	return;
+}
+
+static input_system_error_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 :
+
+			break;
+
+		case INPUT_SYSTEM_SOURCE_PRBS :
+
+			break;
+
+		case INPUT_SYSTEM_SOURCE_FIFO :
+			break;
+
+		default :
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			break;
+
+	} // end of switch (source_type)
+
+	// Set input selector.
+	input_selector_cfg_for_sensor(INPUT_SYSTEM0_ID);
+
+	// Set input switch.
+	input_switch_cfg(INPUT_SYSTEM0_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_error_t input_system_configuration_commit(void)
+{
+	// The last configuration step is to configure the input buffer.
+	input_system_error_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_error_t	input_system_csi_fifo_channel_cfg(
+		uint32_t		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_error_t	input_system_csi_fifo_channel_with_counting_cfg(
+		uint32_t				ch_id,
+		uint32_t				nof_frames,
+		input_system_csi_port_t			port,
+		backend_channel_cfg_t			backend_ch,
+		uint32_t				csi_mem_reg_size,
+		uint32_t				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_error_t	input_system_csi_sram_channel_cfg(
+		uint32_t				ch_id,
+		input_system_csi_port_t			port,
+		backend_channel_cfg_t			backend_ch,
+		uint32_t				csi_mem_reg_size,
+		uint32_t				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_error_t	input_system_csi_xmem_channel_cfg(
+		uint32_t 				ch_id,
+		input_system_csi_port_t			port,
+		backend_channel_cfg_t			backend_ch,
+		uint32_t 				csi_mem_reg_size,
+		uint32_t 				csi_nof_mem_regs,
+		uint32_t 				acq_mem_reg_size,
+		uint32_t 				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_error_t	input_system_csi_xmem_acquire_only_channel_cfg(
+		uint32_t 				ch_id,
+		uint32_t 				nof_frames,
+		input_system_csi_port_t			port,
+		backend_channel_cfg_t			backend_ch,
+		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_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_error_t	input_system_csi_xmem_capture_only_channel_cfg(
+		uint32_t 				ch_id,
+		uint32_t 				nof_frames,
+		input_system_csi_port_t			port,
+		uint32_t 				csi_mem_reg_size,
+		uint32_t 				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.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_error_t	input_system_prbs_channel_cfg(
+		uint32_t 		ch_id,
+		uint32_t		nof_frames,//not used yet
+		uint32_t		seed,
+		uint32_t		sync_gen_width,
+		uint32_t		sync_gen_height,
+		uint32_t		sync_gen_hblank_cycles,
+		uint32_t		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_error_t	input_system_tpg_channel_cfg(
+		uint32_t 		ch_id,
+		uint32_t 		nof_frames,//not used yet
+		uint32_t		x_mask,
+		uint32_t		y_mask,
+		uint32_t		x_delta,
+		uint32_t		y_delta,
+		uint32_t		xy_mask,
+		uint32_t		sync_gen_width,
+		uint32_t		sync_gen_height,
+		uint32_t		sync_gen_hblank_cycles,
+		uint32_t		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_error_t	input_system_gpfifo_channel_cfg(
+		uint32_t 		ch_id,
+		uint32_t 		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_error_t input_system_configure_channel_sensor(
+	const channel_cfg_t channel)
+{
+	const uint32_t port = channel.source_cfg.csi_cfg.csi_port;
+	input_system_error_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 :
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			break;
+		case INPUT_SYSTEM_XMEM_CAPTURE :
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			break;
+		case INPUT_SYSTEM_XMEM_ACQUIRE :
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			break;
+		default :
+			return INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED;
+			break;
+	}
+	return INPUT_SYSTEM_ERR_NO_ERROR;
+}
+
+// Test flags and set structure.
+static input_system_error_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 != NULL);
+	assert(flags != NULL);
+
+	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_error_t set_csi_cfg(
+		csi_cfg_t* const 			lhs,
+		const csi_cfg_t* const 			rhs,
+		input_system_config_flags_t * const 	flags)
+{
+	uint32_t memory_required;
+	uint32_t acq_memory_required;
+
+	assert(lhs != NULL);
+	assert(flags != NULL);
+
+	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_error_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 != NULL);
+	assert(flags != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/input_system_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system_local.h
new file mode 100644
index 000000000000..3e8bd00082dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system_local.h
@@ -0,0 +1,533 @@
+/*
+ * 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 <type_support.h>
+
+#include "input_system_global.h"
+
+#include "input_system_defs.h"		/* HIVE_ISYS_GPREG_MULTICAST_A_IDX,... */
+#include "css_receiver_2400_defs.h"	/* _HRT_CSS_RECEIVER_2400_TWO_PIXEL_EN_REG_IDX, _HRT_CSS_RECEIVER_2400_CSI2_FUNC_PROG_REG_IDX,... */
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+#include "isp_capture_defs.h"
+#elif defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+/* Same name, but keep the distinction,it is a different device */
+#include "isp_capture_defs.h"
+#else
+#error "input_system_local.h: 2400_SYSTEM must be one of {2400, 2401 }"
+#endif
+#include "isp_acquisition_defs.h"
+#include "input_system_ctrl_defs.h"
+
+
+typedef enum {
+	INPUT_SYSTEM_ERR_NO_ERROR = 0,
+	INPUT_SYSTEM_ERR_GENERIC,
+	INPUT_SYSTEM_ERR_CHANNEL_ALREADY_SET,
+	INPUT_SYSTEM_ERR_CONFLICT_ON_RESOURCE,
+	INPUT_SYSTEM_ERR_PARAMETER_NOT_SUPPORTED,
+	N_INPUT_SYSTEM_ERR
+} input_system_error_t;
+
+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 {
+	ib_buffer_t		buffer_mipi[N_CAPTURE_UNIT_ID];
+	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.
+	uint32_t 	dummy_parameter;
+} target_isp_cfg_t;
+
+
+typedef struct {
+// TBD.
+	uint32_t 	dummy_parameter;
+} target_sp_cfg_t;
+
+
+typedef struct {
+// TBD.
+	uint32_t 	dummy_parameter;
+} target_strm2mem_cfg_t;
+
+struct input_switch_cfg_channel_s {
+	uint32_t hsync_data_reg[2];
+	uint32_t vsync_data_reg;
+};
+
+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;
+};
+
+struct backend_channel_cfg_s {
+	uint32_t	fmt_control_word_1; // Format config.
+	uint32_t	fmt_control_word_2;
+	uint32_t	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 {
+	uint32_t hsync_data_reg[N_RX_CHANNEL_ID * 2];
+	uint32_t vsync_data_reg;
+};
+
+// Configuration of a channel.
+struct channel_cfg_s {
+	uint32_t		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.
+	ib_buffer_t			csi_buffer[N_CSI_PORTS];	input_system_config_flags_t	csi_buffer_flags[N_CSI_PORTS];
+	ib_buffer_t			acquisition_buffer_unique;	input_system_config_flags_t	acquisition_buffer_unique_flags;
+	uint32_t			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;
+
+/*
+ * 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;
+
+typedef enum {
+	MIPI_PREDICTOR_NONE = 0,
+	MIPI_PREDICTOR_TYPE1,
+	MIPI_PREDICTOR_TYPE2,
+	N_MIPI_PREDICTOR_TYPES
+} mipi_predictor_t;
+
+typedef enum {
+	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
+} mipi_compressor_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;
+
+typedef struct rx_cfg_s		rx_cfg_t;
+
+/*
+ * Applied per port
+ */
+struct rx_cfg_s {
+	rx_mode_t			mode;	/* The HW config */
+	mipi_port_ID_t		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;
+};
+
+/* NOTE: The base has already an offset of 0x0100 */
+static const hrt_address MIPI_PORT_OFFSET[N_MIPI_PORT_ID] = {
+	0x00000000UL,
+	0x00000100UL,
+	0x00000200UL};
+
+static const mipi_lane_cfg_t MIPI_PORT_MAXLANES[N_MIPI_PORT_ID] = {
+	MIPI_4LANE_CFG,
+	MIPI_1LANE_CFG,
+	MIPI_2LANE_CFG};
+
+static const bool MIPI_PORT_ACTIVE[N_RX_MODE][N_MIPI_PORT_ID] = {
+	{true, true, false},
+	{true, true, false},
+	{true, true, false},
+	{true, true, false},
+	{true, true, true},
+	{true, true, true},
+	{true, true, true},
+	{true, true, true}};
+
+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}};
+
+static const hrt_address 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;
+	uint32_t	timeout_count;
+	uint16_t	init_count;
+	uint16_t	raw16_18;
+	uint32_t	sync_count;		/*4 x uint8_t */
+	uint32_t	rx_count;		/*4 x uint8_t */
+	uint8_t		lane_sync_count[MIPI_4LANE_CFG];
+	uint8_t		lane_rx_count[MIPI_4LANE_CFG];
+};
+
+struct rx_channel_state_s {
+	uint32_t	comp_scheme0;
+	uint32_t	comp_scheme1;
+	mipi_predictor_t		pred[N_MIPI_FORMAT_CUSTOM];
+	mipi_compressor_t		comp[N_MIPI_FORMAT_CUSTOM];
+};
+
+struct receiver_state_s {
+	uint8_t	fs_to_ls_delay;
+	uint8_t	ls_to_data_delay;
+	uint8_t	data_to_le_delay;
+	uint8_t	le_to_fe_delay;
+	uint8_t	fe_to_fs_delay;
+	uint8_t	le_to_fs_delay;
+	bool	is_two_ppc;
+	int	backend_rst;
+	uint16_t	raw18;
+	bool		force_raw8;
+	uint16_t	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/atomisp2/css2400/hive_isp_css_common/host/input_system_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system_private.h
new file mode 100644
index 000000000000..ed1b947b00f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/input_system_private.h
@@ -0,0 +1,116 @@
+/*
+ * 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 mipi_port_ID_t			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 mipi_port_ID_t			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/atomisp2/css2400/hive_isp_css_common/host/irq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq.c
new file mode 100644
index 000000000000..6b58bc13dc1b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq.c
@@ -0,0 +1,448 @@
+/*
+ * 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 */
+
+#include "platform_support.h"			/* hrt_sleep() */
+
+STORAGE_CLASS_INLINE void irq_wait_for_write_complete(
+	const irq_ID_t		ID);
+
+STORAGE_CLASS_INLINE bool any_irq_channel_enabled(
+	const irq_ID_t				ID);
+
+STORAGE_CLASS_INLINE irq_ID_t virq_get_irq_id(
+	const virq_id_t		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 virq_id_t 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 != NULL);
+
+/* 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 != NULL)
+		*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,
+	irq_controller_state_t		*state)
+{
+	assert(ID < N_IRQ_ID);
+	assert(state != NULL);
+
+	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 virq_id_t				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(
+	virq_info_t					*irq_info)
+{
+	enum hrt_isp_css_irq_status irq_status = hrt_isp_css_irq_status_error;
+	irq_ID_t ID;
+
+	assert(irq_info != NULL);
+
+	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(
+	virq_info_t					*irq_info)
+{
+	irq_ID_t ID;
+
+	assert(irq_info != NULL);
+
+	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(
+	virq_id_t					*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 != NULL);
+
+/* 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] == (virq_id_t)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 != NULL)
+		*irq_id = (virq_id_t)idx;
+
+return status;
+}
+
+STORAGE_CLASS_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));
+}
+
+STORAGE_CLASS_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);
+}
+
+STORAGE_CLASS_INLINE irq_ID_t virq_get_irq_id(
+	const virq_id_t		irq_ID,
+	unsigned int		*channel_ID)
+{
+	irq_ID_t ID;
+
+	assert(channel_ID != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/irq_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq_local.h
new file mode 100644
index 000000000000..f522dfd1a9f1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq_local.h
@@ -0,0 +1,136 @@
+/*
+ * 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
+
+typedef struct virq_info_s					virq_info_t;
+typedef struct irq_controller_state_s		irq_controller_state_t;
+
+
+typedef enum {
+	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,
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+	virq_isp_pmem_error        = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_ISP_PMEM_ERROR_BIT_ID,
+#elif defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+	virq_isys_2401             = IRQ0_ID_OFFSET + HIVE_GP_DEV_IRQ_IS2401_BIT_ID,
+#else
+#error "irq_local.h: 2400_SYSTEM must be one of {2400, 2401 }"
+#endif
+	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
+} virq_id_t;
+
+struct virq_info_s {
+	hrt_data		irq_status_reg[N_IRQ_ID];
+};
+
+struct irq_controller_state_s {
+	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/atomisp2/css2400/hive_isp_css_common/host/irq_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq_private.h
new file mode 100644
index 000000000000..eb325e870e88
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/irq_private.h
@@ -0,0 +1,44 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/isp.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp.c
new file mode 100644
index 000000000000..47c21e486c25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp.c
@@ -0,0 +1,129 @@
+/*
+ * 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 "isp.h"
+
+#ifndef __INLINE_ISP__
+#include "isp_private.h"
+#endif /* __INLINE_ISP__ */
+
+#include "assert_support.h"
+#include "platform_support.h"			/* hrt_sleep() */
+
+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 != NULL);
+	assert(stall != NULL);
+
+#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 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 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);
+	hrt_sleep();
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp_local.h
new file mode 100644
index 000000000000..5dcc52dff3dd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp_local.h
@@ -0,0 +1,57 @@
+/*
+ * 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 <stdbool.h>
+
+#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/atomisp2/css2400/hive_isp_css_common/host/isp_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp_private.h
new file mode 100644
index 000000000000..7f63255d3fc5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/isp_private.h
@@ -0,0 +1,157 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 uint16_t		x0,
+	const uint16_t		x1)
+{
+	uint32_t 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/atomisp2/css2400/hive_isp_css_common/host/mmu.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu.c
new file mode 100644
index 000000000000..b75d0f85d524
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu.c
@@ -0,0 +1,50 @@
+/*
+ * 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"
+
+#ifndef __INLINE_MMU__
+#include "mmu_private.h"
+#endif /* __INLINE_MMU__ */
+
+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/atomisp2/css2400/hive_isp_css_common/host/mmu_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu_local.h
new file mode 100644
index 000000000000..7c3ad157189f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/mmu_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu_private.h
new file mode 100644
index 000000000000..392b6cc24e8f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/mmu_private.h
@@ -0,0 +1,44 @@
+/*
+ * 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_PRIVATE_H_INCLUDED__
+#define __MMU_PRIVATE_H_INCLUDED__
+
+#include "mmu_public.h"
+
+#include "device_access.h"
+
+#include "assert_support.h"
+
+STORAGE_CLASS_MMU_H 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;
+}
+
+STORAGE_CLASS_MMU_H 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_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp.c
new file mode 100644
index 000000000000..db694d3a6fbb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp.c
@@ -0,0 +1,81 @@
+/*
+ * 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 != NULL);
+	assert(stall != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_common/host/sp_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp_local.h
new file mode 100644
index 000000000000..3c70b8fdb532
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp_local.h
@@ -0,0 +1,101 @@
+/*
+ * 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)sp_address_of(var), \
+		(uint32_t)(value))
+
+#define store_sp_ptr(var, value) \
+	sp_dmem_store_uint32(SP0_ID, (unsigned)sp_address_of(var), \
+		(uint32_t)(value))
+
+#define load_sp_uint(var) \
+	sp_dmem_load_uint32(SP0_ID, (unsigned)sp_address_of(var))
+
+#define load_sp_array_uint8(array_name, index) \
+	sp_dmem_load_uint8(SP0_ID, (unsigned)sp_address_of(array_name) + \
+		(index)*sizeof(uint8_t))
+
+#define load_sp_array_uint16(array_name, index) \
+	sp_dmem_load_uint16(SP0_ID, (unsigned)sp_address_of(array_name) + \
+		(index)*sizeof(uint16_t))
+
+#define load_sp_array_uint(array_name, index) \
+	sp_dmem_load_uint32(SP0_ID, (unsigned)sp_address_of(array_name) + \
+		(index)*sizeof(uint32_t))
+
+#define store_sp_var(var, data, bytes) \
+	sp_dmem_store(SP0_ID, (unsigned)sp_address_of(var), data, bytes)
+
+#define store_sp_array_uint8(array_name, index, value) \
+	sp_dmem_store_uint8(SP0_ID, (unsigned)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)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)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)sp_address_of(var) + \
+		offset, data, bytes)
+
+#define load_sp_var(var, data, bytes) \
+	sp_dmem_load(SP0_ID, (unsigned)sp_address_of(var), data, bytes)
+
+#define load_sp_var_with_offset(var, offset, data, bytes) \
+	sp_dmem_load(SP0_ID, (unsigned)sp_address_of(var) + offset, \
+		data, bytes)
+
+#endif /* __SP_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp_private.h
new file mode 100644
index 000000000000..e6283bf67ad3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/sp_private.h
@@ -0,0 +1,163 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/system_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/system_local.h
new file mode 100644
index 000000000000..111b346dfafb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/system_local.h
@@ -0,0 +1,306 @@
+/*
+ * 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 __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
+/* This interface is deprecated */
+/*#include "hive_isp_css_custom_host_hrt.h"*/
+#endif
+
+#include "system_global.h"
+
+#ifdef __FIST__
+#define HRT_ADDRESS_WIDTH	32		/* Surprise, this is a local property and even differs per platform */
+#else
+/* HRT assumes 32 by default (see Linux/include/hrt/hive_types.h), overrule it in case it is different */
+#undef HRT_ADDRESS_WIDTH
+#define HRT_ADDRESS_WIDTH	64		/* Surprise, this is a local property */
+#endif
+
+#if !defined(__KERNEL__) || (1==1)
+/* This interface is deprecated */
+#include "hrt/hive_types.h"
+#else  /* __KERNEL__ */
+#include <linux/types.h>
+
+#if HRT_ADDRESS_WIDTH==64
+typedef uint64_t			hrt_address;
+#elif HRT_ADDRESS_WIDTH==32
+typedef uint32_t			hrt_address;
+#else
+#error "system_local.h: HRT_ADDRESS_WIDTH must be one of {32,64}"
+#endif
+
+typedef uint32_t			hrt_vaddress;
+typedef uint32_t			hrt_data;
+#endif /* __KERNEL__ */
+
+/*
+ * Cell specific address maps
+ */
+#if HRT_ADDRESS_WIDTH==64
+
+#define GP_FIFO_BASE   ((hrt_address)0x0000000000090104)		/* This is NOT a base address */
+
+/* DDR */
+static const hrt_address DDR_BASE[N_DDR_ID] = {
+	(hrt_address)0x0000000120000000ULL};
+
+/* ISP */
+static const hrt_address ISP_CTRL_BASE[N_ISP_ID] = {
+	(hrt_address)0x0000000000020000ULL};
+
+static const hrt_address ISP_DMEM_BASE[N_ISP_ID] = {
+	(hrt_address)0x0000000000200000ULL};
+
+static const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID] = {
+	(hrt_address)0x0000000000100000ULL};
+
+static const hrt_address ISP_VAMEM_BASE[N_VAMEM_ID] = {
+	(hrt_address)0x00000000001C0000ULL,
+	(hrt_address)0x00000000001D0000ULL,
+	(hrt_address)0x00000000001E0000ULL};
+
+static const hrt_address ISP_HMEM_BASE[N_HMEM_ID] = {
+	(hrt_address)0x00000000001F0000ULL};
+
+/* SP */
+static const hrt_address SP_CTRL_BASE[N_SP_ID] = {
+	(hrt_address)0x0000000000010000ULL};
+
+static const hrt_address SP_DMEM_BASE[N_SP_ID] = {
+	(hrt_address)0x0000000000300000ULL};
+
+static const hrt_address SP_PMEM_BASE[N_SP_ID] = {
+	(hrt_address)0x00000000000B0000ULL};
+
+/* MMU */
+#if defined (IS_ISP_2400_MAMOIADA_SYSTEM) || defined (IS_ISP_2401_MAMOIADA_SYSTEM)
+/*
+ * MMU0_ID: The data MMU
+ * MMU1_ID: The icache MMU
+ */
+static const hrt_address MMU_BASE[N_MMU_ID] = {
+	(hrt_address)0x0000000000070000ULL,
+	(hrt_address)0x00000000000A0000ULL};
+#else
+#error "system_local.h: SYSTEM must be one of {2400, 2401 }"
+#endif
+
+/* DMA */
+static const hrt_address DMA_BASE[N_DMA_ID] = {
+	(hrt_address)0x0000000000040000ULL};
+
+/* IRQ */
+static const hrt_address IRQ_BASE[N_IRQ_ID] = {
+	(hrt_address)0x0000000000000500ULL,
+	(hrt_address)0x0000000000030A00ULL,
+	(hrt_address)0x000000000008C000ULL,
+	(hrt_address)0x0000000000090200ULL};
+/*
+	(hrt_address)0x0000000000000500ULL};
+ */
+
+/* GDC */
+static const hrt_address GDC_BASE[N_GDC_ID] = {
+	(hrt_address)0x0000000000050000ULL,
+	(hrt_address)0x0000000000060000ULL};
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+static const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID] = {
+	(hrt_address)0x0000000000000000ULL};
+
+/*
+static const hrt_address GP_REGS_BASE[N_GP_REGS_ID] = {
+	(hrt_address)0x0000000000000000ULL};
+
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	(hrt_address)0x0000000000090000ULL};
+*/
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	(hrt_address)0x0000000000000000ULL};
+
+/*GP TIMER , all timer registers are inter-twined,
+ * so, having multiple base addresses for
+ * different timers does not help*/
+static const hrt_address GP_TIMER_BASE =
+	(hrt_address)0x0000000000000600ULL;
+/* GPIO */
+static const hrt_address GPIO_BASE[N_GPIO_ID] = {
+	(hrt_address)0x0000000000000400ULL};
+
+/* TIMED_CTRL */
+static const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID] = {
+	(hrt_address)0x0000000000000100ULL};
+
+
+/* INPUT_FORMATTER */
+static const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID] = {
+	(hrt_address)0x0000000000030000ULL,
+	(hrt_address)0x0000000000030200ULL,
+	(hrt_address)0x0000000000030400ULL,
+	(hrt_address)0x0000000000030600ULL}; /* memcpy() */
+
+/* INPUT_SYSTEM */
+static const hrt_address INPUT_SYSTEM_BASE[N_INPUT_SYSTEM_ID] = {
+	(hrt_address)0x0000000000080000ULL};
+/*	(hrt_address)0x0000000000081000ULL, */ /* capture A */
+/*	(hrt_address)0x0000000000082000ULL, */ /* capture B */
+/*	(hrt_address)0x0000000000083000ULL, */ /* capture C */
+/*	(hrt_address)0x0000000000084000ULL, */ /* Acquisition */
+/*	(hrt_address)0x0000000000085000ULL, */ /* DMA */
+/*	(hrt_address)0x0000000000089000ULL, */ /* ctrl */
+/*	(hrt_address)0x000000000008A000ULL, */ /* GP regs */
+/*	(hrt_address)0x000000000008B000ULL, */ /* FIFO */
+/*	(hrt_address)0x000000000008C000ULL, */ /* IRQ */
+
+/* RX, the MIPI lane control regs start at offset 0 */
+static const hrt_address RX_BASE[N_RX_ID] = {
+	(hrt_address)0x0000000000080100ULL};
+
+#elif HRT_ADDRESS_WIDTH==32
+
+#define GP_FIFO_BASE   ((hrt_address)0x00090104)		/* This is NOT a base address */
+
+/* DDR : Attention, this value not defined in 32-bit */
+static const hrt_address DDR_BASE[N_DDR_ID] = {
+	(hrt_address)0x00000000UL};
+
+/* ISP */
+static const hrt_address ISP_CTRL_BASE[N_ISP_ID] = {
+	(hrt_address)0x00020000UL};
+
+static const hrt_address ISP_DMEM_BASE[N_ISP_ID] = {
+	(hrt_address)0x00200000UL};
+
+static const hrt_address ISP_BAMEM_BASE[N_BAMEM_ID] = {
+	(hrt_address)0x100000UL};
+
+static const hrt_address ISP_VAMEM_BASE[N_VAMEM_ID] = {
+	(hrt_address)0xffffffffUL,
+	(hrt_address)0xffffffffUL,
+	(hrt_address)0xffffffffUL};
+
+static const hrt_address ISP_HMEM_BASE[N_HMEM_ID] = {
+	(hrt_address)0xffffffffUL};
+
+/* SP */
+static const hrt_address SP_CTRL_BASE[N_SP_ID] = {
+	(hrt_address)0x00010000UL};
+
+static const hrt_address SP_DMEM_BASE[N_SP_ID] = {
+	(hrt_address)0x00300000UL};
+
+static const hrt_address SP_PMEM_BASE[N_SP_ID] = {
+	(hrt_address)0x000B0000UL};
+
+/* MMU */
+#if defined (IS_ISP_2400_MAMOIADA_SYSTEM) || defined (IS_ISP_2401_MAMOIADA_SYSTEM)
+/*
+ * MMU0_ID: The data MMU
+ * MMU1_ID: The icache MMU
+ */
+static const hrt_address MMU_BASE[N_MMU_ID] = {
+	(hrt_address)0x00070000UL,
+	(hrt_address)0x000A0000UL};
+#else
+#error "system_local.h: SYSTEM must be one of {2400, 2401 }"
+#endif
+
+/* DMA */
+static const hrt_address DMA_BASE[N_DMA_ID] = {
+	(hrt_address)0x00040000UL};
+
+/* IRQ */
+static const hrt_address IRQ_BASE[N_IRQ_ID] = {
+	(hrt_address)0x00000500UL,
+	(hrt_address)0x00030A00UL,
+	(hrt_address)0x0008C000UL,
+	(hrt_address)0x00090200UL};
+/*
+	(hrt_address)0x00000500UL};
+ */
+
+/* GDC */
+static const hrt_address GDC_BASE[N_GDC_ID] = {
+	(hrt_address)0x00050000UL,
+	(hrt_address)0x00060000UL};
+
+/* FIFO_MONITOR (not a subset of GP_DEVICE) */
+static const hrt_address FIFO_MONITOR_BASE[N_FIFO_MONITOR_ID] = {
+	(hrt_address)0x00000000UL};
+
+/*
+static const hrt_address GP_REGS_BASE[N_GP_REGS_ID] = {
+	(hrt_address)0x00000000UL};
+
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	(hrt_address)0x00090000UL};
+*/
+
+/* GP_DEVICE (single base for all separate GP_REG instances) */
+static const hrt_address GP_DEVICE_BASE[N_GP_DEVICE_ID] = {
+	(hrt_address)0x00000000UL};
+
+/*GP TIMER , all timer registers are inter-twined,
+ * so, having multiple base addresses for
+ * different timers does not help*/
+static const hrt_address GP_TIMER_BASE =
+	(hrt_address)0x00000600UL;
+
+/* GPIO */
+static const hrt_address GPIO_BASE[N_GPIO_ID] = {
+	(hrt_address)0x00000400UL};
+
+/* TIMED_CTRL */
+static const hrt_address TIMED_CTRL_BASE[N_TIMED_CTRL_ID] = {
+	(hrt_address)0x00000100UL};
+
+
+/* INPUT_FORMATTER */
+static const hrt_address INPUT_FORMATTER_BASE[N_INPUT_FORMATTER_ID] = {
+	(hrt_address)0x00030000UL,
+	(hrt_address)0x00030200UL,
+	(hrt_address)0x00030400UL};
+/*	(hrt_address)0x00030600UL, */ /* memcpy() */
+
+/* INPUT_SYSTEM */
+static const hrt_address INPUT_SYSTEM_BASE[N_INPUT_SYSTEM_ID] = {
+	(hrt_address)0x00080000UL};
+/*	(hrt_address)0x00081000UL, */ /* capture A */
+/*	(hrt_address)0x00082000UL, */ /* capture B */
+/*	(hrt_address)0x00083000UL, */ /* capture C */
+/*	(hrt_address)0x00084000UL, */ /* Acquisition */
+/*	(hrt_address)0x00085000UL, */ /* DMA */
+/*	(hrt_address)0x00089000UL, */ /* ctrl */
+/*	(hrt_address)0x0008A000UL, */ /* GP regs */
+/*	(hrt_address)0x0008B000UL, */ /* FIFO */
+/*	(hrt_address)0x0008C000UL, */ /* IRQ */
+
+/* RX, the MIPI lane control regs start at offset 0 */
+static const hrt_address RX_BASE[N_RX_ID] = {
+	(hrt_address)0x00080100UL};
+
+#else
+#error "system_local.h: HRT_ADDRESS_WIDTH must be one of {32,64}"
+#endif
+
+#endif /* __SYSTEM_LOCAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl.c
new file mode 100644
index 000000000000..cd12d74024f7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl.c
@@ -0,0 +1,74 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_local.h
new file mode 100644
index 000000000000..e570813af28d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_private.h
new file mode 100644
index 000000000000..fb0fdbb88435
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/timed_ctrl_private.h
@@ -0,0 +1,34 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/vamem_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vamem_local.h
new file mode 100644
index 000000000000..c4e99afe0d29
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vamem_local.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/host/vamem_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vamem_private.h
new file mode 100644
index 000000000000..5e05258673d5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vamem_private.h
@@ -0,0 +1,37 @@
+/*
+ * 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_PRIVATE_H_INCLUDED__
+#define __VAMEM_PRIVATE_H_INCLUDED__
+
+#include "vamem_public.h"
+
+#include <hrt/api.h>
+
+#include "assert_support.h"
+
+
+STORAGE_CLASS_ISP_C void isp_vamem_store(
+	const vamem_ID_t	ID,
+	vamem_data_t		*addr,
+	const vamem_data_t	*data,
+	const size_t		size) /* in vamem_data_t */
+{
+	assert(ID < N_VAMEM_ID);
+	assert(ISP_VAMEM_BASE[ID] != (hrt_address)-1);
+	hrt_master_port_store(ISP_VAMEM_BASE[ID] + (unsigned)addr, data, size * sizeof(vamem_data_t));
+}
+
+
+#endif /* __VAMEM_PRIVATE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem.c
new file mode 100644
index 000000000000..ea22c23fc7a4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem.c
@@ -0,0 +1,258 @@
+/*
+ * 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"
+#include "platform_support.h"			/* hrt_sleep() */
+
+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 target_bit,
+	hive_uedge src,
+	unsigned src_start,
+	unsigned src_end)
+{
+	unsigned int start_elem = target_bit / uedge_bits;
+	unsigned int start_bit  = target_bit % uedge_bits;
+	unsigned 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 bits_to_write = elem_bits;
+		unsigned start_bit = elem_bits * index;
+		unsigned 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 i;
+	hive_uedge *data;
+	unsigned 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;
+	}
+	hrt_sleep(); /* 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 i;
+	unsigned 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
+	hrt_sleep(); /* 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		elems) /* In t_vmem_elem */
+{
+	unsigned 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		elems) /* In t_vmem_elem */
+{
+	unsigned 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 height,
+	unsigned width,
+	unsigned stride_to,  /* In t_vmem_elem */
+	unsigned stride_from /* In t_vmem_elem */)
+{
+	unsigned 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 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 height,
+	unsigned width,
+	unsigned stride_to,  /* In t_vmem_elem */
+	unsigned stride_from /* In t_vmem_elem */)
+{
+	unsigned 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 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/atomisp2/css2400/hive_isp_css_common/host/vmem_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem_local.h
new file mode 100644
index 000000000000..de85644b885e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem_local.h
@@ -0,0 +1,55 @@
+/*
+ * 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 uint16_t 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		elems); /* In t_vmem_elem */
+
+void isp_vmem_store(
+	const isp_ID_t		ID,
+	t_vmem_elem		*to,
+	const t_vmem_elem	*from,
+	unsigned		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		height,
+	unsigned		width,
+	unsigned		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		height,
+	unsigned		width,
+	unsigned		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/atomisp2/css2400/hive_isp_css_common/host/vmem_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem_private.h
new file mode 100644
index 000000000000..f48d1281b5a7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem_private.h
@@ -0,0 +1,20 @@
+/*
+ * 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/atomisp2/css2400/hive_isp_css_common/input_formatter_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/input_formatter_global.h
new file mode 100644
index 000000000000..5654d911db65
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/input_formatter_global.h
@@ -0,0 +1,130 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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 {
+	uint32_t	start_line;
+	uint32_t	start_column;
+	uint32_t	left_padding;
+	uint32_t	cropped_height;
+	uint32_t	cropped_width;
+	uint32_t	deinterleaving;
+	uint32_t	buf_vecs;
+	uint32_t	buf_start_index;
+	uint32_t	buf_increment;
+	uint32_t	buf_eol_offset;
+	uint32_t	is_yuv420_format;
+	uint32_t	block_no_reqs;
+};
+
+#define DEFAULT_IF_CONFIG \
+{ \
+	0,          /* start_line */\
+	0,          /* start_column */\
+	0,          /* left_padding */\
+	0,          /* cropped_height */\
+	0,          /* cropped_width */\
+	0,          /* deinterleaving */\
+	0,          /*.buf_vecs */\
+	0,          /* buf_start_index */\
+	0,          /* buf_increment */\
+	0,          /* buf_eol_offset */\
+	false,      /* is_yuv420_format */\
+	false       /* 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 uint8_t HIVE_IF_SWITCH_CODE[N_INPUT_FORMATTER_ID];
+
+#endif /* __INPUT_FORMATTER_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/input_system_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/input_system_global.h
new file mode 100644
index 000000000000..9ba36525e8d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/input_system_global.h
@@ -0,0 +1,155 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_GLOBAL_H_INCLUDED__
+#define __INPUT_SYSTEM_GLOBAL_H_INCLUDED__
+
+#define IS_INPUT_SYSTEM_VERSION_2
+
+#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 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 input_system_cfg_s {
+	uint32_t	no_side_band;
+	uint32_t	fmt_type;
+	uint32_t	ch_id;
+	uint32_t	input_mode;
+};
+
+struct sync_generator_cfg_s {
+	uint32_t	width;
+	uint32_t	height;
+	uint32_t	hblank_cycles;
+	uint32_t	vblank_cycles;
+};
+
+/* MW: tpg & prbs are exclusive */
+struct tpg_cfg_s {
+	uint32_t	x_mask;
+	uint32_t	y_mask;
+	uint32_t	x_delta;
+	uint32_t	y_delta;
+	uint32_t	xy_mask;
+	sync_generator_cfg_t sync_gen_cfg;
+};
+
+struct prbs_cfg_s {
+	uint32_t	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 input_system_cfg_s input_system_cfg_t;
+
+struct ib_buffer_s {
+	uint32_t	mem_reg_size;
+	uint32_t	nof_mem_regs;
+	uint32_t	mem_reg_addr;
+};
+
+typedef struct ib_buffer_s	ib_buffer_t;
+
+struct csi_cfg_s {
+	uint32_t			csi_port;
+    buffering_mode_t	buffering_mode;
+	ib_buffer_t			csi_buffer;
+	ib_buffer_t			acquisition_buffer;
+	uint32_t			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.
+} input_system_cfg_flag_t;
+
+typedef uint32_t input_system_config_flags_t; 
+
+#endif /* __INPUT_SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/irq_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/irq_global.h
new file mode 100644
index 000000000000..64554d80dc0b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/irq_global.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+#if defined(IS_ISP_2400_MAMOIADA_SYSTEM)
+/*#define CSS_RECEIVER testbench_isp_inp_sys_csi_receiver*/
+#include "hive_isp_css_irq_types_hrt.h"	/* enum	hrt_isp_css_irq */
+#elif defined(IS_ISP_2401_MAMOIADA_SYSTEM)
+/*#define CSS_RECEIVER testbench_isp_is_2400_inp_sys_csi_receiver*/
+#include "hive_isp_css_2401_irq_types_hrt.h"	/* enum	hrt_isp_css_irq */
+#else
+#error "irq_global.h: 2400_SYSTEM must be one of {2400, 2401 }"
+#endif
+#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/atomisp2/css2400/hive_isp_css_common/isp_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/isp_global.h
new file mode 100644
index 000000000000..14d574849a5b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/isp_global.h
@@ -0,0 +1,115 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.h>
+
+#if defined (HAS_ISP_2401_MAMOIADA)
+#define IS_ISP_2401_MAMOIADA
+
+#include "isp2401_mamoiada_params.h"
+#elif defined (HAS_ISP_2400_MAMOIADA)
+#define IS_ISP_2400_MAMOIADA
+
+#include "isp2400_mamoiada_params.h"
+#else
+#error "isp_global_h: ISP_2400_MAMOIDA must be one of {2400, 2401 }"
+#endif
+
+#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 ISP2401
+#ifdef PIPE_GENERATION
+#define PIPEMEM(x) MEM(x)
+#define ISP_NWAY   (1<<ISP_NWAY_LOG2)
+#else
+#define PIPEMEM(x)
+#endif
+
+#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
+
+#ifdef ISP2401
+#define ISP_BAMEM_ALIGN_ELEM ISP_VMEM_ALIGN_ELEM
+#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
+#endif /* __ISP_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/mmu_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/mmu_global.h
new file mode 100644
index 000000000000..83ca418c8ff2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/mmu_global.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/resource_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/resource_global.h
new file mode 100644
index 000000000000..01c915c033a9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/resource_global.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_GLOBAL_H_INCLUDED__
+#define __RESOURCE_GLOBAL_H_INCLUDED__
+
+#define IS_RESOURCE_VERSION_1
+
+typedef enum {
+	DMA_CHANNEL_RESOURCE_TYPE,
+	IRQ_CHANNEL_RESOURCE_TYPE,
+	MEM_SECTION_RESOURCE_TYPE,
+	N_RESOURCE_TYPE
+} resource_type_ID_t;
+
+typedef enum {
+	PERMANENT_RESOURCE_RESERVATION,
+	PERSISTENT_RESOURCE_RESERVATION,
+	DEDICTATED_RESOURCE_RESERVATION,
+	SHARED_RESOURCE_RESERVATION,
+	N_RESOURCE_RESERVATION
+} resource_reservation_t;
+
+#endif /* __RESOURCE_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/sp_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/sp_global.h
new file mode 100644
index 000000000000..6ec4e590e3b4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/sp_global.h
@@ -0,0 +1,93 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.h>
+
+#if defined(HAS_SP_2401)
+#define IS_SP_2401
+/* 2401 uses 2400 */
+#include <scalar_processor_2400_params.h>
+#elif defined(HAS_SP_2400)
+#define IS_SP_2400
+
+#include <scalar_processor_2400_params.h>
+#else
+#error "sp_global.h: SP_2400 must be one of {2400, 2401 }"
+#endif
+
+#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/atomisp2/css2400/hive_isp_css_common/system_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/system_global.h
new file mode 100644
index 000000000000..d803efd7400a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/system_global.h
@@ -0,0 +1,348 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_GLOBAL_H_INCLUDED__
+#define __SYSTEM_GLOBAL_H_INCLUDED__
+
+#include <hive_isp_css_defs.h>
+#include <type_support.h>
+
+/*
+ * 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
+ */
+#define ISP_DMA_MAX_BURST_LENGTH	128
+
+/*
+ * Create a list of HAS and IS properties that defines the system
+ *
+ * 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 IS_ISP_2400_SYSTEM
+/*
+ * Since this file is visible everywhere and the system definition
+ * macros are not, detect the separate definitions for {host, SP, ISP}
+ *
+ * The 2401 system has the nice property that it uses a vanilla 2400 SP
+ * so the SP will believe it is a 2400 system rather than 2401...
+ */
+//#if defined(SYSTEM_hive_isp_css_2401_system) || defined(__isp2401_mamoiada) || defined(__scalar_processor_2401)
+#if defined(SYSTEM_hive_isp_css_2401_system) || defined(__isp2401_mamoiada)
+#define IS_ISP_2401_MAMOIADA_SYSTEM
+#define HAS_ISP_2401_MAMOIADA
+#define HAS_SP_2400
+//#elif defined(SYSTEM_hive_isp_css_2400_system) || defined(__isp2400_mamoiada) || defined(__scalar_processor_2400)
+#elif defined(SYSTEM_hive_isp_css_2400_system) || defined(__isp2400_mamoiada)
+#define IS_ISP_2400_MAMOIADA_SYSTEM
+#define HAS_ISP_2400_MAMOIADA
+#define HAS_SP_2400
+#else
+#error "system_global.h: 2400_SYSTEM must be one of {2400, 2401 }"
+#endif
+
+#define USE_INPUT_SYSTEM_VERSION_2
+
+#define HAS_MMU_VERSION_2
+#define HAS_DMA_VERSION_2
+#define HAS_GDC_VERSION_2
+#define HAS_VAMEM_VERSION_2
+#define HAS_HMEM_VERSION_1
+#define HAS_BAMEM_VERSION_2
+#define HAS_IRQ_VERSION_2
+#define HAS_IRQ_MAP_VERSION_2
+#define HAS_INPUT_FORMATTER_VERSION_2
+/* 2401: HAS_INPUT_SYSTEM_VERSION_2401 */
+#define HAS_INPUT_SYSTEM_VERSION_2
+#define HAS_BUFFERED_SENSOR
+#define HAS_FIFO_MONITORS_VERSION_2
+/* #define HAS_GP_REGS_VERSION_2 */
+#define HAS_GP_DEVICE_VERSION_2
+#define HAS_GPIO_VERSION_1
+#define HAS_TIMED_CTRL_VERSION_1
+#define HAS_RX_VERSION_2
+
+#define DMA_DDR_TO_VAMEM_WORKAROUND
+#define DMA_DDR_TO_HMEM_WORKAROUND
+
+/*
+ * Semi global. "HRT" is accessible from SP, but the HRT types do not fully apply
+ */
+#define HRT_VADDRESS_WIDTH	32
+//#define HRT_ADDRESS_WIDTH	64		/* Surprise, this is a local property*/
+#define HRT_DATA_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
+
+/* per-frame parameter handling support */
+#define SH_CSS_ENABLE_PER_FRAME_PARAMS
+
+typedef uint32_t			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;
+
+#if defined (IS_ISP_2401_MAMOIADA_SYSTEM)
+typedef enum {
+	MMU0_ID = 0,
+	MMU1_ID,
+	N_MMU_ID
+} mmu_ID_t;
+#elif defined (IS_ISP_2400_MAMOIADA_SYSTEM)
+typedef enum {
+	MMU0_ID = 0,
+	MMU1_ID,
+	N_MMU_ID
+} mmu_ID_t;
+#else
+#error "system_global.h: SYSTEM must be one of {2400, 2401}"
+#endif
+
+typedef enum {
+	DMA0_ID = 0,
+	N_DMA_ID
+} dma_ID_t;
+
+typedef enum {
+	GDC0_ID = 0,
+	GDC1_ID,
+	N_GDC_ID
+} gdc_ID_t;
+
+#define N_GDC_ID_CPP 2 // this extra define is needed because we want to use it also in the preprocessor, and that doesn't work with enums.
+
+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,
+	N_IRQ_ID
+} irq_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;
+
+/*
+ * Deprecated: Since all gp_reg instances are different
+ * and put in the address maps of other devices we cannot
+ * enumerate them as that assumes the instrances are the
+ * same.
+ *
+ * We define a single GP_DEVICE containing all gp_regs
+ * w.r.t. a single base address
+ *
+typedef enum {
+	GP_REGS0_ID = 0,
+	N_GP_REGS_ID
+} gp_regs_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;
+
+typedef enum {
+	MIPI_PORT0_ID = 0,
+	MIPI_PORT1_ID,
+	MIPI_PORT2_ID,
+	N_MIPI_PORT_ID
+} mipi_port_ID_t;
+
+#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 compatability */
+#define N_IA_CSS_ISP_MEMORIES   IA_CSS_NUM_MEMORIES
+#define IA_CSS_NUM_ISP_MEMORIES IA_CSS_NUM_MEMORIES
+
+#if 0
+typedef enum {
+	dev_chn, /* device channels, external resource */
+	ext_mem, /* external memories */
+	int_mem, /* internal memories */
+	int_chn  /* internal channels, user defined */
+} resource_type_t;
+
+/* if this enum is extended with other memory resources, pls also extend the function resource_to_memptr() */
+typedef enum {
+	vied_nci_dev_chn_dma_ext0,
+	int_mem_vmem0,
+	int_mem_dmem0
+} resource_id_t;
+
+/* enum listing the different memories within a program group.
+   This enum is used in the mem_ptr_t type */
+typedef enum {
+	buf_mem_invalid = 0,
+	buf_mem_vmem_prog0,
+	buf_mem_dmem_prog0
+} buf_mem_t;
+
+#endif
+#endif /* __SYSTEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/timed_ctrl_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/timed_ctrl_global.h
new file mode 100644
index 000000000000..c3e8a0104092
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/timed_ctrl_global.h
@@ -0,0 +1,56 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/vamem_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/vamem_global.h
new file mode 100644
index 000000000000..58713c6583b9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/vamem_global.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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		(1<<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 uint16_t				vamem_data_t;
+
+#endif /* __VAMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/vmem_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/vmem_global.h
new file mode 100644
index 000000000000..7867cd137f3f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/vmem_global.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_common/xmem_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/xmem_global.h
new file mode 100644
index 000000000000..1d3a43abe55d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/xmem_global.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __XMEM_GLOBAL_H_INCLUDED__
+#define __XMEM_GLOBAL_H_INCLUDED__
+
+#include "isp.h"
+
+#endif /* __XMEM_GLOBAL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/assert_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/assert_support.h
new file mode 100644
index 000000000000..92fb15d04703
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/assert_support.h
@@ -0,0 +1,103 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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__
+
+#include "storage_class.h"
+
+/**
+ * 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 */
+
+#ifdef NDEBUG
+
+#define assert(cnd) ((void)0)
+
+#else
+
+#if defined(_MSC_VER)
+#ifdef _KERNEL_MODE
+/* Windows kernel mode compilation */
+#include <wdm.h>
+#define assert(cnd) ASSERT(cnd)
+#else
+/* Windows usermode compilation */
+#include <assert.h>
+#endif
+
+#elif defined(__KERNEL__)
+#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)
+
+#elif defined(__FIST__) || defined(__GNUC__)
+
+/* enable assert for crun */
+#include "assert.h"
+
+#else /* default for unknown environments */
+#define assert(cnd) ((void)0)
+#endif
+
+#endif /* NDEBUG */
+
+#ifndef PIPE_GENERATION
+/* Deprecated OP___assert, this is still used in ~1000 places
+ * in the code. This will be removed over time.
+ * The implemenation for the pipe generation tool is in see support.isp.h */
+#define OP___assert(cnd) assert(cnd)
+
+STORAGE_CLASS_INLINE void compile_time_assert (unsigned cond)
+{
+	/* Call undefined function if cond is false */
+	extern 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/atomisp2/css2400/hive_isp_css_include/bamem.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bamem.h
new file mode 100644
index 000000000000..d71e08f27a42
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bamem.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __BAMEM_H_INCLUDED__
+#define __BAMEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the BAMEM 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 "storage_class.h"
+
+#include "system_local.h"
+#include "bamem_local.h"
+
+#ifndef __INLINE_BAMEM__
+#define STORAGE_CLASS_BAMEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_BAMEM_C
+#include "bamem_public.h"
+#else  /* __INLINE_BAMEM__ */
+#define STORAGE_CLASS_BAMEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_BAMEM_C STORAGE_CLASS_INLINE
+#include "bamem_private.h"
+#endif /* __INLINE_BAMEM__ */
+
+#endif /* __BAMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bbb_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bbb_config.h
new file mode 100644
index 000000000000..18bc5ef3d0bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bbb_config.h
@@ -0,0 +1,27 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __BBB_CONFIG_H_INCLUDED__
+#define __BBB_CONFIG_H_INCLUDED__
+/* This header contains BBB defines common to ISP and host */
+
+#define BFA_MAX_KWAY                (49)
+#define BFA_RW_LUT_SIZE             (7)
+
+#define SAD3x3_IN_SHIFT      (2) /* input right shift value for SAD3x3 */
+#define SAD3x3_OUT_SHIFT     (2) /* output right shift value for SAD3x3 */
+
+/* XCU and BMA related defines shared between host and ISP
+ * also need to be moved here */
+#endif /* __BBB_CONFIG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bitop_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bitop_support.h
new file mode 100644
index 000000000000..1b271c3c6a25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/bitop_support.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_include/cpu_mem_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/cpu_mem_support.h
new file mode 100644
index 000000000000..6d014fafb713
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/cpu_mem_support.h
@@ -0,0 +1,59 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __CPU_MEM_SUPPORT_H_INCLUDED__
+#define __CPU_MEM_SUPPORT_H_INCLUDED__
+
+#if defined (__KERNEL__)
+#include <linux/string.h> /* memset */
+#else
+#include <string.h> /* memset */
+#endif
+
+#include "sh_css_internal.h" /* sh_css_malloc and sh_css_free */
+
+static inline void*
+ia_css_cpu_mem_alloc(unsigned int size)
+{
+	return sh_css_malloc(size);
+}
+
+static inline void*
+ia_css_cpu_mem_copy(void* dst, const void* src, unsigned int size)
+{
+	if(!src || !dst)
+		return NULL;
+
+	return memcpy(dst, src, size);
+}
+
+static inline void*
+ia_css_cpu_mem_set_zero(void* dst, unsigned int size)
+{
+	if(!dst)
+		return NULL;
+
+	return memset(dst, 0, size);
+}
+
+static inline void
+ia_css_cpu_mem_free(void* ptr)
+{
+	if(!ptr)
+		return;
+
+	sh_css_free(ptr);
+}
+
+#endif /* __CPU_MEM_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/csi_rx.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/csi_rx.h
new file mode 100644
index 000000000000..0398f5802f05
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/csi_rx.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "csi_rx_local.h"
+
+#ifndef __INLINE_CSI_RX__
+#define STORAGE_CLASS_CSI_RX_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_CSI_RX_C
+#include "csi_rx_public.h"
+#else  /* __INLINE_CSI_RX__ */
+#define STORAGE_CLASS_CSI_RX_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_CSI_RX_C STORAGE_CLASS_INLINE
+#include "csi_rx_private.h"
+#endif /* __INLINE_CSI_RX__ */
+
+#endif /* __CSI_RX_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/debug.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/debug.h
new file mode 100644
index 000000000000..7d8011735033
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/debug.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "debug_local.h"
+
+#ifndef __INLINE_DEBUG__
+#define STORAGE_CLASS_DEBUG_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_DEBUG_C 
+#include "debug_public.h"
+#else  /* __INLINE_DEBUG__ */
+#define STORAGE_CLASS_DEBUG_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_DEBUG_C STORAGE_CLASS_INLINE
+#include "debug_private.h"
+#endif /* __INLINE_DEBUG__ */
+
+#endif /* __DEBUG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/device_access/device_access.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/device_access/device_access.h
new file mode 100644
index 000000000000..834e7c3e0814
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/device_access/device_access.h
@@ -0,0 +1,194 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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_types.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,
+ */
+extern void device_set_base_address(
+	const sys_address		base_addr);
+
+
+/*! Get the (sub)system base address
+
+ \return base_address,
+ */
+extern 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]
+ */
+extern 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]
+ */
+extern 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]
+ */
+extern 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]
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/dma.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/dma.h
new file mode 100644
index 000000000000..b266191f21ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/dma.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "dma_local.h"
+
+#ifndef __INLINE_DMA__
+#define STORAGE_CLASS_DMA_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_DMA_C 
+#include "dma_public.h"
+#else  /* __INLINE_DMA__ */
+#define STORAGE_CLASS_DMA_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_DMA_C STORAGE_CLASS_INLINE
+#include "dma_private.h"
+#endif /* __INLINE_DMA__ */
+
+#endif /* __DMA_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/error_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/error_support.h
new file mode 100644
index 000000000000..6e5e5dd4107d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/error_support.h
@@ -0,0 +1,70 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __ERROR_SUPPORT_H_INCLUDED__
+#define __ERROR_SUPPORT_H_INCLUDED__
+
+#if defined(_MSC_VER)
+#include <errno.h>
+/*
+ * Put here everything _MSC_VER specific not covered in
+ * "errno.h"
+ */
+#define EINVAL  22
+#define EBADE   52
+#define ENODATA 61
+#define ENOTCONN 107
+#define ENOTSUP 252
+#define ENOBUFS 233
+
+
+#elif defined(__KERNEL__)
+#include <linux/errno.h>
+/*
+ * Put here everything __KERNEL__ specific not covered in
+ * "errno.h"
+ */
+#define ENOTSUP 252
+
+#elif defined(__GNUC__)
+#include <errno.h>
+/*
+ * Put here everything __GNUC__ specific not covered in
+ * "errno.h"
+ */
+
+#else /* default is for the FIST environment */
+#include <errno.h>
+/*
+ * Put here everything FIST specific not covered in
+ * "errno.h"
+ */
+
+#endif
+
+#define verifexit(cond,error_tag)  \
+do {                               \
+	if (!(cond)){              \
+		goto EXIT;         \
+	}                          \
+} while(0)
+
+#define verifjmpexit(cond)         \
+do {                               \
+	if (!(cond)){              \
+		goto EXIT;         \
+	}                          \
+} while(0)
+
+#endif /* __ERROR_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/event_fifo.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/event_fifo.h
new file mode 100644
index 000000000000..78827c554cc3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/event_fifo.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "event_fifo_local.h"
+
+#ifndef __INLINE_EVENT__
+#define STORAGE_CLASS_EVENT_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_EVENT_C 
+#include "event_fifo_public.h"
+#else  /* __INLINE_EVENT__ */
+#define STORAGE_CLASS_EVENT_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_EVENT_C STORAGE_CLASS_INLINE
+#include "event_fifo_private.h"
+#endif /* __INLINE_EVENT__ */
+
+#endif /* __EVENT_FIFO_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/fifo_monitor.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/fifo_monitor.h
new file mode 100644
index 000000000000..3bdd260bcaa5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/fifo_monitor.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "fifo_monitor_local.h"
+
+#ifndef __INLINE_FIFO_MONITOR__
+#define STORAGE_CLASS_FIFO_MONITOR_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_FIFO_MONITOR_C 
+#include "fifo_monitor_public.h"
+#else  /* __INLINE_FIFO_MONITOR__ */
+#define STORAGE_CLASS_FIFO_MONITOR_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_FIFO_MONITOR_C STORAGE_CLASS_INLINE
+#include "fifo_monitor_private.h"
+#endif /* __INLINE_FIFO_MONITOR__ */
+
+#endif /* __FIFO_MONITOR_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gdc_device.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gdc_device.h
new file mode 100644
index 000000000000..016132ba0b7f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gdc_device.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "gdc_local.h"
+
+#ifndef __INLINE_GDC__
+#define STORAGE_CLASS_GDC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_GDC_C 
+#include "gdc_public.h"
+#else  /* __INLINE_GDC__ */
+#define STORAGE_CLASS_GDC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_GDC_C STORAGE_CLASS_INLINE
+#include "gdc_private.h"
+#endif /* __INLINE_GDC__ */
+
+#endif /* __GDC_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_device.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_device.h
new file mode 100644
index 000000000000..766d2532d8f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_device.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "gp_device_local.h"
+
+#ifndef __INLINE_GP_DEVICE__
+#define STORAGE_CLASS_GP_DEVICE_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_GP_DEVICE_C 
+#include "gp_device_public.h"
+#else  /* __INLINE_GP_DEVICE__ */
+#define STORAGE_CLASS_GP_DEVICE_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_GP_DEVICE_C STORAGE_CLASS_INLINE
+#include "gp_device_private.h"
+#endif /* __INLINE_GP_DEVICE__ */
+
+#endif /* __GP_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_timer.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_timer.h
new file mode 100644
index 000000000000..ca70f5603bf8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gp_timer.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#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 STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_GP_TIMER_C
+#include "gp_timer_public.h"   /* functions*/
+#else  /* __INLINE_GP_TIMER__ */
+#define STORAGE_CLASS_GP_TIMER_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_GP_TIMER_C STORAGE_CLASS_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/atomisp2/css2400/hive_isp_css_include/gpio.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gpio.h
new file mode 100644
index 000000000000..dec21bcb6f47
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/gpio.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "gpio_local.h"
+
+#ifndef __INLINE_GPIO__
+#define STORAGE_CLASS_GPIO_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_GPIO_C 
+#include "gpio_public.h"
+#else  /* __INLINE_GPIO__ */
+#define STORAGE_CLASS_GPIO_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_GPIO_C STORAGE_CLASS_INLINE
+#include "gpio_private.h"
+#endif /* __INLINE_GPIO__ */
+
+#endif /* __GPIO_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/hmem.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/hmem.h
new file mode 100644
index 000000000000..671dd5b5fca6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/hmem.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "hmem_local.h"
+
+#ifndef __INLINE_HMEM__
+#define STORAGE_CLASS_HMEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_HMEM_C 
+#include "hmem_public.h"
+#else  /* __INLINE_HMEM__ */
+#define STORAGE_CLASS_HMEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_HMEM_C STORAGE_CLASS_INLINE
+#include "hmem_private.h"
+#endif /* __INLINE_HMEM__ */
+
+#endif /* __HMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/csi_rx_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/csi_rx_public.h
new file mode 100644
index 000000000000..396240954bed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/csi_rx_public.h
@@ -0,0 +1,135 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 USE_INPUT_SYSTEM_VERSION_2401
+/*****************************************************
+ *
+ * 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.
+ */
+STORAGE_CLASS_CSI_RX_H 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.
+ */
+STORAGE_CLASS_CSI_RX_H 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.
+ */
+STORAGE_CLASS_CSI_RX_H void csi_rx_fe_ctrl_get_dlane_state(
+		const csi_rx_frontend_ID_t ID,
+		const uint32_t 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.
+ */
+STORAGE_CLASS_CSI_RX_H 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.
+ */
+STORAGE_CLASS_CSI_RX_H 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 offet address of the register.
+ *
+ * @return the value of the register.
+ */
+STORAGE_CLASS_CSI_RX_H 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 offet address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+STORAGE_CLASS_CSI_RX_H 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 offet address of the register.
+ *
+ * @return the value of the register.
+ */
+STORAGE_CLASS_CSI_RX_H 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 offet address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+STORAGE_CLASS_CSI_RX_H 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 /* USE_INPUT_SYSTEM_VERSION_2401 */
+#endif /* __CSI_RX_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/debug_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/debug_public.h
new file mode 100644
index 000000000000..90b4ba7e023f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/debug_public.h
@@ -0,0 +1,99 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "system_types.h"
+
+/*! brief
+ *
+ * Simple queuing trace buffer for debug data
+ * instantiatable in SP DMEM
+ *
+ * The buffer has a remote and 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 hrt_vaddress				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
+ */
+extern void debug_buffer_init(
+	const hrt_address		addr);
+
+/*! Set the offset/address of the (remote) debug buffer
+ 
+ \return none
+ */
+extern void debug_buffer_ddr_init(
+	const hrt_vaddress		addr);
+
+/*! Set the (remote) operating mode of the debug buffer
+ 
+ \return none
+ */
+extern void debug_buffer_setmode(
+	const debug_buf_mode_t	mode);
+
+#endif /* __DEBUG_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/dma_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/dma_public.h
new file mode 100644
index 000000000000..1d5e38ffe938
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/dma_public.h
@@ -0,0 +1,73 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/host/event_fifo_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/event_fifo_public.h
new file mode 100644
index 000000000000..d95bc7070f4c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/event_fifo_public.h
@@ -0,0 +1,79 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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/atomisp2/css2400/hive_isp_css_include/host/fifo_monitor_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/fifo_monitor_public.h
new file mode 100644
index 000000000000..329f5d5049f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/fifo_monitor_public.h
@@ -0,0 +1,110 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/host/gdc_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gdc_public.h
new file mode 100644
index 000000000000..d27f87a719db
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gdc_public.h
@@ -0,0 +1,59 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+ */
+STORAGE_CLASS_EXTERN 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
+ */
+STORAGE_CLASS_EXTERN 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
+ */
+STORAGE_CLASS_EXTERN int gdc_get_unity(
+	const gdc_ID_t		ID);
+
+#endif /* __GDC_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gp_device_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gp_device_public.h
new file mode 100644
index 000000000000..acbce0fd658f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gp_device_public.h
@@ -0,0 +1,58 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/host/gp_timer_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gp_timer_public.h
new file mode 100644
index 000000000000..276e2fa9b1e7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gp_timer_public.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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/atomisp2/css2400/hive_isp_css_include/host/gpio_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gpio_public.h
new file mode 100644
index 000000000000..82eaa0d48bee
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/gpio_public.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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/atomisp2/css2400/hive_isp_css_include/host/hmem_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/hmem_public.h
new file mode 100644
index 000000000000..9b8e7c92442d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/hmem_public.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <stddef.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/atomisp2/css2400/hive_isp_css_include/host/ibuf_ctrl_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ibuf_ctrl_public.h
new file mode 100644
index 000000000000..1ac0e64e539c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ibuf_ctrl_public.h
@@ -0,0 +1,93 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PUBLIC_H_INCLUDED__
+#define __IBUF_CTRL_PUBLIC_H_INCLUDED__
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+/*****************************************************
+ *
+ * Native command interface (NCI).
+ *
+ *****************************************************/
+/**
+ * @brief Get the ibuf-controller state.
+ * Get the state of the ibuf-controller regiester-set.
+ *
+ * @param[in]	id		The global unique ID of the input-buffer controller.
+ * @param[out]	state	Point to the register-state.
+ */
+STORAGE_CLASS_IBUF_CTRL_H void ibuf_ctrl_get_state(
+		const ibuf_ctrl_ID_t ID,
+		ibuf_ctrl_state_t *state);
+
+/**
+ * @brief Get the state of the ibuf-controller process.
+ * Get the state of the register set per buf-controller process.
+ *
+ * @param[in]	id			The global unique ID of the input-buffer controller.
+ * @param[in]	proc_id		The process ID.
+ * @param[out]	state		Point to the process state.
+ */
+STORAGE_CLASS_IBUF_CTRL_H void ibuf_ctrl_get_proc_state(
+		const ibuf_ctrl_ID_t ID,
+		const uint32_t proc_id,
+		ibuf_ctrl_proc_state_t *state);
+/**
+ * @brief Dump the ibuf-controller state.
+ * Dump the state of the ibuf-controller regiester-set.
+ *
+ * @param[in]	id		The global unique ID of the input-buffer controller.
+ * @param[in]	state		Pointer to the register-state.
+ */
+STORAGE_CLASS_IBUF_CTRL_H void ibuf_ctrl_dump_state(
+		const ibuf_ctrl_ID_t ID,
+		ibuf_ctrl_state_t *state);
+/** end of NCI */
+
+/*****************************************************
+ *
+ * Device level interface (DLI).
+ *
+ *****************************************************/
+/**
+ * @brief Load the register value.
+ * Load the value of the register of the ibuf-controller.
+ *
+ * @param[in]	ID	The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg	The offet address of the register.
+ *
+ * @return the value of the register.
+ */
+STORAGE_CLASS_IBUF_CTRL_H hrt_data ibuf_ctrl_reg_load(
+	const ibuf_ctrl_ID_t ID,
+	const hrt_address reg);
+
+/**
+ * @brief Store a value to the register.
+ * Store a value to the registe of the ibuf-controller.
+ *
+ * @param[in]	ID		The global unique ID for the ibuf-controller instance.
+ * @param[in]	reg		The offet address of the register.
+ * @param[in]	value	The value to be stored.
+ *
+ */
+STORAGE_CLASS_IBUF_CTRL_H void ibuf_ctrl_reg_store(
+	const ibuf_ctrl_ID_t ID,
+	const hrt_address reg,
+	const hrt_data value);
+/** end of DLI */
+
+#endif /* USE_INPUT_SYSTEM_VERSION_2401 */
+#endif /* __IBUF_CTRL_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/input_formatter_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/input_formatter_public.h
new file mode 100644
index 000000000000..2db70893daf9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/input_formatter_public.h
@@ -0,0 +1,115 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.h"
+
+/*! Reset INPUT_FORMATTER[ID]
+ 
+ \param	ID[in]				INPUT_FORMATTER identifier
+
+ \return none, reset(INPUT_FORMATTER[ID])
+ */
+extern 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
+ */
+extern 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)
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/host/input_system_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/input_system_public.h
new file mode 100644
index 000000000000..1596757fe9ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/input_system_public.h
@@ -0,0 +1,376 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 USE_INPUT_SYSTEM_VERSION_2401
+#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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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}
+ */
+extern 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])
+ */
+extern void receiver_port_enable(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t		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
+ */
+extern bool is_receiver_port_enabled(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t		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)
+ */
+extern void receiver_irq_enable(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t		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
+ */
+extern rx_irq_info_t receiver_get_irq_info(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t		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)
+ */
+extern void receiver_irq_clear(
+	const rx_ID_t				ID,
+	const mipi_port_ID_t			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 mipi_port_ID_t			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 mipi_port_ID_t		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_error_t input_system_configuration_reset(void);
+
+// Function that commits current configuration.
+// remove the argument since it should be private.
+input_system_error_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_error_t	input_system_csi_fifo_channel_cfg(
+	uint32_t				ch_id,
+	input_system_csi_port_t	port,
+	backend_channel_cfg_t	backend_ch,
+	target_cfg2400_t			target
+);
+
+input_system_error_t	input_system_csi_fifo_channel_with_counting_cfg(
+	uint32_t				ch_id,
+	uint32_t				nof_frame,
+	input_system_csi_port_t	port,
+	backend_channel_cfg_t	backend_ch,
+	uint32_t				mem_region_size,
+	uint32_t				nof_mem_regions,
+	target_cfg2400_t			target
+);
+
+
+// SRAM channel config function user
+
+input_system_error_t	input_system_csi_sram_channel_cfg(
+	uint32_t				ch_id,
+	input_system_csi_port_t	port,
+	backend_channel_cfg_t	backend_ch,
+	uint32_t				csi_mem_region_size,
+	uint32_t				csi_nof_mem_regions,
+	target_cfg2400_t 			target
+);
+
+
+//XMEM channel config function user
+
+input_system_error_t	input_system_csi_xmem_channel_cfg(
+	uint32_t 				ch_id,
+	input_system_csi_port_t port,
+	backend_channel_cfg_t	backend_ch,
+	uint32_t 				mem_region_size,
+	uint32_t 				nof_mem_regions,
+	uint32_t 				acq_mem_region_size,
+	uint32_t 				acq_nof_mem_regions,
+	target_cfg2400_t 			target,
+	uint32_t 				nof_xmem_buffers
+);
+
+input_system_error_t	input_system_csi_xmem_capture_only_channel_cfg(
+	uint32_t 				ch_id,
+	uint32_t 				nof_frames,
+	input_system_csi_port_t port,
+	uint32_t 				csi_mem_region_size,
+	uint32_t 				csi_nof_mem_regions,
+	uint32_t 				acq_mem_region_size,
+	uint32_t 				acq_nof_mem_regions,
+	target_cfg2400_t 			target
+);
+
+input_system_error_t	input_system_csi_xmem_acquire_only_channel_cfg(
+	uint32_t 				ch_id,
+	uint32_t 				nof_frames,
+	input_system_csi_port_t port,
+	backend_channel_cfg_t	backend_ch,
+	uint32_t 				acq_mem_region_size,
+	uint32_t 				acq_nof_mem_regions,
+	target_cfg2400_t 			target
+);
+
+// Non - CSI channel config function user
+
+input_system_error_t	input_system_prbs_channel_cfg(
+	uint32_t 		ch_id,
+	uint32_t		nof_frames,
+	uint32_t		seed,
+	uint32_t		sync_gen_width,
+	uint32_t		sync_gen_height,
+	uint32_t		sync_gen_hblank_cycles,
+	uint32_t		sync_gen_vblank_cycles,
+	target_cfg2400_t	target
+);
+
+
+input_system_error_t	input_system_tpg_channel_cfg(
+	uint32_t 		ch_id,
+	uint32_t 		nof_frames,//not used yet
+	uint32_t		x_mask,
+	uint32_t		y_mask,
+	uint32_t		x_delta,
+	uint32_t		y_delta,
+	uint32_t		xy_mask,
+	uint32_t		sync_gen_width,
+	uint32_t		sync_gen_height,
+	uint32_t		sync_gen_hblank_cycles,
+	uint32_t		sync_gen_vblank_cycles,
+	target_cfg2400_t	target
+);
+
+
+input_system_error_t	input_system_gpfifo_channel_cfg(
+	uint32_t 		ch_id,
+	uint32_t 		nof_frames,
+	target_cfg2400_t	target
+);
+#endif /* #ifdef USE_INPUT_SYSTEM_VERSION_2401 */
+
+#endif /* __INPUT_SYSTEM_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/irq_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/irq_public.h
new file mode 100644
index 000000000000..9aeaf8f082d2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/irq_public.h
@@ -0,0 +1,184 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+ */
+extern void irq_controller_get_state(
+	const irq_ID_t				ID,
+	irq_controller_state_t		*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])
+ */
+extern 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
+ */
+extern 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])
+ */
+extern 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[])
+ */
+extern 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])
+ */
+extern 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])
+ */
+extern 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)
+ */
+extern 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
+ */
+extern void cnd_virq_enable_channel(
+	const virq_id_t				irq_ID,
+	const bool					en);
+
+/*! Clear the state of all IRQ channels of the virtual super IRQ
+
+ \return none, clear(VIRQ.channel[])
+ */
+extern 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
+ */
+extern void virq_clear_info(
+	virq_info_t					*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[...])
+ */
+extern enum hrt_isp_css_irq_status virq_get_channel_id(
+	virq_id_t					*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[...]))
+ */
+extern enum hrt_isp_css_irq_status virq_get_channel_signals(
+	virq_info_t					*irq_info);
+
+#endif /* __IRQ_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2400_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2400_config.h
new file mode 100644
index 000000000000..ab3391716c82
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2400_config.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CONFIG_H_INCLUDED__
+#define __ISP2400_CONFIG_H_INCLUDED__
+
+#define NUM_BITS 14
+#define NUM_SLICE_ELEMS 4
+#define ROUNDMODE           ROUND_NEAREST_EVEN
+#define MAX_SHIFT_1W        (NUM_BITS-1)   /* Max number of bits a 1w input can be shifted */
+#define MAX_SHIFT_2W        (2*NUM_BITS-1) /* Max number of bits a 2w input can be shifted */
+
+#endif /* __ISP2400_CONFIG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2500_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2500_config.h
new file mode 100644
index 000000000000..4fae856f5a23
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2500_config.h
@@ -0,0 +1,29 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __ISP2500_CONFIG_H_INCLUDED__
+#define __ISP2500_CONFIG_H_INCLUDED__
+
+#define NUM_BITS            12
+#define NUM_SLICE_ELEMS     4
+#define ROUNDMODE           ROUND_NEAREST_EVEN
+#define MAX_SHIFT_1W        (NUM_BITS-1)   /* Max number of bits a 1w input can be shifted */
+#define MAX_SHIFT_2W        (2*NUM_BITS-1) /* Max number of bits a 2w input can be shifted */
+
+
+#define HAS_div_unit
+
+#define HAS_vec_sub
+
+#endif /* __ISP2500_CONFIG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2600_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2600_config.h
new file mode 100644
index 000000000000..6086be8cb0d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2600_config.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __ISP2600_CONFIG_H_INCLUDED__
+#define __ISP2600_CONFIG_H_INCLUDED__
+
+
+#define NUM_BITS 16
+
+
+#define NUM_SLICE_ELEMS 8
+#define ROUNDMODE           ROUND_NEAREST_EVEN
+#define MAX_SHIFT_1W        (NUM_BITS-1)   /* Max number of bits a 1w input can be shifted */
+#define MAX_SHIFT_2W        (2*NUM_BITS-1) /* Max number of bits a 2w input can be shifted */
+#define ISP_NWAY		32 /* Number of elements in a vector in ISP 2600 */
+
+#define HAS_div_unit
+#define HAS_1w_sqrt_u_unit
+#define HAS_2w_sqrt_u_unit
+
+#define HAS_vec_sub
+
+#endif /* __ISP2600_CONFIG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2601_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2601_config.h
new file mode 100644
index 000000000000..beceefa24ca0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp2601_config.h
@@ -0,0 +1,70 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __ISP2601_CONFIG_H_INCLUDED__
+#define __ISP2601_CONFIG_H_INCLUDED__
+
+#define NUM_BITS 16
+#define ISP_VEC_ELEMBITS NUM_BITS
+#define ISP_NWAY		32
+#define NUM_SLICE_ELEMS 4
+#define ROUNDMODE           ROUND_NEAREST_EVEN
+#define MAX_SHIFT_1W        (NUM_BITS-1)   /* Max number of bits a 1w input can be shifted */
+#define MAX_SHIFT_2W        (2*NUM_BITS-1) /* Max number of bits a 2w input can be shifted */
+
+#define HAS_div_unit
+#define HAS_bfa_unit
+#define HAS_1w_sqrt_u_unit
+#define HAS_2w_sqrt_u_unit
+
+#define HAS_vec_sub
+
+/* Bit widths and element widths defined in HW implementation of BFA */
+#define BFA_THRESHOLD_BIT_CNT       (8)
+#define BFA_THRESHOLD_MASK          ((1<<BFA_THRESHOLD_BIT_CNT)-1)
+#define BFA_SW_BIT_CNT              (7)
+#define BFA_SW_MASK                 ((1<<BFA_SW_BIT_CNT)-1)
+
+#define BFA_RW_BIT_CNT              (7)
+#define BFA_RW_MASK                 ((1<<BFA_RW_BIT_CNT)-1)
+#define BFA_RW_SLOPE_BIT_POS        (8)
+#define BFA_RW_SLOPE_BIT_SHIFT      (5)
+
+#define BFA_RW_IDX_BIT_CNT          (3)
+#define BFA_RW_FRAC_BIT_CNT         (5)
+#define BFA_RW_LUT0_FRAC_START_BIT  (0)
+#define BFA_RW_LUT0_FRAC_END_BIT    (BFA_RW_LUT0_FRAC_START_BIT+BFA_RW_FRAC_BIT_CNT-1) /* 4 */
+#define BFA_RW_LUT1_FRAC_START_BIT  (2)
+#define BFA_RW_LUT1_FRAC_END_BIT    (BFA_RW_LUT1_FRAC_START_BIT+BFA_RW_FRAC_BIT_CNT-1) /* 6 */
+/* LUT IDX end bit computation, start+idx_bit_cnt-2, one -1 comes as we count
+ * bits from 0, another -1 comes as we use 2 lut table, so idx_bit_cnt is one
+ * bit more */
+#define BFA_RW_LUT0_IDX_START_BIT   (BFA_RW_LUT0_FRAC_END_BIT+1) /* 5 */
+#define BFA_RW_LUT0_IDX_END_BIT     (BFA_RW_LUT0_IDX_START_BIT+BFA_RW_IDX_BIT_CNT-2) /* 6 */
+#define BFA_RW_LUT1_IDX_START_BIT   (BFA_RW_LUT1_FRAC_END_BIT + 1) /* 7 */
+#define BFA_RW_LUT1_IDX_END_BIT     (BFA_RW_LUT1_IDX_START_BIT+BFA_RW_IDX_BIT_CNT-2) /* 8 */
+#define BFA_RW_LUT_THRESHOLD        (1<<(BFA_RW_LUT1_IDX_END_BIT-1)) /* 0x80 : next bit after lut1 end is set */
+#define BFA_RW_LUT1_IDX_OFFSET      ((1<<(BFA_RW_IDX_BIT_CNT-1))-1) /* 3 */
+
+#define BFA_CP_MASK                 (0xFFFFFF80)
+#define BFA_SUBABS_SHIFT            (6)
+#define BFA_SUBABS_BIT_CNT          (8)
+#define BFA_SUBABS_MAX              ((1<<BFA_SUBABS_BIT_CNT)-1)
+#define BFA_SUBABSSAT_BIT_CNT       (9)
+#define BFA_SUBABSSAT_MAX           ((1<<BFA_SUBABSSAT_BIT_CNT)-1)
+#define BFA_WEIGHT_SHIFT            (6)
+
+#endif /* __ISP2601_CONFIG_H_INCLUDED__ */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_config.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_config.h
new file mode 100644
index 000000000000..80506f2419a8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_config.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CONFIG_H_INCLUDED__
+#define __ISP_CONFIG_H_INCLUDED__
+
+#if defined(ISP2400) || defined(ISP2401)
+#include "isp2400_config.h"
+#else
+#error "Please define a core {ISP2400, ISP2401}"
+#endif
+
+#endif /* __ISP_CONFIG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w.h
new file mode 100644
index 000000000000..2251f372145b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w.h
@@ -0,0 +1,845 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_OP1W_H_INCLUDED__
+#define __ISP_OP1W_H_INCLUDED__
+
+/*
+ * This file is part of the Multi-precision vector operations exstension package.
+ */
+
+/*
+ * Single-precision vector operations
+ */
+
+/*
+ * Prerequisites:
+ *
+ */
+#include "storage_class.h"
+
+#ifdef INLINE_ISP_OP1W
+#define STORAGE_CLASS_ISP_OP1W_FUNC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_ISP_OP1W_DATA_H STORAGE_CLASS_INLINE_DATA
+#else /* INLINE_ISP_OP1W */
+#define STORAGE_CLASS_ISP_OP1W_FUNC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_ISP_OP1W_DATA_H STORAGE_CLASS_EXTERN_DATA
+#endif  /* INLINE_ISP_OP1W */
+
+/*
+ * Single-precision data type specification
+ */
+
+#include "isp_op1w_types.h"
+#include "isp_op2w_types.h" // for doubling operations.
+
+/*
+ * Single-precision prototype specification
+ */
+
+/* Arithmetic */
+
+/** @brief bitwise AND
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise and of both input arguments
+ *
+ * This function will calculate the bitwise and.
+ * result = _a & _b
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_and(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief bitwise OR
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise or of both input arguments
+ *
+ * This function will calculate the bitwise or.
+ * result = _a | _b
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_or(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief bitwise XOR
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise xor of both input arguments
+ *
+ * This function will calculate the bitwise xor.
+ * result = _a ^ _b
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_xor(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief bitwise inverse
+ *
+ * @param[in] _a	first argument
+ *
+ * @return		bitwise inverse of both input arguments
+ *
+ * This function will calculate the bitwise inverse.
+ * result = ~_a
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_inv(
+    const tvector1w     _a);
+
+/* Additive */
+
+/** @brief addition
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		sum of both input arguments
+ *
+ * This function will calculate the sum of the input arguments.
+ * in case of overflow it will wrap around.
+ * result = _a + _b
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_add(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief subtraction
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_b subtracted from _a.
+ *
+ * This function will subtract _b from _a.
+ * in case of overflow it will wrap around.
+ * result = _a - _b
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_sub(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief saturated addition
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated sum of both input arguments
+ *
+ * This function will calculate the sum of the input arguments.
+ * in case of overflow it will saturate.
+ * result = CLIP(_a + _b, MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_addsat(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief saturated subtraction
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated subtraction of both input arguments
+ *
+ * This function will subtract _b from _a.
+ * in case of overflow it will saturate.
+ * result = CLIP(_a - _b, MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_subsat(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+#ifdef ISP2401
+/** @brief Unsigned saturated subtraction
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated subtraction of both input arguments
+ *
+ * This function will subtract _b from _a.
+ * in case of overflow it will saturate.
+ * result = CLIP(_a - _b, 0, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w_unsigned OP_1w_subsat_u(
+    const tvector1w_unsigned _a,
+    const tvector1w_unsigned _b);
+
+#endif
+/** @brief subtraction with shift right and rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(a - b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit with rounding.
+ * No overflow can occur.
+ * result = (_a - _b) >> 1
+ *
+ * Note: This function will be deprecated due to
+ * the naming confusion and it will be replaced
+ * by "OP_1w_subhalfrnd".
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_subasr1(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Subtraction with shift right and rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a - _b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit with rounding.
+ * No overflow can occur.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_subhalfrnd(
+    const tvector1w	_a,
+    const tvector1w	_b);
+
+/** @brief Subtraction with shift right and no rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a - _b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit without rounding (i.e. truncation).
+ * No overflow can occur.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_subhalf(
+    const tvector1w	_a,
+    const tvector1w	_b);
+
+
+/** @brief saturated absolute value
+ *
+ * @param[in] _a	input
+ *
+ * @return		saturated absolute value of the input
+ *
+ * This function will calculate the saturated absolute value of the input.
+ * in case of overflow it will saturate.
+ * if (_a > 0) return _a;<br>
+ * else return CLIP(-_a, MIN_RANGE, MAX_RANGE);<br>
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_abs(
+    const tvector1w     _a);
+
+/** @brief saturated absolute difference
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		sat(abs(a-b));
+ *
+ * This function will calculate the saturated absolute value
+ * of the saturated difference of both inputs.
+ * result = sat(abs(sat(_a - _b)));
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_subabssat(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/* Multiplicative */
+
+/** @brief doubling multiply
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		product of _a and _b
+ *
+ * This function will calculate the product
+ * of the input arguments and returns a double
+ * precision result.
+ * No overflow can occur.
+ * result = _a * _b;
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector2w OP_1w_muld(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief integer multiply
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		product of _a and _b
+ *
+ * This function will calculate the product
+ * of the input arguments and returns the LSB
+ * aligned single precision result.
+ * In case of overflow it will wrap around.
+ * result = _a * _b;
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_mul(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief fractional saturating multiply
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated product of _a and _b
+ *
+ * This function will calculate the fixed point
+ * product of the input arguments
+ * and returns a single precision result.
+ * In case of overflow it will saturate.
+ * FP_UNITY * FP_UNITY => FP_UNITY.
+ * result = CLIP(_a * _b >> (NUM_BITS-1), MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_qmul(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief fractional saturating multiply with rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		product of _a and _b
+ *
+ * This function will calculate the fixed point
+ * product of the input arguments
+ * and returns a single precision result.
+ * FP_UNITY * FP_UNITY => FP_UNITY.
+ * Depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ * result = CLIP(_a * _b >> (NUM_BITS-1), MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_qrmul(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/* Comparative */
+
+/** @brief equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a == _b
+ *
+ * This function will return true if both inputs
+ * are equal, and false if not equal.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_eq(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief not equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a != _b
+ *
+ * This function will return false if both inputs
+ * are equal, and true if not equal.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_ne(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief less or equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a <= _b
+ *
+ * This function will return true if _a is smaller
+ * or equal than _b.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_le(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief less then
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a < _b
+ *
+ * This function will return true if _a is smaller
+ * than _b.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_lt(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief greater or equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a >= _b
+ *
+ * This function will return true if _a is greater
+ * or equal than _b.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_ge(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief greater than
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a > _b
+ *
+ * This function will return true if _a is greater
+ * than _b.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tflags OP_1w_gt(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/* Shift */
+
+/** @brief aritmetic shift right
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * This function will shift _a with _b bits to the right,
+ * preserving the sign bit.
+ * It asserts 0 <= _b <= MAX_SHIFT_1W.
+ *
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_asr(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief aritmetic shift right with rounding
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * If _b < NUM_BITS, this function will shift _a with _b bits to the right,
+ * preserving the sign bit, and depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ * If _b >= NUM_BITS, this function will return 0.
+ * It asserts 0 <= _b <= MAX_SHIFT_1W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_asrrnd(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief saturating arithmetic shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * If _b < MAX_BITDEPTH, this function will shift _a with _b bits to the left,
+ * saturating at MIN_RANGE/MAX_RANGE in case of overflow.
+ * If _b >= MAX_BITDEPTH, this function will return MIN_RANGE if _a < 0,
+ * MAX_RANGE if _a > 0, 0 if _a == 0.
+ * (with MAX_BITDEPTH=64)
+ * It asserts 0 <= _b <= MAX_SHIFT_1W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_asl(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief saturating aritmetic shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * This function is identical to OP_1w_asl( )
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_aslsat(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief logical shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * This function will shift _a with _b bits to the left.
+ * It will insert zeroes on the right.
+ * It asserts 0 <= _b <= MAX_SHIFT_1W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_lsl(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief logical shift right
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * This function will shift _a with _b bits to the right.
+ * It will insert zeroes on the left.
+ * It asserts 0 <= _b <= MAX_SHIFT_1W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_lsr(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+#ifdef ISP2401
+/** @brief bidirectional saturating arithmetic shift
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << |_b| if _b is positive
+ *			_a >> |_b| if _b is negative
+ *
+ * If _b > 0, this function will shift _a with _b bits to the left,
+ * saturating at MIN_RANGE/MAX_RANGE in case of overflow.
+ * if _b < 0, this function will shift _a with _b bits to the right.
+ * It asserts -MAX_SHIFT_1W <= _b <= MAX_SHIFT_1W.
+ * If _b = 0, it returns _a.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_ashift_sat(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief bidirectional non-saturating arithmetic shift
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << |_b| if _b is positive
+ *			_a >> |_b| if _b is negative
+ *
+ * If _b > 0, this function will shift _a with _b bits to the left,
+ * no saturation is performed in case of overflow.
+ * if _b < 0, this function will shift _a with _b bits to the right.
+ * It asserts -MAX_SHIFT_1W <= _b <= MAX_SHIFT_1W.
+ * If _b = 0, it returns _a.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_ashift(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+
+/** @brief bidirectional logical shift
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << |_b| if _b is positive
+ *			_a >> |_b| if _b is negative
+ *
+ * This function will shift _a with _b bits to the left if _b is positive.
+ * This function will shift _a with _b bits to the right if _b is negative.
+ * It asserts -MAX_SHIFT_1W <= _b <= MAX_SHIFT_1W.
+ * It inserts zeros on the left or right depending on the shift direction: 
+ * right or left.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_lshift(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+#endif
+/* Cast */
+
+/** @brief Cast from int to 1w
+ *
+ * @param[in] _a	input
+ *
+ * @return		_a
+ *
+ * This function casts the input from integer type to
+ * single precision. It asserts there is no overflow.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_int_cast_to_1w(
+    const int           _a);
+
+/** @brief Cast from 1w to int
+ *
+ * @param[in] _a	input
+ *
+ * @return		_a
+ *
+ * This function casts the input from single precision type to
+ * integer, preserving value and sign.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H int OP_1w_cast_to_int(
+    const tvector1w      _a);
+
+/** @brief Cast from 1w to 2w
+ *
+ * @param[in] _a	input
+ *
+ * @return		_a
+ *
+ * This function casts the input from single precision type to
+ * double precision, preserving value and sign.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector2w OP_1w_cast_to_2w(
+    const tvector1w     _a);
+
+/** @brief Cast from 2w to 1w
+ *
+ * @param[in] _a	input
+ *
+ * @return		_a
+ *
+ * This function casts the input from double precision type to
+ * single precision. In case of overflow it will wrap around.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_2w_cast_to_1w(
+    const tvector2w    _a);
+
+
+/** @brief Cast from 2w to 1w with saturation
+ *
+ * @param[in] _a	input
+ *
+ * @return		_a
+ *
+ * This function casts the input from double precision type to
+ * single precision after saturating it to the range of single
+ * precision.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_2w_sat_cast_to_1w(
+    const tvector2w    _a);
+
+/* clipping */
+
+/** @brief Clip asymmetrical
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a clipped between ~_b and b
+ *
+ * This function will clip the first argument between
+ * (-_b - 1) and _b.
+ * It asserts _b >= 0.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_clip_asym(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Clip zero
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a clipped beteween 0 and _b
+ *
+ * This function will clip the first argument between
+ * zero and _b.
+ * It asserts _b >= 0.
+ *
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_clipz(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/* division */
+
+/** @brief Truncated division
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		trunc( _a / _b )
+ *
+ * This function will divide the first argument by
+ * the second argument, with rounding toward 0.
+ * If _b == 0 and _a <  0, the function will return MIN_RANGE.
+ * If _b == 0 and _a == 0, the function will return 0.
+ * If _b == 0 and _a >  0, the function will return MAX_RANGE.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_div(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Fractional saturating divide
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a / _b
+ *
+ * This function will perform fixed point division of
+ * the first argument by the second argument, with rounding toward 0.
+ * In case of overflow it will saturate.
+ * If _b == 0 and _a <  0, the function will return MIN_RANGE.
+ * If _b == 0 and _a == 0, the function will return 0.
+ * If _b == 0 and _a >  0, the function will return MAX_RANGE.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_qdiv(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Modulo
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a % _b
+ *
+ * This function will return the remainder r = _a - _b * trunc( _a / _b ),
+ * Note that the sign of the remainder is always equal to the sign of _a.
+ * If _b == 0 the function will return _a.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_mod(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Unsigned integer Square root
+ *
+ * @param[in] _a	input
+ *
+ * @return		Integer square root of _a
+ *
+ * This function will calculate the Integer square root of _a
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w_unsigned OP_1w_sqrt_u(
+	const tvector1w_unsigned     _a);
+
+/* Miscellaneous */
+
+/** @brief Multiplexer
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ * @param[in] _c	condition
+ *
+ * @return		_c ? _a : _b
+ *
+ * This function will return _a if the condition _c
+ * is true and _b otherwise.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_mux(
+    const tvector1w     _a,
+    const tvector1w     _b,
+    const tflags           _c);
+
+/** @brief Average without rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a + _b) >> 1
+ *
+ * This function will add _a and _b, and right shift
+ * the result by one without rounding. No overflow
+ * will occur because addition is performed in the
+ * proper precision.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w  OP_1w_avg(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Average with rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a + _b) >> 1
+ *
+ * This function will add _a and _b at full precision,
+ * and right shift with rounding the result with 1 bit.
+ * Depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_avgrnd(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Minimum
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a < _b) ? _a : _b;
+ *
+ * This function will return the smallest of both
+ * input arguments.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_min(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+/** @brief Maximum
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a > _b) ? _a : _b;
+ *
+ * This function will return the largest of both
+ * input arguments.
+ */
+STORAGE_CLASS_ISP_OP1W_FUNC_H tvector1w OP_1w_max(
+    const tvector1w     _a,
+    const tvector1w     _b);
+
+#ifndef INLINE_ISP_OP1W
+#define STORAGE_CLASS_ISP_OP1W_FUNC_C
+#define STORAGE_CLASS_ISP_OP1W_DATA_C const
+#else /* INLINE_ISP_OP1W */
+#define STORAGE_CLASS_ISP_OP1W_FUNC_C STORAGE_CLASS_ISP_OP1W_FUNC_H
+#define STORAGE_CLASS_ISP_OP1W_DATA_C STORAGE_CLASS_ISP_OP1W_DATA_H
+#include "isp_op1w.c"
+#define ISP_OP1W_INLINED
+#endif  /* INLINE_ISP_OP1W */
+
+#endif /* __ISP_OP1W_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w_types.h
new file mode 100644
index 000000000000..c81e587509a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op1w_types.h
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_OP1W_TYPES_H_INCLUDED__
+#define __ISP_OP1W_TYPES_H_INCLUDED__
+
+/*
+ * This file is part of the Multi-precision vector operations exstension package.
+ */
+ 
+/* 
+ * Single-precision vector operations
+ */
+ 
+/*  
+ * Prerequisites:
+ *
+ */
+
+#include "mpmath.h"
+
+/*
+ * Single-precision data type specification
+ */
+
+
+typedef mpsdata_t       tvector1w;
+typedef mpsdata_t       tscalar1w;
+typedef spsdata_t       tflags;
+typedef mpudata_t       tvector1w_unsigned;
+typedef mpsdata_t       tscalar1w_weight;
+typedef mpsdata_t       tvector1w_signed_positive;
+typedef mpsdata_t       tvector1w_weight;
+#ifdef ISP2401
+typedef bool            tscalar_bool;
+#endif
+
+typedef  struct {
+  tvector1w       d;
+  tflags        f;
+} tvector1w_tflags1w;
+
+#endif /* __ISP_OP1W_TYPES_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w.h
new file mode 100644
index 000000000000..1cfe6d717283
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w.h
@@ -0,0 +1,675 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_OP2W_H_INCLUDED__
+#define __ISP_OP2W_H_INCLUDED__
+
+/*
+ * This file is part of the Multi-precision vector operations exstension package.
+ */
+
+/*
+ * Double-precision vector operations
+ */
+
+/*
+ * Prerequisites:
+ *
+ */
+#include "storage_class.h"
+
+#ifdef INLINE_ISP_OP2W
+#define STORAGE_CLASS_ISP_OP2W_FUNC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_ISP_OP2W_DATA_H STORAGE_CLASS_INLINE_DATA
+#else /* INLINE_ISP_OP2W */
+#define STORAGE_CLASS_ISP_OP2W_FUNC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_ISP_OP2W_DATA_H STORAGE_CLASS_EXTERN_DATA
+#endif  /* INLINE_ISP_OP2W */
+
+/*
+ * Double-precision data type specification
+ */
+
+#include "isp_op2w_types.h"
+
+/*
+ * Double-precision prototype specification
+ */
+
+/* Arithmetic */
+
+/** @brief bitwise AND
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise and of both input arguments
+ *
+ * This function will calculate the bitwise and.
+ * result = _a & _b
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_and(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief bitwise OR
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise or of both input arguments
+ *
+ * This function will calculate the bitwise or.
+ * result = _a | _b
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_or(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief bitwise XOR
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		bitwise xor of both input arguments
+ *
+ * This function will calculate the bitwise xor.
+ * result = _a ^ _b
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_xor(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief bitwise inverse
+ *
+ * @param[in] _a	first argument
+ *
+ * @return		bitwise inverse of both input arguments
+ *
+ * This function will calculate the bitwise inverse.
+ * result = ~_a
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_inv(
+    const tvector2w     _a);
+
+/* Additive */
+
+/** @brief addition
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		sum of both input arguments
+ *
+ * This function will calculate the sum of the input arguments.
+ * in case of overflow it will wrap around.
+ * result = _a + _b
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_add(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief subtraction
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_b subtracted from _a.
+ *
+ * This function will subtract _b from _a.
+ * in case of overflow it will wrap around.
+ * result = _a - _b
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_sub(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief saturated addition
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated sum of both input arguments
+ *
+ * This function will calculate the sum of the input arguments.
+ * in case of overflow it will saturate
+ * result = CLIP(_a + _b, MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_addsat(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief saturated subtraction
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated subtraction of both input arguments
+ *
+ * This function will subtract _b from _a.
+ * in case of overflow it will saturate
+ * result = CLIP(_a - _b, MIN_RANGE, MAX_RANGE);
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_subsat(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief subtraction with shift right and rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(a - b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit with rounding.
+ * No overflow can occur.
+ * result = (_a - _b) >> 1
+ *
+ * Note: This function will be deprecated due to
+ * the naming confusion and it will be replaced
+ * by "OP_2w_subhalfrnd".
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_subasr1(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Subtraction with shift right and rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a - _b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit with rounding.
+ * No overflow can occur.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_subhalfrnd(
+    const tvector2w	_a,
+    const tvector2w	_b);
+
+/** @brief Subtraction with shift right and no rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a - _b) >> 1
+ *
+ * This function subtracts _b from _a and right shifts
+ * the result by 1 bit without rounding (i.e. truncation).
+ * No overflow can occur.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_subhalf(
+    const tvector2w	_a,
+    const tvector2w	_b);
+
+/** @brief saturated absolute value
+ *
+ * @param[in] _a	input
+ *
+ * @return		saturated absolute value of the input
+ *
+ * This function will calculate the saturated absolute value of the input.
+ * In case of overflow it will saturate.
+ * if (_a > 0) return _a;<br>
+ * else return CLIP(-_a, MIN_RANGE, MAX_RANGE);<br>
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_abs(
+    const tvector2w     _a);
+
+/** @brief saturated absolute difference
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		sat(abs(sat(a-b)));
+ *
+ * This function will calculate the saturated absolute value
+ * of the saturated difference of both inputs.
+ * result = sat(abs(sat(_a - _b)));
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_subabssat(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/* Multiplicative */
+
+/** @brief integer multiply
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		product of _a and _b
+ *
+ * This function will calculate the product
+ * of the input arguments and returns the LSB
+ * aligned double precision result.
+ * In case of overflow it will wrap around.
+ * result = _a * _b;
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_mul(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief fractional saturating multiply
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		saturated product of _a and _b
+ *
+ * This function will calculate the fixed point
+ * product of the input arguments
+ * and returns a double precision result.
+ * In case of overflow it will saturate.
+ * result =((_a * _b) << 1) >> (2*NUM_BITS);
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_qmul(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief fractional saturating multiply with rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		product of _a and _b
+ *
+ * This function will calculate the fixed point
+ * product of the input arguments
+ * and returns a double precision result.
+ * Depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ * In case of overflow it will saturate.
+ * result = ((_a * _b) << 1) >> (2*NUM_BITS);
+ */
+
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_qrmul(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/* Comparative */
+
+/** @brief equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a == _b
+ *
+ * This function will return true if both inputs
+ * are equal, and false if not equal.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_eq(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief not equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a != _b
+ *
+ * This function will return false if both inputs
+ * are equal, and true if not equal.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_ne(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief less or equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a <= _b
+ *
+ * This function will return true if _a is smaller
+ * or equal than _b.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_le(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief less then
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a < _b
+ *
+ * This function will return true if _a is smaller
+ * than _b.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_lt(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief greater or equal
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a >= _b
+ *
+ * This function will return true if _a is greater
+ * or equal than _b.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_ge(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief greater than
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a > _b
+ *
+ * This function will return true if _a is greater
+ * than _b.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tflags OP_2w_gt(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/* Shift */
+
+/** @brief aritmetic shift right
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * This function will shift _a with _b bits to the right,
+ * preserving the sign bit.
+ * It asserts 0 <= _b <= MAX_SHIFT_2W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_asr(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief aritmetic shift right with rounding
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * If _b < 2*NUM_BITS, this function will shift _a with _b bits to the right,
+ * preserving the sign bit, and depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ * If _b >= 2*NUM_BITS, this function will return 0.
+ * It asserts 0 <= _b <= MAX_SHIFT_2W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_asrrnd(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief saturating aritmetic shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * If _b < MAX_BITDEPTH, this function will shift _a with _b bits to the left,
+ * saturating at MIN_RANGE/MAX_RANGE in case of overflow.
+ * If _b >= MAX_BITDEPTH, this function will return MIN_RANGE if _a < 0,
+ * MAX_RANGE if _a > 0, 0 if _a == 0.
+ * (with MAX_BITDEPTH=64)
+ * It asserts 0 <= _b <= MAX_SHIFT_2W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_asl(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief saturating aritmetic shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * This function is identical to OP_2w_asl( )
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_aslsat(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief logical shift left
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a << _b
+ *
+ * This function will shift _a with _b bits to the left.
+ * It will insert zeroes on the right.
+ * It asserts 0 <= _b <= MAX_SHIFT_2W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_lsl(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief logical shift right
+ *
+ * @param[in] _a	input
+ * @param[in] _b	shift amount
+ *
+ * @return		_a >> _b
+ *
+ * This function will shift _a with _b bits to the right.
+ * It will insert zeroes on the left.
+ * It asserts 0 <= _b <= MAX_SHIFT_2W.
+ * The operation count for this function assumes that
+ * the shift amount is a cloned scalar input.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_lsr(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/* clipping */
+
+/** @brief Clip asymmetrical
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a clipped between ~_b and b
+ *
+ * This function will clip the first argument between
+ * (-_b - 1) and _b.
+ * It asserts _b >= 0.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_clip_asym(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Clip zero
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		_a clipped beteween 0 and _b
+ *
+ * This function will clip the first argument between
+ * zero and _b.
+ * It asserts _b >= 0.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_clipz(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/* division */
+
+/** @brief Truncated division
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		trunc( _a / _b )
+ *
+ * This function will divide the first argument by
+ * the second argument, with rounding toward 0.
+ * If _b == 0 and _a <  0, the function will return MIN_RANGE.
+ * If _b == 0 and _a == 0, the function will return 0.
+ * If _b == 0 and _a >  0, the function will return MAX_RANGE.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_div(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Saturating truncated division
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		CLIP( trunc( _a / _b ), MIN_RANGE1w, MAX_RANGE1w )
+ *
+ * This function will divide the first argument by
+ * the second argument, with rounding toward 0, and
+ * saturate the result to the range of single precision.
+ * If _b == 0 and _a <  0, the function will return MIN_RANGE.
+ * If _b == 0 and _a == 0, the function will return 0.
+ * If _b == 0 and _a >  0, the function will return MAX_RANGE.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector1w OP_2w_divh(
+    const tvector2w     _a,
+    const tvector1w     _b);
+
+/** @brief Modulo
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		n/a
+ *
+ * This function has not yet been implemented.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_mod(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Unsigned Integer Square root
+ *
+ * @param[in] _a	input
+ *
+ * @return		square root of _a
+ *
+ * This function will calculate the unsigned integer square root of _a
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector1w_unsigned OP_2w_sqrt_u(
+	const tvector2w_unsigned     _a);
+
+/* Miscellaneous */
+
+/** @brief Multiplexer
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ * @param[in] _c	condition
+ *
+ * @return		_c ? _a : _b
+ *
+ * This function will return _a if the condition _c
+ * is true and _b otherwise.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_mux(
+    const tvector2w     _a,
+    const tvector2w     _b,
+    const tflags           _c);
+
+/** @brief Average without rounding
+ *
+ * @param[in] _a	first operand
+ * @param[in] _b	second operand
+ *
+ * @return		(_a + _b) >> 1
+ *
+ * This function will add _a and _b, and right shift
+ * the result by one without rounding. No overflow
+ * will occur because addition is performed in the
+ * proper precision.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w  OP_2w_avg(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Average with rounding
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a + _b) >> 1
+ *
+ * This function will add _a and _b at full precision,
+ * and right shift with rounding the result with 1 bit.
+ * Depending on the rounding mode of the core
+ * it will round to nearest or to nearest even.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_avgrnd(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Minimum
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a < _b) ? _a : _b;
+ *
+ * This function will return the smallest of both
+ * input arguments.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_min(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+/** @brief Maximum
+ *
+ * @param[in] _a	first argument
+ * @param[in] _b	second argument
+ *
+ * @return		(_a > _b) ? _a : _b;
+ *
+ * This function will return the largest of both
+ * input arguments.
+ */
+STORAGE_CLASS_ISP_OP2W_FUNC_H tvector2w OP_2w_max(
+    const tvector2w     _a,
+    const tvector2w     _b);
+
+#ifndef INLINE_ISP_OP2W
+#define STORAGE_CLASS_ISP_OP2W_FUNC_C
+#define STORAGE_CLASS_ISP_OP2W_DATA_C const
+#else /* INLINE_ISP_OP2W */
+#define STORAGE_CLASS_ISP_OP2W_FUNC_C STORAGE_CLASS_ISP_OP2W_FUNC_H
+#define STORAGE_CLASS_ISP_OP2W_DATA_C STORAGE_CLASS_ISP_OP2W_DATA_H
+#include "isp_op2w.c"
+#define ISP_OP2W_INLINED
+#endif  /* INLINE_ISP_OP2W */
+
+#endif /* __ISP_OP2W_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w_types.h
new file mode 100644
index 000000000000..7e86083a8a33
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op2w_types.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_OP2W_TYPES_H_INCLUDED__
+#define __ISP_OP2W_TYPES_H_INCLUDED__
+
+/*
+ * This file is part of the Multi-precision vector operations exstension package.
+ */
+
+/*
+ * Double-precision vector operations
+ */
+
+/*
+ * Prerequisites:
+ *
+ */
+#include "mpmath.h"
+#include "isp_op1w_types.h"
+
+/*
+ * Single-precision data type specification
+ */
+
+
+typedef mpsdata_t       tvector2w;
+typedef mpsdata_t       tscalar2w;
+typedef mpsdata_t       tvector2w_signed_positive;
+typedef mpudata_t       tvector2w_unsigned;
+
+
+typedef struct {
+  tvector2w       d;
+  tflags        f;
+} tvector2w_tflags;
+
+#endif /* __ISP_OP2W_TYPES_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op_count.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op_count.h
new file mode 100644
index 000000000000..8e7b48d026b0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_op_count.h
@@ -0,0 +1,226 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_OP_COUNT_H_INCLUDED__
+#define __ISP_OP_COUNT_H_INCLUDED__
+
+#include <stdio.h>
+
+typedef struct {
+	long long bbb_cnt;   /* number of bbb      */
+	int bbb_op;    /* operations per bbb */
+	long long total_cnt; /* bbb_cnt * bbb_op   */
+} bbb_stat_t;
+
+typedef enum {
+	bbb_func_OP_1w_and,
+	bbb_func_OP_1w_or,
+	bbb_func_OP_1w_xor,
+	bbb_func_OP_1w_inv,
+	bbb_func_OP_1w_add,
+	bbb_func_OP_1w_sub,
+	bbb_func_OP_1w_addsat,
+	bbb_func_OP_1w_subsat,
+	bbb_func_OP_1w_subasr1,
+	bbb_func_OP_1w_subhalf,
+	bbb_func_OP_1w_subhalfrnd,
+	bbb_func_OP_1w_abs,
+	bbb_func_OP_1w_subabssat,
+#ifdef ISP2401
+	bbb_func_OP_1w_subsat_u,
+#endif
+	bbb_func_OP_1w_muld,
+	bbb_func_OP_1w_mul,
+	bbb_func_OP_1w_qmul,
+	bbb_func_OP_1w_qrmul,
+	bbb_func_OP_1w_eq,
+	bbb_func_OP_1w_ne,
+	bbb_func_OP_1w_le,
+	bbb_func_OP_1w_lt,
+	bbb_func_OP_1w_ge,
+	bbb_func_OP_1w_gt,
+	bbb_func_OP_1w_asr,
+	bbb_func_OP_1w_asrrnd,
+	bbb_func_OP_1w_asl,
+	bbb_func_OP_1w_aslsat,
+	bbb_func_OP_1w_lsl,
+	bbb_func_OP_1w_lsr,
+#ifdef ISP2401
+	bbb_func_OP_1w_ashift,
+	bbb_func_OP_1w_lshift,
+#endif
+	bbb_func_OP_int_cast_to_1w ,
+	bbb_func_OP_1w_cast_to_int ,
+	bbb_func_OP_1w_cast_to_2w ,
+	bbb_func_OP_2w_cast_to_1w ,
+	bbb_func_OP_2w_sat_cast_to_1w ,
+	bbb_func_OP_1w_clip_asym,
+	bbb_func_OP_1w_clipz,
+	bbb_func_OP_1w_div,
+	bbb_func_OP_1w_qdiv,
+	bbb_func_OP_1w_mod,
+	bbb_func_OP_1w_sqrt_u,
+	bbb_func_OP_1w_mux,
+	bbb_func_OP_1w_avg,
+	bbb_func_OP_1w_avgrnd,
+	bbb_func_OP_1w_min,
+	bbb_func_OP_1w_max,
+	bbb_func_OP_2w_and,
+	bbb_func_OP_2w_or,
+	bbb_func_OP_2w_xor,
+	bbb_func_OP_2w_inv,
+	bbb_func_OP_2w_add,
+	bbb_func_OP_2w_sub,
+	bbb_func_OP_2w_addsat,
+	bbb_func_OP_2w_subsat,
+	bbb_func_OP_2w_subasr1,
+	bbb_func_OP_2w_subhalf,
+	bbb_func_OP_2w_subhalfrnd,
+	bbb_func_OP_2w_abs,
+	bbb_func_OP_2w_subabssat,
+	bbb_func_OP_2w_mul,
+	bbb_func_OP_2w_qmul,
+	bbb_func_OP_2w_qrmul,
+	bbb_func_OP_2w_eq,
+	bbb_func_OP_2w_ne,
+	bbb_func_OP_2w_le,
+	bbb_func_OP_2w_lt,
+	bbb_func_OP_2w_ge,
+	bbb_func_OP_2w_gt,
+	bbb_func_OP_2w_asr,
+	bbb_func_OP_2w_asrrnd,
+	bbb_func_OP_2w_asl,
+	bbb_func_OP_2w_aslsat,
+	bbb_func_OP_2w_lsl,
+	bbb_func_OP_2w_lsr,
+	bbb_func_OP_2w_clip_asym,
+	bbb_func_OP_2w_clipz,
+	bbb_func_OP_2w_div,
+	bbb_func_OP_2w_divh,
+	bbb_func_OP_2w_mod,
+	bbb_func_OP_2w_sqrt_u,
+	bbb_func_OP_2w_mux,
+	bbb_func_OP_2w_avg,
+	bbb_func_OP_2w_avgrnd,
+	bbb_func_OP_2w_min,
+	bbb_func_OP_2w_max,
+	bbb_func_OP_1w_mul_realigning,
+#ifdef ISP2401
+	bbb_func_OP_1w_imax32,
+	bbb_func_OP_1w_imaxidx32,
+	bbb_func_OP_1w_cond_add,
+#endif
+
+	bbb_func_num_functions
+} bbb_functions_t;
+
+typedef enum {
+	core_func_OP_and,
+	core_func_OP_or,
+	core_func_OP_xor,
+	core_func_OP_inv,
+	core_func_OP_add,
+	core_func_OP_sub,
+	core_func_OP_addsat,
+	core_func_OP_subsat,
+	core_func_OP_subasr1,
+	core_func_OP_abs,
+	core_func_OP_subabssat,
+#ifdef ISP2401
+	core_func_OP_subsat_u,
+#endif
+	core_func_OP_muld,
+	core_func_OP_mul,
+	core_func_OP_qrmul,
+	core_func_OP_eq,
+	core_func_OP_ne,
+	core_func_OP_le,
+	core_func_OP_lt,
+	core_func_OP_ge,
+	core_func_OP_gt,
+	core_func_OP_asr,
+	core_func_OP_asl,
+	core_func_OP_asrrnd,
+	core_func_OP_lsl,
+	core_func_OP_lslsat,
+	core_func_OP_lsr,
+	core_func_OP_lsrrnd,
+	core_func_OP_clip_asym,
+	core_func_OP_clipz,
+	core_func_OP_div,
+	core_func_OP_mod,
+	core_func_OP_sqrt,
+	core_func_OP_mux,
+	core_func_OP_avgrnd,
+	core_func_OP_min,
+	core_func_OP_max,
+
+	core_func_num_functions
+
+} core_functions_t;
+
+/* inc_bbb_count() can be used for building blocks that are implemented with one operation
+   inc_bbb_count_ext() will be used in case the operation count is not known or greater than one.
+
+   For some operations there is a difference in operation count for the cloned version and the
+   not cloned version. this difference is not vissible on the reference code side.
+   We could add a min and max operation count for those operations, and keep track of those counts
+   separately. That way in the report the impact can be seen. */
+
+#ifdef DISABLE_OPCNT
+#define inc_bbb_count(func)
+#define inc_bbb_count_ext(func, cnt)
+#define enable_bbb_count()
+#define disable_bbb_count()
+#else
+#define inc_bbb_count(func) _inc_bbb_count(func)
+#define inc_bbb_count_ext(func, cnt) _inc_bbb_count_ext(func, cnt)
+#define enable_bbb_count() _enable_bbb_count()
+#define disable_bbb_count() _disable_bbb_count()
+#endif
+
+void
+inc_core_count_n(
+	core_functions_t func,
+	unsigned n);
+
+void
+_enable_bbb_count(void);
+
+void
+_disable_bbb_count(void);
+
+void
+_inc_bbb_count(
+	bbb_functions_t func);
+
+void
+_inc_bbb_count_ext(
+	bbb_functions_t func,
+	int op_count);
+
+void
+bbb_func_reset_count(void);
+
+void
+bbb_func_print_totals(
+	FILE  * fp,
+	unsigned non_zero_only);
+
+void
+core_func_print_totals(
+	FILE* fp,
+	unsigned non_zero_only);
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_public.h
new file mode 100644
index 000000000000..808ec050efc0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isp_public.h
@@ -0,0 +1,186 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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)
+ */
+extern 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
+ */
+extern 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 uint16_t		x0,
+	const uint16_t		x1);
+
+unsigned isp_is_ready(isp_ID_t ID);
+
+unsigned 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/atomisp2/css2400/hive_isp_css_include/host/isys_dma_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_dma_public.h
new file mode 100644
index 000000000000..4b1603895f06
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_dma_public.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 USE_INPUT_SYSTEM_VERSION_2401
+
+#include "system_types.h"
+#include "type_support.h"
+
+STORAGE_CLASS_ISYS2401_DMA_H void isys2401_dma_reg_store(
+	const isys2401_dma_ID_t dma_id,
+	const unsigned int	reg,
+	const hrt_data		value);
+
+STORAGE_CLASS_ISYS2401_DMA_H hrt_data isys2401_dma_reg_load(
+	const isys2401_dma_ID_t dma_id,
+	const unsigned int	reg);
+
+extern void isys2401_dma_set_max_burst_size(
+	const isys2401_dma_ID_t dma_id,
+	uint32_t		max_burst_size);
+
+#endif /* USE_INPUT_SYSTEM_VERSION_2401 */
+
+#endif /* __ISYS_DMA_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_irq_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_irq_public.h
new file mode 100644
index 000000000000..c3e6f76f8c4d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_irq_public.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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(USE_INPUT_SYSTEM_VERSION_2401)
+
+STORAGE_CLASS_ISYS2401_IRQ_H void isys_irqc_state_get(
+	const isys_irq_ID_t	isys_irqc_id,
+	isys_irqc_state_t	*state);
+
+STORAGE_CLASS_ISYS2401_IRQ_H void isys_irqc_state_dump(
+	const isys_irq_ID_t	isys_irqc_id,
+	const isys_irqc_state_t *state);
+
+STORAGE_CLASS_ISYS2401_IRQ_H void isys_irqc_reg_store(
+	const isys_irq_ID_t	isys_irqc_id,
+	const unsigned int	reg_idx,
+	const hrt_data		value);
+
+STORAGE_CLASS_ISYS2401_IRQ_H hrt_data isys_irqc_reg_load(
+	const isys_irq_ID_t	isys_irqc_id,
+	const unsigned int	reg_idx);
+
+STORAGE_CLASS_ISYS2401_IRQ_H void isys_irqc_status_enable(
+	const isys_irq_ID_t	isys_irqc_id);
+
+#endif /* defined(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#endif	/* __ISYS_IRQ_PUBLIC_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_public.h
new file mode 100644
index 000000000000..097dde852c8a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_public.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 USE_INPUT_SYSTEM_VERSION_2401
+/*! 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 /* USE_INPUT_SYSTEM_VERSION_2401 */
+#endif /* __ISYS_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_stream2mmio_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_stream2mmio_public.h
new file mode 100644
index 000000000000..5624cfcfa015
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/isys_stream2mmio_public.h
@@ -0,0 +1,101 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 offet 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 offet 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/atomisp2/css2400/hive_isp_css_include/host/mmu_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/mmu_public.h
new file mode 100644
index 000000000000..4258fa872087
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/mmu_public.h
@@ -0,0 +1,82 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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
+ */
+STORAGE_CLASS_EXTERN 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
+ */
+STORAGE_CLASS_EXTERN 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
+ */
+STORAGE_CLASS_EXTERN void mmu_invalidate_cache(
+	const mmu_ID_t		ID);
+
+
+/*! Invalidate the page table cache of all MMUs
+
+ \return none
+ */
+STORAGE_CLASS_EXTERN 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
+ */
+STORAGE_CLASS_MMU_H void mmu_reg_store(
+	const mmu_ID_t		ID,
+	const unsigned int	reg,
+	const hrt_data		value);
+
+/*! 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]
+ */
+STORAGE_CLASS_MMU_H hrt_data mmu_reg_load(
+	const mmu_ID_t		ID,
+	const unsigned int	reg);
+
+#endif /* __MMU_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/osys_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/osys_public.h
new file mode 100644
index 000000000000..8695e3c01fa6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/osys_public.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __OSYS_PUBLIC_H_INCLUDED__
+#define __OSYS_PUBLIC_H_INCLUDED__
+
+#include "system_types.h"
+
+#endif /* __OSYS_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pipeline_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pipeline_public.h
new file mode 100644
index 000000000000..32cea582b4c4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pipeline_public.h
@@ -0,0 +1,18 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __PIPELINE_PUBLIC_H_INCLUDED__
+#define __PIPELINE_PUBLIC_H_INCLUDED__
+
+#endif /* __PIPELINE_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pixelgen_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pixelgen_public.h
new file mode 100644
index 000000000000..c0f3f3ea32d7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/pixelgen_public.h
@@ -0,0 +1,79 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 USE_INPUT_SYSTEM_VERSION_2401
+/*****************************************************
+ *
+ * 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 offet 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 offet 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 /* USE_INPUT_SYSTEM_VERSION_2401 */
+#endif /* __PIXELGEN_PUBLIC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func.h
new file mode 100644
index 000000000000..3e955fca2a94
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func.h
@@ -0,0 +1,1222 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef _REF_VECTOR_FUNC_H_INCLUDED_
+#define _REF_VECTOR_FUNC_H_INCLUDED_
+
+#include "storage_class.h"
+
+#ifdef INLINE_VECTOR_FUNC
+#define STORAGE_CLASS_REF_VECTOR_FUNC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_REF_VECTOR_DATA_H STORAGE_CLASS_INLINE_DATA
+#else /* INLINE_VECTOR_FUNC */
+#define STORAGE_CLASS_REF_VECTOR_FUNC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_REF_VECTOR_DATA_H STORAGE_CLASS_EXTERN_DATA
+#endif  /* INLINE_VECTOR_FUNC */
+
+
+#include "ref_vector_func_types.h"
+
+/** @brief Doubling multiply accumulate with saturation
+ *
+ * @param[in] acc accumulator
+ * @param[in] a multiply input
+ * @param[in] b multiply input
+  *
+ * @return		acc + (a*b)
+ *
+ * This function will do a doubling multiply ont
+ * inputs a and b, and will add the result to acc.
+ * in case of an overflow of acc, it will saturate.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector2w OP_1w_maccd_sat(
+	tvector2w acc,
+	tvector1w a,
+	tvector1w b );
+
+/** @brief Doubling multiply accumulate
+ *
+ * @param[in] acc accumulator
+ * @param[in] a multiply input
+ * @param[in] b multiply input
+  *
+ * @return		acc + (a*b)
+ *
+ * This function will do a doubling multiply ont
+ * inputs a and b, and will add the result to acc.
+ * in case of overflow it will not saturate but wrap around.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector2w OP_1w_maccd(
+	tvector2w acc,
+	tvector1w a,
+	tvector1w b );
+
+/** @brief Re-aligning multiply
+ *
+ * @param[in] a multiply input
+ * @param[in] b multiply input
+ * @param[in] shift shift amount
+ *
+ * @return		(a*b)>>shift
+ *
+ * This function will multiply a with b, followed by a right
+ * shift with rounding. the result is saturated and casted
+ * to single precision.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_mul_realigning(
+	tvector1w a,
+	tvector1w b,
+	tscalar1w shift );
+
+/** @brief Leading bit index
+ *
+ * @param[in] a 	input
+ *
+ * @return		index of the leading bit of each element
+ *
+ * This function finds the index of leading one (set) bit of the
+ * input. The index starts with 0 for the LSB and can go upto
+ * ISP_VEC_ELEMBITS-1 for the MSB. For an input equal to zero,
+ * the returned index is -1.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_lod(
+		tvector1w a);
+
+/** @brief Config Unit Input Processing
+ *
+ * @param[in] a 	    input
+ * @param[in] input_scale   input scaling factor
+ * @param[in] input_offset  input offset factor
+ *
+ * @return		    scaled & offset added input	clamped to MAXVALUE
+ *
+ * As part of input processing for piecewise linear estimation config unit,
+ * this function will perform scaling followed by adding offset and
+ * then clamping to the MAX InputValue
+ * It asserts -MAX_SHIFT_1W <= input_scale <= MAX_SHIFT_1W, and
+ * -MAX_SHIFT_1W <= input_offset <= MAX_SHIFT_1W
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_input_scaling_offset_clamping(
+	tvector1w a,
+	tscalar1w_5bit_signed input_scale,
+	tscalar1w_5bit_signed input_offset);
+
+/** @brief Config Unit Output Processing
+ *
+ * @param[in] a 	     output
+ * @param[in] output_scale   output scaling factor
+ *
+ * @return		     scaled & clamped output value
+ *
+ * As part of output processing for piecewise linear estimation config unit,
+ * This function will perform scaling and then clamping to output
+ * MAX value.
+ * It asserts -MAX_SHIFT_1W <= output_scale <= MAX_SHIFT_1W
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_output_scaling_clamping(
+	tvector1w a,
+	tscalar1w_5bit_signed output_scale);
+
+/** @brief Config Unit Piecewiselinear estimation
+ *
+ * @param[in] a 	          input
+ * @param[in] config_points   config parameter structure
+ *
+ * @return		     	   piecewise linear estimated output
+ *
+ * Given a set of N points {(x1,y1),()x2,y2), ....,(xn,yn)}, to find
+ * the functional value at an arbitrary point around the input set,
+ * this function will perform input processing followed by piecewise
+ * linear estimation and then output processing to yield the final value.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_piecewise_estimation(
+	tvector1w a,
+	ref_config_points config_points);
+
+/** @brief Fast Config Unit
+ *
+ * @param[in] x 		input
+ * @param[in] init_vectors	LUT data structure
+ *
+ * @return	piecewise linear estimated output
+ * This block gets an input x and a set of input configuration points stored in a look-up
+ * table of 32 elements. First, the x input is clipped to be within the range [x1, xn+1].
+ * Then, it computes the interval in which the input lies. Finally, the output is computed
+ * by performing linear interpolation based on the interval properties (i.e. x_prev, slope,
+ * and offset). This block assumes that the points are equally spaced and that the interval
+ * size is a power of 2.
+ **/
+STORAGE_CLASS_REF_VECTOR_FUNC_H  tvector1w OP_1w_XCU(
+	tvector1w x,
+	xcu_ref_init_vectors init_vectors);
+
+
+/** @brief LXCU
+ *
+ * @param[in] x 		input
+ * @param[in] init_vectors 	LUT data structure
+ *
+ * @return   logarithmic piecewise linear estimated output.
+ * This block gets an input x and a set of input configuration points stored in a look-up
+ * table of 32 elements. It computes the interval in which the input lies.
+ * Then output is computed by performing linear interpolation based on the interval
+ * properties (i.e. x_prev, slope, * and offset).
+ * This BBB assumes spacing x-coordinates of "init vectors" increase exponentially as
+ * shown below.
+ * interval size :   2^0    2^1      2^2    2^3
+ * x-coordinates: x0<--->x1<---->x2<---->x3<---->
+ **/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_LXCU(
+	tvector1w x,
+	xcu_ref_init_vectors init_vectors);
+
+/** @brief Coring
+ *
+ * @param[in] coring_vec   Amount of coring based on brightness level
+ * @param[in] filt_input   Vector of input pixels on which Coring is applied
+ * @param[in] m_CnrCoring0 Coring Level0
+ *
+ * @return                 vector of filtered pixels after coring is applied
+ *
+ * This function will perform adaptive coring based on brightness level to
+ * remove noise
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w coring(
+	tvector1w coring_vec,
+	tvector1w filt_input,
+	tscalar1w m_CnrCoring0 );
+
+/** @brief Normalised FIR with coefficients [3,4,1]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [3,4,1],
+ *-5dB at Fs/2, -90 degree phase shift (quarter pixel)
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_5dB_m90_nrm (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [1,4,3]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1,4,3],
+ *-5dB at Fs/2, +90 degree phase shift (quarter pixel)
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_5dB_p90_nrm (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [1,2,1]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1,2,1], -6dB at Fs/2
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [13,16,3]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [13,16,3],
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm_ph0 (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [9,16,7]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [9,16,7],
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm_ph1 (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [5,16,11]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [5,16,11],
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm_ph2 (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with coefficients [1,16,15]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1,16,15],
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm_ph3 (
+	const s_1w_1x3_matrix		m);
+
+/** @brief Normalised FIR with programable phase shift
+ *
+ * @param[in] m	1x3 matrix with pixels
+ * @param[in] coeff	phase shift
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [8-coeff,16,8+coeff],
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_6dB_nrm_calc_coeff (
+	const s_1w_1x3_matrix		m, tscalar1w_3bit coeff);
+
+/** @brief 3 tap FIR with coefficients [1,1,1]
+ *
+ * @param[in] m	1x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * FIR with coefficients [1,1,1], -9dB at Fs/2 normalized with factor 1/2
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x3m_9dB_nrm (
+	const s_1w_1x3_matrix		m);
+
+#ifdef ISP2401
+/** @brief      symmetric 3 tap FIR acts as LPF or BSF
+ *
+ * @param[in] m 1x3 matrix with pixels
+ * @param[in] k filter coefficient shift
+ * @param[in] bsf_flag 1 for BSF and 0 for LPF
+ *
+ * @return    filtered output
+ *
+ * This function performs variable coefficient symmetric 3 tap filter which can
+ * be either used as Low Pass Filter or Band Stop Filter.
+ * Symmetric 3tap tap filter with DC gain 1 has filter coefficients [a, 1-2a, a]
+ * For LPF 'a' can be approximated as (1 - 2^(-k))/4, k = 0, 1, 2, ...
+ * and filter output can be approximated as:
+ * out_LPF = ((v00 + v02) - ((v00 + v02) >> k) + (2 * (v01 + (v01 >> k)))) >> 2
+ * For BSF 'a' can be approximated as (1 + 2^(-k))/4, k = 0, 1, 2, ...
+ * and filter output can be approximated as:
+ * out_BSF = ((v00 + v02) + ((v00 + v02) >> k) + (2 * (v01 - (v01 >> k)))) >> 2
+ * For a given filter coefficient shift 'k' and bsf_flag this function
+ * behaves either as LPF or BSF.
+ * All computation is done using 1w arithmetic and implementation does not use
+ * any multiplication.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+sym_fir1x3m_lpf_bsf(s_1w_1x3_matrix m,
+		    tscalar1w k,
+		    tscalar_bool bsf_flag);
+#endif
+
+/** @brief Normalised 2D FIR with coefficients  [1;2;1] * [1,2,1]
+ *
+ * @param[in] m	3x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients  [1;2;1] * [1,2,1]
+ * Unity gain filter through repeated scaling and rounding
+ *	- 6 rotate operations per output
+ *	- 8 vector operations per output
+ * _______
+ *   14 total operations
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir3x3m_6dB_nrm (
+	const s_1w_3x3_matrix		m);
+
+/** @brief Normalised 2D FIR with coefficients  [1;1;1] * [1,1,1]
+ *
+ * @param[in] m	3x3 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1;1;1] * [1,1,1]
+ *
+ * (near) Unity gain filter through repeated scaling and rounding
+ *	- 6 rotate operations per output
+ *	- 8 vector operations per output
+ * _______
+ *   14 operations
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir3x3m_9dB_nrm (
+	const s_1w_3x3_matrix		m);
+
+/** @brief Normalised dual output 2D FIR with coefficients  [1;2;1] * [1,2,1]
+ *
+ * @param[in] m	4x3 matrix with pixels
+ *
+ * @return		two filtered outputs (2x1 matrix)
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients  [1;2;1] * [1,2,1]
+ * and produce two outputs (vertical)
+ * Unity gain filter through repeated scaling and rounding
+ * compute two outputs per call to re-use common intermediates
+ *	- 4 rotate operations per output
+ *	- 6 vector operations per output (alternative possible, but in this
+ *	    form it's not obvious to re-use variables)
+ * _______
+ *   10 total operations
+ */
+ STORAGE_CLASS_REF_VECTOR_FUNC_H s_1w_2x1_matrix fir3x3m_6dB_out2x1_nrm (
+	const s_1w_4x3_matrix		m);
+
+/** @brief Normalised dual output 2D FIR with coefficients [1;1;1] * [1,1,1]
+ *
+ * @param[in] m	4x3 matrix with pixels
+ *
+ * @return		two filtered outputs (2x1 matrix)
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1;1;1] * [1,1,1]
+ * and produce two outputs (vertical)
+ * (near) Unity gain filter through repeated scaling and rounding
+ * compute two outputs per call to re-use common intermediates
+ *	- 4 rotate operations per output
+ *	- 7 vector operations per output (alternative possible, but in this
+ *	    form it's not obvious to re-use variables)
+ * _______
+ *   11 total operations
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H s_1w_2x1_matrix fir3x3m_9dB_out2x1_nrm (
+	const s_1w_4x3_matrix		m);
+
+/** @brief Normalised 2D FIR 5x5
+ *
+ * @param[in] m	5x5 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1;1;1] * [1;2;1] * [1,2,1] * [1,1,1]
+ * and produce a filtered output
+ * (near) Unity gain filter through repeated scaling and rounding
+ *	- 20 rotate operations per output
+ *	- 28 vector operations per output
+ * _______
+ *   48 total operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir5x5m_15dB_nrm (
+	const s_1w_5x5_matrix	m);
+
+/** @brief Normalised FIR 1x5
+ *
+ * @param[in] m	1x5 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1,2,1] * [1,1,1] = [1,4,6,4,1]
+ * and produce a filtered output
+ * (near) Unity gain filter through repeated scaling and rounding
+ *	- 4 rotate operations per output
+ *	- 5 vector operations per output
+ * _______
+ *   9 total operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x5m_12dB_nrm (
+	const s_1w_1x5_matrix m);
+
+/** @brief Normalised 2D FIR 5x5
+ *
+ * @param[in] m	5x5 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will calculate the
+ * Normalised FIR with coefficients [1;2;1] * [1;2;1] * [1,2,1] * [1,2,1]
+ * and produce a filtered output
+ * (near) Unity gain filter through repeated scaling and rounding
+ *	- 20 rotate operations per output
+ *	- 30 vector operations per output
+ * _______
+ *   50 total operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir5x5m_12dB_nrm (
+	const s_1w_5x5_matrix m);
+
+/** @brief Approximate averaging FIR 1x5
+ *
+ * @param[in] m	1x5 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will produce filtered output by
+ * applying the filter coefficients (1/8) * [1,1,1,1,1]
+ * _______
+ *   5 vector operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x5m_box (
+	s_1w_1x5_matrix m);
+
+/** @brief Approximate averaging FIR 1x9
+ *
+ * @param[in] m	1x9 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will produce filtered output by
+ * applying the filter coefficients (1/16) * [1,1,1,1,1,1,1,1,1]
+ * _______
+ *   9 vector operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x9m_box (
+	s_1w_1x9_matrix m);
+
+/** @brief Approximate averaging FIR 1x11
+ *
+ * @param[in] m	1x11 matrix with pixels
+ *
+ * @return		filtered output
+ *
+ * This function will produce filtered output by
+ * applying the filter coefficients (1/16) * [1,1,1,1,1,1,1,1,1,1,1]
+ * _______
+ *   12 vector operations
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w fir1x11m_box (
+	s_1w_1x11_matrix m);
+
+/** @brief Symmetric 7 tap filter with normalization
+ *
+ *  @param[in] in 1x7 matrix with pixels
+ *  @param[in] coeff 1x4 matrix with coefficients
+ *  @param[in] out_shift output pixel shift value for normalization
+ *
+ *  @return symmetric 7 tap filter output
+ *
+ * This function performs symmetric 7 tap filter over input pixels.
+ * Filter sum is normalized by shifting out_shift bits.
+ * Filter sum: p0*c3 + p1*c2 + p2*c1 + p3*c0 + p4*c1 + p5*c2 + p6*c3
+ * is implemented as: (p0 + p6)*c3 + (p1 + p5)*c2 + (p2 + p4)*c1 + p3*c0 to
+ * reduce multiplication.
+ * Input pixels should to be scaled, otherwise overflow is possible during
+ * addition
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x7m_sym_nrm(s_1w_1x7_matrix in,
+		s_1w_1x4_matrix coeff,
+		tvector1w out_shift);
+
+/** @brief Symmetric 7 tap filter with normalization at input side
+ *
+ *  @param[in] in 1x7 matrix with pixels
+ *  @param[in] coeff 1x4 matrix with coefficients
+ *
+ *  @return symmetric 7 tap filter output
+ *
+ * This function performs symmetric 7 tap filter over input pixels.
+ * Filter sum: p0*c3 + p1*c2 + p2*c1 + p3*c0 + p4*c1 + p5*c2 + p6*c3
+ *          = (p0 + p6)*c3 + (p1 + p5)*c2 + (p2 + p4)*c1 + p3*c0
+ * Input pixels and coefficients are in Qn format, where n =
+ * ISP_VEC_ELEMBITS - 1 (ie Q15 for Broxton)
+ * To avoid double precision arithmetic input pixel sum and final sum is
+ * implemented using avgrnd and coefficient multiplication using qrmul.
+ * Final result is in Qm format where m = ISP_VEC_ELEMBITS - 2 (ie Q14 for
+ * Broxton)
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x7m_sym_innrm_approx(s_1w_1x7_matrix in,
+			 s_1w_1x4_matrix coeff);
+
+/** @brief Symmetric 7 tap filter with normalization at output side
+ *
+ *  @param[in] in 1x7 matrix with pixels
+ *  @param[in] coeff 1x4 matrix with coefficients
+ *
+ *  @return symmetric 7 tap filter output
+ *
+ * This function performs symmetric 7 tap filter over input pixels.
+ * Filter sum: p0*c3 + p1*c2 + p2*c1 + p3*c0 + p4*c1 + p5*c2 + p6*c3
+ *          = (p0 + p6)*c3 + (p1 + p5)*c2 + (p2 + p4)*c1 + p3*c0
+ * Input pixels are in Qn and coefficients are in Qm format, where n =
+ * ISP_VEC_ELEMBITS - 2 and m = ISP_VEC_ELEMBITS - 1 (ie Q14 and Q15
+ * respectively for Broxton)
+ * To avoid double precision arithmetic input pixel sum and final sum is
+ * implemented using addsat and coefficient multiplication using qrmul.
+ * Final sum is left shifted by 2 and saturated to produce result is Qm format
+ * (ie Q15 for Broxton)
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x7m_sym_outnrm_approx(s_1w_1x7_matrix in,
+			 s_1w_1x4_matrix coeff);
+
+/** @brief 4 tap filter with normalization
+ *
+ *  @param[in] in 1x4 matrix with pixels
+ *  @param[in] coeff 1x4 matrix with coefficients
+ *  @param[in] out_shift output pixel shift value for normalization
+ *
+ *  @return 4 tap filter output
+ *
+ * This function performs 4 tap filter over input pixels.
+ * Filter sum is normalized by shifting out_shift bits.
+ * Filter sum: p0*c0 + p1*c1 + p2*c2 + p3*c3
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x4m_nrm(s_1w_1x4_matrix in,
+		s_1w_1x4_matrix coeff,
+		tvector1w out_shift);
+
+/** @brief 4 tap filter with normalization for half pixel interpolation
+ *
+ *  @param[in] in 1x4 matrix with pixels
+ *
+ *  @return 4 tap filter output with filter tap [-1 9 9 -1]/16
+ *
+ * This function performs 4 tap filter over input pixels.
+ * Filter sum: -p0 + 9*p1 + 9*p2 - p3
+ * This filter implementation is completely free from multiplication and double
+ * precision arithmetic.
+ * Typical usage of this filter is to half pixel interpolation of Bezier
+ * surface
+ * */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x4m_bicubic_bezier_half(s_1w_1x4_matrix in);
+
+/** @brief 4 tap filter with normalization for quarter pixel interpolation
+ *
+ *  @param[in] in 1x4 matrix with pixels
+ *  @param[in] coeff 1x4 matrix with coefficients
+ *
+ *  @return 4 tap filter output
+ *
+ * This function performs 4 tap filter over input pixels.
+ * Filter sum: p0*c0 + p1*c1 + p2*c2 + p3*c3
+ * To avoid double precision arithmetic we implemented multiplication using
+ * qrmul and addition using avgrnd. Coefficients( c0 to c3) formats are assumed
+ * to be: Qm, Qn, Qo, Qm, where m = n + 2 and o = n + 1.
+ * Typical usage of this filter is to quarter pixel interpolation of Bezier
+ * surface with filter coefficients:[-9 111 29 -3]/128. For which coefficient
+ * values should be: [-9216/2^17  28416/2^15  1484/2^16 -3072/2^17] for
+ * ISP_VEC_ELEMBITS = 16.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x4m_bicubic_bezier_quarter(s_1w_1x4_matrix in,
+			s_1w_1x4_matrix coeff);
+
+
+/** @brief Symmetric 3 tap filter with normalization
+ *
+ *  @param[in] in 1x3 matrix with pixels
+ *  @param[in] coeff 1x2 matrix with coefficients
+ *  @param[in] out_shift output pixel shift value for normalization
+ *
+ *  @return symmetric 3 tap filter output
+ *
+ * This function performs symmetric 3 tap filter input pixels.
+ * Filter sum is normalized by shifting out_shift bits.
+ * Filter sum: p0*c1 + p1*c0 + p2*c1
+ * is implemented as: (p0 + p2)*c1 + p1*c0 to reduce multiplication.
+ * Input pixels should to be scaled, otherwise overflow is possible during
+ * addition
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x3m_sym_nrm(s_1w_1x3_matrix in,
+		s_1w_1x2_matrix coeff,
+		tvector1w out_shift);
+
+/** @brief Symmetric 3 tap filter with normalization
+ *
+ *  @param[in] in 1x3 matrix with pixels
+ *  @param[in] coeff 1x2 matrix with coefficients
+ *
+ *  @return symmetric 3 tap filter output
+ *
+ * This function performs symmetric 3 tap filter over input pixels.
+ * Filter sum: p0*c1 + p1*c0 + p2*c1 = (p0 + p2)*c1 + p1*c0
+ * Input pixels are in Qn and coefficient c0 is in Qm and c1 is in Qn format,
+ * where n = ISP_VEC_ELEMBITS - 1 and m = ISP_VEC_ELEMBITS - 2 ( ie Q15 and Q14
+ * respectively for Broxton)
+ * To avoid double precision arithmetic input pixel sum is implemented using
+ * avgrnd, coefficient multiplication using qrmul and final sum using addsat
+ * Final sum is Qm format (ie Q14 for Broxton)
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+fir1x3m_sym_nrm_approx(s_1w_1x3_matrix in,
+		       s_1w_1x2_matrix coeff);
+
+/** @brief Mean of 1x3 matrix
+ *
+ *  @param[in] m 1x3 matrix with pixels
+ *
+ *  @return mean of 1x3 matrix
+ *
+ * This function calculates the mean of 1x3 pixels,
+ * with a factor of 4/3.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean1x3m(
+	s_1w_1x3_matrix m);
+
+/** @brief Mean of 3x3 matrix
+ *
+ *  @param[in] m 3x3 matrix with pixels
+ *
+ *  @return mean of 3x3 matrix
+ *
+ * This function calculates the mean of 3x3 pixels,
+ * with a factor of 16/9.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean3x3m(
+	s_1w_3x3_matrix m);
+
+/** @brief Mean of 1x4 matrix
+ *
+ *  @param[in] m 1x4 matrix with pixels
+ *
+ *  @return mean of 1x4 matrix
+ *
+ * This function calculates the mean of 1x4 pixels
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean1x4m(
+	s_1w_1x4_matrix m);
+
+/** @brief Mean of 4x4 matrix
+ *
+ *  @param[in] m 4x4 matrix with pixels
+ *
+ *  @return mean of 4x4 matrix
+ *
+ * This function calculates the mean of 4x4 matrix with pixels
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean4x4m(
+	s_1w_4x4_matrix m);
+
+/** @brief Mean of 2x3 matrix
+ *
+ *  @param[in] m 2x3 matrix with pixels
+ *
+ *  @return mean of 2x3 matrix
+ *
+ * This function calculates the mean of 2x3 matrix with pixels
+ * with a factor of 8/6.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean2x3m(
+	s_1w_2x3_matrix m);
+
+/** @brief Mean of 1x5 matrix
+ *
+ *  @param[in] m 1x5 matrix with pixels
+ *
+ *  @return mean of 1x5 matrix
+ *
+ * This function calculates the mean of 1x5 matrix with pixels
+ * with a factor of 8/5.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean1x5m(s_1w_1x5_matrix m);
+
+/** @brief Mean of 1x6 matrix
+ *
+ *  @param[in] m 1x6 matrix with pixels
+ *
+ *  @return mean of 1x6 matrix
+ *
+ * This function calculates the mean of 1x6 matrix with pixels
+ * with a factor of 8/6.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean1x6m(
+	s_1w_1x6_matrix m);
+
+/** @brief Mean of 5x5 matrix
+ *
+ *  @param[in] m 5x5 matrix with pixels
+ *
+ *  @return mean of 5x5 matrix
+ *
+ * This function calculates the mean of 5x5 matrix with pixels
+ * with a factor of 32/25.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean5x5m(
+	s_1w_5x5_matrix m);
+
+/** @brief Mean of 6x6 matrix
+ *
+ *  @param[in] m 6x6 matrix with pixels
+ *
+ *  @return mean of 6x6 matrix
+ *
+ * This function calculates the mean of 6x6 matrix with pixels
+ * with a factor of 64/36.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w mean6x6m(
+	s_1w_6x6_matrix m);
+
+/** @brief Minimum of 4x4 matrix
+ *
+ *  @param[in] m 4x4 matrix with pixels
+ *
+ *  @return minimum of 4x4 matrix
+ *
+ * This function calculates the  minimum of
+ * 4x4 matrix with pixels.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w min4x4m(
+	s_1w_4x4_matrix m);
+
+/** @brief Maximum of 4x4 matrix
+ *
+ *  @param[in] m 4x4 matrix with pixels
+ *
+ *  @return maximum of 4x4 matrix
+ *
+ * This function calculates the  maximum of
+ * 4x4 matrix with pixels.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w max4x4m(
+	s_1w_4x4_matrix m);
+
+/** @brief SAD between two 3x3 matrices
+ *
+ *  @param[in] a 3x3 matrix with pixels
+ *
+ *  @param[in] b 3x3 matrix with pixels
+ *
+ *  @return 3x3 matrix SAD
+ *
+ * This function calculates the sum of absolute difference between two matrices.
+ * Both input pixels and SAD are normalized by a factor of SAD3x3_IN_SHIFT and
+ * SAD3x3_OUT_SHIFT respectively.
+ * Computed SAD is 1/(2 ^ (SAD3x3_IN_SHIFT + SAD3x3_OUT_SHIFT)) ie 1/16 factor
+ * of original SAD and it's more precise than sad3x3m()
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w sad3x3m_precise(
+	s_1w_3x3_matrix a,
+	s_1w_3x3_matrix b);
+
+/** @brief SAD between two 3x3 matrices
+ *
+ *  @param[in] a 3x3 matrix with pixels
+ *
+ *  @param[in] b 3x3 matrix with pixels
+ *
+ *  @return 3x3 matrix SAD
+ *
+ * This function calculates the sum of absolute difference between two matrices.
+ * This version saves cycles by avoiding input normalization and wide vector
+ * operation during sum computation
+ * Input pixel differences are computed by absolute of rounded, halved
+ * subtraction. Normalized sum is computed by rounded averages.
+ * Computed SAD is (1/2)*(1/16) = 1/32 factor of original SAD. Factor 1/2 comes
+ * from input halving operation and factor 1/16 comes from mean operation
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w sad3x3m(
+	s_1w_3x3_matrix a,
+	s_1w_3x3_matrix b);
+
+/** @brief SAD between two 5x5 matrices
+ *
+ *  @param[in] a 5x5 matrix with pixels
+ *
+ *  @param[in] b 5x5 matrix with pixels
+ *
+ *  @return 5x5 matrix SAD
+ *
+ * Computed SAD is = 1/32 factor of original SAD.
+*/
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w sad5x5m(
+	s_1w_5x5_matrix a,
+	s_1w_5x5_matrix b);
+
+/** @brief Absolute gradient between two sets of 1x5 matrices
+ *
+ *  @param[in] m0 first set of 1x5 matrix with pixels
+ *  @param[in] m1 second set of 1x5 matrix with pixels
+ *
+ *  @return absolute gradient between two 1x5 matrices
+ *
+ * This function computes mean of two input 1x5 matrices and returns
+ * absolute difference between two mean values.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w
+absgrad1x5m(s_1w_1x5_matrix m0, s_1w_1x5_matrix m1);
+
+/** @brief Bi-linear Interpolation optimized(approximate)
+ *
+ * @param[in] a input0
+ * @param[in] b input1
+ * @param[in] c cloned weight factor
+  *
+ * @return		(a-b)*c + b
+ *
+ * This function will do bi-linear Interpolation on
+ * inputs a and b using constant weight factor c
+ *
+ * Inputs a,b are assumed in S1.15 format
+ * Weight factor has to be in range [0,1] and is assumed to be in S2.14 format
+ *
+ * The bilinear interpolation equation is (a*c) + b*(1-c),
+ * But this is implemented as (a-b)*c + b for optimization
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_bilinear_interpol_approx_c(
+	tvector1w a,
+	tvector1w b,
+	tscalar1w_weight c);
+
+/** @brief Bi-linear Interpolation optimized(approximate)
+ *
+ * @param[in] a input0
+ * @param[in] b input1
+ * @param[in] c weight factor
+  *
+ * @return		(a-b)*c + b
+ *
+ * This function will do bi-linear Interpolation on
+ * inputs a and b using weight factor c
+ *
+ * Inputs a,b are assumed in S1.15 format
+ * Weight factor has to be in range [0,1] and is assumed to be in S2.14 format
+ *
+ * The bilinear interpolation equation is (a*c) + b*(1-c),
+ * But this is implemented as (a-b)*c + b for optimization
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_bilinear_interpol_approx(
+	tvector1w a,
+	tvector1w b,
+	tvector1w_weight c);
+
+/** @brief Bi-linear Interpolation
+ *
+ * @param[in] a input0
+ * @param[in] b input1
+ * @param[in] c weight factor
+  *
+ * @return		(a*c) + b*(1-c)
+ *
+ * This function will do bi-linear Interpolation on
+ * inputs a and b using weight factor c
+ *
+ * Inputs a,b are assumed in S1.15 format
+ * Weight factor has to be in range [0,1] and is assumed to be in S2.14 format
+ *
+ * The bilinear interpolation equation is (a*c) + b*(1-c),
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_bilinear_interpol(
+	tvector1w a,
+	tvector1w b,
+	tscalar1w_weight c);
+
+/** @brief Generic Block Matching Algorithm
+ * @param[in] search_window pointer to input search window of 16x16 pixels
+ * @param[in] ref_block pointer to input reference block of 8x8 pixels, where N<=M
+ * @param[in] output pointer to output sads
+ * @param[in] search_sz search size for SAD computation
+ * @param[in] ref_sz block size
+ * @param[in] pixel_shift pixel shift to search the data
+ * @param[in] search_block_sz search window block size
+ * @param[in] shift shift value, with which the output is shifted right
+ *
+ * @return   0 when the computation is successful.
+
+ * * This function compares the reference block with a block of size NxN in the search
+ * window. Sum of absolute differences for each pixel in the reference block and the
+ * corresponding pixel in the search block. Whole search window os traversed with the
+ * reference block with the given pixel shift.
+ *
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H int generic_block_matching_algorithm(
+	tscalar1w **search_window,
+	tscalar1w **ref_block,
+	tscalar1w *output,
+	int search_sz,
+	int ref_sz,
+	int pixel_shift,
+	int search_block_sz,
+	tscalar1w_4bit_bma_shift shift);
+
+#ifndef ISP2401
+/** @brief OP_1w_asp_bma_16_1_32way
+#else
+/** @brief OP_1w_asp_bma_16_1_32way_nomask
+#endif
+ *
+ * @param[in] search_area input search window of 16x16 pixels
+ * @param[in] input_block input reference block of 8x8 pixels, where N<=M
+ * @param[in] shift shift value, with which the output is shifted right
+ *
+ * @return   81 SADs for all the search blocks.
+
+ * This function compares the reference block with a block of size 8x8 pixels in the
+ * search window of 16x16 pixels. Sum of absolute differences for each pixel in the
+ * reference block and the corresponding pixel in the search block is calculated.
+ * Whole search window is traversed with the reference block with the pixel shift of 1
+ * pixels. The output is right shifted with the given shift value. The shift value is
+ * a 4 bit value.
+ *
+ */
+
+#ifndef ISP2401
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_16_1 OP_1w_asp_bma_16_1_32way(
+#else
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_16_1 OP_1w_asp_bma_16_1_32way_nomask(
+#endif
+	bma_16x16_search_window search_area,
+	ref_block_8x8 input_block,
+	tscalar1w_4bit_bma_shift shift);
+
+#ifndef ISP2401
+/** @brief OP_1w_asp_bma_16_2_32way
+#else
+/** @brief OP_1w_asp_bma_16_2_32way_nomask
+#endif
+ *
+ * @param[in] search_area input search window of 16x16 pixels
+ * @param[in] input_block input reference block of 8x8 pixels, where N<=M
+ * @param[in] shift shift value, with which the output is shifted right
+ *
+ * @return   25 SADs for all the search blocks.
+ * This function compares the reference block with a block of size 8x8 in the search
+ * window of 16x61. Sum of absolute differences for each pixel in the reference block
+ * and the corresponding pixel in the search block is computed. Whole search window is
+ * traversed with the reference block with the given pixel shift of 2 pixels. The output
+ * is right shifted with the given shift value. The shift value is a 4 bit value.
+ *
+ */
+
+#ifndef ISP2401
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_16_2 OP_1w_asp_bma_16_2_32way(
+#else
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_16_2 OP_1w_asp_bma_16_2_32way_nomask(
+#endif
+	bma_16x16_search_window search_area,
+	ref_block_8x8 input_block,
+	tscalar1w_4bit_bma_shift shift);
+#ifndef ISP2401
+/** @brief OP_1w_asp_bma_14_1_32way
+#else
+/** @brief OP_1w_asp_bma_14_1_32way_nomask
+#endif
+ *
+ * @param[in] search_area input search block of 16x16 pixels with search window of 14x14 pixels
+ * @param[in] input_block input reference block of 8x8 pixels, where N<=M
+ * @param[in] shift shift value, with which the output is shifted right
+ *
+ * @return   49 SADs for all the search blocks.
+ * This function compares the reference block with a block of size 8x8 in the search
+ * window of 14x14. Sum of absolute differences for each pixel in the reference block
+ * and the corresponding pixel in the search block. Whole search window is traversed
+ * with the reference block with 2 pixel shift. The output is right shifted with the
+ * given shift value. The shift value is a 4 bit value. Input is always a 16x16 block
+ * but the search window is 14x14, with last 2 pixels of row and column are not used
+ * for computation.
+ *
+ */
+
+#ifndef ISP2401
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_14_1 OP_1w_asp_bma_14_1_32way(
+#else
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_14_1 OP_1w_asp_bma_14_1_32way_nomask(
+#endif
+	bma_16x16_search_window search_area,
+	ref_block_8x8 input_block,
+	tscalar1w_4bit_bma_shift shift);
+
+#ifndef ISP2401
+/** @brief OP_1w_asp_bma_14_2_32way
+#else
+/** @brief OP_1w_asp_bma_14_2_32way_nomask
+#endif
+ *
+ * @param[in] search_area input search block of 16x16 pixels with search window of 14x14 pixels
+ * @param[in] input_block input reference block of 8x8 pixels, where N<=M
+ * @param[in] shift shift value, with which the output is shifted right
+ *
+ * @return   16 SADs for all the search blocks.
+ * This function compares the reference block with a block of size 8x8 in the search
+ * window of 14x14. Sum of absolute differences for each pixel in the reference block
+ * and the corresponding pixel in the search block. Whole search window is traversed
+ * with the reference block with 2 pixels shift. The output is right shifted with the
+ * given shift value. The shift value is a 4 bit value.
+ *
+ */
+
+#ifndef ISP2401
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_14_2 OP_1w_asp_bma_14_2_32way(
+#else
+STORAGE_CLASS_REF_VECTOR_FUNC_H bma_output_14_2 OP_1w_asp_bma_14_2_32way_nomask(
+#endif
+	bma_16x16_search_window search_area,
+	ref_block_8x8 input_block,
+	tscalar1w_4bit_bma_shift shift);
+
+#ifdef ISP2401
+/** @brief multiplex addition and passing
+ *
+ *  @param[in] _a first pixel
+ *  @param[in] _b second pixel
+ *  @param[in] _c condition flag
+ *
+ *  @return (_a + _b) if condition flag is true
+ *	    _a if condition flag is false
+ *
+ * This function does multiplex addition depending on the input condition flag
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H tvector1w OP_1w_cond_add(
+	tvector1w _a,
+	tvector1w _b,
+	tflags _c);
+
+#endif
+#ifdef HAS_bfa_unit
+/** @brief OP_1w_single_bfa_7x7
+ *
+ * @param[in] weights - spatial and range weight lut
+ * @param[in] threshold - threshold plane, for range weight scaling
+ * @param[in] central_pix - central pixel plane
+ * @param[in] src_plane - src pixel plane
+ *
+ * @return   Bilateral filter output
+ *
+ * This function implements, 7x7 single bilateral filter.
+ * Output = {sum(pixel * weight), sum(weight)}
+ * Where sum is summation over 7x7 block set.
+ * weight = spatial weight * range weight
+ * spatial weights are loaded from spatial_weight_lut depending on src pixel
+ * position in the 7x7 block
+ * range weights are computed by table look up from range_weight_lut depending
+ * on scaled absolute difference between src and central pixels.
+ * threshold is used as scaling factor. range_weight_lut consists of
+ * BFA_RW_LUT_SIZE numbers of LUT entries to model any distribution function.
+ * Piecewise linear approximation technique is used to compute range weight
+ * It computes absolute difference between central pixel and 61 src pixels.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H bfa_7x7_output OP_1w_single_bfa_7x7(
+	bfa_weights weights,
+	tvector1w threshold,
+	tvector1w central_pix,
+	s_1w_7x7_matrix src_plane);
+
+/** @brief OP_1w_joint_bfa_7x7
+ *
+ * @param[in] weights - spatial and range weight lut
+ * @param[in] threshold0 - 1st threshold plane, for range weight scaling
+ * @param[in] central_pix0 - 1st central pixel plane
+ * @param[in] src0_plane - 1st pixel plane
+ * @param[in] threshold1 - 2nd threshold plane, for range weight scaling
+ * @param[in] central_pix1 - 2nd central pixel plane
+ * @param[in] src1_plane - 2nd pixel plane
+ *
+ * @return   Joint bilateral filter output
+ *
+ * This function implements, 7x7 joint bilateral filter.
+ * Output = {sum(pixel * weight), sum(weight)}
+ * Where sum is summation over 7x7 block set.
+ * weight = spatial weight * range weight
+ * spatial weights are loaded from spatial_weight_lut depending on src pixel
+ * position in the 7x7 block
+ * range weights are computed by table look up from range_weight_lut depending
+ * on sum of scaled absolute difference between central pixel and two src pixel
+ * planes. threshold is used as scaling factor. range_weight_lut consists of
+ * BFA_RW_LUT_SIZE numbers of LUT entries to model any distribution function.
+ * Piecewise linear approximation technique is used to compute range weight
+ * It computes absolute difference between central pixel and 61 src pixels.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H bfa_7x7_output OP_1w_joint_bfa_7x7(
+	bfa_weights weights,
+	tvector1w threshold0,
+	tvector1w central_pix0,
+	s_1w_7x7_matrix src0_plane,
+	tvector1w threshold1,
+	tvector1w central_pix1,
+	s_1w_7x7_matrix src1_plane);
+
+/** @brief bbb_bfa_gen_spatial_weight_lut
+ *
+ * @param[in] in - 7x7 matrix of spatial weights
+ * @param[in] out - generated LUT
+ *
+ * @return   None
+ *
+ * This function implements, creates spatial weight look up table used
+ * for bilaterl filter instruction.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H void bbb_bfa_gen_spatial_weight_lut(
+	s_1w_7x7_matrix in,
+	tvector1w out[BFA_MAX_KWAY]);
+
+/** @brief bbb_bfa_gen_range_weight_lut
+ *
+ * @param[in] in - input range weight,
+ * @param[in] out - generated LUT
+ *
+ * @return   None
+ *
+ * This function implements, creates range weight look up table used
+ * for bilaterl filter instruction.
+ * 8 unsigned 7b weights are represented in 7 16bits LUT
+ * LUT formation is done as follows:
+ * higher 8 bit: Point(N) = Point(N+1) - Point(N)
+ * lower 8 bit: Point(N) = Point(N)
+ * Weight function can be any monotonic decreasing function for x >= 0
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H void bbb_bfa_gen_range_weight_lut(
+	tvector1w in[BFA_RW_LUT_SIZE+1],
+	tvector1w out[BFA_RW_LUT_SIZE]);
+#endif
+
+#ifdef ISP2401
+/** @brief OP_1w_imax32
+ *
+ * @param[in] src - structure that holds an array of 32 elements.
+ *
+ * @return  maximum element among input array.
+ *
+ *This function gets maximum element from an array of 32 elements.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H int OP_1w_imax32(
+	imax32_ref_in_vector src);
+
+/** @brief OP_1w_imaxidx32
+ *
+ * @param[in] src - structure that holds a vector of elements.
+ *
+ * @return  index of first element with maximum value among array.
+ *
+ * This function gets index of first element with maximum value
+ * from 32 elements.
+ */
+STORAGE_CLASS_REF_VECTOR_FUNC_H int OP_1w_imaxidx32(
+	imax32_ref_in_vector src);
+
+#endif
+#ifndef INLINE_VECTOR_FUNC
+#define STORAGE_CLASS_REF_VECTOR_FUNC_C
+#define STORAGE_CLASS_REF_VECTOR_DATA_C const
+#else /* INLINE_VECTOR_FUNC */
+#define STORAGE_CLASS_REF_VECTOR_FUNC_C STORAGE_CLASS_REF_VECTOR_FUNC_H
+#define STORAGE_CLASS_REF_VECTOR_DATA_C STORAGE_CLASS_REF_VECTOR_DATA_H
+#include "ref_vector_func.c"
+#define VECTOR_FUNC_INLINED
+#endif  /* INLINE_VECTOR_FUNC */
+
+#endif /*_REF_VECTOR_FUNC_H_INCLUDED_*/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func_types.h
new file mode 100644
index 000000000000..4dd05eba852e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/ref_vector_func_types.h
@@ -0,0 +1,385 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __REF_VECTOR_FUNC_TYPES_H_INCLUDED__
+#define __REF_VECTOR_FUNC_TYPES_H_INCLUDED__
+
+
+/*
+ * Prerequisites:
+ *
+ */
+#include "mpmath.h"
+#include "bbb_config.h"
+#include "isp_op1w_types.h"
+#include "isp_op2w_types.h"
+
+/* Defines for the Config Unit */
+#define MAX_CONFIG_POINTS 5
+#define INPUT_OFFSET_FACTOR 10
+#define INPUT_SCALE_FACTOR 10
+#define OUTPUT_SCALE_FACTOR 10
+#define SLOPE_A_RESOLUTION 10
+#define CONFIG_UNIT_LUT_SIZE_32 32 /*XCU works for ISP_NWAY = 32 */
+#define LXCU_LUT_SIZE      16
+#ifdef ISP2401
+#define IMAX32_ELEM_SIZE   32
+#endif
+
+#define ONE_IN_Q14 (1<<(NUM_BITS-2))
+#define Q29_TO_Q15_SHIFT_VAL (NUM_BITS-2)
+#define Q28_TO_Q15_SHIFT_VAL (NUM_BITS-3)
+#define MAX_ELEM(width_in_bits) ((1<<(width_in_bits))-1)
+
+/* Block matching algorithm related data */
+/* NUM_OF_SADS = ((SEARCH_AREA_HEIGHT - REF_BLOCK_HEIGHT)/PIXEL_SHIFT + 1)* \
+					((SEARCH_AREA_WIDTH - REF_BLOCK_WIDTH)/PIXEL_SHIFT + 1) */
+
+#define SADS(sw_h,sw_w, ref_h, ref_w, p_sh) (((sw_h - ref_h)/p_sh + 1)*((sw_w - ref_w)/p_sh + 1))
+#define SADS_16x16_1	SADS(16, 16, 8, 8, 1)
+#define SADS_16x16_2	SADS(16, 16, 8, 8, 2)
+#define SADS_14x14_1	SADS(14, 14, 8, 8, 1)
+#define SADS_14x14_2	SADS(14, 14, 8, 8, 2)
+
+#define BMA_OUTPUT_MATRIX_DIM(sw_h, ref_h, p_sh)	((sw_h - ref_h)/p_sh + 1)
+#define BMA_OUT_16x16_2_32		BMA_OUTPUT_MATRIX_DIM(16, 8, 2)
+#define BMA_OUT_14x14_2_32		BMA_OUTPUT_MATRIX_DIM(14, 8, 2)
+#define BMA_OUT_16x16_1_32 		BMA_OUTPUT_MATRIX_DIM(16, 8, 1)
+#define BMA_OUT_14x14_1_32 		BMA_OUTPUT_MATRIX_DIM(14, 8, 1)
+#define BMA_SEARCH_BLOCK_SZ_16 	16
+#define BMA_REF_BLOCK_SZ_8    	8
+#define PIXEL_SHIFT_2         	2
+#define PIXEL_SHIFT_1         	1
+#define BMA_SEARCH_WIN_SZ_16  	16
+#define BMA_SEARCH_WIN_SZ_14  	14
+
+
+/*
+ * Struct type specification
+ */
+
+typedef unsigned short tscalar1w_3bit;       /* tscalar1w in interval [0, 2^3)                       */
+typedef short tscalar1w_5bit_signed;         /* tscalar1w in interval [-2^(5-1), 2^(5-1))            */
+typedef unsigned short tscalar1w_5bit;       /* tscalar1w in interval [0, 2^5)                       */
+typedef short tscalar1w_range1wbit;          /* tscalar1w in interval [-NUM_BITS, NUM_BITS]          */
+typedef short tscalar1w_unsigned_range1wbit; /* tscalar1w in interval [0, NUM_BITS]                  */
+typedef unsigned short tvector_8bit;		/* 8 bit positive number */
+typedef unsigned short tvector_5bit;
+typedef unsigned short tvector_4bit;
+typedef unsigned short tscalar1w_16bit;
+typedef unsigned short tscalar1w_4bit_bma_shift;
+
+typedef struct {
+  tvector1w     v0  ;
+  tvector1w     v1 ;
+} s_1w_2x1_matrix;
+
+#define S_1W_2X1_MATRIX_DEFAULT ((s_1w_2x1_matrix)\
+	{ 0, 0 })
+
+typedef struct {
+	tvector1w v00;
+	tvector1w v01;
+} s_1w_1x2_matrix;
+
+#define S_1W_1X2_MATRIX_DEFAULT ((s_1w_1x2_matrix)\
+	{ 0, 0 })
+
+typedef struct {
+  tvector1w     v00  ;
+  tvector1w     v01 ;
+  tvector1w     v02 ;
+} s_1w_1x3_matrix;
+
+#define S_1W_1X3_MATRIX_DEFAULT ((s_1w_1x3_matrix)\
+	{ 0, 0, 0, })
+
+typedef struct {
+  tvector1w v00; tvector1w v01; tvector1w v02;
+  tvector1w v10; tvector1w v11; tvector1w v12;
+} s_1w_2x3_matrix;
+
+#define S_1W_2X3_MATRIX_DEFAULT ((s_1w_2x3_matrix)\
+	{ 0, 0, 0, \
+	  0, 0, 0 })
+
+typedef struct {
+  tvector1w     v00  ; tvector1w     v01 ; tvector1w     v02  ;
+  tvector1w     v10  ; tvector1w     v11 ; tvector1w     v12  ;
+  tvector1w     v20  ; tvector1w     v21 ; tvector1w     v22  ;
+} s_1w_3x3_matrix;
+
+#define S_1W_3X3_MATRIX_DEFAULT ((s_1w_3x3_matrix)\
+	{ 0, 0, 0, \
+	  0, 0, 0, \
+	  0, 0, 0 })
+
+typedef struct {
+  tvector1w     v00  ; tvector1w     v01 ; tvector1w     v02  ;
+  tvector1w     v10  ; tvector1w     v11 ; tvector1w     v12  ;
+  tvector1w     v20  ; tvector1w     v21 ; tvector1w     v22  ;
+  tvector1w     v30  ; tvector1w     v31 ; tvector1w     v32  ;
+} s_1w_4x3_matrix;
+
+#define S_1W_4X3_MATRIX_DEFAULT ((s_1w_4x3_matrix)\
+	{ 0, 0, 0, \
+	  0, 0, 0, \
+	  0, 0, 0, \
+	  0, 0, 0 })
+
+typedef struct {
+  tvector1w     v00 ;
+  tvector1w     v01 ;
+  tvector1w     v02 ;
+  tvector1w     v03 ;
+  tvector1w     v04 ;
+} s_1w_1x5_matrix;
+
+#define S_1W_1X5_MATRIX_DEFAULT ((s_1w_1x5_matrix)\
+	{ 0, 0, 0, 0, 0 })
+
+typedef struct {
+  tvector1w     v00  ; tvector1w     v01 ; tvector1w     v02  ; tvector1w     v03 ; tvector1w     v04  ;
+  tvector1w     v10  ; tvector1w     v11 ; tvector1w     v12  ; tvector1w     v13 ; tvector1w     v14  ;
+  tvector1w     v20  ; tvector1w     v21 ; tvector1w     v22  ; tvector1w     v23 ; tvector1w     v24  ;
+  tvector1w     v30  ; tvector1w     v31 ; tvector1w     v32  ; tvector1w     v33 ; tvector1w     v34  ;
+  tvector1w     v40  ; tvector1w     v41 ; tvector1w     v42  ; tvector1w     v43 ; tvector1w     v44  ;
+} s_1w_5x5_matrix;
+
+#define S_1W_5X5_MATRIX_DEFAULT ((s_1w_5x5_matrix)\
+	{ 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0 })
+#ifndef ISP2401
+	
+#else
+
+#endif
+typedef struct {
+	tvector1w v00;
+	tvector1w v01;
+	tvector1w v02;
+	tvector1w v03;
+	tvector1w v04;
+	tvector1w v05;
+	tvector1w v06;
+} s_1w_1x7_matrix;
+
+#define S_1W_1X7_MATRIX_DEFAULT ((s_1w_1x7_matrix)\
+	{ 0, 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00;
+	tvector1w v01;
+	tvector1w v02;
+	tvector1w v03;
+	tvector1w v04;
+	tvector1w v05;
+	tvector1w v06;
+	tvector1w v07;
+	tvector1w v08;
+} s_1w_1x9_matrix;
+
+#define S_1W_1X9_MATRIX_DEFAULT ((s_1w_1x9_matrix)\
+	{ 0, 0, 0, 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00;
+	tvector1w v01;
+	tvector1w v02;
+	tvector1w v03;
+} s_1w_1x4_matrix;
+
+#define S_1W_1X4_MATRIX ((s_1w_1x4_matrix)\
+	{ 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00; tvector1w v01; tvector1w v02; tvector1w v03;
+	tvector1w v10; tvector1w v11; tvector1w v12; tvector1w v13;
+	tvector1w v20; tvector1w v21; tvector1w v22; tvector1w v23;
+	tvector1w v30; tvector1w v31; tvector1w v32; tvector1w v33;
+} s_1w_4x4_matrix;
+
+#define S_1W_4X4_MATRIX_DEFAULT ((s_1w_4x4_matrix)\
+	{ 0, 0, 0, 0, \
+	  0, 0, 0, 0, \
+	  0, 0, 0, 0, \
+	  0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00;
+	tvector1w v01;
+	tvector1w v02;
+	tvector1w v03;
+	tvector1w v04;
+	tvector1w v05;
+} s_1w_1x6_matrix;
+
+#define S_1W_1X6_MATRIX_DEFAULT ((s_1w_1x6_matrix)\
+	{ 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00; tvector1w v01; tvector1w v02; tvector1w v03; tvector1w v04; tvector1w v05;
+	tvector1w v10; tvector1w v11; tvector1w v12; tvector1w v13; tvector1w v14; tvector1w v15;
+	tvector1w v20; tvector1w v21; tvector1w v22; tvector1w v23; tvector1w v24; tvector1w v25;
+	tvector1w v30; tvector1w v31; tvector1w v32; tvector1w v33; tvector1w v34; tvector1w v35;
+	tvector1w v40; tvector1w v41; tvector1w v42; tvector1w v43; tvector1w v44; tvector1w v45;
+	tvector1w v50; tvector1w v51; tvector1w v52; tvector1w v53; tvector1w v54; tvector1w v55;
+} s_1w_6x6_matrix;
+
+#define S_1W_6X6_MATRIX_DEFAULT ((s_1w_6x6_matrix)\
+	{ 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00; tvector1w v01; tvector1w v02; tvector1w v03; tvector1w v04;
+	tvector1w v05; tvector1w v06; tvector1w v07; tvector1w v08;
+	tvector1w v10; tvector1w v11; tvector1w v12; tvector1w v13; tvector1w v14;
+	tvector1w v15; tvector1w v16; tvector1w v17; tvector1w v18;
+	tvector1w v20; tvector1w v21; tvector1w v22; tvector1w v23; tvector1w v24;
+	tvector1w v25; tvector1w v26; tvector1w v27; tvector1w v28;
+	tvector1w v30; tvector1w v31; tvector1w v32; tvector1w v33; tvector1w v34;
+	tvector1w v35; tvector1w v36; tvector1w v37; tvector1w v38;
+	tvector1w v40; tvector1w v41; tvector1w v42; tvector1w v43; tvector1w v44;
+	tvector1w v45; tvector1w v46; tvector1w v47; tvector1w v48;
+	tvector1w v50; tvector1w v51; tvector1w v52; tvector1w v53; tvector1w v54;
+	tvector1w v55; tvector1w v56; tvector1w v57; tvector1w v58;
+	tvector1w v60; tvector1w v61; tvector1w v62; tvector1w v63; tvector1w v64;
+	tvector1w v65; tvector1w v66; tvector1w v67; tvector1w v68;
+	tvector1w v70; tvector1w v71; tvector1w v72; tvector1w v73; tvector1w v74;
+	tvector1w v75; tvector1w v76; tvector1w v77; tvector1w v78;
+	tvector1w v80; tvector1w v81; tvector1w v82; tvector1w v83; tvector1w v84;
+	tvector1w v85; tvector1w v86; tvector1w v87; tvector1w v88;
+} s_1w_9x9_matrix;
+
+#define S_1W_9X9_MATRIX_DEFAULT ((s_1w_9x9_matrix)\
+	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v00; tvector1w v01; tvector1w v02; tvector1w v03; tvector1w v04;
+	tvector1w v05; tvector1w v06;
+	tvector1w v10; tvector1w v11; tvector1w v12; tvector1w v13; tvector1w v14;
+	tvector1w v15; tvector1w v16;
+	tvector1w v20; tvector1w v21; tvector1w v22; tvector1w v23; tvector1w v24;
+	tvector1w v25; tvector1w v26;
+	tvector1w v30; tvector1w v31; tvector1w v32; tvector1w v33; tvector1w v34;
+	tvector1w v35; tvector1w v36;
+	tvector1w v40; tvector1w v41; tvector1w v42; tvector1w v43; tvector1w v44;
+	tvector1w v45; tvector1w v46;
+	tvector1w v50; tvector1w v51; tvector1w v52; tvector1w v53; tvector1w v54;
+	tvector1w v55; tvector1w v56;
+	tvector1w v60; tvector1w v61; tvector1w v62; tvector1w v63; tvector1w v64;
+	tvector1w v65; tvector1w v66;
+} s_1w_7x7_matrix;
+
+#define S_1W_7X7_MATRIX_DEFAULT ((s_1w_7x7_matrix)\
+	{ 0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0, \
+	  0, 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w v0_0;
+	tvector1w v0_1;
+	tvector1w v0_2;
+	tvector1w v0_3;
+	tvector1w v0_4;
+	tvector1w v0_5;
+	tvector1w v0_6;
+	tvector1w v0_7;
+	tvector1w v0_8;
+	tvector1w v0_9;
+	tvector1w v0_10;
+} s_1w_1x11_matrix;
+
+#define S_1W_1X11_MATRIX_DEFAULT ((s_1w_1x11_matrix)\
+	{ 0, 0, 0, 0, 0, 0, 0, 0, 0 })
+
+typedef struct {
+	tvector1w x_cord[MAX_CONFIG_POINTS];
+	tvector1w slope[MAX_CONFIG_POINTS-1];
+	tvector1w y_offset[MAX_CONFIG_POINTS-1];
+} ref_config_points;
+
+typedef struct {
+	tscalar1w_range1wbit slope_vec[CONFIG_UNIT_LUT_SIZE_32];
+	tscalar1w_range1wbit offset_vec[CONFIG_UNIT_LUT_SIZE_32];
+	tscalar1w_16bit x_cord_vec[CONFIG_UNIT_LUT_SIZE_32];
+	tscalar1w_16bit x_cord_max;
+	tscalar1w_5bit exponent;
+	tscalar1w_5bit slope_resolution;
+} xcu_ref_init_vectors;
+
+typedef struct {
+#ifdef ISP2401
+	tvector1w elem[IMAX32_ELEM_SIZE];
+} imax32_ref_in_vector;
+
+typedef struct {
+#endif
+	tscalar1w search[BMA_SEARCH_BLOCK_SZ_16][BMA_SEARCH_BLOCK_SZ_16];
+} bma_16x16_search_window;
+
+typedef struct {
+	tscalar1w ref[BMA_REF_BLOCK_SZ_8][BMA_REF_BLOCK_SZ_8];
+} ref_block_8x8;
+
+typedef struct {
+	tscalar1w sads[SADS_16x16_1];
+} bma_output_16_1;
+
+typedef struct {
+	tscalar1w sads[SADS_16x16_2];
+} bma_output_16_2;
+
+typedef struct {
+	tscalar1w sads[SADS_14x14_2];
+} bma_output_14_2;
+
+typedef struct {
+	tscalar1w sads[SADS_14x14_1];
+} bma_output_14_1;
+
+typedef struct {
+	tvector1w spatial_weight_lut[BFA_MAX_KWAY]; /* spatial weight LUT */
+	/* range weight LUT, (BFA_RW_LUT_SIZE + 1) numbers of LUT values are compressed in BFA_RW_LUT_SIZE buffer.
+	 * range_weight_lut[k] = packed(drop[k], range_weight[k])
+	 * where, drop[k] = range_weight[k+1] - range_weight[k]
+	 * pack(msb, lsb): two 8bits numbers packed in one 16bits number */
+	tvector1w range_weight_lut[BFA_RW_LUT_SIZE];
+} bfa_weights;
+
+/* Return type for BFA BBBs */
+typedef struct {
+	tvector2w sop; /* weighted sum of pixels */
+	tvector1w sow; /* sum of weights */
+} bfa_7x7_output;
+#endif /* __REF_VECTOR_FUNC_TYPES_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/sp_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/sp_public.h
new file mode 100644
index 000000000000..974ce6a33b4b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/sp_public.h
@@ -0,0 +1,223 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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)
+ */
+extern 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
+ */
+extern 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/atomisp2/css2400/hive_isp_css_include/host/tag_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/tag_public.h
new file mode 100644
index 000000000000..22ef747f3d4a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/tag_public.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_include/host/timed_ctrl_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/timed_ctrl_public.h
new file mode 100644
index 000000000000..b3becac16f49
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/timed_ctrl_public.h
@@ -0,0 +1,59 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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);
+
+extern 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);
+
+extern 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);
+
+extern 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/atomisp2/css2400/hive_isp_css_include/host/vamem_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/vamem_public.h
new file mode 100644
index 000000000000..cee15d0ab2d8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/vamem_public.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_include/host/vmem_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/vmem_public.h
new file mode 100644
index 000000000000..e9801c0fe147
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/host/vmem_public.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_include/ibuf_ctrl.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/ibuf_ctrl.h
new file mode 100644
index 000000000000..f5de0df7981e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/ibuf_ctrl.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_H_INCLUDED__
+#define __IBUF_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 "storage_class.h"
+
+#include "system_local.h"
+#include "ibuf_ctrl_local.h"
+
+#ifndef __INLINE_IBUF_CTRL__
+#define STORAGE_CLASS_IBUF_CTRL_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_IBUF_CTRL_C
+#include "ibuf_ctrl_public.h"
+#else  /* __INLINE_IBUF_CTRL__ */
+#define STORAGE_CLASS_IBUF_CTRL_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_IBUF_CTRL_C STORAGE_CLASS_INLINE
+#include "ibuf_ctrl_private.h"
+#endif /* __INLINE_IBUF_CTRL__ */
+
+#endif /* __IBUF_CTRL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_formatter.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_formatter.h
new file mode 100644
index 000000000000..041c8b660aa4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_formatter.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "input_formatter_local.h"
+
+#ifndef __INLINE_INPUT_FORMATTER__
+#define STORAGE_CLASS_INPUT_FORMATTER_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_INPUT_FORMATTER_C 
+#include "input_formatter_public.h"
+#else  /* __INLINE_INPUT_FORMATTER__ */
+#define STORAGE_CLASS_INPUT_FORMATTER_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_INPUT_FORMATTER_C STORAGE_CLASS_INLINE
+#include "input_formatter_private.h"
+#endif /* __INLINE_INPUT_FORMATTER__ */
+
+#endif /* __INPUT_FORMATTER_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_system.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_system.h
new file mode 100644
index 000000000000..182867367b48
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/input_system.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "input_system_local.h"
+
+#ifndef __INLINE_INPUT_SYSTEM__
+#define STORAGE_CLASS_INPUT_SYSTEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_INPUT_SYSTEM_C 
+#include "input_system_public.h"
+#else  /* __INLINE_INPUT_SYSTEM__ */
+#define STORAGE_CLASS_INPUT_SYSTEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_INPUT_SYSTEM_C STORAGE_CLASS_INLINE
+#include "input_system_private.h"
+#endif /* __INLINE_INPUT_SYSTEM__ */
+
+#endif /* __INPUT_SYSTEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/irq.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/irq.h
new file mode 100644
index 000000000000..1dc443892cc5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/irq.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "irq_local.h"
+
+#ifndef __INLINE_IRQ__
+#define STORAGE_CLASS_IRQ_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_IRQ_C 
+#include "irq_public.h"
+#else  /* __INLINE_IRQ__ */
+#define STORAGE_CLASS_IRQ_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_IRQ_C STORAGE_CLASS_INLINE
+#include "irq_private.h"
+#endif /* __INLINE_IRQ__ */
+
+#endif /* __IRQ_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isp.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isp.h
new file mode 100644
index 000000000000..49190d0abc30
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isp.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "isp_local.h"
+
+#ifndef __INLINE_ISP__
+#define STORAGE_CLASS_ISP_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_ISP_C 
+#include "isp_public.h"
+#else  /* __INLINE_iSP__ */
+#define STORAGE_CLASS_ISP_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_ISP_C STORAGE_CLASS_INLINE
+#include "isp_private.h"
+#endif /* __INLINE_ISP__ */
+
+#endif /* __ISP_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_dma.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_dma.h
new file mode 100644
index 000000000000..9a608f07adcb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_dma.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_H_INCLUDED__
+#define __ISYS_DMA_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 "storage_class.h"
+
+#include "system_local.h"
+#include "isys_dma_local.h"
+
+#ifndef __INLINE_ISYS2401_DMA__
+#define STORAGE_CLASS_ISYS2401_DMA_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_ISYS2401_DMA_C
+#include "isys_dma_public.h"
+#else  /* __INLINE_ISYS2401_DMA__ */
+#define STORAGE_CLASS_ISYS2401_DMA_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_ISYS2401_DMA_C STORAGE_CLASS_INLINE
+#include "isys_dma_private.h"
+#endif /* __INLINE_ISYS2401_DMA__ */
+
+#endif /* __ISYS_DMA_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_irq.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_irq.h
new file mode 100644
index 000000000000..cf858bcc8e45
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_irq.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <storage_class.h>
+#include <system_local.h>
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+
+#ifndef __INLINE_ISYS2401_IRQ__
+
+#define STORAGE_CLASS_ISYS2401_IRQ_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_ISYS2401_IRQ_C STORAGE_CLASS_EXTERN
+#include "isys_irq_public.h"
+
+#else  /* __INLINE_ISYS2401_IRQ__ */
+
+#define STORAGE_CLASS_ISYS2401_IRQ_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_ISYS2401_IRQ_C STORAGE_CLASS_INLINE
+#include "isys_irq_private.h"
+
+#endif /* __INLINE_ISYS2401_IRQ__ */
+
+#endif /* defined(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#endif	/* __IA_CSS_ISYS_IRQ_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_stream2mmio.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_stream2mmio.h
new file mode 100644
index 000000000000..3e8cfe555ad5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/isys_stream2mmio.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "isys_stream2mmio_local.h"
+
+#ifndef __INLINE_STREAM2MMIO__
+#define STORAGE_CLASS_STREAM2MMIO_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_STREAM2MMIO_C
+#include "isys_stream2mmio_public.h"
+#else  /* __INLINE_STREAM2MMIO__ */
+#define STORAGE_CLASS_STREAM2MMIO_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_STREAM2MMIO_C STORAGE_CLASS_INLINE
+#include "isys_stream2mmio_private.h"
+#endif /* __INLINE_STREAM2MMIO__ */
+
+#endif /* __ISYS_STREAM2MMIO_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h
new file mode 100644
index 000000000000..48d84bc0ad9e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/math_support.h
@@ -0,0 +1,224 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h" /* for STORAGE_CLASS_INLINE */
+#if defined(__KERNEL__)
+#include <linux/kernel.h> /* Override the definition of max/min from linux kernel*/
+#endif /*__KERNEL__*/
+
+#if defined(_MSC_VER)
+#include <stdlib.h> /* Override the definition of max/min from stdlib.h*/
+#endif /* _MSC_VER */
+
+/* in case we have min/max/MIN/MAX macro's undefine them */
+#ifdef min
+#undef min
+#endif
+#ifdef max
+#undef max
+#endif
+#ifdef MIN /* also defined in include/hrt/numeric.h from SDK */
+#undef MIN
+#endif
+#ifdef MAX
+#undef MAX
+#endif
+#ifdef ABS
+#undef ABS
+#endif
+
+#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)
+
+#ifdef ISP2401
+/* If the number is odd, find the next even number */
+#define EVEN_CEIL(x)         ((IS_ODD(x)) ? ((x) + 1) : (x))
+
+#endif
+/* A => B */
+#define IMPLIES(a, b)        (!(a) || (b))
+
+#define ABS(a)               ((a) >= 0 ? (a) : -(a))
+
+/* 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))
+#ifdef ISP2401
+#define ROUND_DIV(a, b)      ((b) ? ((a) + ((b) >> 1)) / (b) : 0)
+#endif
+#define CEIL_DIV(a, b)       ((b) ? ((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))
+#ifdef ISP2401
+#define ROUND_HALF_DOWN_DIV(a, b)	((b) ? ((a) + (b / 2) - 1) / (b) : 0)
+#define ROUND_HALF_DOWN_MUL(a, b)	(ROUND_HALF_DOWN_DIV(a, b) * (b))
+#endif
+
+
+/*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)
+
+#ifndef INLINE_MATH_SUPPORT_UTILS
+/*
+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 max(a, b)		(MAX(a, b))
+#define min(a, b)		(MIN(a, b))
+#define ceil_div(a, b)		(CEIL_DIV(a, b))
+
+#else /* !defined(INLINE_MATH_SUPPORT_UTILS) */
+
+STORAGE_CLASS_INLINE int max(int a, int b)
+{
+	return MAX(a, b);
+}
+
+STORAGE_CLASS_INLINE int min(int a, int b)
+{
+	return MIN(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int ceil_div(unsigned int a, unsigned int b)
+{
+	return CEIL_DIV(a, b);
+}
+#endif /* !defined(INLINE_MATH_SUPPORT_UTILS) */
+
+STORAGE_CLASS_INLINE unsigned int umax(unsigned int a, unsigned int b)
+{
+	return MAX(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int umin(unsigned int a, unsigned int b)
+{
+	return MIN(a, b);
+}
+
+
+STORAGE_CLASS_INLINE unsigned int ceil_mul(unsigned int a, unsigned int b)
+{
+	return CEIL_MUL(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int ceil_mul2(unsigned int a, unsigned int b)
+{
+	return CEIL_MUL2(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int ceil_shift(unsigned int a, unsigned int b)
+{
+	return CEIL_SHIFT(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int ceil_shift_mul(unsigned int a, unsigned int b)
+{
+	return CEIL_SHIFT_MUL(a, b);
+}
+
+#ifdef ISP2401
+STORAGE_CLASS_INLINE unsigned int round_half_down_div(unsigned int a, unsigned int b)
+{
+	return ROUND_HALF_DOWN_DIV(a, b);
+}
+
+STORAGE_CLASS_INLINE unsigned int round_half_down_mul(unsigned int a, unsigned int b)
+{
+	return ROUND_HALF_DOWN_MUL(a, b);
+}
+#endif
+
+/** @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))
+ *
+ */
+
+STORAGE_CLASS_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) */
+
+#if !defined(__ISP)
+/*
+ * 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 /* !defined(__ISP) */
+
+#if !defined(__KERNEL__)
+#define clamp(a, min_val, max_val) MIN(MAX((a), (min_val)), (max_val))
+#endif /* !defined(__KERNEL__) */
+
+#endif /* __MATH_SUPPORT_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_access/memory_access.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_access/memory_access.h
new file mode 100644
index 000000000000..195c4a5bceeb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_access/memory_access.h
@@ -0,0 +1,174 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015-2017, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __MEMORY_ACCESS_H_INCLUDED__
+#define __MEMORY_ACCESS_H_INCLUDED__
+
+/*!
+ * \brief
+ * Define the public interface for virtual memory
+ * access functions. Access types are limited to
+ * those defined in <stdint.h>
+ *
+ * The address representation is private to the system
+ * and represented as "hrt_vaddress" rather than a
+ * pointer, as the memory allocation cannot be accessed
+ * by dereferencing but reaquires load and store access
+ * functions
+ *
+ * The page table selection or virtual memory context;
+ * The page table base index; Is implicit. This page
+ * table base index must be set by the implementation
+ * of the access function
+ *
+ * "store" is a transfer to the system
+ * "load" is a transfer from the system
+ *
+ * Allocation properties can be specified by setting
+ * attributes (see below) in case of multiple physical
+ * memories the memory ID is encoded on the attribute
+ *
+ * Allocations in the same physical memory, but in a
+ * different (set of) page tables can be shared through
+ * a page table information mapping function
+ */
+
+#include <type_support.h>
+#include "platform_support.h"	/* for __func__ */
+
+/*
+ * User provided file that defines the (sub)system address types:
+ *	- hrt_vaddress	a type that can hold the (sub)system virtual address range
+ */
+#include "system_types.h"
+
+/*
+ * The MMU base address is a physical address, thus the same type is used
+ * as for the device base address
+ */
+#include "device_access.h"
+
+#include "hmm/hmm.h"
+
+/*!
+ * \brief
+ * Bit masks for specialised allocation functions
+ * the default is "uncached", "not contiguous",
+ * "not page aligned" and "not cleared"
+ *
+ * Forcing alignment (usually) returns a pointer
+ * at an alignment boundary that is offset from
+ * the allocated pointer. Without storing this
+ * pointer/offset, we cannot free it. The memory
+ * manager is responsible for the bookkeeping, e.g.
+ * the allocation function creates a sentinel
+ * within the allocation referencable from the
+ * returned pointer/address.
+ */
+#define MMGR_ATTRIBUTE_MASK		0x000f
+#define MMGR_ATTRIBUTE_CACHED		0x0001
+#define MMGR_ATTRIBUTE_CONTIGUOUS	0x0002
+#define MMGR_ATTRIBUTE_PAGEALIGN	0x0004
+#define MMGR_ATTRIBUTE_CLEARED		0x0008
+#define MMGR_ATTRIBUTE_UNUSED		0xfff0
+
+/* #define MMGR_ATTRIBUTE_DEFAULT	(MMGR_ATTRIBUTE_CACHED) */
+#define MMGR_ATTRIBUTE_DEFAULT	0
+
+extern const hrt_vaddress	mmgr_NULL;
+extern const hrt_vaddress	mmgr_EXCEPTION;
+
+/*! Return the address of an allocation in memory
+
+ \param	size[in]		Size in bytes of the allocation
+ \param	caller_func[in]		Caller function name
+ \param	caller_line[in]		Caller function line number
+
+ \return vaddress
+ */
+extern hrt_vaddress mmgr_malloc(const size_t size);
+
+/*! Return the address of a zero initialised allocation in memory
+
+ \param	N[in]			Horizontal dimension of array
+ \param	size[in]		Vertical dimension of array  Total size is N*size
+
+ \return vaddress
+ */
+extern hrt_vaddress mmgr_calloc(const size_t N, const size_t size);
+
+/*! Return the address of an allocation in memory
+
+ \param	size[in]		Size in bytes of the allocation
+ \param	attribute[in]		Bit vector specifying the properties
+				of the allocation including zero initialisation
+
+ \return vaddress
+ */
+
+extern hrt_vaddress mmgr_alloc_attr(const size_t size, const uint16_t attribute);
+
+/*! Return the address of a mapped existing allocation in memory
+
+ \param	ptr[in]			Pointer to an allocation in a different
+				virtual memory page table, but the same
+				physical memory
+ \param size[in]		Size of the memory of the pointer
+ \param	attribute[in]		Bit vector specifying the properties
+				of the allocation
+ \param context			Pointer of a context provided by
+				client/driver for additonal parameters
+				needed by the implementation
+ \Note
+	This interface is tentative, limited to the desired function
+	the actual interface may require furhter parameters
+
+ \return vaddress
+ */
+extern hrt_vaddress mmgr_mmap(
+	const void *ptr,
+	const size_t size,
+	uint16_t attribute,
+	void *context);
+
+/*! Zero initialise an allocation in memory
+
+ \param	vaddr[in]		Address of an allocation
+ \param	size[in]		Size in bytes of the area to be cleared
+
+ \return none
+ */
+extern void mmgr_clear(hrt_vaddress vaddr, const size_t	size);
+
+/*! Read an array of bytes from a virtual memory address
+
+ \param	vaddr[in]		Address of an allocation
+ \param	data[out]		pointer to the destination array
+ \param	size[in]		number of bytes to read
+
+ \return none
+ */
+extern void mmgr_load(const hrt_vaddress vaddr, void *data, const size_t size);
+
+/*! Write an array of bytes to device registers or memory in the device
+
+ \param	vaddr[in]		Address of an allocation
+ \param	data[in]		pointer to the source array
+ \param	size[in]		number of bytes to write
+
+ \return none
+ */
+extern void mmgr_store(const hrt_vaddress vaddr, const void *data, const size_t size);
+
+#endif /* __MEMORY_ACCESS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_realloc.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_realloc.h
new file mode 100644
index 000000000000..f3b7273fed1b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/memory_realloc.h
@@ -0,0 +1,38 @@
+/**
+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 __MEMORY_REALLOC_H_INCLUDED__
+#define __MEMORY_REALLOC_H_INCLUDED__
+
+/*!
+ * \brief
+ * Define the internal reallocation of private css memory
+ *
+ */
+
+#include <type_support.h>
+/*
+ * User provided file that defines the (sub)system address types:
+ *	- hrt_vaddress	a type that can hold the (sub)system virtual address range
+ */
+#include "system_types.h"
+#include "ia_css_err.h"
+
+bool reallocate_buffer(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err);
+
+#endif /*__MEMORY_REALLOC_H_INCLUDED__*/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/misc_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/misc_support.h
new file mode 100644
index 000000000000..38db1ecef3c8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/misc_support.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_include/mmu_device.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/mmu_device.h
new file mode 100644
index 000000000000..1b2017b029f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/mmu_device.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "mmu_local.h"
+
+#ifndef __INLINE_MMU__
+#define STORAGE_CLASS_MMU_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_MMU_C 
+#include "mmu_public.h"
+#else  /* __INLINE_MMU__ */
+#define STORAGE_CLASS_MMU_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_MMU_C STORAGE_CLASS_INLINE
+#include "mmu_private.h"
+#endif /* __INLINE_MMU__ */
+
+#endif /* __MMU_DEVICE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/mpmath.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/mpmath.h
new file mode 100644
index 000000000000..565983aafa4d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/mpmath.h
@@ -0,0 +1,330 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __MPMATH_H_INCLUDED__
+#define __MPMATH_H_INCLUDED__
+
+#include "storage_class.h"
+
+#ifdef INLINE_MPMATH
+#define STORAGE_CLASS_MPMATH_FUNC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_MPMATH_DATA_H STORAGE_CLASS_INLINE_DATA
+#else /* INLINE_MPMATH */
+#define STORAGE_CLASS_MPMATH_FUNC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_MPMATH_DATA_H STORAGE_CLASS_EXTERN_DATA
+#endif  /* INLINE_MPMATH */
+
+#include <type_support.h>
+
+/*
+ * Implementation limits
+ */
+#define MIN_BITDEPTH            1
+#define MAX_BITDEPTH            64
+
+#define ROUND_NEAREST_EVEN  0
+#define ROUND_NEAREST       1
+
+/*
+ * The MP types
+ *
+ * "vector lane data" is scalar. With "scalar data" for limited range shift and address values
+ */
+typedef unsigned long long      mpudata_t;   /* Type of reference MP scalar / vector lane data; unsigned */
+typedef long long               mpsdata_t;   /* Type of reference MP scalar / vector lane data; signed */
+typedef unsigned short          spudata_t;   /* Type of reference SP scalar / vector lane data; unsigned */
+typedef short                   spsdata_t;   /* Type of reference SP scalar / vector lane data; signed */
+typedef unsigned short          bitdepth_t;
+
+typedef enum {
+    mp_zero_ID,
+    mp_one_ID,
+    mp_mone_ID,
+    mp_smin_ID,
+    mp_smax_ID,
+    mp_umin_ID,
+    mp_umax_ID,
+    N_mp_const_ID
+} mp_const_ID_t;
+
+#ifdef ISP2401
+/* _isValidMpudata is for internal use by mpmath and bbb's.
+ * isValidMpudata is for external use by functions on top.
+ */
+#ifndef ENABLE_VALID_MP_DATA_CHECK
+#define _isValidMpsdata(data,bitdepth) (1)
+#define _isValidMpudata(data,bitdepth) (1)
+#else
+#define _isValidMpsdata(data,bitdepth) isValidMpsdata(data,bitdepth)
+#define _isValidMpudata(data,bitdepth) isValidMpsdata(data,bitdepth)
+
+#endif
+#endif
+STORAGE_CLASS_MPMATH_FUNC_H bool isValidMpsdata(
+    const mpsdata_t             data,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H bool isValidMpudata(
+    const mpudata_t             data,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_castd (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_casth (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_scasth (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qcastd (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qcasth (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qrcasth (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_abs (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_limit (
+    const mpsdata_t             bnd_low,
+    const mpsdata_t             in0,
+    const mpsdata_t             bnd_high,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_max (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_min (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_mux (
+    const spudata_t             sel,
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_rmux (
+    const spudata_t             sel,
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_add (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_sadd (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_sub (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_ssub (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_addasr1 (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_subasr1 (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_lsr (
+    const mpsdata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_asr (
+    const mpsdata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_rasr (
+    const mpsdata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+/* "mp_rasr_u()" is implemented by "mp_rasr()" */
+STORAGE_CLASS_MPMATH_FUNC_H mpudata_t mp_rasr_u (
+    const mpudata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_lsl (
+    const mpsdata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_asl (
+    const mpsdata_t             in0,
+    const spsdata_t             shft,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_muld (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_mul (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qmul (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qrmul (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qdiv (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_qdivh (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_div (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_divh (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_and (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_compl (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_or (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_xor (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isEQ (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isNE (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isGT (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isGE (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isLT (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isLE (
+    const mpsdata_t             in0,
+    const mpsdata_t             in1,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isEQZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isNEZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isGTZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isGEZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isLTZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H spudata_t mp_isLEZ (
+    const mpsdata_t             in0,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpsdata_t mp_const (
+    const mp_const_ID_t         ID,
+    const bitdepth_t            bitdepth);
+
+STORAGE_CLASS_MPMATH_FUNC_H mpudata_t mp_sqrt_u(
+	const mpudata_t     in0,
+	const bitdepth_t    bitdepth);
+
+#ifndef INLINE_MPMATH
+#define STORAGE_CLASS_MPMATH_FUNC_C 
+#define STORAGE_CLASS_MPMATH_DATA_C const
+#else /* INLINE_MPMATH */
+#define STORAGE_CLASS_MPMATH_FUNC_C STORAGE_CLASS_MPMATH_FUNC_H
+#define STORAGE_CLASS_MPMATH_DATA_C STORAGE_CLASS_MPMATH_DATA_H
+#include "mpmath.c"
+#define MPMATH_INLINED
+#endif  /* INLINE_MPMATH */
+
+#endif /* __MPMATH_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/osys.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/osys.h
new file mode 100644
index 000000000000..6e48ea9afc29
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/osys.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __OSYS_H_INCLUDED__
+#define __OSYS_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the OSYS 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 "storage_class.h"
+
+#include "system_local.h"
+#include "osys_local.h"
+
+#ifndef __INLINE_OSYS__
+#define STORAGE_CLASS_OSYS_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_OSYS_C
+#include "osys_public.h"
+#else  /* __INLINE_OSYS__ */
+#define STORAGE_CLASS_OSYS_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_OSYS_C STORAGE_CLASS_INLINE
+#include "osys_private.h"
+#endif /* __INLINE_OSYS__ */
+
+#endif /* __OSYS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/pixelgen.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/pixelgen.h
new file mode 100644
index 000000000000..67f7f3a14231
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/pixelgen.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "pixelgen_local.h"
+
+#ifndef __INLINE_PIXELGEN__
+#define STORAGE_CLASS_PIXELGEN_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_PIXELGEN_C
+#include "pixelgen_public.h"
+#else  /* __INLINE_PIXELGEN__ */
+#define STORAGE_CLASS_PIXELGEN_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_PIXELGEN_C STORAGE_CLASS_INLINE
+#include "pixelgen_private.h"
+#endif /* __INLINE_PIXELGEN__ */
+
+#endif /* __PIXELGEN_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/platform_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/platform_support.h
new file mode 100644
index 000000000000..02f9eee67ff3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/platform_support.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+/* For definition of hrt_sleep() */
+#include "hive_isp_css_custom_host_hrt.h"
+
+#define UINT16_MAX USHRT_MAX
+#define UINT32_MAX UINT_MAX
+#define UCHAR_MAX  (255)
+
+#define CSS_ALIGN(d, a) d __attribute__((aligned(a)))
+
+/*
+ * Put here everything __KERNEL__ specific not covered in
+ * "assert_support.h", "math_support.h", etc
+ */
+
+#endif /* __PLATFORM_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/print_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/print_support.h
new file mode 100644
index 000000000000..cfbc222ea0c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/print_support.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include <stdarg.h>
+#if !defined(__KERNEL__)
+#include <stdio.h>
+#endif
+
+extern int (*sh_css_printf) (const char *fmt, va_list args);
+/* depends on host supplied print function in ia_css_init() */
+STORAGE_CLASS_INLINE 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/atomisp2/css2400/hive_isp_css_include/queue.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/queue.h
new file mode 100644
index 000000000000..a3d874b9516a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/queue.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <storage_class.h>
+
+#include "queue_local.h"
+
+#ifndef __INLINE_QUEUE__
+#define STORAGE_CLASS_QUEUE_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_QUEUE_C 
+/* #include "queue_public.h" */
+#include "ia_css_queue.h"
+#else  /* __INLINE_QUEUE__ */
+#define STORAGE_CLASS_QUEUE_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_QUEUE_C STORAGE_CLASS_INLINE
+#include "queue_private.h"
+#endif /* __INLINE_QUEUE__ */
+
+#endif /* __QUEUE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/resource.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/resource.h
new file mode 100644
index 000000000000..82c55acd0380
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/resource.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "resource_local.h"
+
+#ifndef __INLINE_RESOURCE__
+#define STORAGE_CLASS_RESOURCE_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_RESOURCE_C 
+#include "resource_public.h"
+#else  /* __INLINE_RESOURCE__ */
+#define STORAGE_CLASS_RESOURCE_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_RESOURCE_C STORAGE_CLASS_INLINE
+#include "resource_private.h"
+#endif /* __INLINE_RESOURCE__ */
+
+#endif /* __RESOURCE_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/socket.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/socket.h
new file mode 100644
index 000000000000..c34c2e75c51f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/socket.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __SOCKET_H_INCLUDED__
+#define __SOCKET_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 "storage_class.h"
+
+#include "system_local.h"
+#include "socket_local.h"
+
+#ifndef __INLINE_SOCKET__
+#define STORAGE_CLASS_SOCKET_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_SOCKET_C
+#include "socket_public.h"
+#else  /* __INLINE_SOCKET__ */
+#define STORAGE_CLASS_SOCKET_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_SOCKET_C STORAGE_CLASS_INLINE
+#include "socket_private.h"
+#endif /* __INLINE_SOCKET__ */
+
+#endif /* __SOCKET_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/sp.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/sp.h
new file mode 100644
index 000000000000..150fc2f6129b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/sp.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "sp_local.h"
+
+#ifndef __INLINE_SP__
+#define STORAGE_CLASS_SP_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_SP_C 
+#include "sp_public.h"
+#else  /* __INLINE_SP__ */
+#define STORAGE_CLASS_SP_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_SP_C STORAGE_CLASS_INLINE
+#include "sp_private.h"
+#endif /* __INLINE_SP__ */
+
+#endif /* __SP_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/storage_class.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/storage_class.h
new file mode 100644
index 000000000000..3908e668dacd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/storage_class.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __STORAGE_CLASS_H_INCLUDED__
+#define __STORAGE_CLASS_H_INCLUDED__
+
+/**
+* @file
+* Platform specific includes and functionality.
+*/
+
+#define STORAGE_CLASS_EXTERN extern
+
+#if defined(_MSC_VER)
+#define STORAGE_CLASS_INLINE static __inline
+#else
+#define STORAGE_CLASS_INLINE static inline
+#endif
+
+#define STORAGE_CLASS_EXTERN_DATA extern const
+#define STORAGE_CLASS_INLINE_DATA static const
+
+#endif /* __STORAGE_CLASS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/stream_buffer.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/stream_buffer.h
new file mode 100644
index 000000000000..8e41f60b5d39
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/stream_buffer.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __STREAM_BUFFER_H_INCLUDED__
+#define __STREAM_BUFFER_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 "storage_class.h"
+
+#include "system_local.h"
+#include "stream_buffer_local.h"
+
+#ifndef __INLINE_STREAM_BUFFER__
+#define STORAGE_CLASS_STREAM_BUFFER_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_STREAM_BUFFER_C
+#include "stream_buffer_public.h"
+#else  /* __INLINE_STREAM_BUFFER__ */
+#define STORAGE_CLASS_STREAM_BUFFER_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_STREAM_BUFFER_C STORAGE_CLASS_INLINE
+#include "stream_buffer_private.h"
+#endif /* __INLINE_STREAM_BUFFER__ */
+
+#endif /* __STREAM_BUFFER_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h
new file mode 100644
index 000000000000..568631698a3d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/string_support.h
@@ -0,0 +1,167 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __STRING_SUPPORT_H_INCLUDED__
+#define __STRING_SUPPORT_H_INCLUDED__
+#include <platform_support.h>
+#include <type_support.h>
+#include <storage_class.h>
+
+#if !defined(_MSC_VER)
+/*
+ * For all non microsoft cases, we need the following functions
+ */
+
+
+/** @brief Copy from src_buf to dest_buf.
+ *
+ * @param[out] dest_buf. Destination buffer to copy to
+ * @param[in]  dest_size. The size of the destination buffer in bytes
+ * @param[in]  src_buf. The source buffer
+ * @param[in]  src_size. The size of the source buffer in bytes
+ * @return     0 on success, error code on failure
+ * @return     EINVAL on Invalid arguments
+ * @return     ERANGE on Destination size too small
+ */
+STORAGE_CLASS_INLINE int memcpy_s(
+	void* dest_buf,
+	size_t dest_size,
+	const void* src_buf,
+	size_t src_size)
+{
+	if ((src_buf == NULL) || (dest_buf == NULL)) {
+		/* Invalid arguments*/
+		return EINVAL;
+	}
+
+	if ((dest_size < src_size) || (src_size == 0)) {
+		/* Destination too small*/
+		return ERANGE;
+	}
+
+	memcpy(dest_buf, src_buf, src_size);
+	return 0;
+}
+
+/** @brief Get the length of the string, excluding the null terminator
+ *
+ * @param[in]  src_str. The source string
+ * @param[in]  max_len. Look only for max_len bytes in the string
+ * @return     Return the string length excluding null character
+ * @return     Return max_len if no null character in the first max_len bytes
+ * @return     Returns 0 if src_str is NULL
+ */
+static size_t strnlen_s(
+	const char* src_str,
+	size_t max_len)
+{
+	size_t ix;
+	if (src_str == NULL) {
+		/* Invalid arguments*/
+		return 0;
+	}
+
+	for (ix=0;
+		((src_str[ix] != '\0') && (ix< max_len));
+		++ix) /*Nothing else to do*/;
+
+	/* On Error, it will return src_size == max_len*/
+	return ix;
+}
+
+/** @brief Copy string from src_str to dest_str
+ *
+ * @param[out] dest_str. Destination buffer to copy to
+ * @param[in]  dest_size. The size of the destination buffer in bytes
+ * @param[in]  src_str. The source buffer
+ * @param[in]  src_size. The size of the source buffer in bytes
+ * @return     Returns 0 on success
+ * @return     Returns EINVAL on invalid arguments
+ * @return     Returns ERANGE on destination size too small
+ */
+STORAGE_CLASS_INLINE int strncpy_s(
+	char* dest_str,
+	size_t dest_size,
+	const char* src_str,
+	size_t src_size)
+{
+	size_t len;
+	if (dest_str == NULL) {
+		/* Invalid arguments*/
+		return EINVAL;
+	}
+
+	if ((src_str == NULL) || (dest_size == 0)) {
+		/* Invalid arguments*/
+		dest_str[0] = '\0';
+		return EINVAL;
+	}
+
+	len = strnlen_s(src_str, src_size);
+
+	if (len >= dest_size) {
+		/* Destination too small*/
+		dest_str[0] = '\0';
+		return ERANGE;
+	}
+
+	/* dest_str is big enough for the len */
+	strncpy(dest_str, src_str, len);
+	dest_str[len+1] = '\0';
+	return 0;
+}
+
+/** @brief Copy string from src_str to dest_str
+ *
+ * @param[out] dest_str. Destination buffer to copy to
+ * @param[in]  dest_size. The size of the destination buffer in bytes
+ * @param[in]  src_str. The source buffer
+ * @return     Returns 0 on success
+ * @return     Returns EINVAL on invalid arguments
+ * @return     Returns ERANGE on destination size too small
+ */
+STORAGE_CLASS_INLINE int strcpy_s(
+	char* dest_str,
+	size_t dest_size,
+	const char* src_str)
+{
+	size_t len;
+	if (dest_str == NULL) {
+		/* Invalid arguments*/
+		return EINVAL;
+	}
+
+	if ((src_str == NULL) || (dest_size == 0)) {
+		/* Invalid arguments*/
+		dest_str[0] = '\0';
+		return EINVAL;
+	}
+
+	len = strnlen_s(src_str, dest_size);
+
+	if (len >= dest_size) {
+		/* Destination too small*/
+		dest_str[0] = '\0';
+		return ERANGE;
+	}
+
+	/* dest_str is big enough for the len */
+	strncpy(dest_str, src_str, len);
+	dest_str[len+1] = '\0';
+	return 0;
+}
+
+#endif /*!defined(_MSC_VER)*/
+
+#endif /* __STRING_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/system_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/system_types.h
new file mode 100644
index 000000000000..a8c19cee17da
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/system_types.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_TYPES_H_INCLUDED__
+#define __SYSTEM_TYPES_H_INCLUDED__
+
+/**
+* @file
+* Platform specific types.
+*/
+
+
+#include "system_local.h"
+
+#endif /* __SYSTEM_TYPES_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/tag.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/tag.h
new file mode 100644
index 000000000000..7385fd11c95f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/tag.h
@@ -0,0 +1,46 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "tag_local.h"
+
+#ifndef __INLINE_TAG__
+#define STORAGE_CLASS_TAG_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_TAG_C 
+#include "tag_public.h"
+#else  /* __INLINE_TAG__ */
+#define STORAGE_CLASS_TAG_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_TAG_C STORAGE_CLASS_INLINE
+#include "tag_private.h"
+#endif /* __INLINE_TAG__ */
+
+#endif /* __TAG_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/timed_ctrl.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/timed_ctrl.h
new file mode 100644
index 000000000000..ed13451c9261
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/timed_ctrl.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "timed_ctrl_local.h"
+
+#ifndef __INLINE_TIMED_CTRL__
+#define STORAGE_CLASS_TIMED_CTRL_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_TIMED_CTRL_C 
+#include "timed_ctrl_public.h"
+#else  /* __INLINE_TIMED_CTRL__ */
+#define STORAGE_CLASS_TIMED_CTRL_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_TIMED_CTRL_C STORAGE_CLASS_INLINE
+#include "timed_ctrl_private.h"
+#endif /* __INLINE_TIMED_CTRL__ */
+
+#endif /* __TIMED_CTRL_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/type_support.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/type_support.h
new file mode 100644
index 000000000000..b82fa3eba79f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/type_support.h
@@ -0,0 +1,82 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+
+#if defined(_MSC_VER)
+#include <stdint.h>
+/* For ATE compilation define the bool */
+#if defined(_ATE_)
+#define bool int
+#define true 1
+#define false 0
+#else
+#include <stdbool.h>
+#endif
+#include <stddef.h>
+#include <limits.h>
+#include <errno.h>
+#if defined(_M_X64)
+#define HOST_ADDRESS(x) (unsigned long long)(x)
+#else
+#define HOST_ADDRESS(x) (unsigned long)(x)
+#endif
+
+#elif defined(__KERNEL__)
+
+#define CHAR_BIT (8)
+
+#include <linux/types.h>
+#include <linux/limits.h>
+#include <linux/errno.h>
+#define HOST_ADDRESS(x) (unsigned long)(x)
+
+#elif defined(__GNUC__)
+#ifndef __STDC_LIMIT_MACROS
+#define __STDC_LIMIT_MACROS 1
+#endif
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <limits.h>
+#include <errno.h>
+#define HOST_ADDRESS(x) (unsigned long)(x)
+
+#else /* default is for the FIST environment */
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <limits.h>
+#include <errno.h>
+#define HOST_ADDRESS(x) (unsigned long)(x)
+
+#endif
+
+#endif /* __TYPE_SUPPORT_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vamem.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vamem.h
new file mode 100644
index 000000000000..acf932e1f563
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vamem.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "vamem_local.h"
+
+#ifndef __INLINE_VAMEM__
+#define STORAGE_CLASS_VAMEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_VAMEM_C 
+#include "vamem_public.h"
+#else  /* __INLINE_VAMEM__ */
+#define STORAGE_CLASS_VAMEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_VAMEM_C STORAGE_CLASS_INLINE
+#include "vamem_private.h"
+#endif /* __INLINE_VAMEM__ */
+
+#endif /* __VAMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_func.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_func.h
new file mode 100644
index 000000000000..5d3be31759e4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_func.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __VECTOR_FUNC_H_INCLUDED__
+#define __VECTOR_FUNC_H_INCLUDED__
+
+#include "storage_class.h"
+
+/* TODO: Later filters will be moved to types directory,
+ * and we should only include matrix_MxN types */
+#include "filters/filters_1.0/filter_2x2.h"
+#include "filters/filters_1.0/filter_3x3.h"
+#include "filters/filters_1.0/filter_4x4.h"
+#include "filters/filters_1.0/filter_5x5.h"
+
+#include "vector_func_local.h"
+
+#ifndef __INLINE_VECTOR_FUNC__
+#define STORAGE_CLASS_VECTOR_FUNC_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_VECTOR_FUNC_C 
+#include "vector_func_public.h"
+#else  /* __INLINE_VECTOR_FUNC__ */
+#define STORAGE_CLASS_VECTOR_FUNC_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_VECTOR_FUNC_C STORAGE_CLASS_INLINE
+#include "vector_func_private.h"
+#endif /* __INLINE_VECTOR_FUNC__ */
+
+#endif /* __VECTOR_FUNC_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_ops.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_ops.h
new file mode 100644
index 000000000000..261f87378ce5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vector_ops.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __VECTOR_OPS_H_INCLUDED__
+#define __VECTOR_OPS_H_INCLUDED__
+
+#include "storage_class.h"
+
+#include "vector_ops_local.h"
+
+#ifndef __INLINE_VECTOR_OPS__
+#define STORAGE_CLASS_VECTOR_OPS_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_VECTOR_OPS_C
+#include "vector_ops_public.h"
+#else  /* __INLINE_VECTOR_OPS__ */
+#define STORAGE_CLASS_VECTOR_OPS_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_VECTOR_OPS_C STORAGE_CLASS_INLINE
+#include "vector_ops_private.h"
+#endif /* __INLINE_VECTOR_OPS__ */
+
+#endif /* __VECTOR_OPS_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vmem.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vmem.h
new file mode 100644
index 000000000000..79a36755bfd9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/vmem.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "storage_class.h"
+
+#include "system_local.h"
+#include "vmem_local.h"
+
+#ifndef __INLINE_VMEM__
+#define STORAGE_CLASS_VMEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_VMEM_C 
+#include "vmem_public.h"
+#else  /* __INLINE_VMEM__ */
+#define STORAGE_CLASS_VMEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_VMEM_C STORAGE_CLASS_INLINE
+#include "vmem_private.h"
+#endif /* __INLINE_VMEM__ */
+
+#endif /* __VMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/xmem.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/xmem.h
new file mode 100644
index 000000000000..9169e04f9b4b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_include/xmem.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __XMEM_H_INCLUDED__
+#define __XMEM_H_INCLUDED__
+
+/*
+ * This file is included on every cell {SP,ISP,host} and on every system
+ * that uses the XMEM 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 "storage_class.h"
+
+#include "system_local.h"
+#include "xmem_local.h"
+
+#ifndef __INLINE_XMEM__
+#define STORAGE_CLASS_XMEM_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_XMEM_C 
+#include "xmem_public.h"
+#else  /* __INLINE_XMEM__ */
+#define STORAGE_CLASS_XMEM_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_XMEM_C STORAGE_CLASS_INLINE
+#include "xmem_private.h"
+#endif /* __INLINE_XMEM__ */
+
+#endif /* __XMEM_H_INCLUDED__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/queue_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/queue_local.h
new file mode 100644
index 000000000000..9f4060319b4b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/queue_local.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/host/queue_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/queue_private.h
new file mode 100644
index 000000000000..2b396955cdad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/queue_private.h
@@ -0,0 +1,18 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/host/tag.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag.c
new file mode 100644
index 000000000000..9aa8c168a803
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag.c
@@ -0,0 +1,95 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL);
+
+	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 != NULL);
+
+	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/atomisp2/css2400/hive_isp_css_shared/host/tag_local.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag_local.h
new file mode 100644
index 000000000000..01a8977c189e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag_local.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/host/tag_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag_private.h
new file mode 100644
index 000000000000..0570a95ec5bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/host/tag_private.h
@@ -0,0 +1,18 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/queue_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/queue_global.h
new file mode 100644
index 000000000000..61330daab734
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/queue_global.h
@@ -0,0 +1,19 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/socket_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/socket_global.h
new file mode 100644
index 000000000000..2b7025e90250
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/socket_global.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __SOCKET_GLOBAL_H_INCLUDED__
+#define __SOCKET_GLOBAL_H_INCLUDED__
+
+#include "stream_buffer.h"
+
+/* define the socket port direction */
+typedef enum {
+	SOCKET_PORT_DIRECTION_NULL,
+	SOCKET_PORT_DIRECTION_IN,
+	SOCKET_PORT_DIRECTION_OUT
+} socket_port_direction_t;
+
+/* pointer to the port's callout function */
+typedef void (*socket_port_callout_fp)(void);
+typedef struct socket_port_s socket_port_t;
+typedef struct socket_s socket_t;
+
+/* data structure of the socket port */
+struct socket_port_s {
+	unsigned				channel;	/* the port entity */
+	socket_port_direction_t direction;	/* the port direction */
+	socket_port_callout_fp	callout;	/* the port callout function */
+
+	socket_t				*socket;	/* point to the socket */
+
+	struct {
+		unsigned data;
+	} buf;								/* the buffer at the port */
+};
+
+/* data structure of the socket */
+struct socket_s {
+	socket_port_t	*in;	/* the in-direction port */
+	socket_port_t	*out;	/* the out-direction port */
+	stream_buffer_t	buf;	/* the buffer between in-ports and out-ports */
+};
+
+#endif /* __SOCKET_GLOBAL_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/stream_buffer_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/stream_buffer_global.h
new file mode 100644
index 000000000000..b9664b9608dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/stream_buffer_global.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#ifndef __STREAM_BUFFER_GLOBAL_H_INCLUDED__
+#define __STREAM_BUFFER_GLOBAL_H_INCLUDED__
+
+typedef struct stream_buffer_s stream_buffer_t;
+struct stream_buffer_s {
+	unsigned	base;
+	unsigned	limit;
+	unsigned	top;
+};
+
+#endif /* __STREAM_BUFFER_GLOBAL_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/sw_event_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/sw_event_global.h
new file mode 100644
index 000000000000..c0d2efadbbe3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/sw_event_global.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/hive_isp_css_shared/tag_global.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/tag_global.h
new file mode 100644
index 000000000000..fda457792c9c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_shared/tag_global.h
@@ -0,0 +1,56 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css.h
new file mode 100644
index 000000000000..2458b3767c90
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css.h
@@ -0,0 +1,57 @@
+/* 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/atomisp2/css2400/ia_css_3a.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_3a.h
new file mode 100644
index 000000000000..a80a7dbaf712
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_3a.h
@@ -0,0 +1,188 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+	uint32_t exp_id;     /**< exposure id, to match statistics to a frame,
+			          see ia_css_event_public.h for more detail. */
+	uint32_t 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 */
+	uint32_t   size;     /**< total size of all data */
+	uint32_t   dmem_size;
+	uint32_t   vmem_size; /**< both lo and hi have this size */
+	uint32_t   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;
+	uint16_t                *vmem_stats_hi;
+	uint16_t                *vmem_stats_lo;
+	struct ia_css_bh_table  *hmem_stats;
+	uint32_t                 size; /**< total size in bytes of data_ptr */
+	uint32_t                 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.
+ */
+enum ia_css_err
+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/atomisp2/css2400/ia_css_acc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_acc_types.h
new file mode 100644
index 000000000000..a2a1873aca83
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_acc_types.h
@@ -0,0 +1,468 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.h>	/* HAS_IRQ_MAP_VERSION_# */
+#include <type_support.h>
+#include <platform_support.h>
+#include <debug_global.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 */
+	uint32_t offset;		/**< Blob offset in fw file */
+	struct ia_css_isp_param_memory_offsets memory_offsets;  /**< offset wrt hdr in bytes */
+	uint32_t prog_name_offset;  /**< offset wrt hdr in bytes */
+	uint32_t size;			/**< Size of blob */
+	uint32_t padding_size;	/**< total cummulative of bytes added due to section alignment */
+	uint32_t icache_source;	/**< Position of icache in blob */
+	uint32_t icache_size;	/**< Size of icache section */
+	uint32_t icache_padding;/**< bytes added due to icache section alignment */
+	uint32_t text_source;	/**< Position of text in blob */
+	uint32_t text_size;		/**< Size of text section */
+	uint32_t text_padding;	/**< bytes added due to text section alignment */
+	uint32_t data_source;	/**< Position of data in blob */
+	uint32_t data_target;	/**< Start of data in SP dmem */
+	uint32_t data_size;		/**< Size of text section */
+	uint32_t data_padding;	/**< bytes added due to data section alignment */
+	uint32_t bss_target;	/**< Start position of bss in SP dmem */
+	uint32_t 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 {
+	uint32_t		min_width;
+	uint32_t		min_height;
+	uint32_t		max_width;
+	uint32_t		max_height;
+	uint32_t		source; /* memory, sensor, variable */
+};
+
+struct ia_css_binary_output_info {
+	uint32_t		min_width;
+	uint32_t		min_height;
+	uint32_t		max_width;
+	uint32_t		max_height;
+	uint32_t		num_chunks;
+	uint32_t		variable_format;
+};
+
+struct ia_css_binary_internal_info {
+	uint32_t		max_width;
+	uint32_t		max_height;
+};
+
+struct ia_css_binary_bds_info {
+	uint32_t		supported_bds_factors;
+};
+
+struct ia_css_binary_dvs_info {
+	uint32_t		max_envelope_width;
+	uint32_t		max_envelope_height;
+};
+
+struct ia_css_binary_vf_dec_info {
+	uint32_t		is_variable;
+	uint32_t		max_log_downscale;
+};
+
+struct ia_css_binary_s3a_info {
+	uint32_t		s3atbl_use_dmem;
+	uint32_t		fixed_s3a_deci_log;
+};
+
+/** DPC related binary info */
+struct ia_css_binary_dpc_info {
+	uint32_t		bnr_lite; /**< bnr lite enable flag */
+};
+
+struct ia_css_binary_iterator_info {
+	uint32_t		num_stripes;
+	uint32_t		row_stripes_height;
+	uint32_t		row_stripes_overlap_lines;
+};
+
+struct ia_css_binary_address_info {
+	uint32_t		isp_addresses;	/* Address in ISP dmem */
+	uint32_t		main_entry;	/* Address of entry fct */
+	uint32_t		in_frame;	/* Address in ISP dmem */
+	uint32_t		out_frame;	/* Address in ISP dmem */
+	uint32_t		in_data;	/* Address in ISP dmem */
+	uint32_t		out_data;	/* Address in ISP dmem */
+	uint32_t		sh_dma_cmd_ptr;     /* In ISP dmem */
+};
+
+struct ia_css_binary_uds_info {
+	uint16_t	bpp;
+	uint16_t	use_bci;
+	uint16_t	use_str;
+	uint16_t	woix;
+	uint16_t	woiy;
+	uint16_t	extra_out_vecs;
+	uint16_t	vectors_per_line_in;
+	uint16_t	vectors_per_line_out;
+	uint16_t	vectors_c_per_line_in;
+	uint16_t	vectors_c_per_line_out;
+	uint16_t	vmem_gdc_in_block_height_y;
+	uint16_t	vmem_gdc_in_block_height_c;
+	/* uint16_t padding; */
+};
+
+struct ia_css_binary_pipeline_info {
+	uint32_t	mode;
+	uint32_t	isp_pipe_version;
+	uint32_t	pipelining;
+	uint32_t	c_subsampling;
+	uint32_t	top_cropping;
+	uint32_t	left_cropping;
+	uint32_t	variable_resolution;
+};
+
+struct ia_css_binary_block_info {
+	uint32_t	block_width;
+	uint32_t	block_height;
+	uint32_t	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(uint32_t			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 {
+#ifdef ISP2401
+		uint8_t	luma_only;
+		uint8_t	input_yuv;
+		uint8_t	input_raw;
+#endif
+		uint8_t	reduced_pipe;
+		uint8_t	vf_veceven;
+		uint8_t	dis;
+		uint8_t	dvs_envelope;
+		uint8_t	uds;
+		uint8_t	dvs_6axis;
+		uint8_t	block_output;
+		uint8_t	streaming_dma;
+		uint8_t	ds;
+		uint8_t	bayer_fir_6db;
+		uint8_t	raw_binning;
+		uint8_t	continuous;
+		uint8_t	s3a;
+		uint8_t	fpnr;
+		uint8_t	sc;
+		uint8_t	macc;
+		uint8_t	output;
+		uint8_t	ref_frame;
+		uint8_t	tnr;
+		uint8_t	xnr;
+		uint8_t	params;
+		uint8_t	ca_gdc;
+		uint8_t	isp_addresses;
+		uint8_t	in_frame;
+		uint8_t	out_frame;
+		uint8_t	high_speed;
+		uint8_t	dpc;
+		uint8_t padding[2];
+	} enable;
+	struct {
+/* DMA channel ID: [0,...,HIVE_ISP_NUM_DMA_CHANNELS> */
+		uint8_t	ref_y_channel;
+		uint8_t	ref_c_channel;
+		uint8_t	tnr_channel;
+		uint8_t	tnr_out_channel;
+		uint8_t	dvs_coords_channel;
+		uint8_t	output_channel;
+		uint8_t	c_channel;
+		uint8_t	vfout_channel;
+		uint8_t	vfout_c_channel;
+		uint8_t	vfdec_bits_per_pixel;
+		uint8_t	claimed_by_isp;
+		uint8_t 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(int32_t	     num_output_formats, 8);
+	enum ia_css_frame_format     output_formats[IA_CSS_FRAME_FORMAT_NUM];
+	CSS_ALIGN(int32_t	     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 */
+	uint8_t			     num_output_pins;
+	ia_css_ptr		     xmem_addr;
+	CSS_ALIGN(const struct ia_css_blob_descr *blob, 8);
+	CSS_ALIGN(uint32_t 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 {
+	uint32_t num_dma_cmds;	/**< Number of cmds sent by CSS */
+	uint32_t dma_cmd_list;	/**< Dma command list sent by CSS */
+	uint32_t sw_state;	/**< Polled from css */
+	/* Entry functions */
+	uint32_t 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 {
+	uint32_t init_dmem_data; /**< data sect config, stored to dmem */
+	uint32_t per_frame_data; /**< Per frame data, stored to dmem */
+	uint32_t group;		/**< Per pipeline data, loaded by dma */
+	uint32_t output;		/**< SP output data, loaded by dmem */
+	uint32_t host_sp_queue;	/**< Host <-> SP queues */
+	uint32_t host_sp_com;/**< Host <-> SP commands */
+	uint32_t isp_started;	/**< Polled from sensor thread, csim only */
+	uint32_t sw_state;	/**< Polled from css */
+	uint32_t host_sp_queues_initialized; /**< Polled from the SP */
+	uint32_t sleep_mode;  /**< different mode to halt SP */
+	uint32_t invalidate_tlb;		/**< inform SP to invalidate mmu TLB */
+#ifndef ISP2401
+	uint32_t stop_copy_preview;       /**< suspend copy and preview pipe when capture */
+#endif
+	uint32_t debug_buffer_ddr_address;	/**< inform SP the address
+	of DDR debug queue */
+	uint32_t perf_counter_input_system_error; /**< input system perf
+	counter array */
+#ifdef HAS_WATCHDOG_SP_THREAD_DEBUG
+	uint32_t debug_wait; /**< thread/pipe post mortem debug */
+	uint32_t debug_stage; /**< thread/pipe post mortem debug */
+	uint32_t debug_stripe; /**< thread/pipe post mortem debug */
+#endif
+	uint32_t threads_stack; /**< sp thread's stack pointers */
+	uint32_t threads_stack_size; /**< sp thread's stack sizes */
+	uint32_t curr_binary_id;        /**< current binary id */
+	uint32_t raw_copy_line_count;   /**< raw copy line counter */
+	uint32_t ddr_parameter_address; /**< acc param ddrptr, sp dmem */
+	uint32_t ddr_parameter_size;    /**< acc param size, sp dmem */
+	/* Entry functions */
+	uint32_t sp_entry;	/**< The SP entry function */
+	uint32_t 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.
+*/
+#if !defined(__ISP)
+/** Accelerator firmware information.
+ */
+struct ia_css_acc_info {
+	uint32_t 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(uint32_t 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(uint32_t       loaded, 8);	/**< Firmware has been loaded */
+	CSS_ALIGN(const uint8_t *isp_code, 8);  /**< ISP pointer to code */
+	/**< Firmware handle between user space and kernel */
+	CSS_ALIGN(uint32_t	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 */
+	uint32_t sp_prog_name_offset;		/**< program name offset wrt hdr in bytes */
+	uint32_t sp_blob_offset;		/**< blob offset wrt hdr in bytes */
+	void	 *entry;			/**< Address of sp entry point */
+	uint32_t *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 uint8_t *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 */
+	uint32_t	isp_prog_name_offset; /**< program name offset wrt
+						   header in bytes */
+	uint32_t	isp_blob_offset;      /**< blob offset wrt header
+						   in bytes */
+	uint32_t	isp_size;	      /**< Size of isp blob */
+	const uint8_t  *isp_code;	      /**< ISP pointer to code */
+	struct ia_css_acc_sp  sp;  /**< Standalone sp code */
+	/**< Firmware handle between user space and kernel */
+	uint32_t	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))
+
+#endif /* !defined(__ISP) */
+
+enum ia_css_sp_sleep_mode {
+	SP_DISABLE_SLEEP_MODE = 0,
+	SP_SLEEP_AFTER_FRAME = 1 << 0,
+	SP_SLEEP_AFTER_IRQ = 1 << 1
+};
+#endif /* _IA_CSS_ACC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_buffer.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_buffer.h
new file mode 100644
index 000000000000..b2ecf3618c15
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_buffer.h
@@ -0,0 +1,84 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+#if !defined(__ISP)
+/** 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. */
+	uint64_t 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 /* !__ISP */
+
+#endif /* __IA_CSS_BUFFER_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_control.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_control.h
new file mode 100644
index 000000000000..a15d3e368341
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_control.h
@@ -0,0 +1,157 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 IA_CSS_ERR_INTERNAL_ERROR in case of any
+ *				errors and IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err ia_css_init(
+	const struct ia_css_env *env,
+	const struct ia_css_fw  *fw,
+	uint32_t                 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 Suspend CSS API for power down
+ * @return	success or faulure code
+ *
+ * suspend shuts down the system by:
+ *  unloading all the streams
+ *  stopping SP
+ *  performing uninit
+ *
+ *  Currently stream memory is deallocated because of rmmgr issues.
+ *  Need to come up with a bypass that will leave the streams intact.
+ */
+enum ia_css_err
+ia_css_suspend(void);
+
+/** @brief Resume CSS API from power down
+ * @return	success or failure code
+ *
+ * After a power cycle, this function will bring the CSS API back into
+ * a state where it can be started.
+ * This will re-initialize the hardware and all the streams.
+ * Call this function only after ia_css_suspend() has been called.
+ */
+enum ia_css_err
+ia_css_resume(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. }
+ */
+enum ia_css_err
+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			IA_CSS_SUCCESS 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().
+ */
+enum ia_css_err
+ia_css_start_sp(void);
+
+
+/** @brief stop SP hardware
+ *
+ * @return			IA_CSS_SUCCESS 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().
+ */
+enum ia_css_err
+ia_css_stop_sp(void);
+
+#endif /* __IA_CSS_CONTROL_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_device_access.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_device_access.c
new file mode 100644
index 000000000000..21b842379acc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_device_access.c
@@ -0,0 +1,95 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_types.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 != NULL);
+
+	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/atomisp2/css2400/ia_css_device_access.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_device_access.h
new file mode 100644
index 000000000000..59459f7a9876
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_device_access.h
@@ -0,0 +1,59 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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/atomisp2/css2400/ia_css_dvs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_dvs.h
new file mode 100644
index 000000000000..147bf81959d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_dvs.h
@@ -0,0 +1,299 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+	uint32_t   hor_size;
+	uint32_t   ver_size;
+	uint32_t   exp_id;   /**< see ia_css_event_public.h for more detail */
+	ia_css_ptr data_ptr; /* base pointer containing all memory */
+	uint32_t   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;
+	int32_t *hor_proj;
+	int32_t *ver_proj;
+	uint32_t size;		 /* total size in bytes */
+	uint32_t 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.
+ */
+enum ia_css_err
+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.
+ */
+enum ia_css_err
+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/atomisp2/css2400/ia_css_env.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_env.h
new file mode 100644
index 000000000000..1ae9daf0be76
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_env.h
@@ -0,0 +1,94 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <stdarg.h> /* va_list */
+#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 = 1 << 0,
+	IA_CSS_MEM_ATTR_ZEROED = 1 << 1,
+	IA_CSS_MEM_ATTR_PAGEALIGN = 1 << 2,
+	IA_CSS_MEM_ATTR_CONTIGUOUS = 1 << 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 (*debug_print)(const char *fmt, va_list args);
+	/**< Print a debug message. */
+	int (*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/atomisp2/css2400/ia_css_err.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_err.h
new file mode 100644
index 000000000000..572e4e55c69e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_err.h
@@ -0,0 +1,63 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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.
+ */
+
+/** Errors, these values are used as the return value for most
+ *  functions in this API.
+ */
+enum ia_css_err {
+	IA_CSS_SUCCESS,
+	IA_CSS_ERR_INTERNAL_ERROR,
+	IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY,
+	IA_CSS_ERR_INVALID_ARGUMENTS,
+	IA_CSS_ERR_SYSTEM_NOT_IDLE,
+	IA_CSS_ERR_MODE_HAS_NO_VIEWFINDER,
+	IA_CSS_ERR_QUEUE_IS_FULL,
+	IA_CSS_ERR_QUEUE_IS_EMPTY,
+	IA_CSS_ERR_RESOURCE_NOT_AVAILABLE,
+	IA_CSS_ERR_RESOURCE_LIST_TO_SMALL,
+	IA_CSS_ERR_RESOURCE_ITEMS_STILL_ALLOCATED,
+	IA_CSS_ERR_RESOURCE_EXHAUSTED,
+	IA_CSS_ERR_RESOURCE_ALREADY_ALLOCATED,
+	IA_CSS_ERR_VERSION_MISMATCH,
+	IA_CSS_ERR_NOT_SUPPORTED
+};
+
+/** 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-queing 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-queing 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/atomisp2/css2400/ia_css_event_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_event_public.h
new file mode 100644
index 000000000000..aaf349772abe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_event_public.h
@@ -0,0 +1,196 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+
+/** 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		= 1 << 0,
+	/**< Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_OUTPUT_FRAME_DONE	= 1 << 1,
+	/**< Second output frame ready. */
+	IA_CSS_EVENT_TYPE_VF_OUTPUT_FRAME_DONE		= 1 << 2,
+	/**< Viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_SECOND_VF_OUTPUT_FRAME_DONE	= 1 << 3,
+	/**< Second viewfinder Output frame ready. */
+	IA_CSS_EVENT_TYPE_3A_STATISTICS_DONE		= 1 << 4,
+	/**< Indication that 3A statistics are available. */
+	IA_CSS_EVENT_TYPE_DIS_STATISTICS_DONE		= 1 << 5,
+	/**< Indication that DIS statistics are available. */
+	IA_CSS_EVENT_TYPE_PIPELINE_DONE			= 1 << 6,
+	/**< Pipeline Done event, sent after last pipeline stage. */
+	IA_CSS_EVENT_TYPE_FRAME_TAGGED			= 1 << 7,
+	/**< Frame tagged. */
+	IA_CSS_EVENT_TYPE_INPUT_FRAME_DONE		= 1 << 8,
+	/**< Input frame ready. */
+	IA_CSS_EVENT_TYPE_METADATA_DONE			= 1 << 9,
+	/**< Metadata ready. */
+	IA_CSS_EVENT_TYPE_LACE_STATISTICS_DONE		= 1 << 10,
+	/**< Indication that LACE statistics are available. */
+	IA_CSS_EVENT_TYPE_ACC_STAGE_COMPLETE		= 1 << 11,
+	/**< Extension stage complete. */
+	IA_CSS_EVENT_TYPE_TIMER				= 1 << 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			= 1 << 13,
+	/**< End Of Frame event, sent when in buffered sensor mode. */
+	IA_CSS_EVENT_TYPE_FW_WARNING			= 1 << 14,
+	/**< Performance warning encounter by FW */
+	IA_CSS_EVENT_TYPE_FW_ASSERT			= 1 << 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. */
+	uint8_t                port;
+	/**< Port number for EOF event (not valid for other events). */
+	uint8_t                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. */
+	uint32_t               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. */
+	uint8_t                fw_assert_module_id;
+	/**< Firmware module id, only for ASSERT events, should be logged by driver. */
+	uint16_t               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 */
+	uint8_t                timer_code;
+	/**< For storing the code of the TIMER event. Valid only for
+	     TIMER event */
+	uint8_t                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		IA_CSS_ERR_QUEUE_IS_EMPTY if no events are
+ *			available or
+ *			IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+ia_css_dequeue_psys_event(struct ia_css_event *event);
+
+/** @brief Dequeue an 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		IA_CSS_ERR_QUEUE_IS_EMPTY if no events are
+ *			available or
+ *			IA_CSS_SUCCESS otherwise.
+ *
+ * deprecated{Use ia_css_dequeue_psys_event instead}.
+ * Unless the isys event queue is explicitly enabled, this function will
+ * dequeue both isys (EOF) and psys events (all others).
+ */
+enum ia_css_err
+ia_css_dequeue_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		IA_CSS_ERR_QUEUE_IS_EMPTY if no events are
+ *			available or
+ *			IA_CSS_SUCCESS 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_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.
+ */
+enum ia_css_err
+ia_css_dequeue_isys_event(struct ia_css_event *event);
+
+#endif /* __IA_CSS_EVENT_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_firmware.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_firmware.h
new file mode 100644
index 000000000000..06d375a09be2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_firmware.h
@@ -0,0 +1,74 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 "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 */
+};
+
+/** @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 IA_CSS_ERR_INTERNAL_ERROR in case of any
+ *				errors and IA_CSS_SUCCESS 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).
+ */
+enum ia_css_err
+ia_css_load_firmware(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);
+
+/** @brief Checks firmware version
+ * @param[in]	fw	Firmware package containing the firmware for all
+ *			predefined ISP binaries.
+ * @return		Returns true when the firmware version matches with the CSS
+ *			host code version and returns false otherwise.
+ * This function checks if the firmware package version matches with the CSS host code version.
+ */
+bool
+ia_css_check_firmware_version(const struct ia_css_fw  *fw);
+
+#endif /* __IA_CSS_FIRMWARE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frac.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frac.h
new file mode 100644
index 000000000000..da9c60144c6d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frac.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 uint32_t ia_css_u0_16;
+/** Unsigned fixed point value, 5 integer bits, 11 fractional bits */
+typedef uint32_t ia_css_u5_11;
+/** Unsigned fixed point value, 8 integer bits, 8 fractional bits */
+typedef uint32_t ia_css_u8_8;
+/** Signed fixed point value, 0 integer bits, 15 fractional bits */
+typedef int32_t ia_css_s0_15;
+
+#endif /* _IA_CSS_FRAC_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frame_format.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frame_format.h
new file mode 100644
index 000000000000..d534fbd91380
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frame_format.h
@@ -0,0 +1,101 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_frame_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frame_public.h
new file mode 100644
index 000000000000..92f2389176b2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_frame_public.h
@@ -0,0 +1,365 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 \
+{ \
+	{0,                      /* width */ \
+	 0},                     /* height */ \
+	0,                       /* padded_width */ \
+	IA_CSS_FRAME_FORMAT_NUM, /* format */ \
+	0,                       /* raw_bit_depth */ \
+	IA_CSS_BAYER_ORDER_NUM,  /* raw_bayer_order */ \
+	{0,                       /*start col */ \
+	 0},                       /*start line */ \
+}
+
+/**
+ *  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 */
+	uint32_t 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 */
+	bool contiguous; /**< memory is allocated physically contiguously */
+	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 \
+{ \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO,	/* info */ \
+	0,					/* data */ \
+	0,					/* data_bytes */ \
+	SH_CSS_INVALID_QUEUE_ID,		/* dynamic_data_index */ \
+	IA_CSS_BUFFER_TYPE_INVALID,			/* buf_type */ \
+	IA_CSS_FRAME_FLASH_STATE_NONE,		/* flash_state */ \
+	0,					/* exp_id */ \
+	0,					/* isp_config_id */ \
+	false,					/* valid */ \
+	false,					/* contiguous  */ \
+	{ 0 }					/* planes */ \
+}
+
+/** @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.
+ */
+enum ia_css_err
+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().
+ */
+enum ia_css_err
+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 contiguous 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.
+ *
+ * Contiguous frame allocation, only for FPGA display driver which needs
+ * physically contiguous memory.
+ * Deprecated.
+ */
+enum ia_css_err
+ia_css_frame_allocate_contiguous(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 contiguous CSS frame from 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_contiguous().
+ * Only for FPGA display driver which needs physically contiguous memory.
+ * Deprecated.
+ */
+enum ia_css_err
+ia_css_frame_allocate_contiguous_from_info(struct ia_css_frame **frame,
+					  const struct ia_css_frame_info *info);
+
+/** @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.
+ */
+enum ia_css_err
+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.
+ */
+enum ia_css_err
+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.
+ */
+enum ia_css_err
+ia_css_frame_map(struct ia_css_frame **frame,
+		 const struct ia_css_frame_info *info,
+		 const void *data,
+		 uint16_t attribute,
+		 void *context);
+
+/** @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/atomisp2/css2400/ia_css_host_data.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_host_data.h
new file mode 100644
index 000000000000..4557e66891df
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_host_data.h
@@ -0,0 +1,46 @@
+/* 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/atomisp2/css2400/ia_css_input_port.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_input_port.h
new file mode 100644
index 000000000000..8a17c3346caa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_input_port.h
@@ -0,0 +1,66 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_INPUT_PORT_H
+#define __IA_CSS_INPUT_PORT_H
+
+/** @file
+ * This file contains information about the possible input ports for CSS
+ */
+
+/** Enumeration of the physical input ports on the CSS hardware.
+ *  There are 3 MIPI CSI-2 ports.
+ */
+enum ia_css_csi2_port {
+	IA_CSS_CSI2_PORT0, /* Implicitly map to MIPI_PORT0_ID */
+	IA_CSS_CSI2_PORT1, /* Implicitly map to MIPI_PORT1_ID */
+	IA_CSS_CSI2_PORT2  /* Implicitly map to MIPI_PORT2_ID */
+};
+
+/** 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 IA_CSS_CSI2_PORT0
+#define	IA_CSS_CSI2_PORT_1LANE IA_CSS_CSI2_PORT1
+#define	IA_CSS_CSI2_PORT_2LANE IA_CSS_CSI2_PORT2
+
+/** 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 ia_css_csi2_port 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/atomisp2/css2400/ia_css_irq.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_irq.h
new file mode 100644
index 000000000000..416ca4d28732
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_irq.h
@@ -0,0 +1,235 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+/** 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            = 1 << 0,
+	/**< the css receiver has encountered an error */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_FIFO_OVERFLOW    = 1 << 1,
+	/**< the FIFO in the csi receiver has overflown */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF              = 1 << 2,
+	/**< the css receiver received the start of frame */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_EOF              = 1 << 3,
+	/**< the css receiver received the end of frame */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SOL              = 1 << 4,
+	/**< the css receiver received the start of line */
+	IA_CSS_IRQ_INFO_PSYS_EVENTS_READY             = 1 << 5,
+	/**< One or more events are available in the PSYS event queue */
+	IA_CSS_IRQ_INFO_EVENTS_READY = IA_CSS_IRQ_INFO_PSYS_EVENTS_READY,
+	/**< deprecated{obsolete version of IA_CSS_IRQ_INFO_PSYS_EVENTS_READY,
+	 * same functionality.} */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_EOL              = 1 << 6,
+	/**< the css receiver received the end of line */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_SIDEBAND_CHANGED = 1 << 7,
+	/**< the css receiver received a change in side band signals */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_GEN_SHORT_0      = 1 << 8,
+	/**< generic short packets (0) */
+	IA_CSS_IRQ_INFO_CSS_RECEIVER_GEN_SHORT_1      = 1 << 9,
+	/**< generic short packets (1) */
+	IA_CSS_IRQ_INFO_IF_PRIM_ERROR                 = 1 << 10,
+	/**< the primary input formatter (A) has encountered an error */
+	IA_CSS_IRQ_INFO_IF_PRIM_B_ERROR               = 1 << 11,
+	/**< the primary input formatter (B) has encountered an error */
+	IA_CSS_IRQ_INFO_IF_SEC_ERROR                  = 1 << 12,
+	/**< the secondary input formatter has encountered an error */
+	IA_CSS_IRQ_INFO_STREAM_TO_MEM_ERROR           = 1 << 13,
+	/**< the stream-to-memory device has encountered an error */
+	IA_CSS_IRQ_INFO_SW_0                          = 1 << 14,
+	/**< software interrupt 0 */
+	IA_CSS_IRQ_INFO_SW_1                          = 1 << 15,
+	/**< software interrupt 1 */
+	IA_CSS_IRQ_INFO_SW_2                          = 1 << 16,
+	/**< software interrupt 2 */
+	IA_CSS_IRQ_INFO_ISP_BINARY_STATISTICS_READY   = 1 << 17,
+	/**< ISP binary statistics are ready */
+	IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR            = 1 << 18,
+	/**< the input system in in error */
+	IA_CSS_IRQ_INFO_IF_ERROR                      = 1 << 19,
+	/**< the input formatter in in error */
+	IA_CSS_IRQ_INFO_DMA_ERROR                     = 1 << 20,
+	/**< the dma in in error */
+	IA_CSS_IRQ_INFO_ISYS_EVENTS_READY             = 1 << 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   = 1U << 0, /**< buffer overrun */
+	IA_CSS_RX_IRQ_INFO_ENTER_SLEEP_MODE = 1U << 1, /**< entering sleep mode */
+	IA_CSS_RX_IRQ_INFO_EXIT_SLEEP_MODE  = 1U << 2, /**< exited sleep mode */
+	IA_CSS_RX_IRQ_INFO_ECC_CORRECTED    = 1U << 3, /**< ECC corrected */
+	IA_CSS_RX_IRQ_INFO_ERR_SOT          = 1U << 4,
+						/**< Start of transmission */
+	IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC     = 1U << 5, /**< SOT sync (??) */
+	IA_CSS_RX_IRQ_INFO_ERR_CONTROL      = 1U << 6, /**< Control (??) */
+	IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE   = 1U << 7, /**< Double ECC */
+	IA_CSS_RX_IRQ_INFO_ERR_CRC          = 1U << 8, /**< CRC error */
+	IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID   = 1U << 9, /**< Unknown ID */
+	IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC   = 1U << 10,/**< Frame sync error */
+	IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA   = 1U << 11,/**< Frame data error */
+	IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT = 1U << 12,/**< Timeout occurred */
+	IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC  = 1U << 13,/**< Unknown escape seq. */
+	IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC    = 1U << 14,/**< Line Sync error */
+	IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT     = 1U << 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
+ *			IA_CSS_SUCCESS.
+ *
+ * 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.
+ */
+enum ia_css_err
+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 ia_css_csi2_port 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 ia_css_csi2_port 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
+ *			IA_CSS_SUCCESS.
+ */
+enum ia_css_err
+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/atomisp2/css2400/ia_css_memory_access.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_memory_access.c
new file mode 100644
index 000000000000..282075942ba6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_memory_access.c
@@ -0,0 +1,83 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015-2017, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_types.h>
+#include <assert_support.h>
+#include <memory_access.h>
+#include <ia_css_env.h>
+#include <hrt/hive_isp_css_mm_hrt.h>
+
+const hrt_vaddress mmgr_NULL = (hrt_vaddress)0;
+const hrt_vaddress mmgr_EXCEPTION = (hrt_vaddress)-1;
+
+hrt_vaddress
+mmgr_malloc(const size_t size)
+{
+	return mmgr_alloc_attr(size, 0);
+}
+
+hrt_vaddress mmgr_alloc_attr(const size_t size, const uint16_t attrs)
+{
+	uint16_t masked_attrs = attrs & MMGR_ATTRIBUTE_MASK;
+	WARN_ON(attrs & MMGR_ATTRIBUTE_CONTIGUOUS);
+
+	if (masked_attrs & MMGR_ATTRIBUTE_CLEARED) {
+		if (masked_attrs & MMGR_ATTRIBUTE_CACHED)
+			return (ia_css_ptr) hrt_isp_css_mm_calloc_cached(size);
+		else
+			return (ia_css_ptr) hrt_isp_css_mm_calloc(size);
+	} else {
+		if (masked_attrs & MMGR_ATTRIBUTE_CACHED)
+			return (ia_css_ptr) hrt_isp_css_mm_alloc_cached(size);
+		else
+			return (ia_css_ptr) hrt_isp_css_mm_alloc(size);
+	}
+}
+
+hrt_vaddress
+mmgr_calloc(const size_t N, const size_t size)
+{
+	return mmgr_alloc_attr(size * N, MMGR_ATTRIBUTE_CLEARED);
+}
+
+void mmgr_clear(hrt_vaddress vaddr, const size_t size)
+{
+	if (vaddr)
+		hmm_set(vaddr, 0, size);
+}
+
+void mmgr_load(const hrt_vaddress vaddr, void *data, const size_t size)
+{
+	if (vaddr && data)
+		hmm_load(vaddr, data, size);
+}
+
+void
+mmgr_store(const hrt_vaddress vaddr, const void *data, const size_t size)
+{
+	if (vaddr && data)
+		hmm_store(vaddr, data, size);
+}
+
+hrt_vaddress
+mmgr_mmap(const void *ptr, const size_t size,
+	  uint16_t attribute, void *context)
+{
+	struct hrt_userbuffer_attr *userbuffer_attr = context;
+	return hrt_isp_css_mm_alloc_user_ptr(
+			size, (void *)ptr, userbuffer_attr->pgnr,
+			userbuffer_attr->type,
+			attribute & HRT_BUF_FLAG_CACHED);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_metadata.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_metadata.h
new file mode 100644
index 000000000000..c40c5a19bfe1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_metadata.h
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 ia_css_stream_format data_type; /**< Data type of CSI-2 embedded
+			data. The default value is IA_CSS_STREAM_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 */
+	uint32_t                 stride;     /**< Stride in bytes */
+	uint32_t                 size;       /**< Total size in bytes */
+};
+
+struct ia_css_metadata {
+	struct ia_css_metadata_info info;    /**< Layout info */
+	ia_css_ptr	            address; /**< CSS virtual address */
+	uint32_t	            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/atomisp2/css2400/ia_css_mipi.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mipi.h
new file mode 100644
index 000000000000..fd2c01b60b28
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mipi.h
@@ -0,0 +1,82 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+/** Backward compatible for CSS API 2.0 only
+ * TO BE REMOVED when all drivers move to CSS API 2.1.
+ */
+/** @brief Specify a CSS MIPI frame buffer.
+ *
+ * @param[in]	size_mem_words	The frame size in memory words (32B).
+ * @param[in]	contiguous	Allocate memory physically contiguously or not.
+ * @return		The error code.
+ *
+ * \deprecated{Use ia_css_mipi_buffer_config instead.}
+ *
+ * Specifies a CSS MIPI frame buffer: size in memory words (32B).
+ */
+enum ia_css_err
+ia_css_mipi_frame_specify(const unsigned int	size_mem_words,
+				const bool contiguous);
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+/** @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.
+ *
+ *
+ */
+enum ia_css_err
+ia_css_mipi_frame_enable_check_on_size(const enum ia_css_csi2_port port,
+				const unsigned int	size_mem_words);
+#endif
+
+/** @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.
+ */
+enum ia_css_err
+ia_css_mipi_frame_calculate_size(const unsigned int width,
+				const unsigned int height,
+				const enum ia_css_stream_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/atomisp2/css2400/ia_css_mmu.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu.h
new file mode 100644
index 000000000000..48f8855d61f6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_mmu_private.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h
new file mode 100644
index 000000000000..7c8500903b5c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_mmu_private.h
@@ -0,0 +1,31 @@
+#ifdef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_morph.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_morph.h
new file mode 100644
index 000000000000..969840da52b2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_morph.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_pipe.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_pipe.h
new file mode 100644
index 000000000000..d0c0e6b92025
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_pipe.h
@@ -0,0 +1,228 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DELAY_FRAMES];
+	struct ia_css_frame *tnr_frames[NUM_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 \
+{ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* copy_binary */\
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* preview_binary */\
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* vf_pp_binary */\
+	{ NULL },			/* dvs_frames */ \
+	{ NULL },			/* tnr_frames */ \
+	NULL,				/* copy_pipe */\
+	NULL,				/* capture_pipe */\
+	NULL,				/* acc_pipe */\
+}
+
+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 \
+{ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* copy_binary */\
+	{IA_CSS_BINARY_DEFAULT_SETTINGS},	/* primary_binary */\
+	0,				/* num_primary_stage */\
+	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 */\
+	NULL,				/* yuv_scaler_binary */ \
+	{ NULL },			/* delay_frames[ref_frames] */ \
+	NULL,				/* is_output_stage */ \
+	0,				/* num_yuv_scaler */ \
+}
+
+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];
+#ifndef ISP2401
+	struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];
+#else
+	struct ia_css_frame *tnr_frames[NUM_TNR_FRAMES];
+#endif
+	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 \
+{ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* copy_binary */ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* video_binary */ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,	/* vf_pp_binary */ \
+	NULL,				/* yuv_scaler_binary */ \
+	{ NULL },			/* delay_frames */ \
+	{ NULL },			/* tnr_frames */ \
+	NULL,				/* vf_pp_in_frame */ \
+	NULL,				/* copy_pipe */ \
+	NULL,				/* capture_pipe */ \
+	NULL,				/* is_output_stage */ \
+	0,				/* num_yuv_scaler */ \
+}
+
+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 \
+{ \
+	IA_CSS_BINARY_DEFAULT_SETTINGS,		/* copy_binary */ \
+	NULL,					/* yuv_scaler_binary */ \
+	NULL,					/* vf_pp_binary */ \
+	NULL,					/* is_output_stage */ \
+	0,					/* num_yuv_scaler */ \
+	0,					/* num_vf_pp */ \
+	0,					/* num_output */ \
+}
+
+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;
+	hrt_vaddress 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 \
+{ \
+	false,					/* stop_requested */ \
+	DEFAULT_PIPE_CONFIG,			/* config */ \
+	DEFAULT_PIPE_EXTRA_CONFIG,		/* extra_config */ \
+	DEFAULT_PIPE_INFO,			/* info */ \
+	IA_CSS_PIPE_ID_ACC,			/* mode (pipe_id) */ \
+	NULL,					/* shading_table */ \
+	DEFAULT_PIPELINE,			/* pipeline */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* 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 */ \
+	NULL,					/* output_stage */ \
+	NULL,					/* vf_stage */ \
+	SH_CSS_BDS_FACTOR_1_00,			/* required_bds_factor */ \
+	1,					/* dvs_frame_delay */ \
+	0,					/* num_invalid_frames */ \
+	{true},					/* enable_viewfinder */ \
+	NULL,					/* stream */ \
+	DEFAULT_FRAME,				/* in_frame_struct */ \
+	DEFAULT_FRAME,				/* out_frame_struct */ \
+	DEFAULT_FRAME,				/* vf_frame_struct */ \
+	{ NULL },				/* continuous_frames */ \
+	{ NULL },				/* cont_md_buffers */ \
+	{ IA_CSS_DEFAULT_PREVIEW_SETTINGS },	/* pipe_settings */ \
+	0,					/* scaler_pp_lut */ \
+	NULL,					/* osys object */ \
+	PIPE_ENTRY_EMPTY_TOKEN,			/* pipe_num */\
+}
+
+void ia_css_pipe_map_queue(struct ia_css_pipe *pipe, bool map);
+
+enum ia_css_err
+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/atomisp2/css2400/ia_css_pipe_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_pipe_public.h
new file mode 100644
index 000000000000..733e0ef3afe8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_pipe_public.h
@@ -0,0 +1,659 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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>
+#ifdef ISP2401
+#include <ia_css_acc_types.h>
+#endif
+
+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;
+#ifndef ISP2401
+	/**< bayer down scaling */
+#else
+	/**< capture post processing input resolution */
+#endif
+	struct ia_css_resolution vf_pp_in_res;
+#ifndef ISP2401
+	/**< bayer down scaling */
+#else
+	/**< 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 */
+#endif
+	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 */
+	uint32_t 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. */
+#ifdef ISP2401
+	bool enable_luma_only;
+	/**< Enabling of monochrome mode for a pipeline. If enabled only luma processing
+	     will be done. */
+	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. */
+#endif
+	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 */
+#ifdef 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. */
+#endif
+};
+
+
+#ifdef ISP2401
+/**
+ * Default origin of internal frame positioned on shading table.
+ */
+#define IA_CSS_PIPE_DEFAULT_INTERNAL_FRAME_ORIGIN_BQS_ON_SCTBL \
+{ \
+	0,					/* x [bqs] */ \
+	0					/* y [bqs] */ \
+}
+
+/**
+ * Default settings for newly created pipe configurations.
+ */
+#define DEFAULT_PIPE_CONFIG \
+{ \
+	IA_CSS_PIPE_MODE_PREVIEW,		/* mode */ \
+	1,					/* isp_pipe_version */ \
+	{ 0, 0 },				/* pipe_effective_input_res */ \
+	{ 0, 0 },				/* bayer_ds_out_res */ \
+	{ 0, 0 },				/* vf_pp_in_res */ \
+	{ 0, 0 },				/* capt_pp_in_res */ \
+	{ 0, 0 },				/* output_system_in_res */ \
+	{ 0, 0 },				/* dvs_crop_out_res */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* output_info */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* vf_output_info */ \
+	NULL,					/* acc_extension */ \
+	NULL,					/* acc_stages */ \
+	0,					/* num_acc_stages */ \
+	DEFAULT_CAPTURE_CONFIG,			/* default_capture_config */ \
+	{ 0, 0 },				/* dvs_envelope */ \
+	IA_CSS_FRAME_DELAY_1,			/* dvs_frame_delay */ \
+	-1,					/* acc_num_execs */ \
+	false,					/* enable_dz */ \
+	false,					/* enable_dpc */ \
+	false,					/* enable_vfpp_bci */ \
+	false,					/* enable_luma_only */ \
+	false,					/* enable_tnr */ \
+	NULL,					/* p_isp_config */\
+	{ 0, 0 },				/* gdc_in_buffer_res */ \
+	{ 0, 0 },				/* gdc_in_buffer_offset */ \
+	IA_CSS_PIPE_DEFAULT_INTERNAL_FRAME_ORIGIN_BQS_ON_SCTBL	/* internal_frame_origin_bqs_on_sctbl */ \
+}
+
+#else
+
+/**
+ * Default settings for newly created pipe configurations.
+ */
+#define DEFAULT_PIPE_CONFIG \
+{ \
+	IA_CSS_PIPE_MODE_PREVIEW,		/* mode */ \
+	1,					/* isp_pipe_version */ \
+	{ 0, 0 },				/* pipe_effective_input_res */ \
+	{ 0, 0 },				/* bayer_ds_out_res */ \
+	{ 0, 0 },				/* vf_pp_in_res */ \
+	{ 0, 0 },				/* capt_pp_in_res */ \
+	{ 0, 0 },				/* dvs_crop_out_res */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* output_info */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* vf_output_info */ \
+	NULL,					/* acc_extension */ \
+	NULL,					/* acc_stages */ \
+	0,					/* num_acc_stages */ \
+	DEFAULT_CAPTURE_CONFIG,			/* default_capture_config */ \
+	{ 0, 0 },				/* dvs_envelope */ \
+	IA_CSS_FRAME_DELAY_1,			/* dvs_frame_delay */ \
+	-1,					/* acc_num_execs */ \
+	false,					/* enable_dz */ \
+	false,					/* enable_dpc */ \
+	false,					/* enable_vfpp_bci */ \
+	NULL,					/* p_isp_config */\
+	{ 0, 0 },				/* gdc_in_buffer_res */ \
+	{ 0, 0 }				/* gdc_in_buffer_offset */ \
+}
+
+#endif
+
+/** 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. */
+#ifdef 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. */
+#endif
+	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.
+ */
+#ifdef ISP2401
+
+#define DEFAULT_PIPE_INFO \
+{ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* output_info */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* vf_output_info */ \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO,	/* raw_output_info */ \
+	{ 0, 0},				/* output system in res */ \
+	DEFAULT_SHADING_INFO,			/* shading_info */ \
+	DEFAULT_GRID_INFO,			/* grid_info */ \
+	0					/* num_invalid_frames */ \
+}
+
+#else
+
+#define DEFAULT_PIPE_INFO \
+{ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* output_info */ \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO},	/* vf_output_info */ \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO,	/* raw_output_info */ \
+	DEFAULT_SHADING_INFO,			/* shading_info */ \
+	DEFAULT_GRID_INFO,			/* grid_info */ \
+	0					/* num_invalid_frames */ \
+}
+
+#endif
+
+/** @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	IA_CSS_SUCCESS or the error code.
+ *
+ * This function will create a pipe with the given
+ * configuration.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or the error code.
+ *
+ * This function will destroy a given pipe.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or IA_CSS_ERR_INVALID_ARGUMENTS.
+ *
+ * This function will provide information about a given pipe.
+ */
+enum ia_css_err
+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		IA_CSS_SUCCESS or error code upon error.
+ *
+ * This function configures the filter coefficients for an image
+ * pipe.
+ */
+enum ia_css_err
+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 occure
+			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		IA_CSS_SUCCESS.
+ *
+ 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 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.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS.
+ *
+ 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
+
+*/
+enum ia_css_err
+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,
+ *			IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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
+ *			 IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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
+ * IA_CSS_SUCCESS 			: Success
+ * IA_CSS_ERR_INVALID_ARGUMENTS		: Invalid Parameters
+ * IA_CSS_ERR_RESOURCE_NOT_AVAILABLE	: 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
+ *
+ */
+enum ia_css_err
+ia_css_pipe_set_qos_ext_state (struct ia_css_pipe *pipe,
+                           uint32_t 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
+ * IA_CSS_SUCCESS 			: Success
+ * IA_CSS_ERR_INVALID_ARGUMENTS		: Invalid Parameters
+ * IA_CSS_ERR_RESOURCE_NOT_AVAILABLE	: 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.
+ *
+ */
+enum ia_css_err
+ia_css_pipe_get_qos_ext_state (struct ia_css_pipe *pipe,
+                           uint32_t fw_handle,
+                           bool * enable);
+
+#ifdef ISP2401
+/** @brief  Update mapped CSS and ISP arguments for QoS pipe during SP runtime.
+ * @param[in] pipe     	Pipe handle.
+ * @param[in] fw_handle	Extension firmware Handle (ia_css_fw_info.handle).
+ * @param[in] css_seg  	Parameter memory descriptors for CSS segments.
+ * @param[in] isp_seg  	Parameter memory descriptors for ISP segments.
+ *
+ * @return
+ * IA_CSS_SUCCESS 			: Success
+ * IA_CSS_ERR_INVALID_ARGUMENTS		: Invalid Parameters
+ * IA_CSS_ERR_RESOURCE_NOT_AVAILABLE	: Inactive QOS Pipe
+ * 					(No active stream with this pipe)
+ *
+ * \deprecated{This interface is used to temporarily support a late-developed,
+ * specific use-case on a specific IPU2 platform. It will not be supported or
+ * maintained on IPU3 or further.}
+ */
+enum ia_css_err
+ia_css_pipe_update_qos_ext_mapped_arg(struct ia_css_pipe *pipe, uint32_t fw_handle,
+			struct ia_css_isp_param_css_segments *css_seg,
+			struct ia_css_isp_param_isp_segments *isp_seg);
+
+#endif
+/** @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
+ * IA_CSS_SUCCESS 			: Success
+ * IA_CSS_ERR_INVALID_ARGUMENTS		: 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
+ *
+ */
+enum ia_css_err
+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);
+
+#ifdef 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
+ * IA_CSS_SUCCESS		: Success
+ * IA_CSS_ERR_INVALID_ARGUMENTS	: Invalid Parameters
+ * IA_CSS_ERR_INTERNAL_ERROR	: 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.
+ */
+enum ia_css_err
+ia_css_pipe_override_frame_format(struct ia_css_pipe *pipe,
+				int output_pin,
+				enum ia_css_frame_format format);
+
+#endif
+#endif /* __IA_CSS_PIPE_PUBLIC_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_prbs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_prbs.h
new file mode 100644
index 000000000000..9b0eeb08ca04
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_prbs.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_properties.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_properties.h
new file mode 100644
index 000000000000..19af4021b24c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_properties.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_shading.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_shading.h
new file mode 100644
index 000000000000..cb0f249e98c8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_shading.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_stream.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream.h
new file mode 100644
index 000000000000..453fe4db0133
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream.h
@@ -0,0 +1,110 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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>
+#if !defined(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+#include <input_system.h>
+#endif
+#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;
+#if !defined(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	rx_cfg_t                       csi_rx_config;
+#endif
+	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;
+#ifndef ISP2401
+	bool                           stop_copy_preview;
+#endif
+	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);
+
+enum ia_css_err
+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/atomisp2/css2400/ia_css_stream_format.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream_format.h
new file mode 100644
index 000000000000..ae608a9c9051
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream_format.h
@@ -0,0 +1,94 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+
+/** The ISP streaming input interface supports the following formats.
+ *  These match the corresponding MIPI formats.
+ */
+enum ia_css_stream_format {
+	IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY,    /**< 8 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV420_8,  /**< 8 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV420_10, /**< 10 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV420_16, /**< 16 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV422_8,  /**< UYVY..UYVY, 8 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV422_10, /**< UYVY..UYVY, 10 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_YUV422_16, /**< UYVY..UYVY, 16 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_RGB_444,  /**< BGR..BGR, 4 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_RGB_555,  /**< BGR..BGR, 5 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_RGB_565,  /**< BGR..BGR, 5 bits B and R, 6 bits G */
+	IA_CSS_STREAM_FORMAT_RGB_666,  /**< BGR..BGR, 6 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_RGB_888,  /**< BGR..BGR, 8 bits per subpixel */
+	IA_CSS_STREAM_FORMAT_RAW_6,    /**< RAW data, 6 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_7,    /**< RAW data, 7 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_8,    /**< RAW data, 8 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_10,   /**< RAW data, 10 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_12,   /**< RAW data, 12 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_14,   /**< RAW data, 14 bits per pixel */
+	IA_CSS_STREAM_FORMAT_RAW_16,   /**< RAW data, 16 bits per pixel, which is
+					    not specified in CSI-MIPI standard*/
+	IA_CSS_STREAM_FORMAT_BINARY_8, /**< Binary byte stream, which is target at
+					    JPEG. */
+
+	/** 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.
+	 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT1,  /**< Generic Short Packet Code 1 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT2,  /**< Generic Short Packet Code 2 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT3,  /**< Generic Short Packet Code 3 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT4,  /**< Generic Short Packet Code 4 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT5,  /**< Generic Short Packet Code 5 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT6,  /**< Generic Short Packet Code 6 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT7,  /**< Generic Short Packet Code 7 */
+	IA_CSS_STREAM_FORMAT_GENERIC_SHORT8,  /**< Generic Short Packet Code 8 */
+
+	/** CSI2-MIPI specific format: YUV data.
+	 */
+	IA_CSS_STREAM_FORMAT_YUV420_8_SHIFT,  /**< YUV420 8-bit (Chroma Shifted Pixel Sampling) */
+	IA_CSS_STREAM_FORMAT_YUV420_10_SHIFT, /**< YUV420 8-bit (Chroma Shifted Pixel Sampling) */
+
+	/** CSI2-MIPI specific format: Generic long packet data
+	 */
+	IA_CSS_STREAM_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.
+	 */
+	IA_CSS_STREAM_FORMAT_USER_DEF1,  /**< User defined 8-bit data type 1 */
+	IA_CSS_STREAM_FORMAT_USER_DEF2,  /**< User defined 8-bit data type 2 */
+	IA_CSS_STREAM_FORMAT_USER_DEF3,  /**< User defined 8-bit data type 3 */
+	IA_CSS_STREAM_FORMAT_USER_DEF4,  /**< User defined 8-bit data type 4 */
+	IA_CSS_STREAM_FORMAT_USER_DEF5,  /**< User defined 8-bit data type 5 */
+	IA_CSS_STREAM_FORMAT_USER_DEF6,  /**< User defined 8-bit data type 6 */
+	IA_CSS_STREAM_FORMAT_USER_DEF7,  /**< User defined 8-bit data type 7 */
+	IA_CSS_STREAM_FORMAT_USER_DEF8,  /**< User defined 8-bit data type 8 */
+};
+
+#define	IA_CSS_STREAM_FORMAT_NUM	IA_CSS_STREAM_FORMAT_USER_DEF8
+
+unsigned int ia_css_util_input_format_bpp(
+	enum ia_css_stream_format format,
+	bool two_ppc);
+
+#endif /* __IA_CSS_STREAM_FORMAT_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream_public.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream_public.h
new file mode 100644
index 000000000000..2c8d9de10a59
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_stream_public.h
@@ -0,0 +1,582 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 ia_css_stream_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 ia_css_stream_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;
+
+#ifdef ISP2401
+	/* Currently, Android and Windows platforms interpret the binning_factor parameter
+	 * differently. In Android, the binning factor is expressed in the form
+	 * 2^N * 2^N, whereas in Windows platform, the binning factor is N*N
+	 * To use the Windows method of specification, the caller has to define
+	 * macro USE_WINDOWS_BINNING_FACTOR. This is for backward compatibility only
+	 * and will be deprecated. In the future,all platforms will use the N*N method
+	 */
+#endif
+	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 init_num_cont_raw_buf; /**< initial number of raw buffers to
+					     allocate */
+	unsigned 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 continous viewfinder for ZSL use case */
+	int32_t 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	IA_CSS_SUCCESS or the error code.
+ *
+ * This function will create a stream with a given configuration and given pipes.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or the error code.
+ *
+ * This function will destroy a given stream.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or the error code.
+ *
+ * This function will destroy a given stream.
+ */
+enum ia_css_err
+ia_css_stream_get_info(const struct ia_css_stream *stream,
+		       struct ia_css_stream_info *stream_info);
+
+/** @brief load (rebuild) a stream that was unloaded.
+ * @param[in]	stream The stream
+ * @return		IA_CSS_SUCCESS or the error code
+ *
+ * Rebuild a stream, including allocating structs, setting configuration and
+ * building the required pipes.
+ */
+enum ia_css_err
+ia_css_stream_load(struct ia_css_stream *stream);
+
+/** @brief Starts the stream.
+ * @param[in]	stream The stream.
+ * @return IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+ia_css_stream_start(struct ia_css_stream *stream);
+
+/** @brief Stop the stream.
+ * @param[in]	stream The stream.
+ * @return	IA_CSS_SUCCESS or the error code.
+ *
+ * NOTE: this function will send stop event to pipes belong to this
+ * stream but will not terminate threads.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS (no other errors are generated now)
+ *
+ * Destroy the stream and all the pipes related to it.
+ */
+enum ia_css_err
+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 ia_css_stream_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)
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or IA_CSS_ERR_INVALID_ARGUMENTS
+ *
+ * This function will return the maximum number of continuous RAW frames
+ * the system can support.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or error code upon error.
+ *
+ * Set the number of continuous frames to use during continuous modes.
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS or IA_CSS_ERR_INVALID_ARGUMENTS
+ *
+ * Get the currently set number of continuous frames
+ * to use during continuous modes.
+ */
+enum ia_css_err
+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			IA_CSS_SUCCESS 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).
+ */
+enum ia_css_err
+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			IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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 ia_css_stream_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		IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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		IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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 IA_CSS_SUCCESS 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
+ */
+enum ia_css_err
+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	IA_CSS_SUCCESS 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
+ */
+enum ia_css_err
+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 IA_CSS_SUCCESS 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.
+ */
+enum ia_css_err
+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/atomisp2/css2400/ia_css_timer.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_timer.h
new file mode 100644
index 000000000000..575bb28b4bec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_timer.h
@@ -0,0 +1,84 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 uint32_t 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 IA_CSS_SUCCESS if success
+*
+*/
+enum ia_css_err
+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/atomisp2/css2400/ia_css_tpg.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_tpg.h
new file mode 100644
index 000000000000..9238a3317a46
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_tpg.h
@@ -0,0 +1,78 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/ia_css_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_types.h
new file mode 100644
index 000000000000..5fec3d5c89d8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_types.h
@@ -0,0 +1,654 @@
+/* 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"
+#ifdef ISP2401
+#include "isp/kernels/tnr/tnr3/ia_css_tnr3_types.h"
+#endif
+#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 uint32_t ia_css_ptr;
+
+/** Generic resolution structure.
+ */
+struct ia_css_resolution {
+	uint32_t width;  /**< Width */
+	uint32_t height; /**< Height */
+};
+
+/** Generic coordinate structure.
+ */
+struct ia_css_coordinate {
+	int32_t x;	/**< Value of a coordinate on the horizontal axis */
+	int32_t 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 {
+	int32_t x; /**< horizontal motion (in pixels) */
+	int32_t 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 */
+	uint32_t   size;    /**< Disabled if 0 */
+};
+
+/** Host data descriptor */
+struct ia_css_host_data {
+	char      *address; /**< Host address */
+	uint32_t   size;    /**< Disabled if 0 */
+};
+
+/** ISP data descriptor */
+struct ia_css_isp_data {
+	uint32_t   address; /**< ISP address */
+	uint32_t   size;    /**< Disabled if 0 */
+};
+
+/** Shading Correction types. */
+enum ia_css_shading_correction_type {
+#ifndef ISP2401
+	IA_CSS_SHADING_CORRECTION_TYPE_1 /**< Shading Correction 1.0 (pipe 1.0 on ISP2300, pipe 2.2 on ISP2400) */
+#else
+	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) */
+#endif
+
+	/**< 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 {
+#ifndef ISP2401
+			uint32_t enable;	/**< Shading correction enabled.
+						     0:disabled, 1:enabled */
+			uint32_t num_hor_grids;	/**< Number of data points per line
+						     per color on shading table. */
+			uint32_t num_ver_grids;	/**< Number of lines of data points
+						     per color on shading table. */
+			uint32_t 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. */
+#else
+			uint32_t num_hor_grids;	/**< Number of data points per line per color on shading table. */
+			uint32_t num_ver_grids;	/**< Number of lines of data points per color on shading table. */
+			uint32_t 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. */
+#endif
+			uint32_t bayer_scale_hor_ratio_in;
+			uint32_t bayer_scale_hor_ratio_out;
+#ifndef ISP2401
+			/**< 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 */
+#else
+				/**< 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 */
+#endif
+			uint32_t bayer_scale_ver_ratio_in;
+			uint32_t bayer_scale_ver_ratio_out;
+#ifndef ISP2401
+			/**< 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 */
+			uint32_t 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. */
+			uint32_t 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. */
+#else
+				/**< 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 */
+			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. */
+			struct ia_css_resolution sensor_data_res_bqs;
+				/**< Sensor data size (in bqs) at shading correction.
+				     This is the size AFTER bayer scaling. */
+			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. */
+#endif
+		} type_1;
+
+		/**< More structures can be added here when more shading correction types will be added
+		     in the future. */
+	} info;
+};
+
+#ifndef ISP2401
+
+/** Default Shading Correction information of Shading Correction Type 1. */
+#define DEFAULT_SHADING_INFO_TYPE_1 \
+{ \
+	IA_CSS_SHADING_CORRECTION_TYPE_1,	/* type */ \
+	{					/* info */ \
+		{ \
+			0,	/* enable */ \
+			0,	/* num_hor_grids */ \
+			0,	/* num_ver_grids */ \
+			0,	/* bqs_per_grid_cell */ \
+			1,	/* bayer_scale_hor_ratio_in */ \
+			1,	/* bayer_scale_hor_ratio_out */ \
+			1,	/* bayer_scale_ver_ratio_in */ \
+			1,	/* bayer_scale_ver_ratio_out */ \
+			0,	/* sc_bayer_origin_x_bqs_on_shading_table */ \
+			0	/* sc_bayer_origin_y_bqs_on_shading_table */ \
+		} \
+	} \
+}
+
+#else
+
+/** Default Shading Correction information of Shading Correction Type 1. */
+#define DEFAULT_SHADING_INFO_TYPE_1 \
+{ \
+	IA_CSS_SHADING_CORRECTION_TYPE_1,	/* type */ \
+	{					/* info */ \
+		{ \
+			0,			/* num_hor_grids */ \
+			0,			/* num_ver_grids */ \
+			0,			/* bqs_per_grid_cell */ \
+			1,			/* bayer_scale_hor_ratio_in */ \
+			1,			/* bayer_scale_hor_ratio_out */ \
+			1,			/* bayer_scale_ver_ratio_in */ \
+			1,			/* bayer_scale_ver_ratio_out */ \
+			{0, 0},			/* isp_input_sensor_data_res_bqs */ \
+			{0, 0},			/* sensor_data_res_bqs */ \
+			{0, 0}			/* sensor_data_origin_bqs_on_sctbl */ \
+		} \
+	} \
+}
+
+#endif
+
+/** 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
+	  * @{
+	  */
+	uint32_t isp_in_width;
+	uint32_t 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 \
+{ \
+	0,				/* isp_in_width */ \
+	0,				/* isp_in_height */ \
+	DEFAULT_3A_GRID_INFO,		/* s3a_grid */ \
+	DEFAULT_DVS_GRID_INFO,		/* dvs_grid */ \
+	IA_CSS_VAMEM_TYPE_1		/* vamem_type */ \
+}
+
+/** 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 {
+	uint32_t enable; /**< To disable GDC, set this field to false. The
+			  coordinates fields can be set to NULL in this case. */
+	uint32_t height; /**< Table height */
+	uint32_t width;  /**< Table width */
+	uint16_t *coordinates_x[IA_CSS_MORPH_TABLE_NUM_PLANES];
+	/**< X coordinates that describe the sensor imperfection */
+	uint16_t *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 */
+	uint32_t width_y;
+	uint32_t height_y;
+	uint32_t width_uv;
+	uint32_t height_uv;
+	uint32_t *xcoords_y;
+	uint32_t *ycoords_y;
+	uint32_t *xcoords_uv;
+	uint32_t *ycoords_uv;
+};
+
+/**
+ * This specifies the coordinates (x,y)
+ */
+struct ia_css_point {
+	int32_t x; /**< x coordinate */
+	int32_t 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 {
+	uint32_t dx; /**< Horizontal zoom factor */
+	uint32_t 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 */
+	uint32_t enable_xnr;	       /**< Enable/disable XNR */
+	uint32_t 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 \
+{ \
+	IA_CSS_CAPTURE_MODE_PRIMARY,	/* mode (capture) */ \
+	false,				/* enable_xnr */ \
+	false,				/* enable_raw_output */ \
+	false				/* enable_capture_pp_bli */ \
+}
+
+
+/** 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 */
+
+#ifdef ISP2401
+	struct ia_css_tnr3_kernel_config         *tnr3_config;           /**< TNR3 config */
+#endif
+	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) */
+	uint32_t			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/atomisp2/css2400/ia_css_version.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_version.h
new file mode 100644
index 000000000000..48c59896e847
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_version.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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.
+ */
+enum ia_css_err
+ia_css_get_version(char *version, int max_size);
+
+#endif /* __IA_CSS_VERSION_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_version_data.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_version_data.h
new file mode 100644
index 000000000000..aad592cb86ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/ia_css_version_data.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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
+
+
+#ifndef ISP2401
+#define 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; "
+#else
+#define CSS_VERSION_STRING "REL:20150911_37.5_1652; API:2.1.20.9; GIT:irci___#ebf437d53a8951bb7ff6d13fdb7270dab393a92a; SDK:; USER:viedifw; "
+#endif
+
+
+#endif
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.c
new file mode 100644
index 000000000000..f7dd256b6f7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.c
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.h
new file mode 100644
index 000000000000..71587d85ff2d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2.host.h
@@ -0,0 +1,27 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_param.h
new file mode 100644
index 000000000000..dbab4d6c6cd5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_param.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t strength;
+};
+
+#endif /* __IA_CSS_AA_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_state.h
new file mode 100644
index 000000000000..cc404018b112
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_state.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_AA2_STATE_H
+
+#include "type_support.h"
+#include "vmem.h" /* for VMEM_ARRAY*/
+
+/* Denotes the maximum number of pixels per line that can be processed:
+* MAX_AA_VECTORS_PER_LINE  = maximum_line_width / ISP_NWAY */
+#ifndef MAX_AA_VECTORS_PER_LINE
+#error Please define MAX_AA_VECTORS_PER_LINE.
+#endif
+
+/* This uses 2 history lines for both y, u and v*/
+#define AA_STATE_Y_BUFFER_HEIGHT	2
+#define AA_STATE_UV_BUFFER_HEIGHT	2
+#define AA_STATE_Y_BUFFER_WIDTH		MAX_AA_VECTORS_PER_LINE
+/* The number of u and v elements is half y due to yuv420 downsampling. */
+#define AA_STATE_UV_BUFFER_WIDTH	(AA_STATE_Y_BUFFER_WIDTH/2)
+
+
+struct ia_css_isp_aa_vmem_state {
+	VMEM_ARRAY(y[AA_STATE_Y_BUFFER_HEIGHT], AA_STATE_Y_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(u[AA_STATE_UV_BUFFER_HEIGHT], AA_STATE_UV_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(v[AA_STATE_UV_BUFFER_HEIGHT], AA_STATE_UV_BUFFER_WIDTH*ISP_NWAY);
+};
+
+#endif /* __IA_CSS_AA2_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_types.h
new file mode 100644
index 000000000000..834eedbbeeff
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/aa/aa_2/ia_css_aa2_types.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t 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/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.c
new file mode 100644
index 000000000000..edc4f1ae6d5e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.c
@@ -0,0 +1,60 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	to->threshold = from->threshold;
+}
+
+void
+ia_css_anr_dump(
+	const struct sh_css_isp_anr_params *anr,
+	unsigned 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 level)
+{
+	ia_css_debug_dtrace(level,
+		"config.threshold=%d\n",
+		config->threshold);
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.h
new file mode 100644
index 000000000000..29566c07653c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr.host.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_anr_dump(
+	const struct sh_css_isp_anr_params *anr,
+	unsigned level);
+
+void
+ia_css_anr_debug_dtrace(
+	const struct ia_css_anr_config *config, unsigned level)
+;
+
+#endif /* __IA_CSS_ANR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_param.h
new file mode 100644
index 000000000000..2621b920c3dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_param.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t threshold;
+};
+
+#endif /* __IA_CSS_ANR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_types.h
new file mode 100644
index 000000000000..e205574098f2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_1.0/ia_css_anr_types.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t threshold; /**< Threshold */
+	int32_t thresholds[4*4*4];
+	int32_t factors[3];
+};
+
+#endif /* __IA_CSS_ANR_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.c
new file mode 100644
index 000000000000..b338c434453e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.c
@@ -0,0 +1,46 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 i;
+
+	(void)size;
+	for (i = 0; i < ANR_PARAM_SIZE; i++) {
+		unsigned 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 level)
+{
+	(void)config;
+	(void)level;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.h
new file mode 100644
index 000000000000..83c37e328591
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2.host.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_anr_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 level)
+;
+
+#endif /* __IA_CSS_ANR2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c
new file mode 100644
index 000000000000..2de51fe45623
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.c
@@ -0,0 +1,52 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h
new file mode 100644
index 000000000000..534119e064c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_table.host.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_types.h
new file mode 100644
index 000000000000..3832ada433ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr2_types.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int16_t data[13*64];
+};
+
+#endif /* __IA_CSS_ANR2_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr_param.h
new file mode 100644
index 000000000000..4a289853367a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/anr/anr_2/ia_css_anr_param.h
@@ -0,0 +1,27 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_load_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_load_param.h
new file mode 100644
index 000000000000..8e1f300bcd39
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_load_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_LOAD_PARAM_H
+#define __IA_CSS_BAYER_LOAD_PARAM_H
+
+#include "ia_css_bayer_ls_param.h"
+
+#endif /* __IA_CSS_BAYER_LOAD_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_ls_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_ls_param.h
new file mode 100644
index 000000000000..75ca7606b95c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_ls_param.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_LS_PARAM_H
+#define __IA_CSS_BAYER_LS_PARAM_H
+
+#include "type_support.h"
+#ifndef ISP2401
+
+#define NUM_BAYER_LS 2
+#define BAYER_IDX_GR 0
+#define BAYER_IDX_R 1
+#define BAYER_IDX_B 2
+#define BAYER_IDX_GB 3
+#define BAYER_QUAD_WIDTH 2
+#define BAYER_QUAD_HEIGHT 2
+#define NOF_BAYER_VECTORS 4
+
+/** bayer load/store */
+struct sh_css_isp_bayer_ls_isp_config {
+	uint32_t base_address[NUM_BAYER_LS];
+	uint32_t width[NUM_BAYER_LS];
+	uint32_t height[NUM_BAYER_LS];
+	uint32_t stride[NUM_BAYER_LS];
+};
+
+#else
+#include "../../io_ls/common/ia_css_common_io_types.h"
+#endif
+
+#endif /* __IA_CSS_BAYER_LS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_store_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_store_param.h
new file mode 100644
index 000000000000..f330be80efa6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bayer_ls/bayer_ls_1.0/ia_css_bayer_store_param.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STORE_PARAM_H
+#define __IA_CSS_BAYER_STORE_PARAM_H
+
+#include "ia_css_bayer_ls_param.h"
+
+
+#endif /* __IA_CSS_BAYER_STORE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.c
new file mode 100644
index 000000000000..99c80d2d8f11
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.c
@@ -0,0 +1,66 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#if !defined(HAS_NO_HMEM)
+
+#include "memory_access.h"
+#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 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);
+}
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.h
new file mode 100644
index 000000000000..cbb09299cf21
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh.host.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#endif /* __IA_CSS_BH_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_param.h
new file mode 100644
index 000000000000..b0a8ef3862e0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_param.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	int32_t y_coef_r;
+	int32_t y_coef_g;
+	int32_t y_coef_b;
+};
+
+/* This should be hmem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_bh_hmem_params {
+	uint32_t bh[ISP_HIST_COMPONENTS][IA_CSS_HMEM_BH_UNIT_SIZE];
+};
+
+#endif /* __IA_CSS_HB_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_types.h
new file mode 100644
index 000000000000..9ae27a9e0baa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bh/bh_2/ia_css_bh_types.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t hmem[ISP_HIST_COMPONENTS][IA_CSS_HMEM_BH_UNIT_SIZE];
+};
+
+#endif /* __IA_CSS_BH_TYPES_H */
+
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.c
new file mode 100644
index 000000000000..6d12e031e6fc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.c
@@ -0,0 +1,183 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 int32_t div_lut_nearests[BNLM_DIV_LUT_SIZE] = {
+	0, 454, 948, 1484, 2070, 2710, 3412, 4184, 5035, 5978, 7025, 8191
+};
+
+static const int32_t div_lut_slopes[BNLM_DIV_LUT_SIZE] = {
+	-7760, -6960, -6216, -5536, -4912, -4344, -3832, -3360, -2936, -2552, -2208, -2208
+};
+
+static const int32_t 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 int32_t *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 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/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.h
new file mode 100644
index 000000000000..b99c0644ab38
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm.host.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+#include "ia_css_bnlm_default.host.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 level);
+#endif
+
+#endif /* __IA_CSS_BNLM_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.c
new file mode 100644
index 000000000000..e2eb88c0f123
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.c
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_bnlm_types.h"
+
+const struct ia_css_bnlm_config default_bnlm_config = {
+
+	.rad_enable = true,
+	.rad_x_origin = 0,
+	.rad_y_origin = 0,
+	.avg_min_th = 127,
+	.max_min_th = 2047,
+
+	.exp_coeff_a = 6048,
+	.exp_coeff_b = 7828,
+	.exp_coeff_c = 0,
+	.exp_exponent = 3,
+
+	.nl_th = {2252, 2251, 2250},
+	.match_quality_max_idx = {2, 3, 3, 1},
+
+	.mu_root_lut_thr = {
+		26, 56, 128, 216, 462, 626, 932, 1108, 1480, 1564, 1824, 1896, 2368, 3428, 4560},
+	.mu_root_lut_val = {
+		384, 320, 320, 264, 248, 240, 224, 192, 192, 160, 160, 160, 136, 130, 96, 80},
+	.sad_norm_lut_thr = {
+		236, 328, 470, 774, 964, 1486, 2294, 3244, 4844, 6524, 6524, 6524, 6524, 6524, 6524},
+	.sad_norm_lut_val = {
+		8064, 7680, 7168, 6144, 5120, 3840, 2560, 2304, 1984, 1792, 1792, 1792, 1792, 1792, 1792, 1792},
+	.sig_detail_lut_thr = {
+		2936, 3354, 3943, 4896, 5230, 5682, 5996, 7299, 7299, 7299, 7299, 7299, 7299, 7299, 7299},
+	.sig_detail_lut_val = {
+		8191, 7680, 7168, 6144, 5120, 4608, 4224, 4032, 4032, 4032, 4032, 4032, 4032, 4032, 4032, 4032},
+	.sig_rad_lut_thr = {
+		18, 19, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20},
+	.sig_rad_lut_val = {
+		2560, 7168, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188, 8188},
+	.rad_pow_lut_thr = {
+		0, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013, 7013},
+	.rad_pow_lut_val = {
+		8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191},
+	.nl_0_lut_thr = {
+		1072, 7000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000},
+	.nl_0_lut_val = {
+		2560, 3072, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120, 5120},
+	.nl_1_lut_thr = {
+		624, 3224, 3392, 7424, 7424, 7424, 7424, 7424, 7424, 7424, 7424, 7424, 7424, 7424, 7424},
+	.nl_1_lut_val = {
+		3584, 4608, 5120, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144, 6144},
+	.nl_2_lut_thr = {
+		745, 2896, 3720, 6535, 7696, 8040, 8040, 8040, 8040, 8040, 8040, 8040, 8040, 8040, 8040},
+	.nl_2_lut_val = {
+		3584, 4608, 6144, 7168, 7936, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191},
+	.nl_3_lut_thr = {
+		4848, 4984, 5872, 6000, 6517, 6960, 7944, 8088, 8161, 8161, 8161, 8161, 8161, 8161, 8161},
+	.nl_3_lut_val = {
+		3072, 4104, 4608, 5120, 6144, 7168, 7680, 8128, 8191, 8191, 8191, 8191, 8191, 8191, 8191, 8191},
+
+};
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.h
new file mode 100644
index 000000000000..f18c8070abba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_default.host.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DEFAULT_HOST_H
+#define __IA_CSS_BNLM_DEFAULT_HOST_H
+
+#include "ia_css_bnlm_types.h"
+extern const struct ia_css_bnlm_config default_bnlm_config;
+
+#endif /* __IA_CSS_BNLM_DEFAULT_HOST_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_param.h
new file mode 100644
index 000000000000..2f4be43e594e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_param.h
@@ -0,0 +1,63 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+	int32_t rad_x_origin;
+	int32_t rad_y_origin;
+	int32_t avg_min_th;
+	int32_t max_min_th;
+
+	int32_t exp_coeff_a;
+	uint32_t exp_coeff_b;
+	int32_t exp_coeff_c;
+	uint32_t exp_exponent;
+};
+
+#endif /* __IA_CSS_BNLM_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_state.h
new file mode 100644
index 000000000000..79cce0e40e82
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_state.h
@@ -0,0 +1,31 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_BNLM_STATE_H
+
+
+#include "type_support.h"
+#include "vmem.h" /* for VMEM_ARRAY*/
+#include "bnlm.isp.h"
+
+struct bnlm_vmem_state {
+	/* State buffers required for BNLM */
+	VMEM_ARRAY(buf[BNLM_STATE_BUF_HEIGHT], BNLM_STATE_BUF_WIDTH*ISP_NWAY);
+};
+
+
+
+#endif /* __IA_CSS_BNLM_STATE_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_types.h
new file mode 100644
index 000000000000..219fb835cb26
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnlm/ia_css_bnlm_types.h
@@ -0,0 +1,106 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	int32_t		rad_x_origin;	/**< Initial x coordinate for a radius calculation */
+	int32_t		rad_y_origin;	/**< Initial x coordinate for a radius calculation */
+	/* a threshold for average of weights if this < Th, do not denoise pixel */
+	int32_t		avg_min_th;
+	/* minimum weight for denoising if max < th, do not denoise pixel */
+	int32_t		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
+	 * */
+	int32_t		exp_coeff_a;
+	uint32_t	exp_coeff_b;
+	int32_t		exp_coeff_c;
+	uint32_t	exp_exponent;
+	/**@}*/
+
+	int32_t nl_th[3];	/**< Detail thresholds */
+
+	/** Index for n-th maximum candidate weight for each detail group */
+	int32_t match_quality_max_idx[4];
+
+	/**@{*/
+	/** A lookup table for 1/sqrt(1+mu) approximation */
+	int32_t mu_root_lut_thr[15];
+	int32_t mu_root_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** A lookup table for SAD normalization */
+	int32_t sad_norm_lut_thr[15];
+	int32_t sad_norm_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** A lookup table that models a weight's dependency on textures */
+	int32_t sig_detail_lut_thr[15];
+	int32_t sig_detail_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** A lookup table that models a weight's dependency on a pixel's radial distance */
+	int32_t sig_rad_lut_thr[15];
+	int32_t sig_rad_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** A lookup table to control denoise power depending on a pixel's radial distance */
+	int32_t rad_pow_lut_thr[15];
+	int32_t rad_pow_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** Non linear transfer functions to calculate the blending coefficient depending on detail group */
+	/** detail group 0 */
+	/**@{*/
+	int32_t nl_0_lut_thr[15];
+	int32_t nl_0_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** detail group 1 */
+	int32_t nl_1_lut_thr[15];
+	int32_t nl_1_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** detail group 2 */
+	int32_t nl_2_lut_thr[15];
+	int32_t nl_2_lut_val[16];
+	/**@}*/
+	/**@{*/
+	/** detail group 3 */
+	int32_t nl_3_lut_thr[15];
+	int32_t nl_3_lut_val[16];
+	/**@}*/
+	/**@}*/
+};
+
+#endif /* __IA_CSS_BNLM_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c
new file mode 100644
index 000000000000..a7de6ecb950d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.c
@@ -0,0 +1,122 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h
new file mode 100644
index 000000000000..c94b366b8142
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2.host.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 level);
+#endif
+
+#endif /* __IA_CSS_BNR2_2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h
new file mode 100644
index 000000000000..6dec27a99d8f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_param.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t d_var_gain_r;
+	int32_t d_var_gain_g;
+	int32_t d_var_gain_b;
+	int32_t d_var_gain_slope_r;
+	int32_t d_var_gain_slope_g;
+	int32_t d_var_gain_slope_b;
+	int32_t n_var_gain_r;
+	int32_t n_var_gain_g;
+	int32_t n_var_gain_b;
+	int32_t n_var_gain_slope_r;
+	int32_t n_var_gain_slope_g;
+	int32_t n_var_gain_slope_b;
+	int32_t dir_thres;
+	int32_t dir_thres_w;
+	int32_t var_offset_coef;
+	int32_t dir_gain;
+	int32_t detail_gain;
+	int32_t detail_gain_divisor;
+	int32_t detail_level_offset;
+	int32_t d_var_th_min;
+	int32_t d_var_th_max;
+	int32_t n_var_th_min;
+	int32_t n_var_th_max;
+};
+
+#endif /* __IA_CSS_BNR2_2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h
new file mode 100644
index 000000000000..be80f705d8a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr2_2/ia_css_bnr2_2_types.h
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	int32_t d_var_gain_r;
+	int32_t d_var_gain_g;
+	int32_t d_var_gain_b;
+	/**@}*/
+	/**@{*/
+	/** Slope of Directional variance gain between dark and bright region */
+	int32_t d_var_gain_slope_r;
+	int32_t d_var_gain_slope_g;
+	int32_t d_var_gain_slope_b;
+	/**@}*/
+	/**@{*/
+	/** Non-Directional variance gain for R/G/B components in dark region */
+	int32_t n_var_gain_r;
+	int32_t n_var_gain_g;
+	int32_t n_var_gain_b;
+	/**@}*/
+	/**@{*/
+	/** Slope of Non-Directional variance gain between dark and bright region */
+	int32_t n_var_gain_slope_r;
+	int32_t n_var_gain_slope_g;
+	int32_t n_var_gain_slope_b;
+	/**@}*/
+
+	int32_t dir_thres;		/**< Threshold for directional filtering */
+	int32_t dir_thres_w;		/**< Threshold width for directional filtering */
+	int32_t var_offset_coef;	/**< Variance offset coefficient */
+	int32_t dir_gain;		/**< Gain for directional coefficient */
+	int32_t detail_gain;		/**< Gain for low contrast texture control */
+	int32_t detail_gain_divisor;	/**< Gain divisor for low contrast texture control */
+	int32_t detail_level_offset;	/**< Bias value for low contrast texture control */
+	int32_t d_var_th_min;		/**< Minimum clipping value for directional variance*/
+	int32_t d_var_th_max;		/**< Maximum clipping value for diretional variance*/
+	int32_t n_var_th_min;		/**< Minimum clipping value for non-directional variance*/
+	int32_t 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/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c
new file mode 100644
index 000000000000..d1baca54c3ad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.c
@@ -0,0 +1,64 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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((unsigned)16384, 16, SH_CSS_BAYER_BITS);
+}
+
+void
+ia_css_bnr_dump(
+	const struct sh_css_isp_bnr_params *bnr,
+	unsigned 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/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h
new file mode 100644
index 000000000000..ccd2abc60537
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr.host.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_bnr_dump(
+	const struct sh_css_isp_bnr_params *bnr,
+	unsigned level);
+
+#endif /* __IA_CSS_DP_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h
new file mode 100644
index 000000000000..331e05885ef4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/bnr/bnr_1.0/ia_css_bnr_param.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t gain_all;
+	int32_t gain_dir;
+	int32_t threshold_low;
+	int32_t threshold_width_log2;
+	int32_t threshold_width;
+	int32_t clip;
+};
+
+#endif /* __IA_CSS_BNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c
new file mode 100644
index 000000000000..d14fd8fc08b1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.c
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h
new file mode 100644
index 000000000000..6f00d280b7d6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr.host.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h
new file mode 100644
index 000000000000..c1af207cbf9a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_param.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_state.h
new file mode 100644
index 000000000000..795fba76bb20
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_1.0/ia_css_cnr_state.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_CNR_STATE_H
+
+#include "type_support.h"
+
+#include "vmem.h"
+
+typedef struct
+{
+  VMEM_ARRAY(u, ISP_NWAY);
+  VMEM_ARRAY(v, ISP_NWAY);
+} s_cnr_buf;
+
+/* CNR (color noise reduction) */
+struct sh_css_isp_cnr_vmem_state {
+	s_cnr_buf cnr_buf[2][MAX_VECTORS_PER_BUF_LINE/2];
+};
+
+#endif /* __IA_CSS_CNR_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c
new file mode 100644
index 000000000000..4b4b2b715407
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.c
@@ -0,0 +1,76 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 level);
+
+void
+ia_css_cnr_debug_dtrace(
+	const struct ia_css_cnr_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h
new file mode 100644
index 000000000000..abcf0eba706f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2.host.h
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_cnr_dump(
+	const struct sh_css_isp_cnr_params *cnr,
+	unsigned level);
+
+void
+ia_css_cnr_debug_dtrace(
+	const struct ia_css_cnr_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h
new file mode 100644
index 000000000000..d6f490e26c94
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_param.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t coring_u;
+	int32_t coring_v;
+	int32_t sense_gain_vy;
+	int32_t sense_gain_vu;
+	int32_t sense_gain_vv;
+	int32_t sense_gain_hy;
+	int32_t sense_gain_hu;
+	int32_t sense_gain_hv;
+};
+
+#endif /* __IA_CSS_CNR2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h
new file mode 100644
index 000000000000..6df6c2be9a70
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr2_types.h
@@ -0,0 +1,55 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t coring_u;	/**< Coring level of U.
+				u0.13, [0,8191], default/ineffective 0 */
+	uint16_t coring_v;	/**< Coring level of V.
+				u0.13, [0,8191], default/ineffective 0 */
+	uint16_t sense_gain_vy;	/**< Sensitivity of horizontal edge of Y.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	uint16_t sense_gain_vu;	/**< Sensitivity of horizontal edge of U.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	uint16_t sense_gain_vv;	/**< Sensitivity of horizontal edge of V.
+				u13.0, [0,8191], default 100, ineffective 8191 */
+	uint16_t sense_gain_hy;	/**< Sensitivity of vertical edge of Y.
+				u13.0, [0,8191], default 50, ineffective 8191 */
+	uint16_t sense_gain_hu;	/**< Sensitivity of vertical edge of U.
+				u13.0, [0,8191], default 50, ineffective 8191 */
+	uint16_t 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/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_param.h
new file mode 100644
index 000000000000..56651ba62598
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CNRX_PARAM_H
+#define __IA_CSS_CNRX_PARAM_H
+
+#include  "ia_css_cnr2_param.h"
+
+#endif /* __IA_CSS_CNRX_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_state.h
new file mode 100644
index 000000000000..e533e2fa8cd5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/cnr/cnr_2/ia_css_cnr_state.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_CNR2_STATE_H
+
+#include "type_support.h"
+#include "vmem.h"
+
+typedef struct
+{
+  VMEM_ARRAY(y, (MAX_VECTORS_PER_BUF_LINE/2)*ISP_NWAY);
+  VMEM_ARRAY(u, (MAX_VECTORS_PER_BUF_LINE/2)*ISP_NWAY);
+  VMEM_ARRAY(v, (MAX_VECTORS_PER_BUF_LINE/2)*ISP_NWAY);
+} s_cnr_buf;
+
+/* CNR (color noise reduction) */
+struct sh_css_isp_cnr_vmem_state {
+	s_cnr_buf cnr_buf;
+};
+
+#endif /* __IA_CSS_CNR2_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c
new file mode 100644
index 000000000000..8f25ee180cda
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.c
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h
new file mode 100644
index 000000000000..da7a0a034a71
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion.host.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#ifdef ISP2401
+/* workaround until code generation in isp_kernelparameters.host.c is fixed */
+#define ia_css_conversion_par_encode(to, from, size) ia_css_conversion_encode(to, from, size)
+#endif
+#endif /* __IA_CSS_CONVERSION_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h
new file mode 100644
index 000000000000..301d506f447e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_param.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t en;
+	uint32_t dummy0;
+	uint32_t dummy1;
+	uint32_t dummy2;
+};
+
+#endif /* __IA_CSS_CONVERSION_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h
new file mode 100644
index 000000000000..3f11442500f0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/conversion/conversion_1.0/ia_css_conversion_types.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t en;     /**< en parameter */
+	uint32_t dummy0; /**< dummy0 dummy parameter 0 */
+	uint32_t dummy1; /**< dummy1 dummy parameter 1 */
+	uint32_t dummy2; /**< dummy2 dummy parameter 2 */
+};
+
+#endif /* __IA_CSS_CONVERSION_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c
new file mode 100644
index 000000000000..45e1ea8b1fb0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.c
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	to->enable = from->enable;
+}
+
+void
+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;
+
+	ia_css_configure_copy_output(binary, &config);
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h
new file mode 100644
index 000000000000..3eb77365f8d0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output.host.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+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/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h
new file mode 100644
index 000000000000..622d9181e13f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/copy_output/copy_output_1.0/ia_css_copy_output_param.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t enable;
+};
+
+#endif /* __IA_CSS_COPY_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.c
new file mode 100644
index 000000000000..92905220d862
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.c
@@ -0,0 +1,64 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	to->crop_pos = from->crop_pos;
+}
+
+void
+ia_css_crop_config(
+	struct sh_css_isp_crop_isp_config *to,
+	const struct ia_css_crop_configuration  *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, from->info);
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	assert (elems_a % to->port_b.elems == 0);
+}
+
+void
+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;
+
+	ia_css_configure_crop(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.h
new file mode 100644
index 000000000000..9c1a4c7cac98
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop.host.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_crop_config(
+	struct sh_css_isp_crop_isp_config      *to,
+	const struct ia_css_crop_configuration *from,
+	unsigned size);
+
+void
+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/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_param.h
new file mode 100644
index 000000000000..8bfc8dad37a8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_param.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t 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/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_types.h
new file mode 100644
index 000000000000..8091ad4d4602
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/crop/crop_1.0/ia_css_crop_types.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.c
new file mode 100644
index 000000000000..9f94ef1de572
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.c
@@ -0,0 +1,132 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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 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 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/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.h
new file mode 100644
index 000000000000..eb10d8a5709d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc.host.h
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_csc_encode(
+	struct sh_css_isp_csc_params *to,
+	const struct ia_css_cc_config *from,
+	unsigned size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_cc_dump(
+	const struct sh_css_isp_csc_params *csc, unsigned level,
+	const char *name);
+
+void
+ia_css_csc_dump(
+	const struct sh_css_isp_csc_params *csc,
+	unsigned level);
+
+void
+ia_css_cc_config_debug_dtrace(
+	const struct ia_css_cc_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_param.h
new file mode 100644
index 000000000000..0b054a939baf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_param.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t	m_shift;
+	int16_t		m00;
+	int16_t		m01;
+	int16_t		m02;
+	int16_t		m10;
+	int16_t		m11;
+	int16_t		m12;
+	int16_t		m20;
+	int16_t		m21;
+	int16_t		m22;
+};
+
+
+#endif /* __IA_CSS_CSC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_types.h
new file mode 100644
index 000000000000..54ced072467f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/csc/csc_1.0/ia_css_csc_types.h
@@ -0,0 +1,78 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t fraction_bits;/**< Fractional bits of matrix.
+					u8.0, [0,13] */
+	int32_t 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/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c
new file mode 100644
index 000000000000..e27648c46a25
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.c
@@ -0,0 +1,120 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL);
+	assert(shift != NULL);
+
+	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 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 level);
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h
new file mode 100644
index 000000000000..d943aff28152
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5.host.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_ctc_dump(
+	const struct sh_css_isp_ctc_params *ctc,
+	unsigned level);
+
+#endif /* __IA_CSS_CTC1_5_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h
new file mode 100644
index 000000000000..8d9ac2b1832c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc1_5_param.h
@@ -0,0 +1,46 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t y0;
+	int32_t y1;
+	int32_t y2;
+	int32_t y3;
+	int32_t y4;
+	int32_t y5;
+	int32_t ce_gain_exp;
+	int32_t x1;
+	int32_t x2;
+	int32_t x3;
+	int32_t x4;
+	int32_t dydx0;
+	int32_t dydx0_shift;
+	int32_t dydx1;
+	int32_t dydx1_shift;
+	int32_t dydx2;
+	int32_t dydx2_shift;
+	int32_t dydx3;
+	int32_t dydx3_shift;
+	int32_t dydx4;
+	int32_t dydx4_shift;
+};
+
+#endif /* __IA_CSS_CTC1_5_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc_param.h
new file mode 100644
index 000000000000..dcd471f9bd66
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc1_5/ia_css_ctc_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_CTCX_PARAM_H
+#define __IA_CSS_CTCX_PARAM_H
+
+#include "ia_css_ctc1_5_param.h"
+
+#endif /* __IA_CSS_CTCX_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c
new file mode 100644
index 000000000000..07bd24edc7bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.c
@@ -0,0 +1,156 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 paramater 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 i, j;
+	const unsigned shffl_blck = 4;
+	const unsigned lenght_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 < lenght_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/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h
new file mode 100644
index 000000000000..3733aee24dcd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2.host.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h
new file mode 100644
index 000000000000..c66e823618f6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_param.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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*/
+	int32_t uv_y0;
+	int32_t uv_y1;
+
+	/** Kneepoints by UV(Chroma)- uv_x0 and uv_x1*/
+	int32_t uv_x0;
+	int32_t uv_x1;
+
+	/** Slope of line interconnecting uv_x0 -> uv_x1*/
+	int32_t uv_dydx;
+
+};
+#endif /* __IA_CSS_CTC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h
new file mode 100644
index 000000000000..7b75f01e2ad2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc2/ia_css_ctc2_types.h
@@ -0,0 +1,55 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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)
+	*/
+	int32_t y_y0;
+	int32_t y_y1;
+	int32_t y_y2;
+	int32_t y_y3;
+	int32_t y_y4;
+	int32_t 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
+	*/
+	int32_t y_x1;
+	int32_t y_x2;
+	int32_t y_x3;
+	int32_t y_x4;
+	/** Gains by UV(Chroma) under threholds uv_x0 and uv_x1
+	*   --default/ineffective value: 4096(0.5f)
+	*/
+	int32_t uv_y0;
+	int32_t uv_y1;
+	/** Minimum and Maximum Thresholds by UV(Chroma)- uv_x0 and uv_x1
+	*   --default/ineffective value: n/a
+	*/
+	int32_t uv_x0;
+	int32_t uv_x1;
+	};
+
+#endif /* __IA_CSS_CTC2_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c
new file mode 100644
index 000000000000..7c1a367918a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.c
@@ -0,0 +1,63 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h
new file mode 100644
index 000000000000..bec52a6519f9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc.host.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_ctc_debug_dtrace(
+	const struct ia_css_ctc_config *config, unsigned level)
+;
+
+#endif /* __IA_CSS_CTC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h
new file mode 100644
index 000000000000..6e88ad3d2420
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_param.h
@@ -0,0 +1,44 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+#if defined(HAS_VAMEM_VERSION_2)
+#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
+#elif defined(HAS_VAMEM_VERSION_1)
+#define SH_CSS_ISP_CTC_TABLE_SIZE_LOG2       IA_CSS_VAMEM_1_CTC_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_CTC_TABLE_SIZE            IA_CSS_VAMEM_1_CTC_TABLE_SIZE
+#else
+#error "VAMEM should be {VERSION1, VERSION2}" 
+#endif
+
+#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 {
+	uint16_t ctc[SH_CSS_ISP_CTC_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_CTC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c
new file mode 100644
index 000000000000..edf85aba7716
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.c
@@ -0,0 +1,215 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 <string_support.h> /* memcpy */
+#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;
+
+#if defined(HAS_VAMEM_VERSION_2)
+
+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
+};
+
+#elif defined(HAS_VAMEM_VERSION_1)
+
+/* Default Parameters */
+static const uint16_t
+default_ctc_table_data[IA_CSS_VAMEM_1_CTC_TABLE_SIZE] = {
+		0, 0, 256, 384, 384, 497, 765, 806,
+		837, 851, 888, 901, 957, 981, 993, 1001,
+		1011, 1029, 1028, 1039, 1062, 1059, 1073, 1080,
+		1083, 1085, 1085, 1098, 1080, 1084, 1085, 1093,
+		1078, 1073, 1070, 1069, 1077, 1066, 1072, 1063,
+		1053, 1044, 1046, 1053, 1039, 1028, 1025, 1024,
+		1012, 1013, 1016, 996, 992, 990, 990, 980,
+		969, 968, 961, 955, 951, 949, 933, 930,
+		929, 925, 921, 916, 906, 901, 895, 893,
+		886, 877, 872, 869, 866, 861, 857, 849,
+		845, 838, 836, 832, 823, 821, 815, 813,
+		809, 805, 796, 793, 790, 785, 784, 778,
+		772, 768, 766, 763, 758, 752, 749, 745,
+		741, 740, 736, 730, 726, 724, 723, 718,
+		711, 709, 706, 704, 701, 698, 691, 689,
+		688, 683, 683, 678, 675, 673, 671, 669,
+		666, 663, 661, 660, 656, 656, 653, 650,
+		648, 647, 646, 643, 639, 638, 637, 635,
+		633, 632, 629, 627, 626, 625, 622, 621,
+		618, 618, 614, 614, 612, 609, 606, 606,
+		603, 600, 600, 597, 594, 591, 590, 586,
+		582, 581, 578, 575, 572, 569, 563, 560,
+		557, 554, 551, 548, 545, 539, 536, 533,
+		529, 527, 524, 519, 516, 513, 510, 507,
+		504, 501, 498, 493, 491, 488, 485, 484,
+		480, 476, 474, 471, 467, 466, 464, 460,
+		459, 455, 453, 449, 447, 446, 443, 441,
+		438, 435, 432, 432, 429, 427, 426, 422,
+		419, 418, 416, 414, 412, 410, 408, 406,
+		404, 402, 401, 398, 397, 395, 393, 390,
+		389, 388, 387, 384, 382, 380, 378, 377,
+		376, 375, 372, 370, 368, 368, 366, 364,
+		363, 361, 360, 358, 357, 355, 354, 352,
+		351, 350, 349, 346, 345, 344, 344, 342,
+		340, 339, 337, 337, 336, 335, 333, 331,
+		330, 329, 328, 326, 326, 324, 324, 322,
+		321, 320, 318, 318, 318, 317, 315, 313,
+		312, 311, 311, 310, 308, 307, 306, 306,
+		304, 304, 302, 301, 300, 300, 299, 297,
+		297, 296, 296, 294, 294, 292, 291, 291,
+		291, 290, 288, 287, 286, 286, 287, 285,
+		284, 283, 282, 282, 281, 281, 279, 278,
+		278, 278, 276, 276, 275, 274, 274, 273,
+		271, 270, 269, 268, 268, 267, 265, 262,
+		261, 260, 260, 259, 257, 254, 252, 252,
+		251, 251, 249, 246, 245, 244, 243, 242,
+		240, 239, 239, 237, 235, 235, 233, 231,
+		232, 230, 229, 226, 225, 224, 225, 224,
+		223, 220, 219, 219, 218, 217, 217, 214,
+		213, 213, 212, 211, 209, 209, 209, 208,
+		206, 205, 204, 203, 204, 203, 201, 200,
+		199, 197, 198, 198, 197, 195, 194, 194,
+		193, 192, 192, 191, 189, 190, 189, 188,
+		186, 187, 186, 185, 185, 184, 183, 181,
+		183, 182, 181, 180, 179, 178, 178, 178,
+		177, 176, 175, 176, 175, 174, 174, 173,
+		172, 173, 172, 171, 170, 170, 169, 169,
+		169, 168, 167, 166, 167, 167, 166, 165,
+		164, 164, 164, 163, 164, 163, 162, 163,
+		162, 161, 160, 161, 160, 160, 160, 159,
+		158, 157, 158, 158, 157, 157, 156, 156,
+		156, 156, 155, 155, 154, 154, 154, 154,
+		154, 153, 152, 153, 152, 152, 151, 152,
+		151, 152, 151, 150, 150, 149, 149, 150,
+		149, 149, 148, 148, 148, 149, 148, 147,
+		146, 146, 147, 146, 147, 146, 145, 146,
+		146, 145, 144, 145, 144, 145, 144, 144,
+		143, 143, 143, 144, 143, 142, 142, 142,
+		142, 142, 142, 141, 141, 141, 141, 140,
+		140, 141, 140, 140, 141, 140, 139, 139,
+		139, 140, 139, 139, 138, 138, 137, 139,
+		138, 138, 138, 137, 138, 137, 137, 137,
+		137, 136, 137, 136, 136, 136, 136, 135,
+		136, 135, 135, 135, 135, 136, 135, 135,
+		134, 134, 133, 135, 134, 134, 134, 133,
+		134, 133, 134, 133, 133, 132, 133, 133,
+		132, 133, 132, 132, 132, 132, 131, 131,
+		131, 132, 131, 131, 130, 131, 130, 132,
+		131, 130, 130, 129, 130, 129, 130, 129,
+		129, 129, 130, 129, 128, 128, 128, 128,
+		129, 128, 128, 127, 127, 128, 128, 127,
+		127, 126, 126, 127, 127, 126, 126, 126,
+		127, 126, 126, 126, 125, 125, 126, 125,
+		125, 124, 124, 124, 125, 125, 124, 124,
+		123, 124, 124, 123, 123, 122, 122, 122,
+		122, 122, 121, 120, 120, 119, 118, 118,
+		118, 117, 117, 116, 115, 115, 115, 114,
+		114, 113, 113, 112, 111, 111, 111, 110,
+		110, 109, 109, 108, 108, 108, 107, 107,
+		106, 106, 105, 105, 105, 104, 104, 103,
+		103, 102, 102, 102, 102, 101, 101, 100,
+		100, 99, 99, 99, 99, 99, 99, 98,
+		97, 98, 97, 97, 97, 96, 96, 95,
+		96, 95, 96, 95, 95, 94, 94, 95,
+		94, 94, 94, 93, 93, 92, 93, 93,
+		93, 93, 92, 92, 91, 92, 92, 92,
+		91, 91, 90, 90, 91, 91, 91, 90,
+		90, 90, 90, 91, 90, 90, 90, 89,
+		89, 89, 90, 89, 89, 89, 89, 89,
+		88, 89, 89, 88, 88, 88, 88, 87,
+		89, 88, 88, 88, 88, 88, 87, 88,
+		88, 88, 87, 87, 87, 87, 87, 88,
+		87, 87, 87, 87, 87, 87, 88, 87,
+		87, 87, 87, 86, 86, 87, 87, 87,
+		87, 86, 86, 86, 87, 87, 86, 87,
+		86, 86, 86, 87, 87, 86, 86, 86,
+		86, 86, 87, 87, 86, 85, 85, 85,
+		84, 85, 85, 84, 84, 83, 83, 82,
+		82, 82, 81, 81, 80, 79, 79, 79,
+		78, 77, 77, 76, 76, 76, 75, 74,
+		74, 74, 73, 73, 72, 71, 71, 71,
+		70, 70, 69, 69, 68, 68, 67, 67,
+		67, 66, 66, 65, 65, 64, 64, 63,
+		62, 62, 62, 61, 60, 60, 59, 59,
+		58, 58, 57, 57, 56, 56, 56, 55,
+		55, 54, 55, 55, 54, 53, 53, 52,
+		53, 53, 52, 51, 51, 50, 51, 50,
+		49, 49, 50, 49, 49, 48, 48, 47,
+		47, 48, 46, 45, 45, 45, 46, 45,
+		45, 44, 45, 45, 45, 43, 42, 42,
+		41, 43, 41, 40, 40, 39, 40, 41,
+		39, 39, 39, 39, 39, 38, 35, 35,
+		34, 37, 36, 34, 33, 33, 33, 35,
+		34, 32, 32, 31, 32, 30, 29, 26,
+		25, 25, 27, 26, 23, 23, 23, 25,
+		24, 24, 22, 21, 20, 19, 16, 14,
+		13, 13, 13, 10, 9, 7, 7, 7,
+		12, 12, 12, 7, 0, 0, 0, 0
+};
+
+#else
+#error "VAMEM version must be one of {VAMEM_VERSION_1, VAMEM_VERSION_2}"
+#endif
+
+void
+ia_css_config_ctc_table(void)
+{
+#if defined(HAS_VAMEM_VERSION_2)
+	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;
+#else
+	memcpy(default_ctc_table.data.vamem_1, default_ctc_table_data,
+	       sizeof(default_ctc_table_data));
+	default_ctc_table.vamem_type     = 1IA_CSS_VAMEM_TYPE_1;
+#endif
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h
new file mode 100644
index 000000000000..a350dec8b4ad
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_table.host.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h
new file mode 100644
index 000000000000..1da215bb966d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ctc/ctc_1.0/ia_css_ctc_types.h
@@ -0,0 +1,110 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+
+/** @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           (1U<<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 {
+	uint16_t y0;	/**< 1st kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t y1;	/**< 2nd kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t y2;	/**< 3rd kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t y3;	/**< 4th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t y4;	/**< 5th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t y5;	/**< 6th kneepoint gain.
+				u[ce_gain_exp].[13-ce_gain_exp], [0,8191],
+				default/ineffective 4096(0.5) */
+	uint16_t ce_gain_exp;	/**< Common exponent of y-axis gain.
+				u8.0, [0,13],
+				default/ineffective 1 */
+	uint16_t x1;	/**< 2nd kneepoint luma.
+				u0.13, [0,8191], constraints: 0<x1<x2,
+				default/ineffective 1024 */
+	uint16_t x2;	/**< 3rd kneepoint luma.
+				u0.13, [0,8191], constraints: x1<x2<x3,
+				default/ineffective 2048 */
+	uint16_t x3;	/**< 4th kneepoint luma.
+				u0.13, [0,8191], constraints: x2<x3<x4,
+				default/ineffective 6144 */
+	uint16_t x4;	/**< 5tn kneepoint luma.
+				u0.13, [0,8191], constraints: x3<x4<8191,
+				default/ineffective 7168 */
+};
+
+union ia_css_ctc_data {
+	uint16_t vamem_1[IA_CSS_VAMEM_1_CTC_TABLE_SIZE];
+	uint16_t 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/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.c
new file mode 100644
index 000000000000..fbab2f1c396c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.c
@@ -0,0 +1,79 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.h
new file mode 100644
index 000000000000..5dd6f06f2bf1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de.host.h
@@ -0,0 +1,44 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_de_dump(
+	const struct sh_css_isp_de_params *de,
+	unsigned level);
+
+void
+ia_css_de_debug_dtrace(
+	const struct ia_css_de_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_param.h
new file mode 100644
index 000000000000..833c80afc7a8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_param.h
@@ -0,0 +1,27 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t pixelnoise;
+	int32_t c1_coring_threshold;
+	int32_t c2_coring_threshold;
+};
+
+#endif /* __IA_CSS_DE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_state.h
new file mode 100644
index 000000000000..d64511763436
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_state.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_DE_STATE_H
+
+#include "type_support.h"
+#include "vmem.h"
+
+/* DE (Demosaic) */
+struct sh_css_isp_de_vmem_state {
+	VMEM_ARRAY(de_buf[4], MAX_VECTORS_PER_BUF_LINE*ISP_NWAY);
+};
+
+#endif /* __IA_CSS_DE_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_types.h
new file mode 100644
index 000000000000..525c838d5a99
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_1.0/ia_css_de_types.h
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.c
new file mode 100644
index 000000000000..a5247a57bafb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.c
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 level);
+
+void
+ia_css_ecd_debug_dtrace(
+	const struct ia_css_ecd_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.h
new file mode 100644
index 000000000000..f7cd8448cb30
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2.host.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_ecd_dump(
+	const struct sh_css_isp_ecd_params *ecd,
+	unsigned level);
+
+void
+ia_css_ecd_debug_dtrace(
+	const struct ia_css_ecd_config *config, unsigned level);
+
+#endif /* __IA_CSS_DE2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_param.h
new file mode 100644
index 000000000000..ea2da73a4927
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_param.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t zip_strength;
+	int32_t fc_strength;
+	int32_t fc_debias;
+};
+
+#endif /* __IA_CSS_DE2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_types.h
new file mode 100644
index 000000000000..eac1b2779857
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de2_types.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t zip_strength;	/**< Strength of zipper reduction.
+				u0.13, [0,8191],
+				default 5489(0.67), ineffective 0 */
+	uint16_t fc_strength;	/**< Strength of false color reduction.
+				u0.13, [0,8191],
+				default 8191(almost 1.0), ineffective 0 */
+	uint16_t 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/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_param.h
new file mode 100644
index 000000000000..59af9523604d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DEX_PARAM_H
+#define __IA_CSS_DEX_PARAM_H
+
+#include "ia_css_de2_param.h"
+
+#endif /* __IA_CSS_DEX_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_state.h
new file mode 100644
index 000000000000..f2c65ba58983
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/de/de_2/ia_css_de_state.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_DE2_STATE_H
+
+/* Reuse DE1 states */
+#include "../de_1.0/ia_css_de_state.h"
+
+#endif /* __IA_CSS_DE2_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.c
new file mode 100644
index 000000000000..b1f9dc8d662d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.c
@@ -0,0 +1,132 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+#ifdef ISP2401
+/* 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
+};
+#endif
+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 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 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 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/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.h
new file mode 100644
index 000000000000..db21814ad3db
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp.host.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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;
+#ifdef ISP2401
+extern const struct ia_css_dp_config default_dp_10bpp_config;
+#endif
+
+void
+ia_css_dp_encode(
+	struct sh_css_isp_dp_params *to,
+	const struct ia_css_dp_config *from,
+	unsigned size);
+
+void
+ia_css_dp_dump(
+	const struct sh_css_isp_dp_params *dp,
+	unsigned level);
+
+void
+ia_css_dp_debug_dtrace(
+	const struct ia_css_dp_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_param.h
new file mode 100644
index 000000000000..fc9035a98d92
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_param.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t threshold_single;
+	int32_t threshold_2adjacent;
+	int32_t gain;
+	int32_t coef_rr_gr;
+	int32_t coef_rr_gb;
+	int32_t coef_bb_gb;
+	int32_t coef_bb_gr;
+	int32_t coef_gr_rr;
+	int32_t coef_gr_bb;
+	int32_t coef_gb_bb;
+	int32_t coef_gb_rr;
+};
+
+#endif /* __IA_CSS_DP_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_state.h
new file mode 100644
index 000000000000..f832b3697908
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_state.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_DP_STATE_H
+
+#include "type_support.h"
+
+#include "vmem.h"
+#ifndef ISP2401
+#if NEED_BDS_OTHER_THAN_1_00
+#else
+#if ENABLE_FIXED_BAYER_DS
+#endif
+#define MAX_VECTORS_PER_DP_LINE MAX_VECTORS_PER_BUF_INPUT_LINE
+#else
+#define MAX_VECTORS_PER_DP_LINE MAX_VECTORS_PER_BUF_LINE
+#endif
+
+/* DP (Defect Pixel Correction) */
+struct sh_css_isp_dp_vmem_state {
+	VMEM_ARRAY(dp_buf[4], MAX_VECTORS_PER_DP_LINE*ISP_NWAY);
+};
+
+#endif /* __IA_CSS_DP_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_types.h
new file mode 100644
index 000000000000..b5d7b6b175b6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dp/dp_1.0/ia_css_dp_types.h
@@ -0,0 +1,50 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	uint32_t gr;	/* unsigned <integer_bits>.<16-integer_bits> */
+	uint32_t r;	/* unsigned <integer_bits>.<16-integer_bits> */
+	uint32_t b;	/* unsigned <integer_bits>.<16-integer_bits> */
+	uint32_t gb;	/* unsigned <integer_bits>.<16-integer_bits> */
+};
+
+#endif /* __IA_CSS_DP_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.c
new file mode 100644
index 000000000000..bc14b85cf952
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.c
@@ -0,0 +1,65 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 level)
+{
+	(void)config;
+	(void)level;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.h
new file mode 100644
index 000000000000..641564b4af8e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2.host.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+#include "ia_css_dpc2_default.host.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 level);
+#endif
+
+#endif /* __IA_CSS_DPC2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.c
new file mode 100644
index 000000000000..c102601cc635
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.c
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_types.h"
+
+const struct ia_css_dpc2_config default_dpc2_config = {
+	.metric1 = 1638,
+	.metric2 =  128,
+	.metric3 = 1638,
+	.wb_gain_gr = 512,
+	.wb_gain_r  = 512,
+	.wb_gain_b  = 512,
+	.wb_gain_gb = 512
+};
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.h
new file mode 100644
index 000000000000..a1527ce3eddc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_default.host.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DEFAULT_HOST_H
+#define __IA_CSS_DPC2_DEFAULT_HOST_H
+
+#include "ia_css_dpc2_types.h"
+
+extern const struct ia_css_dpc2_config default_dpc2_config;
+
+#endif /* __IA_CSS_DPC2_DEFAULT_HOST_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_param.h
new file mode 100644
index 000000000000..ef668d54fe16
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_param.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t metric1;
+	int32_t metric2;
+	int32_t metric3;
+	int32_t wb_gain_gr;
+	int32_t wb_gain_r;
+	int32_t wb_gain_b;
+	int32_t wb_gain_gb;
+};
+
+#endif /* __IA_CSS_DPC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_state.h
new file mode 100644
index 000000000000..cbf1e81e83a6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_state.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_DPC2_STATE_H
+
+#include "type_support.h"
+#include "vmem.h" /* for VMEM_ARRAY*/
+
+#include "ia_css_dpc2_param.h"
+
+struct sh_css_isp_dpc2_vmem_state {
+	VMEM_ARRAY(dpc2_input_lines[DPC2_STATE_INPUT_BUFFER_HEIGHT], DPC2_STATE_INPUT_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(dpc2_local_deviations[DPC2_STATE_LOCAL_DEVIATION_BUFFER_HEIGHT], DPC2_STATE_LOCAL_DEVIATION_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(dpc2_second_min[DPC2_STATE_SECOND_MINMAX_BUFFER_HEIGHT], DPC2_STATE_SECOND_MINMAX_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(dpc2_second_max[DPC2_STATE_SECOND_MINMAX_BUFFER_HEIGHT], DPC2_STATE_SECOND_MINMAX_BUFFER_WIDTH*ISP_NWAY);
+};
+
+#endif /* __IA_CSS_DPC2_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_types.h
new file mode 100644
index 000000000000..b2c974196ce8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dpc2/ia_css_dpc2_types.h
@@ -0,0 +1,59 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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	(1<<12)
+#define METRIC2_ONE_FP	(1<<5)
+#define METRIC3_ONE_FP	(1<<12)
+#define WBGAIN_ONE_FP	(1<<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 {
+	/**@{*/
+	int32_t metric1;
+	int32_t metric2;
+	int32_t metric3;
+	int32_t wb_gain_gr;
+	int32_t wb_gain_r;
+	int32_t wb_gain_b;
+	int32_t wb_gain_gb;
+	/**@}*/
+};
+/**@}*/
+
+#endif /* __IA_CSS_DPC2_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c
new file mode 100644
index 000000000000..955adc4d6ab0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.c
@@ -0,0 +1,306 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_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 "memory_access.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 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);
+}
+
+void
+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;
+
+	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;
+#ifndef ISP2401
+	/* Coverity CID 298073 - initialize */
+#endif
+	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 width, height;
+	unsigned int *xbuff = NULL;
+	unsigned int *ybuff = NULL;
+	struct gdc_warp_param_mem_s *ptr;
+
+	assert(config != NULL);
+	assert(gdc_warp_table != NULL);
+	assert(gdc_warp_table->address != NULL);
+
+	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;
+	struct gdc_warp_param_mem_s *isp_data_ptr;
+
+	assert(binary != NULL);
+	assert(dvs_6axis_config != NULL);
+	assert(dvs_in_frame_info != NULL);
+
+	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));
+
+	isp_data_ptr = (struct gdc_warp_param_mem_s *)me->address;
+
+	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;
+}
+
+enum ia_css_err
+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,
+	hrt_vaddress ddr_addr_y)
+{
+
+	struct ia_css_host_data *me;
+	assert(dvs_6axis_config != NULL);
+	assert(ddr_addr_y != mmgr_NULL);
+	assert(dvs_in_frame_info != NULL);
+
+	me = convert_allocate_dvs_6axis_config(dvs_6axis_config,
+				 binary,
+				 dvs_in_frame_info);
+
+	if (!me) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+
+	ia_css_params_store_ia_css_host_data(
+				ddr_addr_y,
+				me);
+	ia_css_host_data_free(me);
+
+	return IA_CSS_SUCCESS;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h
new file mode 100644
index 000000000000..2f513e29d88c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs.host.h
@@ -0,0 +1,60 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+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);
+
+enum ia_css_err
+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,
+	hrt_vaddress ddr_addr_y);
+
+#endif /* __IA_CSS_DVS_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h
new file mode 100644
index 000000000000..4d0abfe4d0fd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_param.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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>
+#ifdef ISP2401
+
+#if !defined(ENABLE_TPROXY) && !defined(ENABLE_CRUN_FOR_TD) && !defined(PARAMBIN_GENERATION)
+#endif
+#include "dma.h"
+#ifdef ISP2401
+#endif /* !defined(ENABLE_TPROXY) && !defined(ENABLE_CRUN_FOR_TD) */
+
+#endif
+#include "uds/uds_1.0/ia_css_uds_param.h"
+
+#ifdef ISP2401
+
+#endif
+/** dvserence frame */
+struct sh_css_isp_dvs_isp_config {
+	uint32_t num_horizontal_blocks;
+	uint32_t num_vertical_blocks;
+};
+
+#endif /* __IA_CSS_DVS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h
new file mode 100644
index 000000000000..216c54a21ea5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/dvs/dvs_1.0/ia_css_dvs_types.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.c
new file mode 100644
index 000000000000..682f8b709ff9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.c
@@ -0,0 +1,321 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+
+const int16_t chgrinv_x[NUMBER_OF_CHGRINV_POINTS] = {
+0, 16, 64, 144, 272, 448, 672, 976,
+1376, 1888, 2528, 3312, 4256, 5376, 6688};
+
+const int16_t chgrinv_a[NUMBER_OF_CHGRINV_POINTS] = {
+-7171, -256, -29, -3456, -1071, -475, -189, -102,
+-48, -38, -10, -9, -7, -6, 0};
+
+const int16_t chgrinv_b[NUMBER_OF_CHGRINV_POINTS] = {
+8191, 1021, 256, 114, 60, 37, 24, 17,
+12, 9, 6, 5, 4, 3, 2};
+
+const int16_t chgrinv_c[NUMBER_OF_CHGRINV_POINTS] = {
+1, 1, 1, 0, 0, 0, 0, 0,
+0, 0, 0, 0, 0, 0, 0};
+
+const int16_t tcinv_x[NUMBER_OF_TCINV_POINTS] = {
+0, 4, 11, 23, 42, 68, 102, 148, 205};
+
+const int16_t tcinv_a[NUMBER_OF_TCINV_POINTS] = {
+-6364, -631, -126, -34, -13, -6, -4452, -2156, 0};
+
+const int16_t tcinv_b[NUMBER_OF_TCINV_POINTS] = {
+8191, 1828, 726, 352, 197, 121, 80, 55, 40};
+
+const int16_t tcinv_c[NUMBER_OF_TCINV_POINTS] = {
+1, 1, 1, 1, 1, 1, 0, 0, 0};
+
+const int16_t fcinv_x[NUMBER_OF_FCINV_POINTS] = {
+0, 80, 216, 456, 824, 1344, 2040, 2952, 4096};
+
+const int16_t fcinv_a[NUMBER_OF_FCINV_POINTS] = {
+-5244, -486, -86, -2849, -961, -400, -180, -86, 0};
+
+const int16_t fcinv_b[NUMBER_OF_FCINV_POINTS] = {
+8191, 1637, 607, 287, 159, 98, 64, 44, 32};
+
+const int16_t 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 i, j, base;
+	const unsigned total_blocks = 4;
+	const unsigned 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(max(from->dew_enhance_seg_x[j], 0), 8191);
+			to->e_dew_enh_y[0][base + j] = min(max(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(max(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(max(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 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/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.h
new file mode 100644
index 000000000000..355ff13273b0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8.host.h
@@ -0,0 +1,46 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+#include "ia_css_eed1_8_default.host.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 level);
+#endif
+
+#endif /* __IA_CSS_EED1_8_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.c
new file mode 100644
index 000000000000..3622719dafa5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.c
@@ -0,0 +1,94 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_eed1_8_types.h"
+
+/* The default values for the kernel parameters are based on
+ * ISP261 CSS API public parameter list_all.xlsx from 12-09-2014
+ * The parameter list is available on the ISP261 sharepoint
+ */
+
+/* Default kernel parameters. */
+const struct ia_css_eed1_8_config default_eed1_8_config = {
+	.rbzp_strength = 5489,
+	.fcstrength = 6554,
+	.fcthres_0 = 0,
+	.fcthres_1 = 0,
+	.fc_sat_coef = 8191,
+	.fc_coring_prm = 128,
+	.aerel_thres0 = 0,
+	.aerel_gain0 = 8191,
+	.aerel_thres1 = 16,
+	.aerel_gain1 = 20,
+	.derel_thres0 = 1229,
+	.derel_gain0 = 1,
+	.derel_thres1 = 819,
+	.derel_gain1 = 1,
+	.coring_pos0 = 0,
+	.coring_pos1 = 0,
+	.coring_neg0 = 0,
+	.coring_neg1 = 0,
+	.gain_exp = 2,
+	.gain_pos0 = 6144,
+	.gain_pos1 = 2048,
+	.gain_neg0 = 2048,
+	.gain_neg1 = 6144,
+	.pos_margin0 = 1475,
+	.pos_margin1 = 1475,
+	.neg_margin0 = 1475,
+	.neg_margin1 = 1475,
+	.dew_enhance_seg_x = {
+		0,
+		64,
+		272,
+		688,
+		1376,
+		2400,
+		3840,
+		5744,
+		8191
+		},
+	.dew_enhance_seg_y = {
+		0,
+		144,
+		480,
+		1040,
+		1852,
+		2945,
+		4357,
+		6094,
+		8191
+		},
+	.dew_enhance_seg_slope = {
+		4608,
+		3308,
+		2757,
+		2417,
+		2186,
+		8033,
+		7473,
+		7020
+		},
+	.dew_enhance_seg_exp = {
+		2,
+		2,
+		2,
+		2,
+		2,
+		0,
+		0,
+		0
+		},
+	.dedgew_max = 6144
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.h
new file mode 100644
index 000000000000..782f739ca8b5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_default.host.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DEFAULT_HOST_H
+#define __IA_CSS_EED1_8_DEFAULT_HOST_H
+
+#include "ia_css_eed1_8_types.h"
+
+extern const struct ia_css_eed1_8_config default_eed1_8_config;
+
+#endif /* __IA_CSS_EED1_8_DEFAULT_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_param.h
new file mode 100644
index 000000000000..bc3a07fd07eb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_param.h
@@ -0,0 +1,154 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t rbzp_strength;
+
+	int32_t fcstrength;
+	int32_t fcthres_0;
+	int32_t fc_sat_coef;
+	int32_t fc_coring_prm;
+	int32_t fc_slope;
+
+	int32_t aerel_thres0;
+	int32_t aerel_gain0;
+	int32_t aerel_thres_diff;
+	int32_t aerel_gain_diff;
+
+	int32_t derel_thres0;
+	int32_t derel_gain0;
+	int32_t derel_thres_diff;
+	int32_t derel_gain_diff;
+
+	int32_t coring_pos0;
+	int32_t coring_pos_diff;
+	int32_t coring_neg0;
+	int32_t coring_neg_diff;
+
+	int32_t gain_exp;
+	int32_t gain_pos0;
+	int32_t gain_pos_diff;
+	int32_t gain_neg0;
+	int32_t gain_neg_diff;
+
+	int32_t margin_pos0;
+	int32_t margin_pos_diff;
+	int32_t margin_neg0;
+	int32_t margin_neg_diff;
+
+	int32_t e_dew_enh_asr;
+	int32_t dedgew_max;
+};
+
+#endif /* __IA_CSS_EED1_8_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_state.h
new file mode 100644
index 000000000000..47e451b15044
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_state.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_EED1_8_STATE_H
+
+#include "type_support.h"
+#include "vmem.h" /* for VMEM_ARRAY*/
+
+#include "ia_css_eed1_8_param.h"
+
+struct eed1_8_vmem_state {
+	VMEM_ARRAY(eed1_8_input_lines[EED1_8_STATE_INPUT_BUFFER_HEIGHT], EED1_8_STATE_INPUT_BUFFER_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_LD_H[EED1_8_STATE_LD_H_HEIGHT], EED1_8_STATE_LD_H_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_LD_V[EED1_8_STATE_LD_V_HEIGHT], EED1_8_STATE_LD_V_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_D_Hr[EED1_8_STATE_D_HR_HEIGHT], EED1_8_STATE_D_HR_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_D_Hb[EED1_8_STATE_D_HB_HEIGHT], EED1_8_STATE_D_HB_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_D_Vr[EED1_8_STATE_D_VR_HEIGHT], EED1_8_STATE_D_VR_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_D_Vb[EED1_8_STATE_D_VB_HEIGHT], EED1_8_STATE_D_VB_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_rb_zipped[EED1_8_STATE_RB_ZIPPED_HEIGHT], EED1_8_STATE_RB_ZIPPED_WIDTH*ISP_NWAY);
+#if EED1_8_FC_ENABLE_MEDIAN
+	VMEM_ARRAY(eed1_8_Yc[EED1_8_STATE_YC_HEIGHT], EED1_8_STATE_YC_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_Cg[EED1_8_STATE_CG_HEIGHT], EED1_8_STATE_CG_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_Co[EED1_8_STATE_CO_HEIGHT], EED1_8_STATE_CO_WIDTH*ISP_NWAY);
+	VMEM_ARRAY(eed1_8_AbsK[EED1_8_STATE_ABSK_HEIGHT], EED1_8_STATE_ABSK_WIDTH*ISP_NWAY);
+#endif
+};
+
+#endif /* __IA_CSS_EED1_8_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_types.h
new file mode 100644
index 000000000000..07651f0ac558
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/eed1_8/ia_css_eed1_8_types.h
@@ -0,0 +1,86 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t rbzp_strength;	/**< Strength of zipper reduction. */
+
+	int32_t fcstrength;	/**< Strength of false color reduction. */
+	int32_t fcthres_0;	/**< Threshold to prevent chroma coring due to noise or green disparity in dark region. */
+	int32_t fcthres_1;	/**< Threshold to prevent chroma coring due to noise or green disparity in bright region. */
+	int32_t fc_sat_coef;	/**< How much color saturation to maintain in high color saturation region. */
+	int32_t fc_coring_prm;	/**< Chroma coring coefficient for tint color suppression. */
+
+	int32_t aerel_thres0;	/**< Threshold for Non-Directional Reliability at dark region. */
+	int32_t aerel_gain0;	/**< Gain for Non-Directional Reliability at dark region. */
+	int32_t aerel_thres1;	/**< Threshold for Non-Directional Reliability at bright region. */
+	int32_t aerel_gain1;	/**< Gain for Non-Directional Reliability at bright region. */
+
+	int32_t derel_thres0;	/**< Threshold for Directional Reliability at dark region. */
+	int32_t derel_gain0;	/**< Gain for Directional Reliability at dark region. */
+	int32_t derel_thres1;	/**< Threshold for Directional Reliability at bright region. */
+	int32_t derel_gain1;	/**< Gain for Directional Reliability at bright region. */
+
+	int32_t coring_pos0;	/**< Positive Edge Coring Threshold in dark region. */
+	int32_t coring_pos1;	/**< Positive Edge Coring Threshold in bright region. */
+	int32_t coring_neg0;	/**< Negative Edge Coring Threshold in dark region. */
+	int32_t coring_neg1;	/**< Negative Edge Coring Threshold in bright region. */
+
+	int32_t gain_exp;	/**< Common Exponent of Gain. */
+	int32_t gain_pos0;	/**< Gain for Positive Edge in dark region. */
+	int32_t gain_pos1;	/**< Gain for Positive Edge in bright region. */
+	int32_t gain_neg0;	/**< Gain for Negative Edge in dark region. */
+	int32_t gain_neg1;	/**< Gain for Negative Edge in bright region. */
+
+	int32_t pos_margin0;	/**< Margin for Positive Edge in dark region. */
+	int32_t pos_margin1;	/**< Margin for Positive Edge in bright region. */
+	int32_t neg_margin0;	/**< Margin for Negative Edge in dark region. */
+	int32_t neg_margin1;	/**< Margin for Negative Edge in bright region. */
+
+	int32_t dew_enhance_seg_x[IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS];		/**< Segment data for directional edge weight: X. */
+	int32_t dew_enhance_seg_y[IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS];		/**< Segment data for directional edge weight: Y. */
+	int32_t dew_enhance_seg_slope[(IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1)];	/**< Segment data for directional edge weight: Slope. */
+	int32_t dew_enhance_seg_exp[(IA_CSS_NUMBER_OF_DEW_ENHANCE_SEGMENTS - 1)];	/**< Segment data for directional edge weight: Exponent. */
+	int32_t dedgew_max;	/**< Max Weight for Directional Edge. */
+};
+
+#endif /* __IA_CSS_EED1_8_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.c
new file mode 100644
index 000000000000..94631eee8614
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.c
@@ -0,0 +1,62 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.h
new file mode 100644
index 000000000000..8a90cd83b248
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats.host.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+#ifndef IA_CSS_NO_DEBUG
+/* FIXME: See BZ 4427 */
+void
+ia_css_formats_dump(
+	const struct sh_css_isp_formats_params *formats,
+	unsigned 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 level);
+#endif /*IA_CSS_NO_DEBUG*/
+
+#endif /* __IA_CSS_FORMATS_HOST_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_param.h
new file mode 100644
index 000000000000..2eb6030b6081
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_param.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t video_full_range_flag;
+};
+
+#endif /* __IA_CSS_FORMATS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_types.h
new file mode 100644
index 000000000000..df1565a5914c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fc/fc_1.0/ia_css_formats_types.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t 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/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h
new file mode 100644
index 000000000000..cc8dd1a7007f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+#ifdef ISP2401
+#define BDS_UNIT 8
+#define FRAC_LOG 3
+#define FRAC_ACC (1<<FRAC_LOG)
+#if FRAC_ACC != BDS_UNIT
+#error "FRAC_ACC and BDS_UNIT need to be merged into one define"
+#endif
+
+#endif
+struct sh_css_isp_bds_params {
+	int baf_strength;
+};
+
+#endif /* __IA_CSS_FIXEDBDS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h
new file mode 100644
index 000000000000..5b59d9dec4d0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fixedbds/fixedbds_1.0/ia_css_fixedbds_types.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 numerator;
+	unsigned denominator;
+	unsigned int bds_factor;
+};
+
+
+#endif	/*__IA_CSS_FIXEDBDS_TYPES_H*/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c
new file mode 100644
index 000000000000..1fb9f27540f3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.c
@@ -0,0 +1,89 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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);
+}
+
+void
+ia_css_fpn_config(
+	struct sh_css_isp_fpn_isp_config *to,
+	const struct ia_css_fpn_configuration *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, from->info);
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	assert (elems_a % to->port_b.elems == 0);
+}
+
+void
+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;
+
+	ia_css_configure_fpn(binary, &config);
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h
new file mode 100644
index 000000000000..bb905c8db8c8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn.host.h
@@ -0,0 +1,44 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_fpn_dump(
+	const struct sh_css_isp_fpn_params *fpn,
+	unsigned level);
+
+void
+ia_css_fpn_config(
+	struct sh_css_isp_fpn_isp_config      *to,
+	const struct ia_css_fpn_configuration *from,
+	unsigned size);
+
+void
+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/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h
new file mode 100644
index 000000000000..68765c3f3bf7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_param.h
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t shift;
+	int32_t enabled;
+};
+
+struct sh_css_isp_fpn_isp_config {
+	uint32_t width_a_over_b;
+	struct dma_port_config port_b;
+};
+
+#endif /* __IA_CSS_FPN_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h
new file mode 100644
index 000000000000..5a2f0c06a80d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/fpn/fpn_1.0/ia_css_fpn_types.h
@@ -0,0 +1,52 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int16_t *data;		/**< Table content (fixed patterns noise).
+					u0.[13-shift], [0,63] */
+	uint32_t width;		/**< Table width (in pixels).
+					This is the input frame width. */
+	uint32_t height;	/**< Table height (in pixels).
+					This is the input frame height. */
+	uint32_t shift;		/**< Common exponent of table content.
+					u8.0, [0,13] */
+	uint32_t 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/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.c
new file mode 100644
index 000000000000..0cfb5c94447f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.c
@@ -0,0 +1,118 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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 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 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 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 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/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.h
new file mode 100644
index 000000000000..06f08840563e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc.host.h
@@ -0,0 +1,65 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_gc_vamem_encode(
+	struct sh_css_isp_gc_vamem_params *to,
+	const struct ia_css_gamma_table *from,
+	unsigned size);
+
+void
+ia_css_ce_encode(
+	struct sh_css_isp_ce_params *to,
+	const struct ia_css_ce_config *from,
+	unsigned size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_gc_dump(
+	const struct sh_css_isp_gc_params *gc,
+	unsigned level);
+
+void
+ia_css_ce_dump(
+	const struct sh_css_isp_ce_params *ce,
+	unsigned level);
+
+void
+ia_css_gc_debug_dtrace(
+	const struct ia_css_gc_config *config,
+	unsigned level);
+
+void
+ia_css_ce_debug_dtrace(
+	const struct ia_css_ce_config *config,
+	unsigned level);
+
+#endif
+
+#endif /* __IA_CSS_GC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_param.h
new file mode 100644
index 000000000000..52972b1a07ff
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_param.h
@@ -0,0 +1,61 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t gain_k1;
+	int32_t gain_k2;
+};
+
+/* CE (Chroma Enhancement) */
+struct sh_css_isp_ce_params {
+	int32_t uv_level_min;
+	int32_t uv_level_max;
+};
+
+/* This should be vamem_data_t, but that breaks the pipe generator */
+struct sh_css_isp_gc_vamem_params {
+	uint16_t gc[SH_CSS_ISP_GC_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_GC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c
new file mode 100644
index 000000000000..082db22a5329
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.c
@@ -0,0 +1,214 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 <string_support.h> /* memcpy */
+#include "system_global.h"
+#include "vamem.h"
+#include "ia_css_types.h"
+#include "ia_css_gc_table.host.h"
+
+#if defined(HAS_VAMEM_VERSION_2)
+
+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
+};
+
+#elif defined(HAS_VAMEM_VERSION_1)
+
+static const uint16_t
+default_gamma_table_data[IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE] = {
+		0, 1, 2, 3, 4, 5, 6, 7,
+		8, 9, 10, 11, 12, 13, 14, 16,
+		17, 18, 19, 20, 21, 23, 24, 25,
+		27, 28, 29, 31, 32, 33, 35, 36,
+		38, 39, 41, 42, 44, 45, 47, 48,
+		49, 51, 52, 54, 55, 57, 58, 60,
+		61, 62, 64, 65, 66, 68, 69, 70,
+		71, 72, 74, 75, 76, 77, 78, 79,
+		80, 81, 82, 83, 84, 85, 86, 87,
+		88, 89, 90, 91, 92, 93, 93, 94,
+		95, 96, 97, 98, 98, 99, 100, 101,
+		102, 102, 103, 104, 105, 105, 106, 107,
+		108, 108, 109, 110, 110, 111, 112, 112,
+		113, 114, 114, 115, 116, 116, 117, 118,
+		118, 119, 120, 120, 121, 121, 122, 123,
+		123, 124, 125, 125, 126, 126, 127, 127,	/* 128 */
+		128, 129, 129, 130, 130, 131, 131, 132,
+		132, 133, 134, 134, 135, 135, 136, 136,
+		137, 137, 138, 138, 139, 139, 140, 140,
+		141, 141, 142, 142, 143, 143, 144, 144,
+		145, 145, 145, 146, 146, 147, 147, 148,
+		148, 149, 149, 150, 150, 150, 151, 151,
+		152, 152, 152, 153, 153, 154, 154, 155,
+		155, 155, 156, 156, 156, 157, 157, 158,
+		158, 158, 159, 159, 160, 160, 160, 161,
+		161, 161, 162, 162, 162, 163, 163, 163,
+		164, 164, 164, 165, 165, 165, 166, 166,
+		166, 167, 167, 167, 168, 168, 168, 169,
+		169, 169, 170, 170, 170, 170, 171, 171,
+		171, 172, 172, 172, 172, 173, 173, 173,
+		174, 174, 174, 174, 175, 175, 175, 176,
+		176, 176, 176, 177, 177, 177, 177, 178,	/* 256 */
+		178, 178, 178, 179, 179, 179, 179, 180,
+		180, 180, 180, 181, 181, 181, 181, 182,
+		182, 182, 182, 182, 183, 183, 183, 183,
+		184, 184, 184, 184, 184, 185, 185, 185,
+		185, 186, 186, 186, 186, 186, 187, 187,
+		187, 187, 187, 188, 188, 188, 188, 188,
+		189, 189, 189, 189, 189, 190, 190, 190,
+		190, 190, 191, 191, 191, 191, 191, 192,
+		192, 192, 192, 192, 192, 193, 193, 193,
+		193, 193, 194, 194, 194, 194, 194, 194,
+		195, 195, 195, 195, 195, 195, 196, 196,
+		196, 196, 196, 196, 197, 197, 197, 197,
+		197, 197, 198, 198, 198, 198, 198, 198,
+		198, 199, 199, 199, 199, 199, 199, 200,
+		200, 200, 200, 200, 200, 200, 201, 201,
+		201, 201, 201, 201, 201, 202, 202, 202,	/* 384 */
+		202, 202, 202, 202, 203, 203, 203, 203,
+		203, 203, 203, 204, 204, 204, 204, 204,
+		204, 204, 204, 205, 205, 205, 205, 205,
+		205, 205, 205, 206, 206, 206, 206, 206,
+		206, 206, 206, 207, 207, 207, 207, 207,
+		207, 207, 207, 208, 208, 208, 208, 208,
+		208, 208, 208, 209, 209, 209, 209, 209,
+		209, 209, 209, 209, 210, 210, 210, 210,
+		210, 210, 210, 210, 210, 211, 211, 211,
+		211, 211, 211, 211, 211, 211, 212, 212,
+		212, 212, 212, 212, 212, 212, 212, 213,
+		213, 213, 213, 213, 213, 213, 213, 213,
+		214, 214, 214, 214, 214, 214, 214, 214,
+		214, 214, 215, 215, 215, 215, 215, 215,
+		215, 215, 215, 216, 216, 216, 216, 216,
+		216, 216, 216, 216, 216, 217, 217, 217,	/* 512 */
+		217, 217, 217, 217, 217, 217, 217, 218,
+		218, 218, 218, 218, 218, 218, 218, 218,
+		218, 219, 219, 219, 219, 219, 219, 219,
+		219, 219, 219, 220, 220, 220, 220, 220,
+		220, 220, 220, 220, 220, 221, 221, 221,
+		221, 221, 221, 221, 221, 221, 221, 221,
+		222, 222, 222, 222, 222, 222, 222, 222,
+		222, 222, 223, 223, 223, 223, 223, 223,
+		223, 223, 223, 223, 223, 224, 224, 224,
+		224, 224, 224, 224, 224, 224, 224, 224,
+		225, 225, 225, 225, 225, 225, 225, 225,
+		225, 225, 225, 226, 226, 226, 226, 226,
+		226, 226, 226, 226, 226, 226, 226, 227,
+		227, 227, 227, 227, 227, 227, 227, 227,
+		227, 227, 228, 228, 228, 228, 228, 228,
+		228, 228, 228, 228, 228, 228, 229, 229,
+		229, 229, 229, 229, 229, 229, 229, 229,
+		229, 229, 230, 230, 230, 230, 230, 230,
+		230, 230, 230, 230, 230, 230, 231, 231,
+		231, 231, 231, 231, 231, 231, 231, 231,
+		231, 231, 231, 232, 232, 232, 232, 232,
+		232, 232, 232, 232, 232, 232, 232, 233,
+		233, 233, 233, 233, 233, 233, 233, 233,
+		233, 233, 233, 233, 234, 234, 234, 234,
+		234, 234, 234, 234, 234, 234, 234, 234,
+		234, 235, 235, 235, 235, 235, 235, 235,
+		235, 235, 235, 235, 235, 235, 236, 236,
+		236, 236, 236, 236, 236, 236, 236, 236,
+		236, 236, 236, 236, 237, 237, 237, 237,
+		237, 237, 237, 237, 237, 237, 237, 237,
+		237, 237, 238, 238, 238, 238, 238, 238,
+		238, 238, 238, 238, 238, 238, 238, 238,
+		239, 239, 239, 239, 239, 239, 239, 239,
+		239, 239, 239, 239, 239, 239, 240, 240,
+		240, 240, 240, 240, 240, 240, 240, 240,
+		240, 240, 240, 240, 241, 241, 241, 241,
+		241, 241, 241, 241, 241, 241, 241, 241,
+		241, 241, 241, 242, 242, 242, 242, 242,
+		242, 242, 242, 242, 242, 242, 242, 242,
+		242, 242, 243, 243, 243, 243, 243, 243,
+		243, 243, 243, 243, 243, 243, 243, 243,
+		243, 244, 244, 244, 244, 244, 244, 244,
+		244, 244, 244, 244, 244, 244, 244, 244,
+		245, 245, 245, 245, 245, 245, 245, 245,
+		245, 245, 245, 245, 245, 245, 245, 246,
+		246, 246, 246, 246, 246, 246, 246, 246,
+		246, 246, 246, 246, 246, 246, 246, 247,
+		247, 247, 247, 247, 247, 247, 247, 247,
+		247, 247, 247, 247, 247, 247, 247, 248,
+		248, 248, 248, 248, 248, 248, 248, 248,
+		248, 248, 248, 248, 248, 248, 248, 249,
+		249, 249, 249, 249, 249, 249, 249, 249,
+		249, 249, 249, 249, 249, 249, 249, 250,
+		250, 250, 250, 250, 250, 250, 250, 250,
+		250, 250, 250, 250, 250, 250, 250, 251,
+		251, 251, 251, 251, 251, 251, 251, 251,
+		251, 251, 251, 251, 251, 251, 251, 252,
+		252, 252, 252, 252, 252, 252, 252, 252,
+		252, 252, 252, 252, 252, 252, 252, 253,
+		253, 253, 253, 253, 253, 253, 253, 253,
+		253, 253, 253, 253, 253, 253, 253, 253,
+		254, 254, 254, 254, 254, 254, 254, 254,
+		254, 254, 254, 254, 254, 254, 254, 254,
+		255, 255, 255, 255, 255, 255, 255, 255
+};
+
+#else
+#error "VAMEM version must be one of {VAMEM_VERSION_1, VAMEM_VERSION_2}"
+#endif
+
+void
+ia_css_config_gamma_table(void)
+{
+#if defined(HAS_VAMEM_VERSION_2)
+	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;
+#else
+	memcpy(default_gamma_table.data.vamem_1, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	default_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_1;
+#endif
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h
new file mode 100644
index 000000000000..9686623d9cdd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_table.host.h
@@ -0,0 +1,24 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_types.h
new file mode 100644
index 000000000000..dd9f0eda3353
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_1.0/ia_css_gc_types.h
@@ -0,0 +1,97 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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         (1U<<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 {
+	uint16_t vamem_1[IA_CSS_VAMEM_1_GAMMA_TABLE_SIZE];
+	/**< Y(Luma) Gamma table on vamem type 1. u0.8, [0,255] */
+	uint16_t 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 {
+	uint16_t gain_k1; /**< Gain to adjust U after YUV Gamma Correction.
+				u0.16, [0,65535],
+				default/ineffective 19000(0.29) */
+	uint16_t 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 {
+	uint8_t uv_level_min; /**< Minimum of chroma output level.
+				u0.8, [0,255], default/ineffective 0 */
+	uint8_t 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 {
+	uint8_t 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/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.c
new file mode 100644
index 000000000000..0fb1a919fccd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.c
@@ -0,0 +1,110 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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 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 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 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 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 level)
+{
+	(void)config;
+	(void)level;
+}
+#endif
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.h
new file mode 100644
index 000000000000..ba140eefd525
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2.host.h
@@ -0,0 +1,79 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_rgb2yuv_encode(
+	struct sh_css_isp_csc_params *to,
+	const struct ia_css_cc_config *from,
+	unsigned 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 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 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 size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_yuv2rgb_dump(
+	const struct sh_css_isp_csc_params *yuv2rgb,
+	unsigned level);
+
+void
+ia_css_rgb2yuv_dump(
+	const struct sh_css_isp_csc_params *rgb2yuv,
+	unsigned level);
+
+void
+ia_css_rgb_gamma_table_debug_dtrace(
+	const struct ia_css_rgb_gamma_table *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_param.h
new file mode 100644
index 000000000000..d25239f4d86f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_param.h
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t gc[SH_CSS_ISP_RGB_GAMMA_TABLE_SIZE];
+};
+
+#endif /* __IA_CSS_GC2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c
new file mode 100644
index 000000000000..f14a66b78714
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.c
@@ -0,0 +1,132 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 <string_support.h> /* memcpy */
+#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. */
+
+#if defined(HAS_VAMEM_VERSION_2)
+
+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
+};
+#elif defined(HAS_VAMEM_VERSION_1)
+
+static const uint16_t
+default_gamma_table_data[IA_CSS_VAMEM_1_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
+};
+#else
+#error "VAMEM version must be one of {VAMEM_VERSION_1, VAMEM_VERSION_2}"
+#endif
+
+void
+ia_css_config_rgb_gamma_tables(void)
+{
+#if defined(HAS_VAMEM_VERSION_2)
+	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));
+#else
+	memcpy(default_r_gamma_table.data.vamem_1, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	memcpy(default_g_gamma_table.data.vamem_1, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	memcpy(default_b_gamma_table.data.vamem_1, default_gamma_table_data,
+	       sizeof(default_gamma_table_data));
+	default_r_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_1;
+	default_g_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_1;
+	default_b_gamma_table.vamem_type   = IA_CSS_VAMEM_TYPE_1;
+#endif
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h
new file mode 100644
index 000000000000..8686e6e3586c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_table.host.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_types.h
new file mode 100644
index 000000000000..e439583bdfb6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/gc/gc_2/ia_css_gc2_types.h
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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      (1U<<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 {
+	uint16_t 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. */
+	uint16_t 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/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.c
new file mode 100644
index 000000000000..8215ae47d384
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.c
@@ -0,0 +1,41 @@
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/* Release Version: irci_ecr-master_20150911_0724 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.h
new file mode 100644
index 000000000000..8f89bc8f1ca2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr.host.h
@@ -0,0 +1,31 @@
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/* Release Version: irci_ecr-master_20150911_0724 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#endif /* __IA_CSS_HDR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_param.h
new file mode 100644
index 000000000000..1c053af7d0d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_param.h
@@ -0,0 +1,53 @@
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/* Release Version: irci_ecr-master_20150911_0724 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t test_irr;
+	int32_t match_shift[HDR_NUM_INPUT_FRAMES - 1];  /* Histogram matching shift parameter */
+	int32_t match_mul[HDR_NUM_INPUT_FRAMES - 1];    /* Histogram matching multiplication parameter */
+	int32_t thr_low[HDR_NUM_INPUT_FRAMES - 1];      /* Weight map soft threshold low bound parameter */
+	int32_t thr_high[HDR_NUM_INPUT_FRAMES - 1];     /* Weight map soft threshold high bound parameter */
+	int32_t thr_coeff[HDR_NUM_INPUT_FRAMES - 1];    /* Soft threshold linear function coefficient */
+	int32_t thr_shift[HDR_NUM_INPUT_FRAMES - 1];    /* Soft threshold precision shift parameter */
+	int32_t weight_bpp;                             /* Weight map bits per pixel */
+};
+
+/* HDR deghosting parameters on ISP */
+struct sh_css_hdr_deghost_params {
+	int32_t test_deg;
+};
+
+/* HDR exclusion parameters on ISP */
+struct sh_css_hdr_exclusion_params {
+	int32_t 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/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_types.h
new file mode 100644
index 000000000000..c3345b32e3e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/hdr/ia_css_hdr_types.h
@@ -0,0 +1,64 @@
+/* Release Version: irci_stable_candrpv_0415_20150521_0458 */
+/* Release Version: irci_ecr-master_20150911_0724 */
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 paramters 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 paramters are used only for testing purposes
+ */
+struct ia_css_hdr_deghost_params {
+	int test_deg; /**< Test parameter */
+};
+
+/**
+ * \brief HDR Exclusion Parameters
+ * \detail Currently HDR paramters 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 paramters for HDR that can be seet from
+ * the CSS API. Currenly, only test paramters are defined.
+ */
+struct ia_css_hdr_config {
+	struct ia_css_hdr_irradiance_params irradiance; /**< HDR irradiance paramaters */
+	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/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.c
new file mode 100644
index 000000000000..a31c9e828e22
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.c
@@ -0,0 +1,86 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+void
+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 ddr_bits_per_element = sizeof(short) * 8;
+	const unsigned ddr_elems_per_word = ceil_div(HIVE_ISP_DDR_WORD_BITS, ddr_bits_per_element);
+	unsigned size_get = 0, size_put = 0;
+	unsigned offset = 0;
+
+	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
+
+		ia_css_dma_configure_from_info(&config, in_frame_info);
+		// 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
+
+		ia_css_dma_configure_from_info(&config, &out_frames[0]->info);
+		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
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.h
new file mode 100644
index 000000000000..7e5d4cfe3454
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.h
@@ -0,0 +1,31 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+
+void
+ia_css_bayer_io_config(
+	const struct ia_css_binary     *binary,
+	const struct sh_css_binary_args *args);
+
+#endif /*__BAYER_IO_HOST_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_param.h
new file mode 100644
index 000000000000..7b6f581c4a80
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_param.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_types.h
new file mode 100644
index 000000000000..2291b01452f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io_types.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_param.h
new file mode 100644
index 000000000000..f1ce03aa7951
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_param.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_types.h
new file mode 100644
index 000000000000..8a9a97063264
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/common/ia_css_common_io_types.h
@@ -0,0 +1,31 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 2
+
+struct ia_css_common_io_config {
+	unsigned base_address;
+	unsigned width;
+	unsigned height;
+	unsigned stride;
+	unsigned ddr_elems_per_word;
+	unsigned dma_channel[MAX_IO_DMA_CHANNELS];
+};
+
+#endif /* __IA_CSS_COMMON_IO_TYPES */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_param.h
new file mode 100644
index 000000000000..213ef3b385aa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_param.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PLANE_IO_PARAM_H
+#define __IA_CSS_PLANE_IO_PARAM_H
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif /* __IA_CSS_PLANE_IO_PARAM_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_types.h
new file mode 100644
index 000000000000..d635741505e2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/plane_io_ls/ia_css_plane_io_types.h
@@ -0,0 +1,30 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PLANE_IO_TYPES_H
+#define __IA_CSS_PLANE_IO_TYPES_H
+
+#include "../common/ia_css_common_io_types.h"
+
+#define PLANE_IO_LS_NUM_PLANES       3
+
+struct ia_css_plane_io_config {
+	struct ia_css_common_io_config get_plane_io_config[PLANE_IO_LS_NUM_PLANES];
+	struct ia_css_common_io_config put_plane_io_config[PLANE_IO_LS_NUM_PLANES];
+};
+
+#endif /* __IA_CSS_PLANE_IO_TYPES_H */
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h
new file mode 100644
index 000000000000..52450a9a55a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YUV420_IO_PARAM
+#define __IA_CSS_YUV420_IO_PARAM
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h
new file mode 100644
index 000000000000..99ec1143b214
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YUV420_IO_TYPES
+#define __IA_CSS_YUV420_IO_TYPES
+
+#include "../common/ia_css_common_io_types.h"
+
+#endif
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
new file mode 100644
index 000000000000..91fb5168c357
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
new file mode 100644
index 000000000000..dac440309394
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
@@ -0,0 +1,22 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c
new file mode 100644
index 000000000000..78e159c04851
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.c
@@ -0,0 +1,86 @@
+#ifdef ISP2401
+/**
+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"
+
+void
+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 ddr_bits_per_element = sizeof(short) * 8;
+	const unsigned ddr_elems_per_word = ceil_div(HIVE_ISP_DDR_WORD_BITS, ddr_bits_per_element);
+	unsigned size_get = 0, size_put = 0;
+	unsigned offset = 0;
+
+	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
+
+		ia_css_dma_configure_from_info(&config, in_frame_info);
+		// 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
+
+		ia_css_dma_configure_from_info(&config, &out_frames[0]->info);
+		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
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h
new file mode 100644
index 000000000000..ab9fa31bfc5e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h
@@ -0,0 +1,31 @@
+#ifdef ISP2401
+/**
+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"
+
+
+void
+ia_css_bayer_io_config(
+	const struct ia_css_binary     *binary,
+	const struct sh_css_binary_args *args);
+
+#endif /*__BAYER_IO_HOST_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h
new file mode 100644
index 000000000000..bf5a3eccb330
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_param.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h
new file mode 100644
index 000000000000..9e3c622db4d4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io_types.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h
new file mode 100644
index 000000000000..e5fdcfff0cf7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_param.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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 */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h
new file mode 100644
index 000000000000..0a19e2d1aff4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/common/ia_css_common_io_types.h
@@ -0,0 +1,31 @@
+#ifdef ISP2401
+/**
+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 base_address;
+	unsigned width;
+	unsigned height;
+	unsigned stride;
+	unsigned ddr_elems_per_word;
+	unsigned dma_channel[MAX_IO_DMA_CHANNELS];
+};
+
+#endif /* __IA_CSS_COMMON_IO_TYPES */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_param.h
new file mode 100644
index 000000000000..881b7e5236dc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_param.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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_PLANE_IO_PARAM_H
+#define __IA_CSS_PLANE_IO_PARAM_H
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif /* __IA_CSS_PLANE_IO_PARAM_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_types.h
new file mode 100644
index 000000000000..f4b9e8de3d8e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/plane_io_ls/ia_css_plane_io_types.h
@@ -0,0 +1,30 @@
+#ifdef ISP2401
+/**
+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_PLANE_IO_TYPES_H
+#define __IA_CSS_PLANE_IO_TYPES_H
+
+#include "../common/ia_css_common_io_types.h"
+
+#define PLANE_IO_LS_NUM_PLANES       3
+
+struct ia_css_plane_io_config {
+	struct ia_css_common_io_config get_plane_io_config[PLANE_IO_LS_NUM_PLANES];
+	struct ia_css_common_io_config put_plane_io_config[PLANE_IO_LS_NUM_PLANES];
+};
+
+#endif /* __IA_CSS_PLANE_IO_TYPES_H */
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h
new file mode 100644
index 000000000000..86184b545fed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_param.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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_YUV420_IO_PARAM
+#define __IA_CSS_YUV420_IO_PARAM
+
+#include "../common/ia_css_common_io_param.h"
+
+#endif
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h
new file mode 100644
index 000000000000..ad750f530013
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv420_io_ls/ia_css_yuv420_io_types.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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_YUV420_IO_TYPES
+#define __IA_CSS_YUV420_IO_TYPES
+
+#include "../common/ia_css_common_io_types.h"
+
+#endif
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c
new file mode 100644
index 000000000000..f7e1a632c47e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.c
@@ -0,0 +1,86 @@
+#ifdef ISP2401
+/**
+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"
+
+void
+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 ddr_bits_per_element = sizeof(short) * 8;
+	const unsigned ddr_elems_per_word = ceil_div(HIVE_ISP_DDR_WORD_BITS, ddr_bits_per_element);
+	unsigned size_get = 0, size_put = 0;
+	unsigned offset = 0;
+
+	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
+
+		ia_css_dma_configure_from_info(&config, in_frame_info);
+		// 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
+
+		ia_css_dma_configure_from_info(&config, &out_frames[0]->info);
+		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
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h
new file mode 100644
index 000000000000..480172d39aee
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io.host.h
@@ -0,0 +1,31 @@
+#ifdef ISP2401
+/**
+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"
+
+
+void
+ia_css_yuv444_io_config(
+	const struct ia_css_binary     *binary,
+	const struct sh_css_binary_args *args);
+
+#endif /*__YUV44_IO_HOST_H */
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
new file mode 100644
index 000000000000..cc8eda19c6e8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_param.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
new file mode 100644
index 000000000000..343325a111e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ipu2_io_ls/yuv444_io_ls/ia_css_yuv444_io_types.h
@@ -0,0 +1,22 @@
+#ifdef ISP2401
+/**
+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
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c
new file mode 100644
index 000000000000..9e41cc0a307f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.c
@@ -0,0 +1,80 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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);
+}
+
+enum ia_css_err
+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;
+	}
+
+	ia_css_configure_iterator(binary, &config);
+
+	return IA_CSS_SUCCESS;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h
new file mode 100644
index 000000000000..d8f249c5a53b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator.host.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+enum ia_css_err
+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/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h
new file mode 100644
index 000000000000..d308126e41d3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/iterator/iterator_1.0/ia_css_iterator_param.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c
new file mode 100644
index 000000000000..5ddf61fc95fa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.c
@@ -0,0 +1,74 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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;
+	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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h
new file mode 100644
index 000000000000..53ef18f7e912
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5.host.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h
new file mode 100644
index 000000000000..41a2da460dcf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_param.h
@@ -0,0 +1,31 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t 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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c
new file mode 100644
index 000000000000..89714bf87b52
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.c
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h
new file mode 100644
index 000000000000..10a50aa82be8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_table.host.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h
new file mode 100644
index 000000000000..3d510bf5886a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc1_5/ia_css_macc1_5_types.h
@@ -0,0 +1,74 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int16_t 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 {
+	uint8_t 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/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.c
new file mode 100644
index 000000000000..1f7e9e4eec3c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.c
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	to->exp = from->exp;
+}
+
+void
+ia_css_macc_dump(
+	const struct sh_css_isp_macc_params *macc,
+	unsigned level);
+
+void
+ia_css_macc_debug_dtrace(
+	const struct ia_css_macc_config *config,
+	unsigned level)
+{
+	ia_css_debug_dtrace(level,
+		"config.exp=%d\n",
+		config->exp);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.h
new file mode 100644
index 000000000000..044b01d38ad6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc.host.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+	
+
+void
+ia_css_macc_dump(
+	const struct sh_css_isp_macc_params *macc,
+	unsigned level);
+
+void
+ia_css_macc_debug_dtrace(
+	const struct ia_css_macc_config *config,
+	unsigned level);
+
+#endif /* __IA_CSS_MACC_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_param.h
new file mode 100644
index 000000000000..6a12b922c485
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_param.h
@@ -0,0 +1,25 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t exp;
+};
+
+#endif /* __IA_CSS_MACC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c
new file mode 100644
index 000000000000..8a6c3cafabdc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.c
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h
new file mode 100644
index 000000000000..96d62c9912b8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_table.host.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_types.h
new file mode 100644
index 000000000000..a25581c6f3ac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/macc/macc_1.0/ia_css_macc_types.h
@@ -0,0 +1,63 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int16_t 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/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.c
new file mode 100644
index 000000000000..2c2c5a5854a0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.h
new file mode 100644
index 000000000000..42b5143ef78f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm.host.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_param.h
new file mode 100644
index 000000000000..85dc6fc0a56b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_param.h
@@ -0,0 +1,19 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_types.h
new file mode 100644
index 000000000000..5581bddf9f9b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/norm/norm_1.0/ia_css_norm_types.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_TYPES_H
+#define __IA_CSS_NORM_TYPES_H
+
+
+#endif /* __IA_CSS_NORM_TYPES_H */
+
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.c
new file mode 100644
index 000000000000..f77aff13f8e3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.c
@@ -0,0 +1,79 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.h
new file mode 100644
index 000000000000..06846502eca3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2.host.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ob2_dump(
+	const struct sh_css_isp_ob2_params *ob2,
+	unsigned level);
+
+void
+ia_css_ob2_debug_dtrace(
+	const struct ia_css_ob2_config *config, unsigned level);
+#endif
+
+#endif /* __IA_CSS_OB2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_param.h
new file mode 100644
index 000000000000..5c21d6a3911b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_param.h
@@ -0,0 +1,29 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t blacklevel_gr;
+	int32_t blacklevel_r;
+	int32_t blacklevel_b;
+	int32_t blacklevel_gb;
+};
+
+#endif /* __IA_CSS_OB2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_types.h
new file mode 100644
index 000000000000..eeaadfeb5a1e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob2/ia_css_ob2_types.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.c
new file mode 100644
index 000000000000..fd891ac092ed
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.c
@@ -0,0 +1,159 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 i;
+		unsigned sp_obarea_start_bq  = ob->area_start_bq;
+		unsigned sp_obarea_length_bq = ob->area_length_bq;
+		unsigned low = sp_obarea_start_bq;
+		unsigned high = low + sp_obarea_length_bq;
+		uint16_t 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 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 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/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.h
new file mode 100644
index 000000000000..4af181470f8d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob.host.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 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 size);
+
+void
+ia_css_ob_dump(
+	const struct sh_css_isp_ob_params *ob,
+	unsigned level);
+
+void
+ia_css_ob_debug_dtrace(
+	const struct ia_css_ob_config *config, unsigned level)
+;
+
+#endif /* __IA_CSS_OB_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_param.h
new file mode 100644
index 000000000000..a60a644bb4ff
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_param.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 (1<<12)
+
+
+/* OB (Optical Black) */
+struct sh_css_isp_ob_stream_config {
+	unsigned isp_pipe_version;
+	unsigned raw_bit_depth;
+};
+
+struct sh_css_isp_ob_params {
+	int32_t blacklevel_gr;
+	int32_t blacklevel_r;
+	int32_t blacklevel_b;
+	int32_t blacklevel_gb;
+	int32_t area_start_bq;
+	int32_t area_length_bq;
+	int32_t 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/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_types.h
new file mode 100644
index 000000000000..88459b6c003d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ob/ob_1.0/ia_css_ob_types.h
@@ -0,0 +1,69 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	uint16_t start_position; /**< Start position of OB area
+					(used for Raster Mode only).
+					u16.0, [0,63],
+					default/ineffective 0 */
+	uint16_t 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/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.c
new file mode 100644
index 000000000000..8fdf47c9310c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.c
@@ -0,0 +1,162 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	to->enable_hflip = from->enable_hflip;
+	to->enable_vflip = from->enable_vflip;
+}
+
+void
+ia_css_output_config(
+	struct sh_css_isp_output_isp_config *to,
+	const struct ia_css_output_configuration  *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, from->info);
+	to->width_a_over_b = elems_a / to->port_b.elems;
+	to->height = from->info->res.height;
+	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. */
+	assert (elems_a % to->port_b.elems == 0);
+}
+
+void
+ia_css_output0_config(
+	struct sh_css_isp_output_isp_config       *to,
+	const struct ia_css_output0_configuration *from,
+	unsigned size)
+{
+	ia_css_output_config (
+		to, (const struct ia_css_output_configuration *)from, size);
+}
+
+void
+ia_css_output1_config(
+	struct sh_css_isp_output_isp_config       *to,
+	const struct ia_css_output1_configuration *from,
+	unsigned size)
+{
+	ia_css_output_config (
+		to, (const struct ia_css_output_configuration *)from, size);
+}
+
+void
+ia_css_output_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame_info *info)
+{
+	if (NULL != info) {
+		struct ia_css_output_configuration config =
+				default_output_configuration;
+
+		config.info = info;
+
+		ia_css_configure_output(binary, &config);
+	}
+}
+
+void
+ia_css_output0_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame_info *info)
+{
+	if (NULL != info) {
+		struct ia_css_output0_configuration config =
+				default_output0_configuration;
+
+		config.info = info;
+
+		ia_css_configure_output0(binary, &config);
+	}
+}
+
+void
+ia_css_output1_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame_info *info)
+{
+
+	if (NULL != info) {
+		struct ia_css_output1_configuration config =
+				default_output1_configuration;
+
+		config.info = info;
+
+		ia_css_configure_output1(binary, &config);
+	}
+}
+
+void
+ia_css_output_dump(
+	const struct sh_css_isp_output_params *output,
+	unsigned 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 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/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.h
new file mode 100644
index 000000000000..530f934ce81e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output.host.h
@@ -0,0 +1,75 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_output_config(
+	struct sh_css_isp_output_isp_config      *to,
+	const struct ia_css_output_configuration *from,
+	unsigned size);
+
+void
+ia_css_output0_config(
+	struct sh_css_isp_output_isp_config       *to,
+	const struct ia_css_output0_configuration *from,
+	unsigned size);
+
+void
+ia_css_output1_config(
+	struct sh_css_isp_output_isp_config       *to,
+	const struct ia_css_output1_configuration *from,
+	unsigned size);
+
+void
+ia_css_output_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame_info *from);
+
+void
+ia_css_output0_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame_info *from);
+
+void
+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 level);
+
+void
+ia_css_output_debug_dtrace(
+	const struct ia_css_output_config *config,
+	unsigned level);
+
+#endif /* __IA_CSS_OUTPUT_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_param.h
new file mode 100644
index 000000000000..26ec27e085c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_param.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t width_a_over_b;
+	uint32_t height;
+	uint32_t enable;
+	struct ia_css_frame_sp_info info;
+	struct dma_port_config port_b;
+};
+
+struct sh_css_isp_output_params {
+	uint8_t enable_hflip;
+	uint8_t enable_vflip;
+};
+
+#endif /* __IA_CSS_OUTPUT_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_types.h
new file mode 100644
index 000000000000..4335ac28b31d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/output/output_1.0/ia_css_output_types.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint8_t enable_hflip;  /**< enable horizontal output mirroring */
+	uint8_t enable_vflip;  /**< enable vertical output mirroring */
+};
+
+#endif /* __IA_CSS_OUTPUT_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c
new file mode 100644
index 000000000000..d1fb4b116003
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.c
@@ -0,0 +1,61 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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,
+};
+
+void
+ia_css_qplane_config(
+	struct sh_css_isp_qplane_isp_config *to,
+	const struct ia_css_qplane_configuration  *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, from->info);
+	to->width_a_over_b = elems_a / to->port_b.elems;
+
+	/* Assume divisiblity here, may need to generalize to fixed point. */
+	assert (elems_a % to->port_b.elems == 0);
+
+	to->inout_port_config = from->pipe->inout_port_config;
+	to->format = from->info->format;
+}
+
+void
+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;
+
+	ia_css_configure_qplane(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h
new file mode 100644
index 000000000000..c41e9e5e0fd7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane.host.h
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+void
+ia_css_qplane_config(
+	struct sh_css_isp_qplane_isp_config      *to,
+	const struct ia_css_qplane_configuration *from,
+	unsigned size);
+
+void
+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/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h
new file mode 100644
index 000000000000..5885f621de88
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_param.h
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t width_a_over_b;
+	struct dma_port_config port_b;
+	uint32_t inout_port_config;
+	uint32_t input_needs_raw_binning;
+	uint32_t format; /* enum ia_css_frame_format */
+};
+
+#endif /* __IA_CSS_QPLANE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h
new file mode 100644
index 000000000000..955fd472a241
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/qplane/qplane_2/ia_css_qplane_types.h
@@ -0,0 +1,33 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.c
new file mode 100644
index 000000000000..68a27f0cfba0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.c
@@ -0,0 +1,136 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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,
+};
+
+static inline unsigned
+sh_css_elems_bytes_from_info (unsigned raw_bit_depth)
+{
+	return CEIL_DIV(raw_bit_depth,8);
+}
+
+/* MW: These areMIPI / ISYS properties, not camera function properties */
+static enum sh_stream_format
+css2isp_stream_format(enum ia_css_stream_format from)
+{
+	switch (from) {
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+		return sh_stream_format_yuv420_legacy;
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+		return sh_stream_format_yuv420;
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+		return sh_stream_format_yuv422;
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_FORMAT_RGB_888:
+		return sh_stream_format_rgb;
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+		return sh_stream_format_raw;
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+	default:
+		return sh_stream_format_raw;
+	}
+}
+
+void
+ia_css_raw_config(
+	struct sh_css_isp_raw_isp_config *to,
+	const struct ia_css_raw_configuration  *from,
+	unsigned size)
+{
+	unsigned 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;
+
+	(void)size;
+#if !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	/* 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
+	ia_css_dma_configure_from_info(&to->port_b, in_info);
+
+	/* 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(USE_INPUT_SYSTEM_VERSION_2401) || defined(CONFIG_CSI2_PLUS))
+	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
+}
+
+void
+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)
+{
+	uint8_t 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;
+
+	ia_css_configure_raw(binary, &config);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.h
new file mode 100644
index 000000000000..ac6b7f6b59c6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw.host.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+void
+ia_css_raw_config(
+	struct sh_css_isp_raw_isp_config      *to,
+	const struct ia_css_raw_configuration *from,
+	unsigned size);
+
+void
+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/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_param.h
new file mode 100644
index 000000000000..12168b2dec2d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_param.h
@@ -0,0 +1,38 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t width_a_over_b;
+	struct dma_port_config port_b;
+	uint32_t inout_port_config;
+	uint32_t input_needs_raw_binning;
+	uint32_t format; /* enum ia_css_frame_format */
+	uint32_t required_bds_factor;
+	uint32_t two_ppc;
+	uint32_t stream_format; /* enum sh_stream_format */
+	uint32_t deinterleaved;
+	uint32_t start_column; /*left crop offset*/
+	uint32_t start_line; /*top crop offset*/
+	uint8_t enable_left_padding; /*need this for multiple binary case*/
+};
+
+#endif /* __IA_CSS_RAW_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_types.h
new file mode 100644
index 000000000000..54f8c299d227
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw/raw_1.0/ia_css_raw_types.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 ia_css_stream_format stream_format;
+	bool deinterleaved;
+	uint8_t enable_left_padding;
+};
+
+#endif /* __IA_CSS_RAW_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c
new file mode 100644
index 000000000000..92168211683d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.c
@@ -0,0 +1,35 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#if !defined(HAS_NO_HMEM)
+
+#include "memory_access.h"
+#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 size)
+{
+	(void)size;
+	(void)to;
+	(void)from;
+}
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h
new file mode 100644
index 000000000000..b4f245c19f18
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/raw_aa_binning/raw_aa_binning_1.0/ia_css_raa.host.h
@@ -0,0 +1,27 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#endif /* __IA_CSS_RAA_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.c
new file mode 100644
index 000000000000..4c0ed5d4d971
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.c
@@ -0,0 +1,74 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+void
+ia_css_ref_config(
+	struct sh_css_isp_ref_isp_config *to,
+	const struct ia_css_ref_configuration  *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS, i;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, &(from->ref_frames[0]->info));
+	to->width_a_over_b = elems_a / to->port_b.elems;
+	to->dvs_frame_delay = from->dvs_frame_delay;
+	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. */
+	assert (elems_a % to->port_b.elems == 0);
+}
+
+void
+ia_css_ref_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame **ref_frames,
+	const uint32_t dvs_frame_delay)
+{
+	struct ia_css_ref_configuration config;
+	unsigned 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;
+	ia_css_configure_ref(binary, &config);
+}
+
+void
+ia_css_init_ref_state(
+	struct sh_css_isp_ref_dmem_state *state,
+	unsigned 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/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.h
new file mode 100644
index 000000000000..3c6d728d49ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref.host.h
@@ -0,0 +1,41 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+void
+ia_css_ref_config(
+	struct sh_css_isp_ref_isp_config      *to,
+	const struct ia_css_ref_configuration *from,
+	unsigned size);
+
+void
+ia_css_ref_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame **ref_frames,
+	const uint32_t dvs_frame_delay);
+
+void
+ia_css_init_ref_state(
+	struct sh_css_isp_ref_dmem_state *state,
+	unsigned size);
+#endif /* __IA_CSS_REF_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_param.h
new file mode 100644
index 000000000000..1f1b72a417d1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_param.h
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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];
+	uint32_t dvs_frame_delay;
+};
+
+struct sh_css_isp_ref_isp_config {
+	uint32_t width_a_over_b;
+	struct dma_port_config port_b;
+	hrt_vaddress ref_frame_addr_y[MAX_NUM_VIDEO_DELAY_FRAMES];
+	hrt_vaddress ref_frame_addr_c[MAX_NUM_VIDEO_DELAY_FRAMES];
+	uint32_t dvs_frame_delay;
+};
+
+#endif /* __IA_CSS_REF_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_state.h
new file mode 100644
index 000000000000..7867be8a7958
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_state.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t ref_in_buf_idx;
+	int32_t ref_out_buf_idx;
+};
+
+#endif /* __IA_CSS_REF_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_types.h
new file mode 100644
index 000000000000..ce0eaeeee9c6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ref/ref_1.0/ia_css_ref_types.h
@@ -0,0 +1,28 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c
new file mode 100644
index 000000000000..8ef6c54ee813
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.c
@@ -0,0 +1,386 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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 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 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 != NULL);
+
+	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 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 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 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 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 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)
+{
+#if defined(HAS_NO_HMEM)
+	(void)host_stats;
+	(void)hmem_buf;
+#else
+	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 != NULL);
+	assert(host_stats->rgby_data != NULL);
+	assert(hmem_buf != NULL);
+
+	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;
+#endif
+}
+
+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 != NULL);
+	assert(host_stats->data != NULL);
+	assert(isp_stats != NULL);
+
+	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 */
+STORAGE_CLASS_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 uint16_t *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 uint16_t *hi, *lo;
+	struct ia_css_3a_output *output;
+
+	assert(host_stats!= NULL);
+	assert(host_stats->data != NULL);
+	assert(isp_stats_hi != NULL);
+	assert(isp_stats_lo != NULL);
+
+	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/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h
new file mode 100644
index 000000000000..4bc6c0bf478f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a.host.h
@@ -0,0 +1,77 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#ifndef IA_CSS_NO_DEBUG
+void
+ia_css_ae_dump(
+	const struct sh_css_isp_ae_params *ae,
+	unsigned level);
+
+void
+ia_css_awb_dump(
+	const struct sh_css_isp_awb_params *awb,
+	unsigned level);
+
+void
+ia_css_af_dump(
+	const struct sh_css_isp_af_params *af,
+	unsigned level);
+
+void
+ia_css_s3a_dump(
+	const struct sh_css_isp_s3a_params *s3a,
+	unsigned level);
+
+void
+ia_css_s3a_debug_dtrace(
+	const struct ia_css_3a_config *config,
+	unsigned 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 uint16_t *isp_stats_hi,
+	const uint16_t *isp_stats_lo);
+
+#endif /* __IA_CSS_S3A_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h
new file mode 100644
index 000000000000..35fb0a2c921a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_param.h
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	int32_t y_coef_r;
+	int32_t y_coef_g;
+	int32_t y_coef_b;
+};
+
+/* AWB (3A Support) */
+struct sh_css_isp_awb_params {
+	int32_t lg_high_raw;
+	int32_t lg_low;
+	int32_t lg_high;
+};
+
+/* AF (3A Support) */
+struct sh_css_isp_af_params {
+	int32_t fir1[7];
+	int32_t 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/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h
new file mode 100644
index 000000000000..f57ed1ec5981
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a/s3a_1.0/ia_css_s3a_types.h
@@ -0,0 +1,266 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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)
+	uint32_t 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*/
+
+  	uint32_t 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*/
+
+  	uint32_t af_enable;					/**< af enabled in binary,
+								   0:disabled, 1:enabled */
+	struct af_public_grid_config		af_grd_info;	/**< see description in af_public.h*/
+
+  	uint32_t 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*/
+  
+        uint32_t 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
+	uint32_t enable;            /**< 3A statistics enabled.
+					0:disabled, 1:enabled */
+	uint32_t use_dmem;          /**< DMEM or VMEM determines layout.
+					0:3A statistics are stored to VMEM,
+					1:3A statistics are stored to DMEM */
+	uint32_t has_histogram;     /**< Statistics include histogram.
+					0:no histogram, 1:has histogram */
+	uint32_t width;	    	    /**< Width of 3A grid table.
+					(= Horizontal number of grid cells
+					in table, which cells have effective
+					statistics.) */
+	uint32_t height;	    /**< Height of 3A grid table.
+					(= Vertical number of grid cells
+					in table, which cells have effective
+					statistics.) */
+	uint32_t aligned_width;     /**< Horizontal stride (for alloc).
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	uint32_t aligned_height;    /**< Vertical stride (for alloc).
+					(= Vertical number of grid cells
+					in table, which means
+					the allocated height.) */
+	uint32_t 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. */
+	uint32_t deci_factor_log2;  /**< log2 of bqs_per_grid_cell. */
+	uint32_t 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
+};
+
+
+#if defined(SYSTEM_css_skycam_c0_system)
+#if defined USE_NEW_AE_STRUCT || defined USE_NEW_AWB_STRUCT
+#define DEFAULT_3A_GRID_INFO \
+{ \
+	0,				/* ae_enable */ \
+	{0,0,0,0,0,0,0},	        /* AE:     width,height,b_width,b_height,x_start,y_start*/ \
+	0,				/* awb_enable */ \
+	{0,0,0,0,0,0},			/* AWB:    width,height,b_width,b_height,x_start,y_start*/ \
+	0,				/* af_enable */ \
+	{0,0,0,0,0,0,0},		/* AF:     width,height,b_width,b_height,x_start,y_start,ff_en*/ \
+	0,				/* awb_fr_enable */ \
+	{0,0,0,0,0,0,0},                  /* AWB_FR: width,height,b_width,b_height,x_start,y_start,ff_en*/ \
+	0,				/* elem_bit_depth */ \
+}
+#else
+#define DEFAULT_3A_GRID_INFO \
+{ \
+	0,				/* ae_enable */ \
+	{0,0,0,0,0,0,0,0,0},	        /* AE:     width,height,b_width,b_height,x_start,y_start,x_end,y_end*/ \
+	0,				/* awb_enable */ \
+	{0,0,0,0,0,0,0,0},              /* AWB:    width,height,b_width,b_height,x_start,y_start,x_end,y_end*/ \
+	0,				/* af_enable */ \
+	{0,0,0,0,0,0,0},		/* AF:     width,height,b_width,b_height,x_start,y_start,ff_en*/ \
+	0,				/* awb_fr_enable */ \
+	{0,0,0,0,0,0,0},                  /* AWB_FR: width,height,b_width,b_height,x_start,y_start,ff_en*/ \
+	0,				/* elem_bit_depth */ \
+}
+#endif /* USE_NEW_AE_STRUCT || defined USE_NEW_AWB_STRUCT */
+
+#else
+#define DEFAULT_3A_GRID_INFO \
+{ \
+	0,				/* enable */ \
+	0,				/* use_dmem */ \
+	0,				/* has_histogram */ \
+	0,				/* width */ \
+	0,				/* height */ \
+	0,				/* aligned_width */ \
+	0,				/* aligned_height */ \
+	0,				/* bqs_per_grid_cell */ \
+	0,				/* deci_factor_log2 */ \
+	0,				/* elem_bit_depth */ \
+}
+
+#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 {
+	int32_t ae_y;    /**< Sum of Y in a statistics window, for AE.
+				(u19.13) */
+	int32_t awb_cnt; /**< Number of effective pixels
+				in a statistics window.
+				Pixels passed by the AWB level gate check are
+				judged as "effective". (u32) */
+	int32_t awb_gr;  /**< Sum of Gr in a statistics window, for AWB.
+				All Gr pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	int32_t awb_r;   /**< Sum of R in a statistics window, for AWB.
+				All R pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	int32_t awb_b;   /**< Sum of B in a statistics window, for AWB.
+				All B pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	int32_t awb_gb;  /**< Sum of Gb in a statistics window, for AWB.
+				All Gb pixels (not only for effective pixels)
+				are summed. (u19.13) */
+	int32_t af_hpf1; /**< Sum of |Y| following high pass filter af_fir1
+				within a statistics window, for AF. (u19.13) */
+	int32_t 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 {
+	uint32_t r;	/**< Number of R of one bin of the histogram R. (u24) */
+	uint32_t g;	/**< Number of G of one bin of the histogram G. (u24) */
+	uint32_t b;	/**< Number of B of one bin of the histogram B. (u24) */
+	uint32_t 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/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_ls_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_ls_param.h
new file mode 100644
index 000000000000..8b2b56b0310b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_ls_param.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STAT_LS_PARAM_H
+#define __IA_CSS_S3A_STAT_LS_PARAM_H
+
+#include "type_support.h"
+#ifdef ISP2401
+#include "../../io_ls/common/ia_css_common_io_types.h"
+#endif
+
+#define NUM_S3A_LS 1
+
+/** s3a statistics store */
+#ifdef ISP2401
+struct ia_css_s3a_stat_ls_configuration {
+	uint32_t s3a_grid_size_log2;
+};
+
+#endif
+struct sh_css_isp_s3a_stat_ls_isp_config {
+#ifndef ISP2401
+	uint32_t base_address[NUM_S3A_LS];
+	uint32_t width[NUM_S3A_LS];
+	uint32_t height[NUM_S3A_LS];
+	uint32_t stride[NUM_S3A_LS];
+#endif
+	uint32_t s3a_grid_size_log2[NUM_S3A_LS];
+};
+
+#ifndef ISP2401
+
+#endif
+#endif /* __IA_CSS_S3A_STAT_LS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_store_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_store_param.h
new file mode 100644
index 000000000000..676b42d364e8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/s3a_stat_ls/ia_css_s3a_stat_store_param.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STAT_STORE_PARAM_H
+#define __IA_CSS_S3A_STAT_STORE_PARAM_H
+
+#include "ia_css_s3a_stat_ls_param.h"
+
+
+#endif /* __IA_CSS_S3A_STAT_STORE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.c
new file mode 100644
index 000000000000..565ae45b7541
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.c
@@ -0,0 +1,130 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "math_support.h"	/* min() */
+
+#define IA_CSS_INCLUDE_CONFIGURATIONS
+#include "ia_css_isp_configs.h"
+#endif
+
+#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 size)
+{
+	(void)size;
+	to->gain_shift = (*from)->fraction_bits;
+}
+
+void
+ia_css_sc_dump(
+	const struct sh_css_isp_sc_params *sc,
+	unsigned 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);
+}
+
+#ifdef ISP2401
+void
+ia_css_sc_config(
+	struct sh_css_isp_sc_isp_config *to,
+	const struct ia_css_sc_configuration *from,
+	unsigned size)
+{
+	uint32_t internal_org_x_bqs = from->internal_frame_origin_x_bqs_on_sctbl;
+	uint32_t internal_org_y_bqs = from->internal_frame_origin_y_bqs_on_sctbl;
+	uint32_t slice, rest, i;
+
+	(void)size;
+
+	/* The internal_frame_origin_x_bqs_on_sctbl is separated to 8 times of slice_vec. */
+	rest = internal_org_x_bqs;
+	for (i = 0; i < SH_CSS_SC_INTERPED_GAIN_HOR_SLICE_TIMES; i++) {
+		slice = min(rest, ((uint32_t)ISP_SLICE_NELEMS));
+		rest = rest - slice;
+		to->interped_gain_hor_slice_bqs[i] = slice;
+	}
+
+	to->internal_frame_origin_y_bqs_on_sctbl = internal_org_y_bqs;
+}
+
+void
+ia_css_sc_configure(
+	const struct ia_css_binary *binary,
+	uint32_t internal_frame_origin_x_bqs_on_sctbl,
+	uint32_t internal_frame_origin_y_bqs_on_sctbl)
+{
+	const struct ia_css_sc_configuration config = {
+		internal_frame_origin_x_bqs_on_sctbl,
+		internal_frame_origin_y_bqs_on_sctbl };
+
+	ia_css_configure_sc(binary, &config);
+}
+
+#endif
+/* ------ 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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.h
new file mode 100644
index 000000000000..44e3c43a5d4a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc.host.h
@@ -0,0 +1,77 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_sc_dump(
+	const struct sh_css_isp_sc_params *sc,
+	unsigned level);
+
+#ifdef ISP2401
+/** @brief Configure the shading correction.
+ * @param[out]	to	Parameters used in the shading correction kernel in the isp.
+ * @param[in]	from	Parameters passed from the host.
+ * @param[in]	size	Size of the sh_css_isp_sc_isp_config structure.
+ *
+ * This function passes the parameters for the shading correction from the host to the isp.
+ */
+void
+ia_css_sc_config(
+	struct sh_css_isp_sc_isp_config *to,
+	const struct ia_css_sc_configuration *from,
+	unsigned size);
+
+/** @brief Configure the shading correction.
+ * @param[in]	binary	The binary, which has the shading correction.
+ * @param[in]	internal_frame_origin_x_bqs_on_sctbl
+ *			X coordinate (in bqs) of the origin of the internal frame on the shading table.
+ * @param[in]	internal_frame_origin_y_bqs_on_sctbl
+ *			Y coordinate (in bqs) of the origin of the internal frame on the shading table.
+ *
+ * This function calls the ia_css_configure_sc() function.
+ * (The ia_css_configure_sc() function is automatically generated in ia_css_isp.configs.c.)
+ * The ia_css_configure_sc() function calls the ia_css_sc_config() function
+ * to pass the parameters for the shading correction from the host to the isp.
+ */
+void
+ia_css_sc_configure(
+	const struct ia_css_binary *binary,
+	uint32_t internal_frame_origin_x_bqs_on_sctbl,
+	uint32_t internal_frame_origin_y_bqs_on_sctbl);
+
+#endif
+/* ------ 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/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_param.h
new file mode 100644
index 000000000000..d997d5137634
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_param.h
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+
+#ifdef ISP2401
+/* 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 _ISP_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 _ISP_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)
+
+/* 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)
+
+/* 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)
+
+#endif
+/* SC (Shading Corrction) */
+struct sh_css_isp_sc_params {
+	int32_t gain_shift;
+};
+
+#ifdef ISP2401
+/* 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 {
+	uint32_t interped_gain_hor_slice_bqs[SH_CSS_SC_INTERPED_GAIN_HOR_SLICE_TIMES];
+	uint32_t internal_frame_origin_y_bqs_on_sctbl;
+};
+
+#endif
+#endif /* __IA_CSS_SC_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_types.h
new file mode 100644
index 000000000000..5a833bc48af1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sc/sc_1.0/ia_css_sc_types.h
@@ -0,0 +1,136 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t enable; /**< Set to false for no shading correction.
+		          The data field can be NULL when enable == true */
+/* ------ deprecated(bz675) : from ------ */
+	uint32_t sensor_width;  /**< Native sensor width in pixels. */
+	uint32_t 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 ------ */
+	uint32_t width;  /**< Number of data points per line per color.
+				u8.0, [0,81] */
+	uint32_t height; /**< Number of lines of data points per color.
+				u8.0, [0,61] */
+	uint32_t fraction_bits; /**< Bits of fractional part in the data
+				points.
+				u8.0, [0,13] */
+	uint16_t *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 {
+	uint32_t 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 ------ */
+
+#ifdef ISP2401
+
+/** Shading Correction configuration.
+ *
+ *  NOTE: The shading table size is larger than or equal to the internal frame size.
+ */
+struct ia_css_sc_configuration {
+	uint32_t internal_frame_origin_x_bqs_on_sctbl; /**< Origin X (in bqs) of internal frame on shading table. */
+	uint32_t internal_frame_origin_y_bqs_on_sctbl; /**< Origin Y (in bqs) of internal frame on shading table. */
+						/**< 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. */
+};
+#endif
+
+#endif /* __IA_CSS_SC_TYPES_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/scale/scale_1.0/ia_css_scale_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/scale/scale_1.0/ia_css_scale_param.h
new file mode 100644
index 000000000000..fd19f008ff91
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/scale/scale_1.0/ia_css_scale_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_SCALE_PARAM_H
+#define _IA_CSS_SCALE_PARAM_H
+
+#include "uds/uds_1.0/ia_css_uds_param.h"
+
+#endif /* _IA_CSS_SCALE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common.host.h
new file mode 100644
index 000000000000..4eb4910798fa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common.host.h
@@ -0,0 +1,99 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+  int32_t tbl[ISP_DVS_NUM_COEF_TYPES * ISP_MAX_SDIS_HOR_PROJ_NUM_ISP];
+#if DVS2_PROJ_MARGIN > 0
+  int32_t margin[DVS2_PROJ_MARGIN];
+#endif
+};
+
+struct sh_css_isp_sdis_vert_proj_tbl {
+  int32_t tbl[ISP_DVS_NUM_COEF_TYPES * ISP_MAX_SDIS_VER_PROJ_NUM_ISP];
+#if DVS2_PROJ_MARGIN > 0
+  int32_t 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 horicoef_vectors;
+  unsigned vertcoef_vectors;
+  unsigned horiproj_num;
+  unsigned 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/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common_types.h
new file mode 100644
index 000000000000..295dc60b778c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_common_types.h
@@ -0,0 +1,232 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t width;		/**< Width of DVS grid table in cells */
+	uint32_t 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;
+	uint32_t deci_factor_log2;
+};
+
+#define IA_CSS_DEFAULT_SDIS_INFO \
+	{	\
+		{	{ 0, 0 },	/* dim */ \
+			{ 0, 0 },	/* pad */ \
+		},	/* grid */ \
+		{	{ 0, 0 },	/* dim */ \
+			{ 0, 0 },	/* pad */ \
+		},	/* coef */ \
+		{	{ 0, 0 },	/* dim */ \
+			{ 0, 0 },	/* pad */ \
+		},	/* proj */ \
+		0,	/* dis_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 {
+	uint32_t 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. */
+	uint32_t aligned_width; /**< Stride of each grid line.
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	uint32_t 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. */
+	uint32_t 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 {
+	uint32_t enable;        /**< DVS statistics enabled.
+					0:disabled, 1:enabled */
+	uint32_t 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. */
+	uint32_t aligned_width; /**< Stride of each grid line.
+					(= Horizontal number of grid cells
+					in table, which means
+					the allocated width.) */
+	uint32_t 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. */
+	uint32_t aligned_height;/**< Stride of each grid column.
+					(= Vertical number of grid cells
+					in table, which means
+					the allocated height.) */
+	uint32_t 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. */
+	uint32_t num_hor_coefs;	/**< Number of horizontal coefficients. */
+	uint32_t 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 { \
+{ \
+	{ 0, 0, 0},	/* GBL CFG reg: kappa, match_shifrt, binning mode*/ \
+	{{{0, 0, 0, 0}, {0, 0, 0}, {0, 0} }, \
+	{{0, 0, 0, 0}, {0, 0, 0}, {0, 0} }, \
+	{{0, 0, 0, 0}, {0, 0, 0}, {0, 0} } }, \
+	{{0, 0, 0, 0}, {4, 0, 0, 0}, {0, 0, 0, 0} } } \
+}
+
+
+/** 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/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_param.h
new file mode 100644
index 000000000000..586cc4315c1f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/common/ia_css_sdis_param.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PARAM_COMMON_H
+#define __IA_CSS_SDIS_PARAM_COMMON_H
+
+
+#include "sdis/common/ia_css_sdis_common.host.h"
+
+#endif /* __IA_CSS_SDIS_PARAM_COMMON_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
new file mode 100644
index 000000000000..0daab1176865
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.c
@@ -0,0 +1,424 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "memory_access.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 width, unsigned 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 size)
+{
+	unsigned aligned_width = from->grid.aligned_width * from->grid.bqs_per_grid_cell;
+	unsigned width         = from->grid.num_hor_coefs;
+	int      padding       = aligned_width-width;
+	unsigned stride        = size/IA_CSS_DVS_NUM_COEF_TYPES/sizeof(short);
+	unsigned total_bytes   = aligned_width*IA_CSS_DVS_NUM_COEF_TYPES*sizeof(short);
+	short   *public        = from->hor_coefs;
+	short   *private       = (short*)to;
+	unsigned 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 size)
+{
+	unsigned aligned_height = from->grid.aligned_height * from->grid.bqs_per_grid_cell;
+	unsigned height         = from->grid.num_ver_coefs;
+	int      padding        = aligned_height-height;
+	unsigned stride         = size/IA_CSS_DVS_NUM_COEF_TYPES/sizeof(short);
+	unsigned total_bytes    = aligned_height*IA_CSS_DVS_NUM_COEF_TYPES*sizeof(short);
+	short   *public         = from->ver_coefs;
+	short   *private        = (short*)to;
+	unsigned 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 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 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 != NULL);
+	assert(vertical_coefficients != NULL);
+
+	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 sc_3a_dis_width,
+	unsigned sc_3a_dis_padded_width,
+	unsigned sc_3a_dis_height,
+	unsigned isp_pipe_version,
+	unsigned enabled)
+{
+	if (!enabled) {
+		struct ia_css_sdis_info default_dis = IA_CSS_DEFAULT_SDIS_INFO;
+		*dis = default_dis;
+		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;
+}
+
+enum ia_css_err
+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;
+	enum ia_css_err ret = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats != NULL);
+	assert(isp_stats != NULL);
+
+	map = ia_css_isp_dvs_statistics_map_allocate(isp_stats, NULL);
+	if (map) {
+		mmgr_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 = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+
+	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;
+	int32_t *hor_ptr_dvs, *ver_ptr_dvs, *hor_ptr_isp, *ver_ptr_isp;
+
+	assert(host_stats != NULL);
+	assert(host_stats->hor_proj != NULL);
+	assert(host_stats->ver_proj != NULL);
+	assert(isp_stats != NULL);
+	assert(isp_stats->hor_proj != NULL);
+	assert(isp_stats->ver_proj != NULL);
+
+	IA_CSS_ENTER("hproj=%p, vproj=%p, haddr=%x, vaddr=%x",
+			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 != NULL);
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	if (!grid->enable)
+		return NULL;
+
+	me = sh_css_calloc(1,sizeof(*me));
+	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 = mmgr_malloc(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 = sh_css_malloc(sizeof(*me));
+	if (!me) {
+		IA_CSS_LOG("cannot allocate memory");
+		goto err;
+	}
+
+	me->data_ptr = data_ptr;
+	me->data_allocated = data_ptr == NULL;
+
+	if (!me->data_ptr) {
+		me->data_ptr = sh_css_malloc(isp_stats->size);
+		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:
+	if (me)
+		sh_css_free(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)
+			sh_css_free(me->data_ptr);
+		sh_css_free(me);
+	}
+}
+
+void
+ia_css_isp_dvs_statistics_free(struct ia_css_isp_dvs_statistics *me)
+{
+	if (me != NULL) {
+		hmm_free(me->data_ptr);
+		sh_css_free(me);
+	}
+}
+
+void ia_css_sdis_horicoef_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_vertcoef_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_horiproj_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis_vertproj_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h
new file mode 100644
index 000000000000..95e2c61bbcba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis.host.h
@@ -0,0 +1,101 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 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 size);
+
+void ia_css_sdis_horiproj_encode (
+	struct sh_css_isp_sdis_hori_proj_tbl *to,
+	const struct ia_css_dvs_coefficients *from,
+	unsigned size);
+
+void ia_css_sdis_vertproj_encode (
+	struct sh_css_isp_sdis_vert_proj_tbl *to,
+	const struct ia_css_dvs_coefficients *from,
+	unsigned size);
+
+void ia_css_get_isp_dis_coefficients(
+	struct ia_css_stream *stream,
+	short *horizontal_coefficients,
+	short *vertical_coefficients);
+
+enum ia_css_err
+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 sc_3a_dis_width,
+	unsigned sc_3a_dis_padded_width,
+	unsigned sc_3a_dis_height,
+	unsigned isp_pipe_version,
+	unsigned 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 level);
+
+void ia_css_sdis_vertcoef_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level);
+
+void ia_css_sdis_horiproj_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level);
+
+void ia_css_sdis_vertproj_debug_dtrace(
+	const struct ia_css_dvs_coefficients *config, unsigned level);
+
+#endif /* __IA_CSS_SDIS_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_param.h
new file mode 100644
index 000000000000..2dd8696802d0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_param.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PARAM_H
+#define __IA_CSS_SDIS_PARAM_H
+
+#include "sdis.isp.h"
+
+#endif /* __IA_CSS_SDIS_PARAM_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h
new file mode 100644
index 000000000000..d408b58a027d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_1.0/ia_css_sdis_types.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 */
+	int16_t *hor_coefs;	/**< the pointer to int16_t[grid.num_hor_coefs * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the horizontal coefficients */
+	int16_t *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 */
+	int32_t *hor_proj;	/**< the pointer to int16_t[grid.height * IA_CSS_DVS_NUM_COEF_TYPES]
+				     containing the horizontal projections */
+	int32_t *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/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
new file mode 100644
index 000000000000..5a0c103e9eb7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.c
@@ -0,0 +1,338 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "memory_access.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 width, unsigned 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 size)
+{
+	unsigned aligned_width = from->grid.aligned_width * from->grid.bqs_per_grid_cell;
+	unsigned width         = from->grid.num_hor_coefs;
+	int      padding       = aligned_width-width;
+	unsigned stride        = size/IA_CSS_DVS2_NUM_COEF_TYPES/sizeof(short);
+	unsigned 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 size)
+{
+	unsigned aligned_height = from->grid.aligned_height * from->grid.bqs_per_grid_cell;
+	unsigned height         = from->grid.num_ver_coefs;
+	int      padding        = aligned_height-height;
+	unsigned stride         = size/IA_CSS_DVS2_NUM_COEF_TYPES/sizeof(short);
+	unsigned 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 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 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;
+	unsigned int hor_num_isp, ver_num_isp;
+	struct ia_css_binary *dvs_binary;
+
+	IA_CSS_ENTER("void");
+
+	assert(stream != NULL);
+	assert(hor_coefs_odd_real  != NULL);
+	assert(hor_coefs_odd_imag  != NULL);
+	assert(hor_coefs_even_real != NULL);
+	assert(hor_coefs_even_imag != NULL);
+	assert(ver_coefs_odd_real  != NULL);
+	assert(ver_coefs_odd_imag  != NULL);
+	assert(ver_coefs_even_real != NULL);
+	assert(ver_coefs_even_imag != NULL);
+
+	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;
+	hor_num_isp = dvs_binary->dis.coef.pad.width;
+	ver_num_isp = dvs_binary->dis.coef.pad.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;
+}
+
+enum ia_css_err
+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;
+	enum ia_css_err ret = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats != NULL);
+	assert(isp_stats != NULL);
+
+	map = ia_css_isp_dvs_statistics_map_allocate(isp_stats, NULL);
+	if (map) {
+		mmgr_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 = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+
+	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;
+	int32_t *htemp_ptr, *vtemp_ptr;
+
+	assert(host_stats != NULL);
+	assert(host_stats->hor_prod.odd_real  != NULL);
+	assert(host_stats->hor_prod.odd_imag  != NULL);
+	assert(host_stats->hor_prod.even_real != NULL);
+	assert(host_stats->hor_prod.even_imag != NULL);
+	assert(host_stats->ver_prod.odd_real  != NULL);
+	assert(host_stats->ver_prod.odd_imag  != NULL);
+	assert(host_stats->ver_prod.even_real != NULL);
+	assert(host_stats->ver_prod.even_imag != NULL);
+	assert(isp_stats != NULL);
+	assert(isp_stats->hor_proj != NULL);
+	assert(isp_stats->ver_proj != NULL);
+
+	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=%x, vaddr=%x",
+		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 != NULL);
+
+	IA_CSS_ENTER("grid=%p", grid);
+
+	if (!grid->enable)
+		return NULL;
+
+	me = sh_css_calloc(1,sizeof(*me));
+	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 = mmgr_malloc(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 != NULL) {
+		hmm_free(me->data_ptr);
+		sh_css_free(me);
+	}
+}
+
+void ia_css_sdis2_horicoef_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_vertcoef_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_horiproj_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
+
+void ia_css_sdis2_vertproj_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h
new file mode 100644
index 000000000000..60198d4279b4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2.host.h
@@ -0,0 +1,95 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 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 size);
+
+void ia_css_sdis2_horiproj_encode (
+	struct sh_css_isp_sdis_hori_proj_tbl *to,
+	const struct ia_css_dvs2_coefficients *from,
+	unsigned size);
+
+void ia_css_sdis2_vertproj_encode (
+	struct sh_css_isp_sdis_vert_proj_tbl *to,
+	const struct ia_css_dvs2_coefficients *from,
+	unsigned 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);
+
+enum ia_css_err
+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 level);
+
+void ia_css_sdis2_vertcoef_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level);
+
+void ia_css_sdis2_horiproj_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level);
+
+void ia_css_sdis2_vertproj_debug_dtrace(
+	const struct ia_css_dvs2_coefficients *config, unsigned level);
+
+#endif /* __IA_CSS_SDIS2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h
new file mode 100644
index 000000000000..7db7dd10fe00
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis2_types.h
@@ -0,0 +1,69 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int16_t *odd_real; /**< real part of the odd coefficients*/
+	int16_t *odd_imag; /**< imaginary part of the odd coefficients*/
+	int16_t *even_real;/**< real part of the even coefficients*/
+	int16_t *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 {
+	int32_t *odd_real; /**< real part of the odd statistics*/
+	int32_t *odd_imag; /**< imaginary part of the odd statistics*/
+	int32_t *even_real;/**< real part of the even statistics*/
+	int32_t *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/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis_param.h
new file mode 100644
index 000000000000..cea352e45713
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/sdis/sdis_2/ia_css_sdis_param.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_PARAM_H
+#define __IA_CSS_SDIS2_PARAM_H
+
+#include "sdis.isp.h"
+
+#endif /* __IA_CSS_SDIS2_PARAM_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c
new file mode 100644
index 000000000000..e775af51c0c0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.c
@@ -0,0 +1,76 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+const int16_t 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 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 level)
+{
+	(void)config;
+	(void)level;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h
new file mode 100644
index 000000000000..1b3e759e41a3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf.host.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+#include "ia_css_tdf_default.host.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 level)
+;
+
+#endif /* __IA_CSS_TDF_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.c
new file mode 100644
index 000000000000..9bb42daf070d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.c
@@ -0,0 +1,36 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_tdf_types.h"
+
+const struct ia_css_tdf_config default_tdf_config = {
+	.thres_flat_table = {0},
+	.thres_detail_table = {0},
+	.epsilon_0 = 4095,
+	.epsilon_1 = 5733,
+	.eps_scale_text = 409,
+	.eps_scale_edge = 3686,
+	.sepa_flat = 1294,
+	.sepa_edge = 4095,
+	.blend_flat = 819,
+	.blend_text = 819,
+	.blend_edge = 8191,
+	.shading_gain = 1024,
+	.shading_base_gain = 8191,
+	.local_y_gain = 0,
+	.local_y_base_gain = 2047,
+	.rad_x_origin = 0,
+	.rad_y_origin = 0
+};
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.h
new file mode 100644
index 000000000000..cd8fb70e5a87
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_default.host.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_DEFAULT_HOST_H
+#define __IA_CSS_TDF_DEFAULT_HOST_H
+
+#include "ia_css_tdf_types.h"
+
+extern const struct ia_css_tdf_config default_tdf_config;
+
+#endif /* __IA_CSS_TDF_DEFAULT_HOST_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h
new file mode 100644
index 000000000000..9334f2e0698b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_param.h
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t Epsilon_0;
+	int32_t Epsilon_1;
+	int32_t EpsScaleText;
+	int32_t EpsScaleEdge;
+	int32_t Sepa_flat;
+	int32_t Sepa_Edge;
+	int32_t Blend_Flat;
+	int32_t Blend_Text;
+	int32_t Blend_Edge;
+	int32_t Shading_Gain;
+	int32_t Shading_baseGain;
+	int32_t LocalY_Gain;
+	int32_t LocalY_baseGain;
+};
+
+#endif /* __IA_CSS_TDF_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h
new file mode 100644
index 000000000000..cc47a50e5ad5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tdf/tdf_1.0/ia_css_tdf_types.h
@@ -0,0 +1,53 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t thres_flat_table[64];	/**< Final optimized strength table of NR for flat region. */
+	int32_t thres_detail_table[64];	/**< Final optimized strength table of NR for detail region. */
+	int32_t epsilon_0;		/**< Coefficient to control variance for dark area (for flat region). */
+	int32_t epsilon_1;		/**< Coefficient to control variance for bright area (for flat region). */
+	int32_t eps_scale_text;		/**< Epsilon scaling coefficient for texture region. */
+	int32_t eps_scale_edge;		/**< Epsilon scaling coefficient for edge region. */
+	int32_t sepa_flat;		/**< Threshold to judge flat (edge < m_Flat_thre). */
+	int32_t sepa_edge;		/**< Threshold to judge edge (edge > m_Edge_thre). */
+	int32_t blend_flat;		/**< Blending ratio at flat region. */
+	int32_t blend_text;		/**< Blending ratio at texture region. */
+	int32_t blend_edge;		/**< Blending ratio at edge region. */
+	int32_t shading_gain;		/**< Gain of Shading control. */
+	int32_t shading_base_gain;	/**< Base Gain of Shading control. */
+	int32_t local_y_gain;		/**< Gain of local luminance control. */
+	int32_t local_y_base_gain;	/**< Base gain of local luminance control. */
+	int32_t rad_x_origin;		/**< Initial x coord. for radius computation. */
+	int32_t rad_y_origin;		/**< Initial y coord. for radius computation. */
+};
+
+#endif /* __IA_CSS_TDF_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h
new file mode 100644
index 000000000000..135563f52174
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr3/ia_css_tnr3_types.h
@@ -0,0 +1,61 @@
+#ifdef ISP2401
+/**
+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
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
new file mode 100644
index 000000000000..804c19ab4485
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.c
@@ -0,0 +1,130 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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);
+}
+
+void
+ia_css_tnr_config(
+	struct sh_css_isp_tnr_isp_config *to,
+	const struct ia_css_tnr_configuration *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+	unsigned i;
+
+	(void)size;
+	ia_css_dma_configure_from_info(&to->port_b, &from->tnr_frames[0]->info);
+	to->width_a_over_b = elems_a / to->port_b.elems;
+	to->frame_height = from->tnr_frames[0]->info.res.height;
+#ifndef ISP2401
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+#else
+	for (i = 0; i < NUM_TNR_FRAMES; i++) {
+#endif
+		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. */
+	assert (elems_a % to->port_b.elems == 0);
+}
+
+void
+ia_css_tnr_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame **frames)
+{
+	struct ia_css_tnr_configuration config;
+	unsigned i;
+
+#ifndef ISP2401
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++)
+#else
+	for (i = 0; i < NUM_TNR_FRAMES; i++)
+#endif
+		config.tnr_frames[i] = frames[i];
+
+	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;
+
+#ifndef ISP2401
+	assert(NUM_VIDEO_TNR_FRAMES >= 2);
+#endif
+	assert(sizeof(*state) == size);
+	state->tnr_in_buf_idx = 0;
+	state->tnr_out_buf_idx = 1;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h
new file mode 100644
index 000000000000..9290dfad574e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr.host.h
@@ -0,0 +1,56 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_tnr_dump(
+	const struct sh_css_isp_tnr_params *tnr,
+	unsigned level);
+
+void
+ia_css_tnr_debug_dtrace(
+	const struct ia_css_tnr_config *config,
+	unsigned level);
+
+void
+ia_css_tnr_config(
+	struct sh_css_isp_tnr_isp_config      *to,
+	const struct ia_css_tnr_configuration *from,
+	unsigned size);
+
+void
+ia_css_tnr_configure(
+	const struct ia_css_binary     *binary,
+	const struct ia_css_frame **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/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h
new file mode 100644
index 000000000000..db4a7cced264
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_param.h
@@ -0,0 +1,48 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t coef;
+	int32_t threshold_Y;
+	int32_t threshold_C;
+};
+
+struct ia_css_tnr_configuration {
+#ifndef ISP2401
+	const struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];
+#else
+	const struct ia_css_frame *tnr_frames[NUM_TNR_FRAMES];
+#endif
+};
+
+struct sh_css_isp_tnr_isp_config {
+	uint32_t width_a_over_b;
+	uint32_t frame_height;
+	struct dma_port_config port_b;
+#ifndef ISP2401
+	hrt_vaddress tnr_frame_addr[NUM_VIDEO_TNR_FRAMES];
+#else
+	hrt_vaddress tnr_frame_addr[NUM_TNR_FRAMES];
+#endif
+};
+
+#endif /* __IA_CSS_TNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h
new file mode 100644
index 000000000000..8b1218f7235d
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_state.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t tnr_in_buf_idx;
+	uint32_t tnr_out_buf_idx;
+};
+
+#endif /* __IA_CSS_TNR_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h
new file mode 100644
index 000000000000..4fd35e6ccd70
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/tnr/tnr_1.0/ia_css_tnr_types.h
@@ -0,0 +1,60 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/uds/uds_1.0/ia_css_uds_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/uds/uds_1.0/ia_css_uds_param.h
new file mode 100644
index 000000000000..26b7b5bc9391
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/uds/uds_1.0/ia_css_uds_param.h
@@ -0,0 +1,31 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.c
new file mode 100644
index 000000000000..5610833ed595
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.c
@@ -0,0 +1,140 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_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"
+
+void
+ia_css_vf_config(
+	struct sh_css_isp_vf_isp_config      *to,
+	const struct ia_css_vf_configuration *from,
+	unsigned size)
+{
+	unsigned elems_a = ISP_VEC_NELEMS;
+
+	(void)size;
+	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);
+		ia_css_dma_configure_from_info(&to->dma.port_b, from->info);
+		to->dma.width_a_over_b = elems_a / to->dma.port_b.elems;
+
+		/* Assume divisiblity here, may need to generalize to fixed point. */
+		assert (elems_a % to->dma.port_b.elems == 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.
+ */
+enum ia_css_err
+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 == NULL) | (vf_info == NULL))
+	       return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+       out_width = out_info->res.width;
+
+       if (out_width == 0)
+	       return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+       /* 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+       *downscale_log2 = ds_log2;
+       return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+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)
+{
+       enum ia_css_err err;
+       unsigned 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 != IA_CSS_SUCCESS)
+		       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 IA_CSS_SUCCESS;
+}
+
+static void
+configure_dma(
+	struct ia_css_vf_configuration *config,
+	const struct ia_css_frame_info *vf_info)
+{
+	config->info = vf_info;
+}
+
+enum ia_css_err
+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)
+{
+	enum ia_css_err 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);
+	configure_dma(&config, vf_info);
+	if (binary) {
+		if (vf_info)
+			vf_info->raw_bit_depth = info->dma.vfdec_bits_per_pixel;
+		ia_css_configure_vf (binary, &config);
+	}
+	return IA_CSS_SUCCESS;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.h
new file mode 100644
index 000000000000..c7c3625a9a96
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf.host.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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.
+ */
+enum ia_css_err
+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);
+
+void
+ia_css_vf_config(
+	struct sh_css_isp_vf_isp_config *to,
+	const struct ia_css_vf_configuration *from,
+	unsigned size);
+
+enum ia_css_err
+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/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_param.h
new file mode 100644
index 000000000000..df5d37c8c946
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_param.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t vf_downscale_bits; /**< Log VF downscale value */
+	uint32_t enable;
+	struct ia_css_frame_sp_info info;
+	struct {
+		uint32_t 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/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_types.h
new file mode 100644
index 000000000000..d8cfdfbc8c0b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/vf/vf_1.0/ia_css_vf_types.h
@@ -0,0 +1,32 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t 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/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.c
new file mode 100644
index 000000000000..b43cb88c6ae4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.c
@@ -0,0 +1,89 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 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/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.h
new file mode 100644
index 000000000000..18666baf9f76
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb.host.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_wb_dump(
+	const struct sh_css_isp_wb_params *wb,
+	unsigned level);
+
+void
+ia_css_wb_debug_dtrace(
+	const struct ia_css_wb_config *wb,
+	unsigned level);
+
+#endif /* __IA_CSS_WB_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_param.h
new file mode 100644
index 000000000000..c95c53a24067
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_param.h
@@ -0,0 +1,29 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t gain_shift;
+	int32_t gain_gr;
+	int32_t gain_r;
+	int32_t gain_b;
+	int32_t gain_gb;
+};
+
+#endif /* __IA_CSS_WB_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_types.h
new file mode 100644
index 000000000000..6bcfa274be88
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/wb/wb_1.0/ia_css_wb_types.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint32_t integer_bits; /**< Common exponent of gains.
+				u8.0, [0,3],
+				default 1, ineffective 1 */
+	uint32_t gr;	/**< Significand of Gr gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	uint32_t r;	/**< Significand of R gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	uint32_t b;	/**< Significand of B gain.
+				u[integer_bits].[16-integer_bits], [0,65535],
+				default/ineffective 32768(u1.15, 1.0) */
+	uint32_t 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/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c
new file mode 100644
index 000000000000..3018100f6f76
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.c
@@ -0,0 +1,66 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 level)
+{
+	(void)config;
+	(void)level;
+}
+
+void
+ia_css_xnr_debug_dtrace(
+	const struct ia_css_xnr_config *config,
+	unsigned level)
+{
+	ia_css_debug_dtrace(level,
+		"config.threshold=%d\n", config->threshold);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h
new file mode 100644
index 000000000000..eb3425eafbbe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr.host.h
@@ -0,0 +1,47 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_xnr_encode(
+	struct sh_css_isp_xnr_params *to,
+	const struct ia_css_xnr_config *from,
+	unsigned size);
+
+void
+ia_css_xnr_table_debug_dtrace(
+	const struct ia_css_xnr_table *s3a,
+	unsigned level);
+
+void
+ia_css_xnr_debug_dtrace(
+	const struct ia_css_xnr_config *config,
+	unsigned level);
+
+#endif /* __IA_CSS_XNR_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h
new file mode 100644
index 000000000000..806c9f8f0e2e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_param.h
@@ -0,0 +1,51 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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
+#if defined(HAS_VAMEM_VERSION_2)
+#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
+#elif defined(HAS_VAMEM_VERSION_1)
+#define SH_CSS_ISP_XNR_TABLE_SIZE_LOG2       IA_CSS_VAMEM_1_XNR_TABLE_SIZE_LOG2
+#define SH_CSS_ISP_XNR_TABLE_SIZE            IA_CSS_VAMEM_1_XNR_TABLE_SIZE
+#else
+#error "Unknown vamem type"
+#endif
+
+
+#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 {
+	uint16_t 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 */
+	uint16_t threshold;
+};
+
+#endif /* __IA_CSS_XNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c
new file mode 100644
index 000000000000..cd5fb72fce3f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.c
@@ -0,0 +1,81 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 <string_support.h> /* memcpy */
+#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;
+
+#if defined(HAS_VAMEM_VERSION_2)
+
+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
+};
+
+#elif defined(HAS_VAMEM_VERSION_1)
+
+static const uint16_t
+default_xnr_table_data[IA_CSS_VAMEM_1_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
+};
+
+#else
+#error "sh_css_params.c: VAMEM version must \
+	be one of {VAMEM_VERSION_1, VAMEM_VERSION_2}"
+#endif
+
+void
+ia_css_config_xnr_table(void)
+{
+#if defined(HAS_VAMEM_VERSION_2)
+	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;
+#else
+	memcpy(default_xnr_table.data.vamem_1, default_xnr_table_data,
+	       sizeof(default_xnr_table_data));
+	default_xnr_table.vamem_type     = IA_CSS_VAMEM_TYPE_1;
+#endif
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h
new file mode 100644
index 000000000000..130086713a7f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_table.host.h
@@ -0,0 +1,22 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h
new file mode 100644
index 000000000000..89e8b0f17e8c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_1.0/ia_css_xnr_types.h
@@ -0,0 +1,71 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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           (1U<<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	        (1U<<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 {
+	uint16_t vamem_1[IA_CSS_VAMEM_1_XNR_TABLE_SIZE];
+	/**< Coefficients table on vamem type1. u0.12, [0,4095] */
+	uint16_t 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 */
+	uint16_t threshold;
+};
+
+#endif /* __IA_CSS_XNR_TYPES_H */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c
new file mode 100644
index 000000000000..955b6c8a8738
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.c
@@ -0,0 +1,265 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "assert_support.h"
+#endif
+#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)
+
+/*
+#ifdef ISP2401
+ * division look-up table
+ * Refers to XNR3.0.5
+ */
+#define XNR3_LOOK_UP_TABLE_POINTS 16
+
+static const int16_t x[XNR3_LOOK_UP_TABLE_POINTS] = {
+1024, 1164, 1320, 1492, 1680, 1884, 2108, 2352,
+2616, 2900, 3208, 3540, 3896, 4276, 4684, 5120};
+
+static const int16_t a[XNR3_LOOK_UP_TABLE_POINTS] = {
+-7213, -5580, -4371, -3421, -2722, -2159, -6950, -5585,
+-4529, -3697, -3010, -2485, -2070, -1727, -1428, 0};
+
+static const int16_t b[XNR3_LOOK_UP_TABLE_POINTS] = {
+4096, 3603, 3178, 2811, 2497, 2226, 1990, 1783,
+1603, 1446, 1307, 1185, 1077, 981, 895, 819};
+
+static const int16_t c[XNR3_LOOK_UP_TABLE_POINTS] = {
+1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+/*
+#endif
+ * 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)
+{
+	int32_t alpha;
+#if defined(XNR_ATE_ROUNDING_BUG)
+	int32_t alpha_unscaled;
+#else
+	int offset = sigma / 2;
+#endif
+	if (sigma < XNR_MIN_SIGMA) {
+		alpha = XNR_MAX_ALPHA;
+	} else {
+#if defined(XNR_ATE_ROUNDING_BUG)
+		/* The scale factor for alpha must be the same as on the ISP,
+		 * For sigma, it must match the public interface. The code
+		 * below mimics the rounding and unintended loss of precision
+		 * of the ATE reference code. It computes an unscaled alpha,
+		 * rounds down, and then scales it to get the required fixed
+		 * point representation. It would have been more precise to
+		 * round after scaling. */
+		alpha_unscaled = IA_CSS_XNR3_SIGMA_SCALE / sigma;
+		alpha = alpha_unscaled * XNR_ALPHA_SCALE_FACTOR;
+#else
+		alpha = ((IA_CSS_XNR3_SIGMA_SCALE * XNR_ALPHA_SCALE_FACTOR) + offset)/ sigma;
+#endif
+
+		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)
+{
+	int32_t isp_coring;
+	int32_t isp_scale = XNR_CORING_SCALE_FACTOR;
+	int32_t host_scale = IA_CSS_XNR3_CORING_SCALE;
+	int32_t 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)
+{
+	int32_t isp_strength;
+	int32_t isp_scale = XNR_BLENDING_SCALE_FACTOR;
+	int32_t host_scale = IA_CSS_XNR3_BLENDING_SCALE;
+	int32_t 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), -XNR_BLENDING_SCALE_FACTOR);
+}
+
+void
+ia_css_xnr3_encode(
+	struct sh_css_isp_xnr3_params *to,
+	const struct ia_css_xnr3_config *from,
+	unsigned 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);
+	int32_t max_diff = (1 << (ISP_VEC_ELEMBITS - 1)) - 1;
+	int32_t min_diff = -(1 << (ISP_VEC_ELEMBITS - 1));
+
+	int32_t alpha_y0 = compute_alpha(from->sigma.y0);
+	int32_t alpha_y1 = compute_alpha(from->sigma.y1);
+	int32_t alpha_u0 = compute_alpha(from->sigma.u0);
+	int32_t alpha_u1 = compute_alpha(from->sigma.u1);
+	int32_t alpha_v0 = compute_alpha(from->sigma.v0);
+	int32_t alpha_v1 = compute_alpha(from->sigma.v1);
+	int32_t alpha_ydiff = (alpha_y1 - alpha_y0) * adjust_factor / kernel_size;
+	int32_t alpha_udiff = (alpha_u1 - alpha_u0) * adjust_factor / kernel_size;
+	int32_t alpha_vdiff = (alpha_v1 - alpha_v0) * adjust_factor / kernel_size;
+
+	int32_t coring_u0 = compute_coring(from->coring.u0);
+	int32_t coring_u1 = compute_coring(from->coring.u1);
+	int32_t coring_v0 = compute_coring(from->coring.v0);
+	int32_t coring_v1 = compute_coring(from->coring.v1);
+	int32_t coring_udiff = (coring_u1 - coring_u0) * adjust_factor / kernel_size;
+	int32_t coring_vdiff = (coring_v1 - coring_v0) * adjust_factor / kernel_size;
+
+	int32_t 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;
+}
+
+#ifdef 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 size)
+{
+	unsigned i, j, base;
+	const unsigned total_blocks = 4;
+	const unsigned 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];
+		}
+	}
+}
+
+#endif
+/* Dummy Function added as the tool expects it*/
+void
+ia_css_xnr3_debug_dtrace(
+	const struct ia_css_xnr3_config *config,
+	unsigned level)
+{
+	(void)config;
+	(void)level;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h
new file mode 100644
index 000000000000..6a86924a71fe
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3.host.h
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+#ifdef ISP2401
+void
+ia_css_xnr3_vmem_encode(
+	struct sh_css_isp_xnr3_vmem_params *to,
+	const struct ia_css_xnr3_config *from,
+	unsigned size);
+
+#endif
+void
+ia_css_xnr3_debug_dtrace(
+	const struct ia_css_xnr3_config *config,
+	unsigned level);
+
+#endif /* __IA_CSS_XNR3_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h
new file mode 100644
index 000000000000..06c24e848234
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_param.h
@@ -0,0 +1,96 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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"
+#ifdef ISP2401
+#include "vmem.h" /* needed for VMEM_ARRAY */
+
+#endif
+
+/* 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      (1 << 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     (1 << 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   (1 << XNR_BLENDING_SCALE_LOG2)
+
+/* XNR3 filter size. Must be 11x11, 9x9 or 5x5. */
+#ifdef FLT_KERNEL_9x9
+#define XNR_FILTER_SIZE             9
+#else
+#ifdef FLT_KERNEL_11x11
+#define XNR_FILTER_SIZE             11
+#else
+#define XNR_FILTER_SIZE             5
+#endif
+#endif
+
+/* 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 {
+	int32_t y0;
+	int32_t u0;
+	int32_t v0;
+	int32_t ydiff;
+	int32_t udiff;
+	int32_t 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 {
+	int32_t u0;
+	int32_t v0;
+	int32_t udiff;
+	int32_t vdiff;
+};
+
+/* XNR3 blending strength on the ISP. */
+struct sh_css_xnr3_blending_params {
+	int32_t 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;
+};
+
+#ifdef 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
+#endif  /*__IA_CSS_XNR3_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h
new file mode 100644
index 000000000000..8f14d1080651
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_types.h
@@ -0,0 +1,98 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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  (1 << 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 (1 << 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 (1 << 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/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_wrapper_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_wrapper_param.h
new file mode 100644
index 000000000000..1a98555fd5d9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/xnr/xnr_3.0/ia_css_xnr3_wrapper_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_WRAPPER_PARAM_H
+#define __IA_CSS_XNR3_WRAPPER_PARAM_H
+
+#include "ia_css_xnr3_param.h"
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c
new file mode 100644
index 000000000000..d8dccce772a9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.c
@@ -0,0 +1,219 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 size)
+{
+	(void)size;
+	/* YNR (Y Noise Reduction) */
+	to->threshold =
+		uDIGIT_FITTING((unsigned)8192, 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 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((unsigned)56, 16, SH_CSS_BAYER_BITS) *
+	     from->ee.threshold) >> 8;
+	to->coring_g =
+	    (uDIGIT_FITTING((unsigned)224, 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((unsigned)32760, 16, SH_CSS_BAYER_BITS);
+	to->clip_plus_g = 0;
+	to->clip_minus_s =
+	    uDIGIT_FITTING((unsigned)504, 16, SH_CSS_BAYER_BITS);
+	to->clip_minus_g =
+	    uDIGIT_FITTING((unsigned)32256, 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 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 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 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 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/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h
new file mode 100644
index 000000000000..b5730df313ef
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr.host.h
@@ -0,0 +1,60 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_yee_encode(
+	struct sh_css_isp_yee_params *to,
+	const struct ia_css_yee_config *from,
+	unsigned size);
+
+void
+ia_css_nr_dump(
+	const struct sh_css_isp_ynr_params *ynr,
+	unsigned level);
+
+void
+ia_css_yee_dump(
+	const struct sh_css_isp_yee_params *yee,
+	unsigned level);
+
+void
+ia_css_nr_debug_dtrace(
+	const struct ia_css_nr_config *config,
+	unsigned level);
+
+void
+ia_css_ee_debug_dtrace(
+	const struct ia_css_ee_config *config,
+	unsigned 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/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h
new file mode 100644
index 000000000000..ad61ec1211e8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_param.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t threshold;
+	int32_t gain_all;
+	int32_t gain_dir;
+	int32_t threshold_cb;
+	int32_t threshold_cr;
+};
+
+/* YEE (Y Edge Enhancement) */
+struct sh_css_isp_yee_params {
+	int32_t dirthreshold_s;
+	int32_t dirthreshold_g;
+	int32_t dirthreshold_width_log2;
+	int32_t dirthreshold_width;
+	int32_t detailgain;
+	int32_t coring_s;
+	int32_t coring_g;
+	int32_t scale_plus_s;
+	int32_t scale_plus_g;
+	int32_t scale_minus_s;
+	int32_t scale_minus_g;
+	int32_t clip_plus_s;
+	int32_t clip_plus_g;
+	int32_t clip_minus_s;
+	int32_t clip_minus_g;
+	int32_t Yclip;
+};
+
+#endif /* __IA_CSS_YNR_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_state.h
new file mode 100644
index 000000000000..b2348b19c3cd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_state.h
@@ -0,0 +1,26 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_YNR_STATE_H
+
+#include "type_support.h"
+#include "vmem.h"
+
+/* YNR (luminance noise reduction) */
+struct sh_css_isp_ynr_vmem_state {
+	VMEM_ARRAY(ynr_buf[4], MAX_VECTORS_PER_BUF_LINE*ISP_NWAY);
+};
+
+#endif /* __IA_CSS_YNR_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h
new file mode 100644
index 000000000000..3f46655bee57
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_1.0/ia_css_ynr_types.h
@@ -0,0 +1,81 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c
new file mode 100644
index 000000000000..44b005004238
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.c
@@ -0,0 +1,125 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 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 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 level);
+
+void
+ia_css_fc_dump(
+	const struct sh_css_isp_fc_params *fc,
+	unsigned level);
+
+void
+ia_css_fc_debug_dtrace(
+	const struct ia_css_fc_config *config,
+	unsigned 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 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/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h
new file mode 100644
index 000000000000..71e89c469e4c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2.host.h
@@ -0,0 +1,56 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 size);
+
+void
+ia_css_fc_encode(
+	struct sh_css_isp_fc_params *to,
+	const struct ia_css_fc_config *from,
+	unsigned size);
+
+void
+ia_css_ynr_dump(
+	const struct sh_css_isp_yee2_params *yee2,
+	unsigned level);
+
+void
+ia_css_fc_dump(
+	const struct sh_css_isp_fc_params *fc,
+	unsigned level);
+
+void
+ia_css_fc_debug_dtrace(
+	const struct ia_css_fc_config *config,
+	unsigned level);
+
+void
+ia_css_ynr_debug_dtrace(
+	const struct ia_css_ynr_config *config,
+	unsigned level);
+
+#endif /* __IA_CSS_YNR2_HOST_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h
new file mode 100644
index 000000000000..e56b695bef27
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_param.h
@@ -0,0 +1,45 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	int32_t edge_sense_gain_0;
+	int32_t edge_sense_gain_1;
+	int32_t corner_sense_gain_0;
+	int32_t corner_sense_gain_1;
+};
+
+/* Fringe Control */
+struct sh_css_isp_fc_params {
+	int32_t gain_exp;
+	uint16_t coring_pos_0;
+	uint16_t coring_pos_1;
+	uint16_t coring_neg_0;
+	uint16_t coring_neg_1;
+	int32_t gain_pos_0;
+	int32_t gain_pos_1;
+	int32_t gain_neg_0;
+	int32_t gain_neg_1;
+	int32_t crop_pos_0;
+	int32_t crop_pos_1;
+	int32_t crop_neg_0;
+	int32_t crop_neg_1;
+};
+
+#endif /* __IA_CSS_YNR2_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h
new file mode 100644
index 000000000000..e0a0b10ac5fa
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr2_types.h
@@ -0,0 +1,94 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t edge_sense_gain_0;   /**< Sensitivity of edge in dark area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	uint16_t edge_sense_gain_1;   /**< Sensitivity of edge in bright area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	uint16_t corner_sense_gain_0; /**< Sensitivity of corner in dark area.
+					u13.0, [0,8191],
+					default 1000, ineffective 0 */
+	uint16_t 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 {
+	uint8_t  gain_exp;   /**< Common exponent of gains.
+				u8.0, [0,13],
+				default 1, ineffective 0 */
+	uint16_t coring_pos_0; /**< Coring threshold for positive edge in dark area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	uint16_t coring_pos_1; /**< Coring threshold for positive edge in bright area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	uint16_t coring_neg_0; /**< Coring threshold for negative edge in dark area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	uint16_t coring_neg_1; /**< Coring threshold for negative edge in bright area.
+				u0.13, [0,8191],
+				default 0(0), ineffective 0 */
+	uint16_t gain_pos_0; /**< Gain for positive edge in dark area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	uint16_t gain_pos_1; /**< Gain for positive edge in bright area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	uint16_t gain_neg_0; /**< Gain for negative edge in dark area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	uint16_t gain_neg_1; /**< Gain for negative edge in bright area.
+				u0.13, [0,8191],
+				default 4096(0.5), ineffective 0 */
+	uint16_t crop_pos_0; /**< Limit for positive edge in dark area.
+				u0.13, [0,8191],
+				default/ineffective 8191(almost 1.0) */
+	uint16_t crop_pos_1; /**< Limit for positive edge in bright area.
+				u0.13, [0,8191],
+				default/ineffective 8191(almost 1.0) */
+	int16_t  crop_neg_0; /**< Limit for negative edge in dark area.
+				s0.13, [-8192,0],
+				default/ineffective -8192(-1.0) */
+	int16_t  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/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_param.h
new file mode 100644
index 000000000000..48fb7d22d7c1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YNRX_PARAM_H
+#define __IA_CSS_YNRX_PARAM_H
+
+#include "ia_css_ynr2_param.h"
+
+#endif /* __IA_CSS_YNRX_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_state.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_state.h
new file mode 100644
index 000000000000..2516dd3dc12b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/ynr/ynr_2/ia_css_ynr_state.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_STATE_H
+#define __IA_CSS_YNR2_STATE_H
+
+/* Reuse YNR1 states */
+#include "../ynr_1.0/ia_css_ynr_state.h"
+
+#endif /* __IA_CSS_YNR2_STATE_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_load_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_load_param.h
new file mode 100644
index 000000000000..400c6790cbf5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_load_param.h
@@ -0,0 +1,20 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YUV_LOAD_PARAM_H
+#define __IA_CSS_YUV_LOAD_PARAM_H
+
+#include "ia_css_yuv_ls_param.h"
+
+#endif /* __IA_CSS_YUV_LOAD_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_ls_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_ls_param.h
new file mode 100644
index 000000000000..63a8703c9c44
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_ls_param.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YUV_LS_PARAM_H
+#define __IA_CSS_YUV_LS_PARAM_H
+
+#include "type_support.h"
+#ifndef ISP2401
+
+/* The number of load/store kernels in a pipeline can be greater than one.
+ * A kernel can consume more than one input or can produce more
+ * than one output.
+ */
+#define NUM_YUV_LS 2
+
+/** YUV load/store */
+struct sh_css_isp_yuv_ls_isp_config {
+	unsigned base_address[NUM_YUV_LS];
+	unsigned width[NUM_YUV_LS];
+	unsigned height[NUM_YUV_LS];
+	unsigned stride[NUM_YUV_LS];
+};
+
+#else
+#include "../../io_ls/common/ia_css_common_io_types.h"
+#endif
+
+#endif /* __IA_CSS_YUV_LS_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_store_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_store_param.h
new file mode 100644
index 000000000000..69c474ea1ffd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/kernels/yuv_ls/yuv_ls_1.0/ia_css_yuv_store_param.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_YUV_STORE_PARAM_H
+#define __IA_CSS_YUV_STORE_PARAM_H
+
+#include "ia_css_yuv_ls_param.h"
+
+
+#endif /* __IA_CSS_YUV_STORE_PARAM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/input_buf.isp.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/input_buf.isp.h
new file mode 100644
index 000000000000..32714d5870cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/input_buf.isp.h
@@ -0,0 +1,73 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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
+#if (defined(__ISP) && (!defined(MODE) || MODE != IA_CSS_BINARY_MODE_COPY))
+
+#if ENABLE_CONTINUOUS
+typedef struct {
+  tmemvectoru  raw[INPUT_BUF_HEIGHT][INPUT_BUF_LINES][MAX_VECTORS_PER_INPUT_LINE_CONT]; /* 2 bayer lines */
+  /* Two more lines for SP raw copy efficiency */
+#ifndef ENABLE_REDUCED_INPUT_BUFFER
+  /* "Workaround" solution in the case that space needed vmem exceeds the size of the vmem. */
+  /* Since in theory this buffer is not needed for IPU 2.2/2.3,  */
+  /* the workaround solution will not be needed (and the whole buffer) after the code refactoring. */
+  tmemvectoru _raw[INPUT_BUF_HEIGHT][INPUT_BUF_LINES][MAX_VECTORS_PER_INPUT_LINE_CONT]; /* 2 bayer lines */
+#endif
+} input_line_type;
+#else /* ENABLE CONTINUOUS == 0 */
+typedef struct {
+  tmemvectoru  raw[INPUT_BUF_HEIGHT][INPUT_BUF_LINES][MAX_VECTORS_PER_INPUT_LINE]; /* 2 bayer lines */
+} input_line_type;
+#endif /* ENABLE_CONTINUOUS */
+
+#endif /*MODE*/
+
+#endif /* _INPUT_BUF_ISP_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h
new file mode 100644
index 000000000000..005eaaa9eb6c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_const.h
@@ -0,0 +1,498 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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++ */
+
+/* ISP binary identifiers.
+   These determine the order in which the binaries are looked up, do not change
+   this!
+   Also, the SP firmware uses this same order (isp_loader.hive.c).
+   Also, gen_firmware.c uses this order in its firmware_header.
+*/
+/* The binary id is used in pre-processor expressions so we cannot
+ * use an enum here. */
+ /* 24xx pipelines*/
+#define SH_CSS_BINARY_ID_COPY                      0
+#define SH_CSS_BINARY_ID_BAYER_DS                  1
+#define SH_CSS_BINARY_ID_VF_PP_FULL                2
+#define SH_CSS_BINARY_ID_VF_PP_OPT                 3
+#define SH_CSS_BINARY_ID_YUV_SCALE                 4
+#define SH_CSS_BINARY_ID_CAPTURE_PP                5
+#define SH_CSS_BINARY_ID_PRE_ISP                   6
+#define SH_CSS_BINARY_ID_PRE_ISP_ISP2              7
+#define SH_CSS_BINARY_ID_GDC                       8
+#define SH_CSS_BINARY_ID_POST_ISP                  9
+#define SH_CSS_BINARY_ID_POST_ISP_ISP2            10
+#define SH_CSS_BINARY_ID_ANR                      11
+#define SH_CSS_BINARY_ID_ANR_ISP2                 12
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_DS          13
+#define SH_CSS_BINARY_ID_PREVIEW_DS               14
+#define SH_CSS_BINARY_ID_PREVIEW_DEC              15
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_BDS125_ISP2 16
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_DPC_BDS150_ISP2 17
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_BDS150_ISP2 18
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_DPC_BDS200_ISP2 19
+#define SH_CSS_BINARY_ID_PREVIEW_CONT_BDS200_ISP2 20
+#define SH_CSS_BINARY_ID_PREVIEW_DZ               21
+#define SH_CSS_BINARY_ID_PREVIEW_DZ_ISP2          22
+#define SH_CSS_BINARY_ID_PRIMARY_DS               23
+#define SH_CSS_BINARY_ID_PRIMARY_VAR              24
+#define SH_CSS_BINARY_ID_PRIMARY_VAR_ISP2         25
+#define SH_CSS_BINARY_ID_PRIMARY_SMALL            26
+#define SH_CSS_BINARY_ID_PRIMARY_STRIPED          27
+#define SH_CSS_BINARY_ID_PRIMARY_STRIPED_ISP2     28
+#define SH_CSS_BINARY_ID_PRIMARY_8MP              29
+#define SH_CSS_BINARY_ID_PRIMARY_14MP             30
+#define SH_CSS_BINARY_ID_PRIMARY_16MP             31
+#define SH_CSS_BINARY_ID_PRIMARY_REF              32
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE0        33
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE1        34
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE2        35
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE3        36
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE4        37
+#define SH_CSS_BINARY_ID_PRIMARY_ISP261_STAGE5        38
+#define SH_CSS_BINARY_ID_VIDEO_OFFLINE            39
+#define SH_CSS_BINARY_ID_VIDEO_DS                 40
+#define SH_CSS_BINARY_ID_VIDEO_YUV_DS             41
+#define SH_CSS_BINARY_ID_VIDEO_DZ                 42
+#define SH_CSS_BINARY_ID_VIDEO_DZ_2400_ONLY       43
+#define SH_CSS_BINARY_ID_VIDEO_HIGH               44
+#define SH_CSS_BINARY_ID_VIDEO_NODZ               45
+#define SH_CSS_BINARY_ID_VIDEO_CONT_MULTIBDS_ISP2_MIN 46
+#define SH_CSS_BINARY_ID_VIDEO_CONT_BDS_300_600_ISP2_MIN 47
+#define SH_CSS_BINARY_ID_VIDEO_CONT_DPC_BDS150_ISP2_MIN 48
+#define SH_CSS_BINARY_ID_VIDEO_CONT_BDS150_ISP2_MIN   49
+#define SH_CSS_BINARY_ID_VIDEO_CONT_DPC_BDS200_ISP2_MIN   50
+#define SH_CSS_BINARY_ID_VIDEO_CONT_BDS200_ISP2_MIN   51
+#define SH_CSS_BINARY_ID_VIDEO_CONT_NOBDS_ISP2_MIN    52
+#define SH_CSS_BINARY_ID_VIDEO_DZ_ISP2_MIN      53
+#define SH_CSS_BINARY_ID_VIDEO_DZ_ISP2          54
+#define SH_CSS_BINARY_ID_VIDEO_LP_ISP2          55
+#define SH_CSS_BINARY_ID_RESERVED1              56
+#define SH_CSS_BINARY_ID_ACCELERATION           57
+#define SH_CSS_BINARY_ID_PRE_DE_ISP2            58
+#define SH_CSS_BINARY_ID_KERNEL_TEST_LOAD_STORE 59
+#define SH_CSS_BINARY_ID_CAPTURE_PP_BLI         60
+#define SH_CSS_BINARY_ID_CAPTURE_PP_LDC         61
+#ifdef ISP2401
+#define SH_CSS_BINARY_ID_PRIMARY_STRIPED_ISP2_XNR      62
+#endif
+
+/* skycam kerneltest pipelines */
+#ifndef ISP2401
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_NORM              120
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_NORM_STRIPED      121
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_LIN               122
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_LIN_STRIPED       123
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_SHD           124
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_SHD_STRIPED   125
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AWB           126
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_3A                127
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_3A_STRIPED        128
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AF            129
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OBGRID            130
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_BAYER_DENOISE       131
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_BAYER_DENOISE_STRIPED 132
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_DEMOSAIC            133
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP1_C0            134
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP2               135
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_REF               136
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_REF_STRIPED       137
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_REF           138
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DVS               139
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR               140
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_STRIPED       141
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_BLENDING      142
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR_BLOCK         143
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AE            144
+#define SH_CSS_BINARY_ID_VIDEO_RAW                          145
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AWB_FR        146
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DM_RGBPP          147
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DM_RGBPP_STRIPED  148
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_ANR                 149
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_IF                150
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_IF_STRIPED        151
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SYSTEM     152
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR_STRIPED       153
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DVS_STRIPED       154
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OBGRID_STRIPED    155
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV          156
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV_BLOCK    157
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV16_BLOCK  158
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV16_STRIPED 159
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_BLOCK_STRIPED 160
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_INPUT_YUV         161
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_YUV        162
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_YUV_16     163
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SPLIT      164
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SYSTEM_STRIPED 165
+
+#else
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_NORM              121
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_NORM_STRIPED      122
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OBGRID            123
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OBGRID_STRIPED    124
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_LIN               125
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_LIN_STRIPED       126
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_SHD           127
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_SHD_STRIPED   128
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AE            129
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AWB           130
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AF            131
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_ACC_AWB_FR        132
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_3A                133
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_3A_STRIPED        134
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_BAYER_DENOISE       135
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_BAYER_DENOISE_STRIPED 136
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_ANR                 137
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_ANR_STRIPED         138
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_DEMOSAIC            139
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DM_RGBPP          140
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DM_RGBPP_STRIPED  141
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP1_C0            142
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP2               143
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP2_STRIPED       144
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_REF           145
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR               146
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_STRIPED       147
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_XNR_BLENDING      148
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_REF               149
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_REF_STRIPED       150
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DVS               151
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_DVS_STRIPED       152
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_DVS_STAT_C0         153
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR_BLOCK         154
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR_STRIPED       155
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SYSTEM     156
+#define SH_CSS_BINARY_ID_VIDEO_RAW                          157
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV          158
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV_BLOCK    159
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV16_BLOCK  160
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_YUV16_STRIPED 161
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_COPY_BLOCK_STRIPED 162
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_INPUT_YUV         163
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_YUV        164
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_YUV_16     165
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SPLIT      166
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_OUTPUT_SYSTEM_STRIPED 167
+#define SH_CSS_BINARY_ID_COPY_KERNELTEST_OUTPUT_SYSTEM      168
+#endif
+
+/* skycam partial test pipelines*/
+#ifndef ISP2401
+#define SH_CSS_BINARY_ID_IF_TO_DPC                          201
+#define SH_CSS_BINARY_ID_IF_TO_BDS                          202
+#else
+#define SH_CSS_BINARY_ID_IF_TO_BDS                          201
+#define SH_CSS_BINARY_ID_IF_TO_BDS_STRIPED                  202
+#endif
+#define SH_CSS_BINARY_ID_IF_TO_NORM                         203
+#ifndef ISP2401
+#define SH_CSS_BINARY_ID_IF_TO_OB                           204
+#define SH_CSS_BINARY_ID_IF_TO_LIN                          205
+#define SH_CSS_BINARY_ID_IF_TO_SHD                          206
+#define SH_CSS_BINARY_ID_IF_TO_BNR                          207
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP_NV12_16                208
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP                        210
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1                        211
+#define SH_CSS_BINARY_ID_IF_TO_DM                           214
+#define SH_CSS_BINARY_ID_IF_TO_YUVP2_C0                     216
+#define SH_CSS_BINARY_ID_IF_TO_YUVP2_ANR_VIA_ISP            217
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_DVS                    218
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_TNR                    219
+#define SH_CSS_BINARY_ID_IF_TO_BDS_STRIPED                  224
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_ANR_STRIPED         225
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_YUVP2_STRIPED       227
+#define SH_CSS_BINARY_ID_IF_TO_BDS_RGBP_DVS_STAT_C0         228
+#define SH_CSS_BINARY_ID_IF_TO_BDS_RGBP_DVS_STAT_C0_STRIPED 229
+#define SH_CSS_BINARY_ID_IF_TO_REF                          236
+#define SH_CSS_BINARY_ID_IF_TO_DVS_STRIPED                  237
+#define SH_CSS_BINARY_ID_IF_TO_YUVP2_STRIPED                238
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_STRIPED                239
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP_STRIPED                240
+#define SH_CSS_BINARY_ID_IF_TO_ANR_STRIPED                  241
+#define SH_CSS_BINARY_ID_IF_TO_BNR_STRIPED                  242
+#define SH_CSS_BINARY_ID_IF_TO_SHD_STRIPED                  243
+#define SH_CSS_BINARY_ID_IF_TO_LIN_STRIPED                  244
+#define SH_CSS_BINARY_ID_IF_TO_OB_STRIPED                   245
+#define SH_CSS_BINARY_ID_IF_TO_NORM_STRIPED                 248
+#define SH_CSS_BINARY_ID_COPY_KERNELTEST_OUTPUT_SYSTEM      253
+#define SH_CSS_BINARY_ID_IF_TO_XNR                          256
+#define SH_CSS_BINARY_ID_IF_TO_XNR_STRIPED                  257
+#define SH_CSS_BINARY_ID_IF_TO_REF_STRIPED                  258
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_OSYS                   259
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_C0                     262
+#define SH_CSS_BINARY_ID_IF_TO_XNR_PRIMARY                  263
+#define SH_CSS_BINARY_ID_IF_TO_XNR_PRIMARY_STRIPED          264
+#define SH_CSS_BINARY_ID_IF_TO_ANR                          265
+#define SH_CSS_BINARY_ID_VIDEO_TEST_ACC_DVS_STAT_C0         266
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_OSYS_STRIPED           270
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PRIMARY                 276
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PRIMARY_STRIPED         277
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_C0_STRIPED             278
+#else
+#define SH_CSS_BINARY_ID_IF_TO_NORM_STRIPED                 204
+#define SH_CSS_BINARY_ID_IF_TO_OB                           205
+#define SH_CSS_BINARY_ID_IF_TO_OB_STRIPED                   206
+#define SH_CSS_BINARY_ID_IF_TO_LIN                          207
+#define SH_CSS_BINARY_ID_IF_TO_LIN_STRIPED                  208
+#define SH_CSS_BINARY_ID_IF_TO_SHD                          209
+#define SH_CSS_BINARY_ID_IF_TO_SHD_STRIPED                  210
+#define SH_CSS_BINARY_ID_IF_TO_BNR                          211
+#define SH_CSS_BINARY_ID_IF_TO_BNR_STRIPED                  212
+#define SH_CSS_BINARY_ID_IF_TO_ANR                          213
+#define SH_CSS_BINARY_ID_IF_TO_ANR_STRIPED                  214
+#define SH_CSS_BINARY_ID_IF_TO_DM                           215
+#define SH_CSS_BINARY_ID_IF_TO_BDS_RGBP_DVS_STAT_C0         216
+#define SH_CSS_BINARY_ID_IF_TO_BDS_RGBP_DVS_STAT_C0_STRIPED 217
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP                        218
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP_NV12_16                219
+#define SH_CSS_BINARY_ID_IF_TO_RGBPP_STRIPED                220
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1                        221
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_STRIPED                222
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_C0                     223
+#define SH_CSS_BINARY_ID_IF_TO_YUVP2_C0                     224
+#define SH_CSS_BINARY_ID_IF_TO_YUVP2_STRIPED                225
+#define SH_CSS_BINARY_ID_IF_TO_XNR                          226
+#define SH_CSS_BINARY_ID_IF_TO_XNR_STRIPED                  227
+#define SH_CSS_BINARY_ID_IF_TO_XNR_PRIMARY                  228
+#define SH_CSS_BINARY_ID_IF_TO_XNR_PRIMARY_STRIPED          229
+#define SH_CSS_BINARY_ID_IF_TO_REF                          230
+#define SH_CSS_BINARY_ID_IF_TO_REF_STRIPED                  231
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_DVS                    232
+#define SH_CSS_BINARY_ID_IF_TO_DVS_STRIPED                  233
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_TNR                    234
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_OSYS                   235
+#define SH_CSS_BINARY_ID_VIDEO_IF_TO_OSYS_STRIPED           236
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PRIMARY                 237
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PRIMARY_STRIPED         238
+#define SH_CSS_BINARY_ID_IF_TO_YUVP1_C0_STRIPED             239
+#define SH_CSS_BINARY_ID_VIDEO_YUVP1_TO_OSYS                240
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PREVIEW                 241
+#define SH_CSS_BINARY_ID_IF_TO_OSYS_PREVIEW_STRIPED         242
+#endif
+
+/* Skycam IR camera binaries */
+#ifndef ISP2401
+#define SH_CSS_BINARY_ID_IR_IF_TO_OSYS_NO_XNR               300
+#define SH_CSS_BINARY_ID_VIDEO_IR_IF_TO_OSYS_NO_DVS_NO_TNR_NO_XNR    301
+#define SH_CSS_BINARY_ID_IR_IF_TO_OSYS_NO_XNR_NO_DVS_PRIMARY         302
+#else
+#define SH_CSS_BINARY_ID_IR_IF_TO_OSYS                      300
+#define SH_CSS_BINARY_ID_IR_IF_TO_OSYS_NO_TNR3              301
+#define SH_CSS_BINARY_ID_IR_IF_TO_OSYS_PRIMARY              302
+
+/* Binaries under development */
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR3              401
+#define SH_CSS_BINARY_ID_VIDEO_KERNELTEST_TNR3_STRIPED      402
+
+#endif
+
+#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)
+
+#if ISP_VEC_NELEMS == 64
+#define ISP_NWAY_LOG2  6
+#elif ISP_VEC_NELEMS == 32
+#define ISP_NWAY_LOG2  5
+#elif ISP_VEC_NELEMS == 16
+#define ISP_NWAY_LOG2  4
+#elif ISP_VEC_NELEMS == 8
+#define ISP_NWAY_LOG2  3
+#else
+#error "isp_const.h ISP_VEC_NELEMS must be one of {8, 16, 32, 64}"
+#endif
+
+/* *****************************
+ * 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])
+ */
+#if defined(HAS_RES_MGR)
+#define MAX_VECTORS_PER_OUTPUT_LINE \
+	(CEIL_DIV(CEIL_DIV(ISP_MAX_OUTPUT_WIDTH, ISP_NUM_STRIPES) + ISP_LEFT_PADDING, ISP_VEC_NELEMS) + \
+	ITERATOR_VECTOR_INCREMENT)
+
+#define MAX_VECTORS_PER_INPUT_LINE	CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS)
+#define MAX_VECTORS_PER_INPUT_STRIPE	(CEIL_ROUND_DIV_STRIPE(CEIL_DIV(ISP_MAX_INPUT_WIDTH, ISP_VEC_NELEMS) , \
+							      ISP_NUM_STRIPES, \
+							      ISP_LEFT_PADDING_VECS) + \
+							      ITERATOR_VECTOR_INCREMENT)
+#else /* !defined(HAS_RES_MGR)*/
+#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)
+#endif /* HAS_RES_MGR */
+
+
+/* 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)
+
+#if defined(HAS_RES_MGR)
+#define RAW_BUF_STRIDE (MAX_VECTORS_PER_INPUT_STRIPE)
+#else /* !defined(HAS_RES_MGR) */
+#define RAW_BUF_STRIDE \
+	(BINARY_ID == SH_CSS_BINARY_ID_POST_ISP ? MAX_VECTORS_PER_INPUT_CHUNK : \
+	 ISP_NUM_STRIPES > 1 ? MAX_VECTORS_PER_INPUT_STRIPE+_ISP_EXTRA_PADDING_VECS : \
+	 !ENABLE_CONTINUOUS ? MAX_VECTORS_PER_INPUT_LINE : \
+	 MAX_VECTORS_PER_INPUT_CHUNK)
+#endif /* HAS_RES_MGR */
+
+/* [isp vmem] table size[vectors] per line per color (GR,R,B,GB),
+   multiples of NWAY */
+#define 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/atomisp2/css2400/isp/modes/interface/isp_exprs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h
new file mode 100644
index 000000000000..8b59a8caec52
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_exprs.h
@@ -0,0 +1,309 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef _COMMON_ISP_EXPRS_H_
+#define _COMMON_ISP_EXPRS_H_
+
+/* Binary independent pre-processor expressions */
+
+#include "sh_css_defs.h"
+#include "isp_const.h"
+
+#ifdef __HOST
+#error "isp_exprs.h: Do not include on HOST, contains ISP specific defines"
+#endif
+
+#ifndef __ISP
+#if defined(MODE)
+#define MODE aap
+#error "isp_exprs.h: is mode independent, but MODE is set"
+#endif
+#if defined(VARIABLE_RESOLUTION)
+#define VARIABLE_RESOLUTION noot
+#error "isp_exprs.h: is mode independent, but VARIABLE_RESOLUTION is set"
+#endif
+#if defined(DECI_FACTOR_LOG2)
+#define DECI_FACTOR_LOG2 mies
+#error "isp_exprs.h: is mode independent, but DECI_FACTOR_LOG2 is set"
+#endif
+#endif
+
+#define LOG_VECTOR_STEP        _ISP_LOG_VECTOR_STEP(MODE)
+/* should be even and multiple of vf downscaling */
+#define ISP_OUTPUT_CHUNK_LOG_FACTOR (MAX_VF_LOG_DOWNSCALE<=1 ? LOG_VECTOR_STEP : \
+					umax(VF_LOG_DOWNSCALE, LOG_VECTOR_STEP))
+
+#define CEIL_DIV_CHUNKS(n,c)    ((c) == 1 ? (n) \
+		  		          : CEIL_SHIFT(CEIL_DIV((n), (c)), ISP_OUTPUT_CHUNK_LOG_FACTOR)<<ISP_OUTPUT_CHUNK_LOG_FACTOR)
+
+
+#define ISP_VARIABLE_INPUT     (ISP_INPUT == IA_CSS_BINARY_INPUT_VARIABLE)
+
+/* Binary independent versions, see isp_defs.h for binary dependent ones */
+#ifndef __ISP 
+#define IMAGEFORMAT_IS_RAW(fmt)			((fmt) == IA_CSS_FRAME_FORMAT_RAW)
+
+#define IMAGEFORMAT_IS_RAW_INTERLEAVED(fmt) 	((fmt) == IA_CSS_FRAME_FORMAT_RAW)
+
+#define IMAGEFORMAT_IS_RGB(fmt) 		((fmt) == IA_CSS_FRAME_FORMAT_RGBA888 || (fmt) == IA_CSS_FRAME_FORMAT_PLANAR_RGB888 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_RGB565)
+
+#define IMAGEFORMAT_IS_RGB_INTERLEAVED(fmt) 	((fmt) == IA_CSS_FRAME_FORMAT_RGBA888 || (fmt) == IA_CSS_FRAME_FORMAT_RGB565)
+
+#define IMAGEFORMAT_UV_INTERLEAVED(fmt) 	((fmt) == IA_CSS_FRAME_FORMAT_NV11    || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV12    || (fmt) == IA_CSS_FRAME_FORMAT_NV21 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV16    || (fmt) == IA_CSS_FRAME_FORMAT_NV61 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_UYVY    || (fmt) == IA_CSS_FRAME_FORMAT_YUYV || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV12_16 || (fmt) == IA_CSS_FRAME_FORMAT_NV12_TILEY)
+
+#define IMAGEFORMAT_YUV_INTERLEAVED(fmt)	((fmt) == IA_CSS_FRAME_FORMAT_UYVY    || (fmt) == IA_CSS_FRAME_FORMAT_YUYV)
+
+#define IMAGEFORMAT_INTERLEAVED(fmt)		(IMAGEFORMAT_UV_INTERLEAVED(fmt) || IMAGEFORMAT_IS_RGB_INTERLEAVED(fmt))
+
+#define IMAGEFORMAT_SUB_SAMPL_420(fmt)		((fmt) == IA_CSS_FRAME_FORMAT_YUV420 || (fmt) == IA_CSS_FRAME_FORMAT_YV12 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV12   || (fmt) == IA_CSS_FRAME_FORMAT_NV21 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV12_16 || (fmt) == IA_CSS_FRAME_FORMAT_NV12TILEY)
+
+#define IMAGEFORMAT_SUB_SAMPL_422(fmt)		((fmt) == IA_CSS_FRAME_FORMAT_YUV422 || (fmt) == IA_CSS_FRAME_FORMAT_YV16 || \
+						 (fmt) == IA_CSS_FRAME_FORMAT_NV16   || (fmt) == IA_CSS_FRAME_FORMAT_NV61)
+
+#define IMAGEFORMAT_SUB_SAMPL_444(fmt) 		((fmt) == IA_CSS_FRAME_FORMAT_YUV444)
+
+#define IMAGEFORMAT_UV_SWAPPED(fmt)		((fmt) == IA_CSS_FRAME_FORMAT_NV21 || (fmt) == IA_CSS_FRAME_FORMAT_NV61)
+
+#define IMAGEFORMAT_IS_RGBA(fmt)		((fmt) == IA_CSS_FRAME_FORMAT_RGBA888)
+
+#define IMAGEFORMAT_IS_NV11(fmt)		((fmt) == IA_CSS_FRAME_FORMAT_NV11)
+
+#define IMAGEFORMAT_IS_16BIT(fmt)               ((fmt) == IA_CSS_FRAME_FORMAT_YUV420_16 || (fmt) == IA_CSS_FRAME_FORMAT_NV12_16 || (fmt) == IA_CSS_FRAME_FORMAT_YUV422_16)
+
+#endif
+
+
+/******** GDCAC settings *******/
+#define GDCAC_BPP			ISP_VEC_ELEMBITS  /* We use 14 bits per pixel component for the GDCAC mode */
+#define GDC_INPUT_BLOCK_WIDTH		2 /* Two vectors are needed */
+#define GDC_OUTPUT_BLOCK_WIDTH		1 /* One vector is produced */
+
+#if ISP_VEC_NELEMS == 16
+/* For 16*16 output block, the distortion fits in 13.312 lines __ALWAYS__ */
+#define GDC_INPUT_BLOCK_HEIGHT		14
+#elif ISP_VEC_NELEMS == 64
+/* For 64*64 output block, the distortion fits in 47.    lines __ALWAYS__ */
+#define GDC_INPUT_BLOCK_HEIGHT		48
+#endif
+/*******************************/
+
+
+#define ENABLE_HUP ((isp_input_width  - isp_envelope_width)  < isp_output_width)
+#define ENABLE_VUP ((isp_input_height - isp_envelope_height) < isp_output_height)
+
+#define ISP_INPUT_WIDTH  (ENABLE_DS | ENABLE_HUP ? isp_input_width  : ISP_INTERNAL_WIDTH)
+#define ISP_INPUT_HEIGHT (ENABLE_DS | ENABLE_VUP ? isp_input_height : isp_internal_height)
+
+#define DECI_FACTOR_LOG2 (ISP_FIXED_S3A_DECI_LOG ? ISP_FIXED_S3A_DECI_LOG : isp_deci_log_factor)
+
+#define ISP_S3ATBL_WIDTH \
+  _ISP_S3ATBL_ISP_WIDTH(_ISP_S3A_ELEMS_ISP_WIDTH((ENABLE_HUP ? ISP_INTERNAL_WIDTH : ISP_INPUT_WIDTH), ISP_LEFT_CROPPING), \
+    DECI_FACTOR_LOG2)
+#define S3ATBL_WIDTH_BYTES   (sizeof(struct ia_css_3a_output) * ISP_S3ATBL_WIDTH)
+#define S3ATBL_WIDTH_SHORTS  (S3ATBL_WIDTH_BYTES / sizeof(short))
+
+/* should be even?? */
+#define ISP_UV_OUTPUT_CHUNK_VECS   	CEIL_DIV(ISP_OUTPUT_CHUNK_VECS, 2)
+
+
+#if defined(__ISP) || defined(INIT_VARS)
+
+#define ISP_USE_IF	(ISP_INPUT == IA_CSS_BINARY_INPUT_MEMORY ? 0 : \
+	       	         ISP_INPUT == IA_CSS_BINARY_INPUT_SENSOR ? 1 : \
+	                 isp_online)
+
+#define ISP_DVS_ENVELOPE_WIDTH  0
+#define ISP_DVS_ENVELOPE_HEIGHT 0
+
+#define _ISP_INPUT_WIDTH_VECS	_ISP_VECS(ISP_INPUT_WIDTH)
+
+#if !defined(__ISP) || (VARIABLE_RESOLUTION && !__HOST)
+#define ISP_INPUT_WIDTH_VECS	isp_vectors_per_input_line
+#else
+#define ISP_INPUT_WIDTH_VECS	_ISP_INPUT_WIDTH_VECS
+#endif
+
+#if !defined(__ISP) || VARIABLE_RESOLUTION
+#define ISP_INTERNAL_WIDTH_VECS		isp_vectors_per_line
+#else
+#define ISP_INTERNAL_WIDTH_VECS		_ISP_INTERNAL_WIDTH_VECS
+#endif
+
+#define _ISP_INTERNAL_HEIGHT	__ISP_INTERNAL_HEIGHT(isp_output_height, ISP_TOP_CROPPING, ISP_DVS_ENVELOPE_HEIGHT)
+
+#define ISP_INTERNAL_HEIGHT	isp_internal_height
+
+#define _ISP_INTERNAL_WIDTH	__ISP_INTERNAL_WIDTH(ISP_OUTPUT_WIDTH, ISP_DVS_ENVELOPE_WIDTH, \
+			     			     ISP_LEFT_CROPPING, MODE, ISP_C_SUBSAMPLING, \
+						     OUTPUT_NUM_CHUNKS, ISP_PIPELINING)
+
+#define ISP_UV_INTERNAL_WIDTH	(ISP_INTERNAL_WIDTH / 2)
+#define ISP_UV_INTERNAL_HEIGHT	(ISP_INTERNAL_HEIGHT / 2)
+
+#define _ISP_INTERNAL_WIDTH_VECS	(_ISP_INTERNAL_WIDTH / ISP_VEC_NELEMS)
+#define _ISP_UV_INTERNAL_WIDTH_VECS	CEIL_DIV(ISP_UV_INTERNAL_WIDTH, ISP_VEC_NELEMS)
+
+#define ISP_VF_OUTPUT_WIDTH		_ISP_VF_OUTPUT_WIDTH(ISP_VF_OUTPUT_WIDTH_VECS)
+#define ISP_VF_OUTPUT_HEIGHT		_ISP_VF_OUTPUT_HEIGHT(isp_output_height, VF_LOG_DOWNSCALE)
+
+#if defined (__ISP) && !VARIABLE_RESOLUTION
+#define ISP_INTERNAL_WIDTH         _ISP_INTERNAL_WIDTH
+#define ISP_VF_OUTPUT_WIDTH_VECS   _ISP_VF_OUTPUT_WIDTH_VECS
+#else
+#define ISP_INTERNAL_WIDTH         (VARIABLE_RESOLUTION ? isp_internal_width : _ISP_INTERNAL_WIDTH)
+#define ISP_VF_OUTPUT_WIDTH_VECS   (VARIABLE_RESOLUTION ? isp_vf_output_width_vecs : _ISP_VF_OUTPUT_WIDTH_VECS)
+#endif
+
+#if defined(__ISP) && !VARIABLE_RESOLUTION
+#define ISP_OUTPUT_WIDTH        ISP_MAX_OUTPUT_WIDTH
+#define VF_LOG_DOWNSCALE        MAX_VF_LOG_DOWNSCALE
+#else
+#define ISP_OUTPUT_WIDTH        isp_output_width
+#define VF_LOG_DOWNSCALE        isp_vf_downscale_bits
+#endif
+
+#if !defined(__ISP) || VARIABLE_RESOLUTION
+#define _ISP_MAX_VF_OUTPUT_WIDTH	__ISP_MAX_VF_OUTPUT_WIDTH(2*SH_CSS_MAX_VF_WIDTH, ISP_LEFT_CROPPING)
+#elif defined(MODE) && MODE == IA_CSS_BINARY_MODE_PRIMARY && ISP_OUTPUT_WIDTH > 3328
+/* Because of vmem issues, should be fixed later */
+#define _ISP_MAX_VF_OUTPUT_WIDTH	(SH_CSS_MAX_VF_WIDTH - 2*ISP_VEC_NELEMS + (ISP_LEFT_CROPPING ? 2 * ISP_VEC_NELEMS : 0))
+#else
+#define _ISP_MAX_VF_OUTPUT_WIDTH	(ISP_VF_OUTPUT_WIDTH + (ISP_LEFT_CROPPING ? (2 >> VF_LOG_DOWNSCALE) * ISP_VEC_NELEMS : 0))
+#endif
+
+#define ISP_MAX_VF_OUTPUT_VECS 		CEIL_DIV(_ISP_MAX_VF_OUTPUT_WIDTH, ISP_VEC_NELEMS)
+
+
+
+#define ISP_MIN_STRIPE_WIDTH (ISP_PIPELINING * (1<<_ISP_LOG_VECTOR_STEP(MODE)))
+
+/******* STRIPING-RELATED MACROS *******/
+#define NO_STRIPING (ISP_NUM_STRIPES == 1)
+
+#if defined(HAS_RES_MGR)
+
+#define ISP_OUTPUT_CHUNK_VECS ISP_INTERNAL_WIDTH_VECS
+
+#if defined(__ISP)
+#define VECTORS_PER_LINE ISP_INTERNAL_WIDTH_VECS
+#else
+#define VECTORS_PER_LINE \
+	(NO_STRIPING 	? ISP_INTERNAL_WIDTH_VECS \
+				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INTERNAL_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
+#endif
+
+#define VECTORS_PER_INPUT_LINE \
+	(NO_STRIPING 	? ISP_INPUT_WIDTH_VECS \
+				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INPUT_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
+
+#else
+
+#define ISP_OUTPUT_CHUNK_VECS \
+	(NO_STRIPING 	? CEIL_DIV_CHUNKS(ISP_OUTPUT_VECS_EXTRA_CROP, OUTPUT_NUM_CHUNKS) \
+				: ISP_IO_STRIPE_WIDTH_VECS(ISP_OUTPUT_VECS_EXTRA_CROP, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
+
+#define VECTORS_PER_LINE \
+	(NO_STRIPING 	? ISP_INTERNAL_WIDTH_VECS \
+				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INTERNAL_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH) )
+
+#define VECTORS_PER_INPUT_LINE \
+	(NO_STRIPING 	? ISP_INPUT_WIDTH_VECS \
+				: ISP_IO_STRIPE_WIDTH_VECS(ISP_INPUT_WIDTH_VECS, ISP_LEFT_PADDING_VECS, ISP_NUM_STRIPES, ISP_MIN_STRIPE_WIDTH)+_ISP_EXTRA_PADDING_VECS)
+
+#endif
+
+#define ISP_MAX_VF_OUTPUT_STRIPE_VECS \
+	(NO_STRIPING 	? ISP_MAX_VF_OUTPUT_VECS \
+				: CEIL_MUL(CEIL_DIV(ISP_MAX_VF_OUTPUT_VECS, ISP_NUM_STRIPES), 2))
+#define _ISP_VF_OUTPUT_WIDTH_VECS \
+	(NO_STRIPING 	? __ISP_VF_OUTPUT_WIDTH_VECS(ISP_OUTPUT_WIDTH, VF_LOG_DOWNSCALE) \
+				: __ISP_VF_OUTPUT_WIDTH_VECS(CEIL_DIV(ISP_OUTPUT_WIDTH, ISP_NUM_STRIPES), VF_LOG_DOWNSCALE))
+
+#define ISP_IO_STRIPE_WIDTH_VECS(width, padding, num_stripes, min_stripe) \
+	MAX(CEIL_MUL(padding + CEIL_DIV(width-padding, num_stripes) \
+		   , 2) \
+	  , min_stripe)
+////////// INPUT & INTERNAL
+/* should be even */
+#define INPUT_NUM_CHUNKS	OUTPUT_NUM_CHUNKS
+
+#define INPUT_VECTORS_PER_CHUNK	CEIL_DIV_CHUNKS(VECTORS_PER_INPUT_LINE, INPUT_NUM_CHUNKS)
+
+/* only for ISP code, will be removed: */
+#define VECTORS_PER_FULL_LINE         	ISP_INTERNAL_WIDTH_VECS
+#define VECTORS_PER_INPUT_FULL_LINE   	ISP_INPUT_WIDTH_VECS
+
+////////// OUTPUT
+/* should at least even and also multiple of vf scaling */
+#define ISP_OUTPUT_VECS_EXTRA_CROP	CEIL_DIV(ISP_OUTPUT_WIDTH_EXTRA_CROP, ISP_VEC_NELEMS)
+
+/* Output is decoupled from input */
+#define ISP_OUTPUT_WIDTH_EXTRA_CROP	CEIL_MUL(CEIL_MUL((ENABLE_DVS_ENVELOPE ? ISP_OUTPUT_WIDTH : ISP_INTERNAL_WIDTH), 2*ISP_VEC_NELEMS), \
+		 				ISP_C_SUBSAMPLING * OUTPUT_NUM_CHUNKS *  HIVE_ISP_DDR_WORD_BYTES)
+
+#define ISP_MAX_VF_OUTPUT_CHUNK_VECS \
+        (NO_CHUNKING ? ISP_MAX_VF_OUTPUT_STRIPE_VECS \
+                                : 2*CEIL_DIV(ISP_MAX_VF_OUTPUT_STRIPE_VECS, 2*OUTPUT_NUM_CHUNKS))
+
+#define OUTPUT_VECTORS_PER_CHUNK	CEIL_DIV_CHUNKS(VECTORS_PER_LINE,OUTPUT_NUM_CHUNKS)
+
+/* should be even?? */
+#if !defined(HAS_RES_MGR)
+#define OUTPUT_C_VECTORS_PER_CHUNK  	CEIL_DIV(OUTPUT_VECTORS_PER_CHUNK, 2)
+#else
+#define OUTPUT_C_VECTORS_PER_CHUNK  	CEIL_DIV(MAX_VECTORS_PER_CHUNK, 2)
+#endif
+
+#ifndef ISP2401
+/**** SCTBL defs *******/
+#define ISP_SCTBL_HEIGHT \
+	_ISP_SCTBL_HEIGHT(ISP_INPUT_HEIGHT, DECI_FACTOR_LOG2)
+
+#endif
+/**** UDS defs *********/
+#define UDS_DMACH_STRIDE_B_IN_Y           (( ISP_INTERNAL_WIDTH   /BITS8_ELEMENTS_PER_XMEM_ADDR)*HIVE_ISP_DDR_WORD_BYTES)
+#define UDS_DMACH_STRIDE_B_IN_C           (((ISP_INTERNAL_WIDTH/2)/BITS8_ELEMENTS_PER_XMEM_ADDR)*HIVE_ISP_DDR_WORD_BYTES)
+
+#else /* defined(__ISP) || defined(INIT_VARS) */
+
+#define ISP_INTERNAL_WIDTH         isp_internal_width
+#define ISP_INTERNAL_HEIGHT        isp_internal_height
+
+#endif /* defined(__ISP) || defined(INIT_VARS) */
+
+#endif /* _COMMON_ISP_EXPRS_H_ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_types.h
new file mode 100644
index 000000000000..37a7d28f6d9f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/isp/modes/interface/isp_types.h
@@ -0,0 +1,128 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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;
+
+#if defined(__ISP)
+struct isp_uds_config {
+	int      hive_dx;
+	int      hive_dy;
+	unsigned hive_woix;
+	unsigned hive_bpp; /* gdc_bits_per_pixel */
+	unsigned hive_bci;
+};
+
+struct s_isp_gdcac_config {
+	unsigned nbx;
+	unsigned nby;
+};
+
+/* output.hive.c request information */
+typedef enum {
+  output_y_channel,
+  output_c_channel,
+  OUTPUT_NUM_CHANNELS
+} output_channel_type;
+
+typedef struct s_output_dma_info {
+  unsigned            cond;		/* Condition for transfer */
+  output_channel_type channel_type;
+  dma_channel         channel;
+  unsigned            width_a;
+  unsigned            width_b;
+  unsigned            stride;
+  unsigned            v_delta;	        /* Offset for v address to do cropping */
+  char               *x_base;           /* X base address */
+} output_dma_info_type;
+#endif
+
+/* 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/atomisp2/css2400/memory_realloc.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/memory_realloc.c
new file mode 100644
index 000000000000..e814f1bf19f7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/memory_realloc.c
@@ -0,0 +1,81 @@
+/**
+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 "memory_realloc.h"
+#include "ia_css_debug.h"
+#include "ia_css_refcount.h"
+#include "memory_access.h"
+
+static bool realloc_isp_css_mm_buf(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err,
+	uint16_t mmgr_attribute);
+
+
+bool reallocate_buffer(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err)
+{
+	bool ret;
+	uint16_t	mmgr_attribute = MMGR_ATTRIBUTE_DEFAULT;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	ret = realloc_isp_css_mm_buf(curr_buf,
+		curr_size, needed_size, force, err, mmgr_attribute);
+
+	IA_CSS_LEAVE_PRIVATE("ret=%d", ret);
+	return ret;
+}
+
+static bool realloc_isp_css_mm_buf(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err,
+	uint16_t mmgr_attribute)
+{
+	int32_t id;
+
+	*err = IA_CSS_SUCCESS;
+	/* Possible optimization: add a function sh_css_isp_css_mm_realloc()
+	 * and implement on top of hmm. */
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (ia_css_refcount_is_single(*curr_buf) && !force && *curr_size >= needed_size) {
+		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, mmgr_alloc_attr(needed_size,
+							mmgr_attribute));
+
+	if (!*curr_buf) {
+		*err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		*curr_size = 0;
+	} else {
+		*curr_size = needed_size;
+	}
+	IA_CSS_LEAVE_PRIVATE("true");
+	return true;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/interface/ia_css_binary.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/interface/ia_css_binary.h
new file mode 100644
index 000000000000..c65194619a34
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/interface/ia_css_binary.h
@@ -0,0 +1,333 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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;
+};
+
+#define IA_CSS_DEFAULT_CAS_BINARY_DESCR \
+{ \
+	0,		\
+	0,		\
+	NULL,		\
+	NULL,		\
+	NULL,		\
+	NULL,		\
+	NULL,		\
+}
+
+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;
+#ifdef ISP2401
+	bool enable_luma_only;
+	bool enable_tnr;
+#endif
+	bool enable_capture_pp_bli;
+	struct ia_css_resolution dvs_env;
+	enum ia_css_stream_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 ia_css_stream_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;
+#ifdef ISP2401
+	int                      sctbl_legacy_width_per_color;
+	int                      sctbl_legacy_height;
+#endif
+	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;
+};
+
+#ifdef ISP2401
+
+#define IA_CSS_BINARY_DEFAULT_SETTINGS \
+{ \
+	NULL, \
+	IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY, \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	{ 0, 0},/* effective_in_frame_res */ \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	0,	/* input_buf_vectors */ \
+	0,	/* deci_factor_log2 */ \
+	0,	/* vf_downscale_log2 */ \
+	0,	/* s3atbl_width */ \
+	0,	/* s3atbl_height */ \
+	0,	/* s3atbl_isp_width */ \
+	0,	/* s3atbl_isp_height */ \
+	0,	/* morph_tbl_width */ \
+	0,	/* morph_tbl_aligned_width */ \
+	0,	/* morph_tbl_height */ \
+	0,	/* sctbl_width_per_color */ \
+	0,	/* sctbl_aligned_width_per_color */ \
+	0,	/* sctbl_height */ \
+	0,	/* sctbl_legacy_width_per_color */ \
+	0,	/* sctbl_legacy_height */ \
+	IA_CSS_DEFAULT_SDIS_INFO, /* dis */ \
+	{ 0, 0},/* dvs_envelope_info */ \
+	false,	/* online */ \
+	0,	/* uds_xc */ \
+	0,	/* uds_yc */ \
+	0,	/* left_padding */ \
+	DEFAULT_BINARY_METRICS,	/* metrics */ \
+	IA_CSS_DEFAULT_ISP_MEM_PARAMS, /* mem_params */ \
+	IA_CSS_DEFAULT_ISP_CSS_PARAMS, /* css_params */ \
+}
+
+#else
+
+#define IA_CSS_BINARY_DEFAULT_SETTINGS \
+{ \
+	NULL, \
+	IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY, \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	{IA_CSS_BINARY_DEFAULT_FRAME_INFO}, \
+	{ 0, 0},/* effective_in_frame_res */ \
+	IA_CSS_BINARY_DEFAULT_FRAME_INFO, \
+	0,	/* input_buf_vectors */ \
+	0,	/* deci_factor_log2 */ \
+	0,	/* vf_downscale_log2 */ \
+	0,	/* s3atbl_width */ \
+	0,	/* s3atbl_height */ \
+	0,	/* s3atbl_isp_width */ \
+	0,	/* s3atbl_isp_height */ \
+	0,	/* morph_tbl_width */ \
+	0,	/* morph_tbl_aligned_width */ \
+	0,	/* morph_tbl_height */ \
+	0,	/* sctbl_width_per_color */ \
+	0,	/* sctbl_aligned_width_per_color */ \
+	0,	/* sctbl_height */ \
+	IA_CSS_DEFAULT_SDIS_INFO, /* dis */ \
+	{ 0, 0},/* dvs_envelope_info */ \
+	false,	/* online */ \
+	0,	/* uds_xc */ \
+	0,	/* uds_yc */ \
+	0,	/* left_padding */ \
+	DEFAULT_BINARY_METRICS,	/* metrics */ \
+	IA_CSS_DEFAULT_ISP_MEM_PARAMS, /* mem_params */ \
+	IA_CSS_DEFAULT_ISP_CSS_PARAMS, /* css_params */ \
+}
+
+#endif
+
+enum ia_css_err
+ia_css_binary_init_infos(void);
+
+enum ia_css_err
+ia_css_binary_uninit(void);
+
+enum ia_css_err
+ia_css_binary_fill_info(const struct ia_css_binary_xinfo *xinfo,
+		 bool online,
+		 bool two_ppc,
+		 enum ia_css_stream_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);
+
+enum ia_css_err
+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.
+#ifndef ISP2401
+ * @param[out] info: The shading information.
+#else
+ * @param[out] shading_info: The shading information.
+ *		The shading information necessary as API is stored in the shading_info.
+#endif
+ *		The driver needs to get this information to generate
+#ifndef ISP2401
+ *		the shading table directly required in the isp.
+#else
+ *		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.
+#endif
+ * @return	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err
+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,
+#ifndef ISP2401
+			struct ia_css_shading_info *info);
+#else
+			struct ia_css_shading_info *shading_info,
+			struct ia_css_pipe_config *pipe_config);
+#endif
+
+enum ia_css_err
+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/atomisp2/css2400/runtime/binary/src/binary.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
new file mode 100644
index 000000000000..a8b93a756e41
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/binary/src/binary.c
@@ -0,0 +1,1873 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "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 "vf/vf_1.0/ia_css_vf.host.h"
+#ifdef ISP2401
+#include "sc/sc_1.0/ia_css_sc.host.h"
+#endif
+#include "sdis/sdis_1.0/ia_css_sdis.host.h"
+#ifdef ISP2401
+#include "fixedbds/fixedbds_1.0/ia_css_fixedbds_param.h"	/* FRAC_ACC */
+#endif
+
+#include "camera/pipe/interface/ia_css_pipe_binarydesc.h"
+#if defined(HAS_RES_MGR)
+#include <components/resolutions_mgr/src/host/resolutions_mgr.host.h>
+#include <components/acc_cluster/acc_dvs_stat/host/dvs_stat.host.h>
+#endif
+
+#include "memory_access.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 != NULL);
+		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 != NULL) {
+			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 != NULL) && (out_info != NULL) &&
+				/* 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 != NULL) {
+			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);
+#if defined(HAS_RES_MGR)
+	internal_res->height = (bds_out_info == NULL) ? internal_res->height : bds_out_info->res.height;
+	internal_res->width = (bds_out_info == NULL) ? internal_res->width: bds_out_info->res.width;
+#endif
+}
+
+#ifndef ISP2401
+/* Computation results of the origin coordinate of bayer on the shading table. */
+struct sh_css_shading_table_bayer_origin_compute_results {
+	uint32_t bayer_scale_hor_ratio_in;	/* Horizontal ratio (in) of bayer scaling. */
+	uint32_t bayer_scale_hor_ratio_out;	/* Horizontal ratio (out) of bayer scaling. */
+	uint32_t bayer_scale_ver_ratio_in;	/* Vertical ratio (in) of bayer scaling. */
+	uint32_t bayer_scale_ver_ratio_out;	/* Vertical ratio (out) of bayer scaling. */
+	uint32_t sc_bayer_origin_x_bqs_on_shading_table; /* X coordinate (in bqs) of bayer origin on shading table. */
+	uint32_t sc_bayer_origin_y_bqs_on_shading_table; /* Y coordinate (in bqs) of bayer origin on shading table. */
+#else
+/* Requirements for the shading correction. */
+struct sh_css_binary_sc_requirements {
+	/* Bayer scaling factor, for the scaling which is applied before shading correction. */
+	uint32_t bayer_scale_hor_ratio_in;  /* Horizontal ratio (in) of scaling applied BEFORE shading correction. */
+	uint32_t bayer_scale_hor_ratio_out; /* Horizontal ratio (out) of scaling applied BEFORE shading correction. */
+	uint32_t bayer_scale_ver_ratio_in;  /* Vertical ratio (in) of scaling applied BEFORE shading correction. */
+	uint32_t bayer_scale_ver_ratio_out; /* Vertical ratio (out) of scaling applied BEFORE shading correction. */
+
+	/* ISP internal frame is composed of the real sensor data and the padding data. */
+	uint32_t sensor_data_origin_x_bqs_on_internal; /* X origin (in bqs) of sensor data on internal frame
+								at shading correction. */
+	uint32_t sensor_data_origin_y_bqs_on_internal; /* Y origin (in bqs) of sensor data on internal frame
+								at shading correction. */
+#endif
+};
+
+/* Get the requirements for the shading correction. */
+static enum ia_css_err
+#ifndef ISP2401
+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] */
+#else
+sh_css_binary_get_sc_requirements(
+	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_binary_sc_requirements *scr)		/* [out] */
+#endif
+{
+	enum ia_css_err err;
+
+#ifndef ISP2401
+	/* Numerator and denominator of the fixed bayer downscaling factor.
+	(numerator >= denominator) */
+#else
+	/* Numerator and denominator of the fixed bayer downscaling factor. (numerator >= denominator) */
+#endif
+	unsigned int bds_num, bds_den;
+
+#ifndef ISP2401
+	/* 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;
+#else
+	/* Horizontal/Vertical ratio of bayer scaling between input area and output area. */
+	unsigned int bs_hor_ratio_in, bs_hor_ratio_out, bs_ver_ratio_in, bs_ver_ratio_out;
+#endif
+
+	/* Left padding set by InputFormatter. */
+#ifndef ISP2401
+	unsigned int left_padding_bqs;			/* in bqs */
+#else
+	unsigned int left_padding_bqs;
+#endif
+
+#ifndef ISP2401
+	/* 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 != IA_CSS_SUCCESS)
+#else
+	/* Flags corresponding to NEED_BDS_FACTOR_2_00/NEED_BDS_FACTOR_1_50/NEED_BDS_FACTOR_1_25 macros
+	 * defined in isp kernels. */
+	unsigned int need_bds_factor_2_00, need_bds_factor_1_50, need_bds_factor_1_25;
+
+	/* Left padding adjusted inside the isp kernels. */
+	unsigned int left_padding_adjusted_bqs;
+
+	/* Top padding padded inside the isp kernel for bayer downscaling binaries. */
+	unsigned int top_padding_bqs;
+
+	/* Bayer downscaling factor 1.0 by fixed-point. */
+	int bds_frac_acc = FRAC_ACC;	/* FRAC_ACC is defined in ia_css_fixedbds_param.h. */
+
+	/* Right/Down shift amount caused by filters applied BEFORE shading corrertion. */
+	unsigned int right_shift_bqs_before_bs; /* right shift before bayer scaling */
+	unsigned int right_shift_bqs_after_bs;  /* right shift after bayer scaling */
+	unsigned int down_shift_bqs_before_bs;  /* down shift before bayer scaling */
+	unsigned int down_shift_bqs_after_bs;   /* down shift after bayer scaling */
+
+	/* Origin of the real sensor data area on the internal frame at shading correction. */
+	unsigned int sensor_data_origin_x_bqs_on_internal;
+	unsigned int sensor_data_origin_y_bqs_on_internal;
+
+	IA_CSS_ENTER_PRIVATE("binary=%p, required_bds_factor=%d, stream_config=%p",
+		binary, required_bds_factor, stream_config);
+
+	/* Get the numerator and denominator of the required bayer downscaling factor. */
+	err = sh_css_bds_factor_get_numerator_denominator(required_bds_factor, &bds_num, &bds_den);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+#endif
+		return err;
+#ifdef ISP2401
+	}
+#endif
+
+#ifndef ISP2401
+	/* Set the horizontal/vertical ratio of bayer scaling
+	between input area and output area. */
+#else
+	IA_CSS_LOG("bds_num=%d, bds_den=%d", bds_num, bds_den);
+
+	/* Set the horizontal/vertical ratio of bayer scaling between input area and output area. */
+#endif
+	bs_hor_ratio_in  = bds_num;
+	bs_hor_ratio_out = bds_den;
+	bs_ver_ratio_in  = bds_num;
+	bs_ver_ratio_out = bds_den;
+
+#ifndef ISP2401
+	/* Set the left padding set by InputFormatter. (ifmtr.c) */
+#else
+	/* Set the left padding set by InputFormatter. (ia_css_ifmtr_configure() in ifmtr.c) */
+#endif
+	if (stream_config->left_padding == -1)
+		left_padding_bqs = _ISP_BQS(binary->left_padding);
+	else
+#ifndef ISP2401
+		left_padding_bqs = (unsigned int)((int)ISP_VEC_NELEMS
+			- _ISP_BQS(stream_config->left_padding));
+#else
+		left_padding_bqs = (unsigned int)((int)ISP_VEC_NELEMS - _ISP_BQS(stream_config->left_padding));
+#endif
+
+#ifndef ISP2401
+	/* 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.)
+	*/
+#else
+	IA_CSS_LOG("stream.left_padding=%d, binary.left_padding=%d, left_padding_bqs=%d",
+		stream_config->left_padding, binary->left_padding, left_padding_bqs);
+
+	/* Set the left padding adjusted inside the isp kernels.
+	 * When the bds_factor isn't 1.00, the left padding size is adjusted inside the isp,
+	 * before bayer downscaling. (scaled_hor_plane_index(), raw_compute_hphase() in raw.isp.c)
+	 */
+#endif
+	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);
+
+#ifndef ISP2401
+	if (need_bds_factor_2_00 && binary->info->sp.pipeline.left_cropping > 0)
+		left_padding_adjusted_bqs = left_padding_bqs + ISP_VEC_NELEMS;
+	else
+#else
+	need_bds_factor_1_50 = ((binary->info->sp.bds.supported_bds_factors &
+		(PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_1_50) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_2_25) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_3_00) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_4_50) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_6_00))) != 0);
+
+	need_bds_factor_1_25 = ((binary->info->sp.bds.supported_bds_factors &
+		(PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_1_25) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_2_50) |
+		 PACK_BDS_FACTOR(SH_CSS_BDS_FACTOR_5_00))) != 0);
+
+	if (binary->info->sp.pipeline.left_cropping > 0 &&
+	    (need_bds_factor_2_00 || need_bds_factor_1_50 || need_bds_factor_1_25)) {
+		/*
+		 * downscale 2.0  -> first_vec_adjusted_bqs = 128
+		 * downscale 1.5  -> first_vec_adjusted_bqs = 96
+		 * downscale 1.25 -> first_vec_adjusted_bqs = 80
+		 */
+		unsigned int first_vec_adjusted_bqs
+			= ISP_VEC_NELEMS * bs_hor_ratio_in / bs_hor_ratio_out;
+		left_padding_adjusted_bqs = first_vec_adjusted_bqs
+			- _ISP_BQS(binary->info->sp.pipeline.left_cropping);
+	} else
+#endif
+		left_padding_adjusted_bqs = left_padding_bqs;
+
+#ifndef ISP2401
+	/* 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;
+#else
+	IA_CSS_LOG("supported_bds_factors=%d, need_bds_factor:2_00=%d, 1_50=%d, 1_25=%d",
+		binary->info->sp.bds.supported_bds_factors,
+		need_bds_factor_2_00, need_bds_factor_1_50, need_bds_factor_1_25);
+	IA_CSS_LOG("left_cropping=%d, left_padding_adjusted_bqs=%d",
+		binary->info->sp.pipeline.left_cropping, left_padding_adjusted_bqs);
+
+	/* Set the top padding padded inside the isp kernel for bayer downscaling binaries.
+	 * When the bds_factor isn't 1.00, the top padding is padded inside the isp
+	 * before bayer downscaling, because the top cropping size (input margin) is not enough.
+	 * (calculate_input_line(), raw_compute_vphase(), dma_read_raw() in raw.isp.c)
+	 * NOTE: In dma_read_raw(), the factor passed to raw_compute_vphase() is got by get_bds_factor_for_dma_read().
+	 *       This factor is BDS_FPVAL_100/BDS_FPVAL_125/BDS_FPVAL_150/BDS_FPVAL_200.
+	 */
+	top_padding_bqs = 0;
+	if (binary->info->sp.pipeline.top_cropping > 0 &&
+	    (required_bds_factor == SH_CSS_BDS_FACTOR_1_25 ||
+	     required_bds_factor == SH_CSS_BDS_FACTOR_1_50 ||
+	     required_bds_factor == SH_CSS_BDS_FACTOR_2_00)) {
+		/* Calculation from calculate_input_line() and raw_compute_vphase() in raw.isp.c. */
+		int top_cropping_bqs = _ISP_BQS(binary->info->sp.pipeline.top_cropping);
+								/* top cropping (in bqs) */
+		int factor = bds_num * bds_frac_acc / bds_den;	/* downscaling factor by fixed-point */
+		int top_padding_bqsxfrac_acc = (top_cropping_bqs * factor - top_cropping_bqs * bds_frac_acc)
+				+ (2 * bds_frac_acc - factor);	/* top padding by fixed-point (in bqs) */
+
+		top_padding_bqs = (unsigned int)((top_padding_bqsxfrac_acc + bds_frac_acc/2 - 1) / bds_frac_acc);
+	}
+
+	IA_CSS_LOG("top_cropping=%d, top_padding_bqs=%d", binary->info->sp.pipeline.top_cropping, top_padding_bqs);
+
+	/* Set the right/down shift amount caused by filters applied BEFORE bayer scaling,
+	 * which scaling is applied BEFORE shading corrertion.
+	 *
+	 * When the 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 shifts each color plane (Gr/R/B/Gb) to right/down directions by 1 pixel.
+	 */
+	right_shift_bqs_before_bs = 0;
+	down_shift_bqs_before_bs = 0;
+#endif
+
+#ifndef ISP2401
+	/* 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;
+#else
+	if (need_bds_factor_2_00 || need_bds_factor_1_50 || need_bds_factor_1_25) {
+		right_shift_bqs_before_bs = 1;
+		down_shift_bqs_before_bs = 1;
+	}
+
+	IA_CSS_LOG("right_shift_bqs_before_bs=%d, down_shift_bqs_before_bs=%d",
+		right_shift_bqs_before_bs, down_shift_bqs_before_bs);
+
+	/* Set the right/down shift amount caused by filters applied AFTER bayer scaling,
+	 * which scaling is applied BEFORE shading corrertion.
+	 *
+	 * When DPC&BNR is processed between bayer scaling and shading correction,
+	 * DPC&BNR moves each color plane (Gr/R/B/Gb) to right/down directions by 1 pixel.
+	 */
+	right_shift_bqs_after_bs = 0;
+	down_shift_bqs_after_bs = 0;
+#endif
+
+#ifndef ISP2401
+	/* 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;
+#else
+	if (binary->info->mem_offsets.offsets.param->dmem.dp.size != 0) { /* if DPC&BNR is enabled in the binary */
+		right_shift_bqs_after_bs = 1;
+		down_shift_bqs_after_bs = 1;
+	}
+
+	IA_CSS_LOG("right_shift_bqs_after_bs=%d, down_shift_bqs_after_bs=%d",
+		right_shift_bqs_after_bs, down_shift_bqs_after_bs);
+
+	/* Set the origin of the sensor data area on the internal frame at shading correction. */
+	{
+		unsigned int bs_frac = bds_frac_acc;	/* scaling factor 1.0 in fixed point */
+		unsigned int bs_out, bs_in;		/* scaling ratio in fixed point */
+
+		bs_out = bs_hor_ratio_out * bs_frac;
+		bs_in = bs_hor_ratio_in * bs_frac;
+		sensor_data_origin_x_bqs_on_internal
+			= ((left_padding_adjusted_bqs + right_shift_bqs_before_bs) * bs_out + bs_in/2) / bs_in
+				+ right_shift_bqs_after_bs;	/* "+ bs_in/2": rounding */
+
+		bs_out = bs_ver_ratio_out * bs_frac;
+		bs_in = bs_ver_ratio_in * bs_frac;
+		sensor_data_origin_y_bqs_on_internal
+			= ((top_padding_bqs + down_shift_bqs_before_bs) * bs_out + bs_in/2) / bs_in
+				+ down_shift_bqs_after_bs;	/* "+ bs_in/2": rounding */
+	}
+
+	scr->bayer_scale_hor_ratio_in			= (uint32_t)bs_hor_ratio_in;
+	scr->bayer_scale_hor_ratio_out			= (uint32_t)bs_hor_ratio_out;
+	scr->bayer_scale_ver_ratio_in			= (uint32_t)bs_ver_ratio_in;
+	scr->bayer_scale_ver_ratio_out			= (uint32_t)bs_ver_ratio_out;
+	scr->sensor_data_origin_x_bqs_on_internal	= (uint32_t)sensor_data_origin_x_bqs_on_internal;
+	scr->sensor_data_origin_y_bqs_on_internal	= (uint32_t)sensor_data_origin_y_bqs_on_internal;
+
+	IA_CSS_LOG("sc_requirements: %d, %d, %d, %d, %d, %d",
+		scr->bayer_scale_hor_ratio_in, scr->bayer_scale_hor_ratio_out,
+		scr->bayer_scale_ver_ratio_in, scr->bayer_scale_ver_ratio_out,
+		scr->sensor_data_origin_x_bqs_on_internal, scr->sensor_data_origin_y_bqs_on_internal);
+#endif
+
+#ifdef ISP2401
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+#endif
+	return err;
+}
+
+/* Get the shading information of Shading Correction Type 1. */
+static enum ia_css_err
+ia_css_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] */
+#ifndef ISP2401
+			struct ia_css_shading_info *info)			/* [out] */
+#else
+			struct ia_css_shading_info *shading_info,		/* [out] */
+			struct ia_css_pipe_config *pipe_config)			/* [out] */
+#endif
+{
+	enum ia_css_err err;
+#ifndef ISP2401
+	struct sh_css_shading_table_bayer_origin_compute_results res;
+#else
+	struct sh_css_binary_sc_requirements scr;
+	struct ia_css_shading_info default_shading_info_type_1 = DEFAULT_SHADING_INFO_TYPE_1;
+#endif
+
+#ifndef ISP2401
+	assert(binary != NULL);
+	assert(info != NULL);
+#else
+	uint32_t in_width_bqs, in_height_bqs, internal_width_bqs, internal_height_bqs;
+	uint32_t num_hor_grids, num_ver_grids, bqs_per_grid_cell, tbl_width_bqs, tbl_height_bqs;
+	uint32_t sensor_org_x_bqs_on_internal, sensor_org_y_bqs_on_internal, sensor_width_bqs, sensor_height_bqs;
+	uint32_t sensor_center_x_bqs_on_internal, sensor_center_y_bqs_on_internal;
+	uint32_t left, right, upper, lower;
+	uint32_t adjust_left, adjust_right, adjust_upper, adjust_lower, adjust_width_bqs, adjust_height_bqs;
+	uint32_t internal_org_x_bqs_on_tbl, internal_org_y_bqs_on_tbl;
+	uint32_t sensor_org_x_bqs_on_tbl, sensor_org_y_bqs_on_tbl;
+#endif
+
+#ifndef ISP2401
+	info->type = IA_CSS_SHADING_CORRECTION_TYPE_1;
+#else
+	assert(binary != NULL);
+	assert(stream_config != NULL);
+	assert(shading_info != NULL);
+	assert(pipe_config != NULL);
+#endif
+
+#ifndef ISP2401
+	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);
+#else
+	IA_CSS_ENTER_PRIVATE("binary=%p, required_bds_factor=%d, stream_config=%p",
+		binary, required_bds_factor, stream_config);
+#endif
+
+	/* Initialize by default values. */
+#ifndef ISP2401
+	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 != IA_CSS_SUCCESS)
+#else
+	*shading_info = default_shading_info_type_1;
+
+	err = sh_css_binary_get_sc_requirements(binary, required_bds_factor, stream_config, &scr);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+#endif
+		return err;
+#ifdef ISP2401
+	}
+
+	IA_CSS_LOG("binary: id=%d, sctbl=%dx%d, deci=%d",
+		binary->info->sp.id, binary->sctbl_width_per_color, binary->sctbl_height, binary->deci_factor_log2);
+	IA_CSS_LOG("binary: in=%dx%d, in_padded_w=%d, int=%dx%d, int_padded_w=%d, out=%dx%d, out_padded_w=%d",
+		binary->in_frame_info.res.width, binary->in_frame_info.res.height, binary->in_frame_info.padded_width,
+		binary->internal_frame_info.res.width, binary->internal_frame_info.res.height,
+		binary->internal_frame_info.padded_width,
+		binary->out_frame_info[0].res.width, binary->out_frame_info[0].res.height,
+		binary->out_frame_info[0].padded_width);
+
+	/* Set the input size from sensor, which includes left/top crop size. */
+	in_width_bqs	    = _ISP_BQS(binary->in_frame_info.res.width);
+	in_height_bqs	    = _ISP_BQS(binary->in_frame_info.res.height);
+
+	/* Frame size internally used in ISP, including sensor data and padding.
+	 * This is the frame size, to which the shading correction is applied.
+	 */
+	internal_width_bqs  = _ISP_BQS(binary->internal_frame_info.res.width);
+	internal_height_bqs = _ISP_BQS(binary->internal_frame_info.res.height);
+
+	/* Shading table. */
+	num_hor_grids = binary->sctbl_width_per_color;
+	num_ver_grids = binary->sctbl_height;
+	bqs_per_grid_cell = (1 << binary->deci_factor_log2);
+	tbl_width_bqs  = (num_hor_grids - 1) * bqs_per_grid_cell;
+	tbl_height_bqs = (num_ver_grids - 1) * bqs_per_grid_cell;
+#endif
+
+#ifndef ISP2401
+	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;
+#else
+	IA_CSS_LOG("tbl_width_bqs=%d, tbl_height_bqs=%d", tbl_width_bqs, tbl_height_bqs);
+#endif
+
+#ifdef ISP2401
+	/* Real sensor data area on the internal frame at shading correction.
+	 * Filters and scaling are applied to the internal frame before shading correction, depending on the binary.
+	 */
+	sensor_org_x_bqs_on_internal = scr.sensor_data_origin_x_bqs_on_internal;
+	sensor_org_y_bqs_on_internal = scr.sensor_data_origin_y_bqs_on_internal;
+	{
+		unsigned int bs_frac = 8;	/* scaling factor 1.0 in fixed point (8 == FRAC_ACC macro in ISP) */
+		unsigned int bs_out, bs_in;	/* scaling ratio in fixed point */
+
+		bs_out = scr.bayer_scale_hor_ratio_out * bs_frac;
+		bs_in = scr.bayer_scale_hor_ratio_in * bs_frac;
+		sensor_width_bqs  = (in_width_bqs * bs_out + bs_in/2) / bs_in; /* "+ bs_in/2": rounding */
+
+		bs_out = scr.bayer_scale_ver_ratio_out * bs_frac;
+		bs_in = scr.bayer_scale_ver_ratio_in * bs_frac;
+		sensor_height_bqs = (in_height_bqs * bs_out + bs_in/2) / bs_in; /* "+ bs_in/2": rounding */
+	}
+
+	/* Center of the sensor data on the internal frame at shading correction. */
+	sensor_center_x_bqs_on_internal = sensor_org_x_bqs_on_internal + sensor_width_bqs / 2;
+	sensor_center_y_bqs_on_internal = sensor_org_y_bqs_on_internal + sensor_height_bqs / 2;
+
+	/* Size of left/right/upper/lower sides of the sensor center on the internal frame. */
+	left  = sensor_center_x_bqs_on_internal;
+	right = internal_width_bqs - sensor_center_x_bqs_on_internal;
+	upper = sensor_center_y_bqs_on_internal;
+	lower = internal_height_bqs - sensor_center_y_bqs_on_internal;
+
+	/* Align the size of left/right/upper/lower sides to a multiple of the grid cell size. */
+	adjust_left  = CEIL_MUL(left,  bqs_per_grid_cell);
+	adjust_right = CEIL_MUL(right, bqs_per_grid_cell);
+	adjust_upper = CEIL_MUL(upper, bqs_per_grid_cell);
+	adjust_lower = CEIL_MUL(lower, bqs_per_grid_cell);
+
+	/* Shading table should cover the adjusted frame size. */
+	adjust_width_bqs  = adjust_left + adjust_right;
+	adjust_height_bqs = adjust_upper + adjust_lower;
+
+	IA_CSS_LOG("adjust_width_bqs=%d, adjust_height_bqs=%d", adjust_width_bqs, adjust_height_bqs);
+
+	if (adjust_width_bqs > tbl_width_bqs || adjust_height_bqs > tbl_height_bqs) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	/* Origin of the internal frame on the shading table. */
+	internal_org_x_bqs_on_tbl = adjust_left - left;
+	internal_org_y_bqs_on_tbl = adjust_upper - upper;
+
+	/* Origin of the real sensor data area on the shading table. */
+	sensor_org_x_bqs_on_tbl = internal_org_x_bqs_on_tbl + sensor_org_x_bqs_on_internal;
+	sensor_org_y_bqs_on_tbl = internal_org_y_bqs_on_tbl + sensor_org_y_bqs_on_internal;
+
+	/* The shading information necessary as API is stored in the shading_info. */
+	shading_info->info.type_1.num_hor_grids	    = num_hor_grids;
+	shading_info->info.type_1.num_ver_grids	    = num_ver_grids;
+	shading_info->info.type_1.bqs_per_grid_cell = bqs_per_grid_cell;
+
+	shading_info->info.type_1.bayer_scale_hor_ratio_in  = scr.bayer_scale_hor_ratio_in;
+	shading_info->info.type_1.bayer_scale_hor_ratio_out = scr.bayer_scale_hor_ratio_out;
+	shading_info->info.type_1.bayer_scale_ver_ratio_in  = scr.bayer_scale_ver_ratio_in;
+	shading_info->info.type_1.bayer_scale_ver_ratio_out = scr.bayer_scale_ver_ratio_out;
+
+	shading_info->info.type_1.isp_input_sensor_data_res_bqs.width  = in_width_bqs;
+	shading_info->info.type_1.isp_input_sensor_data_res_bqs.height = in_height_bqs;
+
+	shading_info->info.type_1.sensor_data_res_bqs.width  = sensor_width_bqs;
+	shading_info->info.type_1.sensor_data_res_bqs.height = sensor_height_bqs;
+
+	shading_info->info.type_1.sensor_data_origin_bqs_on_sctbl.x = (int32_t)sensor_org_x_bqs_on_tbl;
+	shading_info->info.type_1.sensor_data_origin_bqs_on_sctbl.y = (int32_t)sensor_org_y_bqs_on_tbl;
+
+	/* The shading information related to ISP (but, not necessary as API) is stored in the pipe_config. */
+	pipe_config->internal_frame_origin_bqs_on_sctbl.x = (int32_t)internal_org_x_bqs_on_tbl;
+	pipe_config->internal_frame_origin_bqs_on_sctbl.y = (int32_t)internal_org_y_bqs_on_tbl;
+
+	IA_CSS_LOG("shading_info: grids=%dx%d, cell=%d, scale=%d,%d,%d,%d, input=%dx%d, data=%dx%d, origin=(%d,%d)",
+		shading_info->info.type_1.num_hor_grids,
+		shading_info->info.type_1.num_ver_grids,
+		shading_info->info.type_1.bqs_per_grid_cell,
+		shading_info->info.type_1.bayer_scale_hor_ratio_in,
+		shading_info->info.type_1.bayer_scale_hor_ratio_out,
+		shading_info->info.type_1.bayer_scale_ver_ratio_in,
+		shading_info->info.type_1.bayer_scale_ver_ratio_out,
+		shading_info->info.type_1.isp_input_sensor_data_res_bqs.width,
+		shading_info->info.type_1.isp_input_sensor_data_res_bqs.height,
+		shading_info->info.type_1.sensor_data_res_bqs.width,
+		shading_info->info.type_1.sensor_data_res_bqs.height,
+		shading_info->info.type_1.sensor_data_origin_bqs_on_sctbl.x,
+		shading_info->info.type_1.sensor_data_origin_bqs_on_sctbl.y);
+
+	IA_CSS_LOG("pipe_config: origin=(%d,%d)",
+		pipe_config->internal_frame_origin_bqs_on_sctbl.x,
+		pipe_config->internal_frame_origin_bqs_on_sctbl.y);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+#endif
+	return err;
+}
+
+enum ia_css_err
+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] */
+#ifndef ISP2401
+				struct ia_css_shading_info *info)			/* [out] */
+#else
+				struct ia_css_shading_info *shading_info,		/* [out] */
+				struct ia_css_pipe_config *pipe_config)			/* [out] */
+#endif
+{
+	enum ia_css_err err;
+
+	assert(binary != NULL);
+#ifndef ISP2401
+	assert(info != NULL);
+#else
+	assert(shading_info != NULL);
+
+	IA_CSS_ENTER_PRIVATE("binary=%p, type=%d, required_bds_factor=%d, stream_config=%p",
+		binary, type, required_bds_factor, stream_config);
+#endif
+
+	if (type == IA_CSS_SHADING_CORRECTION_TYPE_1)
+#ifndef ISP2401
+		err = ia_css_binary_get_shading_info_type_1(binary, required_bds_factor, stream_config, info);
+#else
+		err = ia_css_binary_get_shading_info_type_1(binary, required_bds_factor, stream_config,
+								shading_info, pipe_config);
+#endif
+
+	/* Other function calls can be added here when other shading correction types will be added in the future. */
+
+	else
+		err = IA_CSS_ERR_NOT_SUPPORTED;
+
+	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 != NULL);
+	assert(info != NULL);
+
+	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 != NULL);
+	assert(info != NULL);
+
+	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)
+{
+#if defined(HAS_RES_MGR)
+	struct ia_css_dvs_stat_grid_info *dvs_stat_info;
+	unsigned int i;
+
+	assert(binary != NULL);
+	assert(info != NULL);
+	dvs_stat_info = &info->dvs_grid.dvs_stat_grid_info;
+
+	if (binary->info->sp.enable.dvs_stats) {
+		for (i = 0; i < IA_CSS_SKC_DVS_STAT_NUM_OF_LEVELS; i++) {
+			dvs_stat_info->grd_cfg[i].grd_start.enable = 1;
+		}
+		ia_css_dvs_stat_grid_calculate(pipe, dvs_stat_info);
+	}
+	else {
+		memset(dvs_stat_info, 0, sizeof(struct ia_css_dvs_stat_grid_info));
+	}
+
+#endif
+	(void)pipe;
+	sh_css_binary_common_grid_info(binary, info);
+	return;
+}
+
+enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("binary=%p, info=%p, pipe=%p",
+			     binary, info, pipe);
+
+	assert(binary != NULL);
+	assert(info != NULL);
+	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;
+#if defined(HAS_NO_HMEM)
+	s3a_info->has_histogram     = 1;
+#else
+	s3a_info->has_histogram     = 0;
+#endif
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static void
+binary_init_pc_histogram(struct sh_css_pc_histogram *histo)
+{
+	assert(histo != NULL);
+
+	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 != NULL);
+	assert(info != NULL);
+
+	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 != NULL);
+
+	for (i = 0; i < info->num_output_formats; i++) {
+		if (info->output_formats[i] == format)
+			return true;
+	}
+	return false;
+}
+
+#ifdef ISP2401
+static bool
+binary_supports_input_format(const struct ia_css_binary_xinfo *info,
+			     enum ia_css_stream_format format)
+{
+
+	assert(info != NULL);
+	(void)format;
+
+	return true;
+}
+#endif
+
+static bool
+binary_supports_vf_format(const struct ia_css_binary_xinfo *info,
+			  enum ia_css_frame_format format)
+{
+	int i;
+
+	assert(info != NULL);
+
+	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(uint32_t supported_factors,
+		       uint32_t bds_factor)
+{
+	return ((supported_factors & PACK_BDS_FACTOR(bds_factor)) != 0);
+}
+
+static enum ia_css_err
+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 size = sh_css_blob_info[i].header.blob.size;
+
+	if ((info == NULL) || (binary_found == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	*info = sh_css_blob_info[i].header.info.isp;
+	*binary_found = blob != NULL;
+	info->blob_index = i;
+	/* we don't have this binary, skip it */
+	if (!size)
+		return IA_CSS_SUCCESS;
+
+	info->xmem_addr = sh_css_load_blob(blob, size);
+	if (!info->xmem_addr)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	return IA_CSS_SUCCESS;
+}
+
+/* When binaries are put at the beginning, they will only
+ * be selected if no other primary matches.
+ */
+enum ia_css_err
+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 IA_CSS_SUCCESS;
+
+	all_binaries = sh_css_malloc(num_of_isp_binaries *
+						sizeof(*all_binaries));
+	if (all_binaries == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	for (i = 0; i < num_of_isp_binaries; i++) {
+		enum ia_css_err ret;
+		struct ia_css_binary_xinfo *binary = &all_binaries[i];
+		bool binary_found;
+
+		ret = binary_init_info(binary, i, &binary_found);
+		if (ret != IA_CSS_SUCCESS)
+			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 IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+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;
+	}
+	sh_css_free(all_binaries);
+	return IA_CSS_SUCCESS;
+}
+
+/** @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]
+#ifndef ISP2401
+ * 1280 ?c             32                       40 ?c
+ *  640 ?c 1279        16                       40 ?c 80
+ *      ?c  639         8                          ?c 80
+#else
+ * from 1280                   32                 from 40
+ * from  640 to 1279           16                 from 40 to 80
+ *           to  639            8                         to 80
+#endif
+ * ------------------------------------------------------------------
+ */
+/* 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(USE_INPUT_SYSTEM_VERSION_2401)
+	/* 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 defined(HAS_RES_MGR)
+	(void)dvs_env_width;
+#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. */
+#if !defined(HAS_RES_MGR) /* dvs env is included already */
+			in_frame_width += dvs_env_width;
+#endif
+			rval =
+				CEIL_MUL(in_frame_width +
+					(left_cropping ? nr_of_left_paddings : 0),
+					2*ISP_VEC_NELEMS);
+		}
+	} else {
+		rval = isp_internal_width;
+	}
+
+	return rval;
+}
+
+
+enum ia_css_err
+ia_css_binary_fill_info(const struct ia_css_binary_xinfo *xinfo,
+		 bool online,
+		 bool two_ppc,
+		 enum ia_css_stream_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;
+	enum ia_css_err err;
+	unsigned int i;
+	const struct ia_css_frame_info *bin_out_info = NULL;
+
+	assert(info != NULL);
+	assert(binary != NULL);
+
+	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 != IA_CSS_SUCCESS) {
+			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 != NULL && bin_out_info != NULL) {
+		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 != NULL) /* { */
+		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 != NULL) {
+		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;
+
+#if !defined(HAS_RES_MGR) /* dvs env is included already */
+		binary->in_frame_info.res.width += dvs_env_width;
+		binary->in_frame_info.res.height += dvs_env_height;
+#endif
+
+		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] != NULL) {
+			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 != IA_CSS_SUCCESS) {
+			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 != NULL) && (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 == NULL)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		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 != NULL && in_info != NULL &&
+			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  =
+#ifndef ISP2401
+			_ISP_SCTBL_WIDTH_PER_COLOR(sc_3a_dis_padded_width,
+				s3a_log_deci);
+#else
+			_ISP_SCTBL_WIDTH_PER_COLOR(isp_internal_width, s3a_log_deci);
+#endif
+		binary->sctbl_aligned_width_per_color =
+			SH_CSS_MAX_SCTBL_ALIGNED_WIDTH_PER_COLOR;
+		binary->sctbl_height =
+#ifndef ISP2401
+			_ISP_SCTBL_HEIGHT(sc_3a_dis_height, s3a_log_deci);
+#else
+			_ISP_SCTBL_HEIGHT(isp_internal_height, s3a_log_deci);
+		binary->sctbl_legacy_width_per_color  =
+			_ISP_SCTBL_LEGACY_WIDTH_PER_COLOR(sc_3a_dis_padded_width, s3a_log_deci);
+		binary->sctbl_legacy_height =
+			_ISP_SCTBL_LEGACY_HEIGHT(sc_3a_dis_height, s3a_log_deci);
+#endif
+	} else {
+		binary->sctbl_width_per_color         = 0;
+		binary->sctbl_aligned_width_per_color = 0;
+		binary->sctbl_height                  = 0;
+#ifdef ISP2401
+		binary->sctbl_legacy_width_per_color  = 0;
+		binary->sctbl_legacy_height	      = 0;
+#endif
+	}
+	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 IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_binary_find(struct ia_css_binary_descr *descr,
+		   struct ia_css_binary *binary)
+{
+	int mode;
+	bool online;
+	bool two_ppc;
+	enum ia_css_stream_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;
+#ifndef ISP2401
+	bool need_ds, need_dz, need_dvs, need_xnr, need_dpc;
+#else
+	bool need_ds, need_dz, need_dvs, need_xnr, need_dpc, need_tnr;
+#endif
+	bool striped;
+	bool enable_yuv_ds;
+	bool enable_high_speed;
+	bool enable_dvs_6axis;
+	bool enable_reduced_pipe;
+	bool enable_capture_pp_bli;
+#ifdef ISP2401
+	bool enable_luma_only;
+#endif
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+	bool continuous;
+	unsigned int isp_pipe_version;
+	struct ia_css_resolution dvs_env, internal_res;
+	unsigned int i;
+
+	assert(descr != NULL);
+	/* MW: used after an error check, may accept NULL, but doubtfull */
+	assert(binary != NULL);
+
+	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 == NULL)
+		return IA_CSS_ERR_INTERNAL_ERROR;
+#ifndef ISP2401
+	req_vf_info = descr->vf_info;
+#else
+
+	if ((descr->vf_info != NULL) && (descr->vf_info->res.width == 0))
+		/* width==0 means that there is no vf pin (e.g. in SkyCam preview case) */
+		req_vf_info = NULL;
+	else
+		req_vf_info = descr->vf_info;
+#endif
+
+	need_xnr = descr->enable_xnr;
+	need_ds = descr->enable_fractional_ds;
+	need_dz = false;
+	need_dvs = false;
+	need_dpc = descr->enable_dpc;
+#ifdef ISP2401
+	need_tnr = descr->enable_tnr;
+#endif
+	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;
+#ifdef ISP2401
+	enable_luma_only = descr->enable_luma_only;
+#endif
+	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 != NULL) && !(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;
+		}
+#ifdef ISP2401
+		if (!binary_supports_input_format(xcandidate, descr->stream_format)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+					    "ia_css_binary_find() [%d] continue: !%d\n",
+					    __LINE__,
+					    binary_supports_input_format(xcandidate, req_in_info->format));
+			continue;
+		}
+#endif
+		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;
+		}
+
+#ifdef ISP2401
+		if (candidate->enable.luma_only != enable_luma_only) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				"ia_css_binary_find() [%d] continue: %d != %d\n",
+				__LINE__, candidate->enable.luma_only,
+				descr->enable_luma_only);
+			continue;
+		}
+
+		if(!candidate->enable.tnr && need_tnr) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+				"ia_css_binary_find() [%d] continue: !%d && %d\n",
+				__LINE__, candidate->enable.tnr,
+				descr->enable_tnr);
+			continue;
+		}
+
+#endif
+		/* 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 != IA_CSS_SUCCESS)
+			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);
+
+	return err;
+}
+
+unsigned
+ia_css_binary_max_vf_width(void)
+{
+	/* This is (should be) true for IPU1 and IPU2 */
+	/* For IPU3 (SkyCam) this pointer is guarenteed 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 != NULL);
+
+	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/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq.h
new file mode 100644
index 000000000000..034ec15ec4a1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq.h
@@ -0,0 +1,197 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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  Initilize 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	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum  ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum ia_css_err ia_css_bufq_enqueue_psys_event(
+	uint8_t evt_id,
+	uint8_t evt_payload_0,
+	uint8_t 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	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum  ia_css_err ia_css_bufq_dequeue_psys_event(
+	uint8_t item[BUFQ_EVENT_SIZE]
+	);
+
+/**
+ * @brief  Enqueue an event item into host to SP EOF event queue.
+ *
+ * @param[in]	evt_id		      The event ID.
+ * @return	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err 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	IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum  ia_css_err ia_css_bufq_dequeue_isys_event(
+	uint8_t item[BUFQ_EVENT_SIZE]);
+
+/**
+* @brief   Enqueue a tagger command item into tagger command queue..
+ *
+ * @param item	Object to be enqueue.
+ * @return	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum ia_css_err ia_css_bufq_enqueue_tag_cmd(
+	uint32_t item);
+
+/**
+* @brief  Uninitializes bufq module.
+ *
+ * @return	IA_CSS_SUCCESS or error code upon error.
+ *
+*/
+enum ia_css_err 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/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq_comm.h
new file mode 100644
index 000000000000..bb77080591b9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/interface/ia_css_bufq_comm.h
@@ -0,0 +1,66 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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,
+#if defined(HAS_NO_INPUT_SYSTEM)
+	/* input frame queue for skycam */
+	SH_CSS_QUEUE_H_ID,
+#endif
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	SH_CSS_QUEUE_H_ID, /* for metadata */
+#endif
+
+#if defined(HAS_NO_INPUT_SYSTEM) || defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+#define SH_CSS_MAX_NUM_QUEUES (SH_CSS_QUEUE_H_ID+1)
+#else
+#define SH_CSS_MAX_NUM_QUEUES (SH_CSS_QUEUE_G_ID+1)
+#endif
+
+};
+
+#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/atomisp2/css2400/runtime/bufq/src/bufq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c
new file mode 100644
index 000000000000..ed33d4c4c84a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/bufq/src/bufq.c
@@ -0,0 +1,590 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "ia_css_util.h"		/* ia_css_convert_errno()*/
+#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                      */
+/*********************************************************/
+
+#ifndef ISP2401
+
+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;
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	/* 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;
+#endif
+	/* Tagger command queue */
+	ia_css_queue_t host2sp_tag_cmd_queue_handle;
+};
+
+#else
+
+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;
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	/* 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;
+#endif
+};
+
+#endif
+
+struct sh_css_queues  css_queues;
+
+
+/*******************************************************
+*** Static variables
+********************************************************/
+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 == NULL) || (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] == true) {
+			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 = 0;
+
+	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;
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	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;
+#endif		
+	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.
+ */
+STORAGE_CLASS_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);
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	/* 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);
+#endif
+
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+enum ia_css_err ia_css_bufq_enqueue_buffer(
+	int thread_index,
+	int queue_id,
+	uint32_t item)
+{
+	enum ia_css_err return_err = IA_CSS_SUCCESS;
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* Get the queue for communication */
+	q = bufq_get_qhandle(sh_css_host2sp_buffer_queue,
+		queue_id,
+		thread_index);
+	if (q != NULL) {
+		error = ia_css_queue_enqueue(q, item);
+		return_err = ia_css_convert_errno(error);
+	} else {
+		IA_CSS_ERROR("queue is not initialized");
+		return_err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(return_err);
+	return return_err;
+}
+
+enum ia_css_err ia_css_bufq_dequeue_buffer(
+	int queue_id,
+	uint32_t *item)
+{
+	enum ia_css_err return_err;
+	int error = 0;
+	ia_css_queue_t *q;
+
+	IA_CSS_ENTER_PRIVATE("queue_id=%d", queue_id);
+	if ((item == NULL) ||
+	    (queue_id <= SH_CSS_INVALID_QUEUE_ID) ||
+	    (queue_id >= SH_CSS_MAX_NUM_QUEUES)
+	   )
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	q = bufq_get_qhandle(sh_css_sp2host_buffer_queue,
+		queue_id,
+		-1);
+	if (q != NULL) {
+		error = ia_css_queue_dequeue(q, item);
+		return_err = ia_css_convert_errno(error);
+	} else {
+		IA_CSS_ERROR("queue is not initialized");
+		return_err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(return_err);
+	return return_err;
+}
+
+enum ia_css_err ia_css_bufq_enqueue_psys_event(
+	uint8_t evt_id,
+	uint8_t evt_payload_0,
+	uint8_t evt_payload_1,
+	uint8_t evt_payload_2)
+{
+	enum ia_css_err return_err;
+	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 (NULL == q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	error = ia_css_eventq_send(q,
+			evt_id, evt_payload_0, evt_payload_1, evt_payload_2);
+
+	return_err = ia_css_convert_errno(error);
+	IA_CSS_LEAVE_ERR_PRIVATE(return_err);
+	return return_err;
+}
+
+enum  ia_css_err ia_css_bufq_dequeue_psys_event(
+	uint8_t item[BUFQ_EVENT_SIZE])
+{
+	enum ia_css_err;
+	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 == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	q = bufq_get_qhandle(sh_css_sp2host_psys_event_queue, -1, -1);
+	if (NULL == q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+	error = ia_css_eventq_recv(q, item);
+
+	return ia_css_convert_errno(error);
+
+}
+
+enum  ia_css_err ia_css_bufq_dequeue_isys_event(
+	uint8_t item[BUFQ_EVENT_SIZE])
+{
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	enum ia_css_err;
+	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 == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	q = bufq_get_qhandle(sh_css_sp2host_isys_event_queue, -1, -1);
+	if (q == NULL) {
+		IA_CSS_ERROR("queue is not initialized");
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+	error = ia_css_eventq_recv(q, item);
+	return ia_css_convert_errno(error);
+#else
+	(void)item;
+	return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+#endif
+}
+
+enum ia_css_err ia_css_bufq_enqueue_isys_event(uint8_t evt_id)
+{
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	enum ia_css_err return_err;
+	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 == NULL) {
+		IA_CSS_ERROR("queue is not initialized");
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	error = ia_css_eventq_send(q, evt_id, 0, 0, 0);
+	return_err = ia_css_convert_errno(error);
+	IA_CSS_LEAVE_ERR_PRIVATE(return_err);
+	return return_err;
+#else
+	(void)evt_id;
+	return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+#endif
+}
+
+enum ia_css_err ia_css_bufq_enqueue_tag_cmd(
+	uint32_t item)
+{
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	enum ia_css_err return_err;
+	int error = 0;
+	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 (NULL == q) {
+		IA_CSS_ERROR("queue is not initialized");
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+	error = ia_css_queue_enqueue(q, item);
+
+	return_err = ia_css_convert_errno(error);
+	IA_CSS_LEAVE_ERR_PRIVATE(return_err);
+	return return_err;
+#else
+	(void)item;
+	return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+#endif
+}
+
+enum ia_css_err ia_css_bufq_deinit(void)
+{
+	return IA_CSS_SUCCESS;
+}
+
+static void bufq_dump_queue_info(const char *prefix, ia_css_queue_t *qhandle)
+{
+	uint32_t free = 0, used = 0;
+	assert(prefix != NULL && qhandle != NULL);
+	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);
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	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);
+#endif
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h
new file mode 100644
index 000000000000..be7df3a30c21
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug.h
@@ -0,0 +1,508 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <stdarg.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"
+#ifdef ISP2401
+#if defined(IS_ISP_2500_SYSTEM)
+#include "ia_css_pipe.h"
+#endif
+#endif
+
+/* 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 unsigned int ia_css_debug_trace_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 = 1 << 0, /**< FPN table */
+	IA_CSS_DEBUG_DUMP_OB = 1 << 1,  /**< OB table */
+	IA_CSS_DEBUG_DUMP_SC = 1 << 2,  /**< Shading table */
+	IA_CSS_DEBUG_DUMP_WB = 1 << 3,  /**< White balance */
+	IA_CSS_DEBUG_DUMP_DP = 1 << 4,  /**< Defect Pixel */
+	IA_CSS_DEBUG_DUMP_BNR = 1 << 5,  /**< Bayer Noise Reductions */
+	IA_CSS_DEBUG_DUMP_S3A = 1 << 6,  /**< 3A Statistics */
+	IA_CSS_DEBUG_DUMP_DE = 1 << 7,  /**< De Mosaicing */
+	IA_CSS_DEBUG_DUMP_YNR = 1 << 8,  /**< Luma Noise Reduction */
+	IA_CSS_DEBUG_DUMP_CSC = 1 << 9,  /**< Color Space Conversion */
+	IA_CSS_DEBUG_DUMP_GC = 1 << 10,  /**< Gamma Correction */
+	IA_CSS_DEBUG_DUMP_TNR = 1 << 11,  /**< Temporal Noise Reduction */
+	IA_CSS_DEBUG_DUMP_ANR = 1 << 12,  /**< Advanced Noise Reduction */
+	IA_CSS_DEBUG_DUMP_CE = 1 << 13,  /**< Chroma Enhancement */
+	IA_CSS_DEBUG_DUMP_ALL = 1 << 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 enum ia_css_err 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 enum ia_css_err 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
+ */
+STORAGE_CLASS_INLINE void
+ia_css_debug_vdtrace(unsigned int level, const char *fmt, va_list args)
+{
+	if (ia_css_debug_trace_level >= level)
+		sh_css_vprint(fmt, args);
+}
+
+extern 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 input formatter state.
+ * Dumps the input formatter state to tracing output.
+ * @return	None
+ */
+void ia_css_debug_dump_if_state(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);
+
+#ifdef 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);
+
+#endif
+/*! @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);
+
+#ifdef 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);
+
+#if defined(IS_ISP_2500_SYSTEM)
+/*! @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
+#endif
+
+#endif /* _IA_CSS_DEBUG_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_internal.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_internal.h
new file mode 100644
index 000000000000..88d025807201
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_internal.h
@@ -0,0 +1,31 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+/* TO DO: Move debug related code from ia_css_internal.h in */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_pipe.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_pipe.h
new file mode 100644
index 000000000000..72ac0e32ebf7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/interface/ia_css_debug_pipe.h
@@ -0,0 +1,84 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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
+ */
+extern void ia_css_debug_pipe_graph_dump_prologue(void);
+
+/**
+ * @brief Internal debug support for constructing a pipe graph.
+ *
+ * @return	None
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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
+ */
+extern 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/atomisp2/css2400/runtime/debug/src/ia_css_debug.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c
new file mode 100644
index 000000000000..030810bd0878
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/debug/src/ia_css_debug.c
@@ -0,0 +1,3611 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "memory_access.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 "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"
+#ifdef ISP2401
+#include "ia_css_queue.h"
+#endif
+
+#include "ia_css_isp_params.h"
+
+#include "system_local.h"
+#include "assert_support.h"
+#include "print_support.h"
+#include "string_support.h"
+#ifdef ISP2401
+#include "ia_css_system_ctrl.h"
+#endif
+
+#include "fifo_monitor.h"
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+#include "input_formatter.h"
+#endif
+#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 */
+#if defined(HAS_INPUT_FORMATTER_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+#include "input_system.h"	/* input_formatter_reg_load */
+#endif
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+#include "ia_css_tagger_common.h"
+#endif
+
+#include "sh_css_internal.h"
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "ia_css_isys.h"
+#endif
+#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"
+
+/* Global variable to store the dtrace verbosity level */
+unsigned int ia_css_debug_trace_level = IA_CSS_DEBUG_WARNING;
+
+/* Assumes that IA_CSS_STREAM_FORMAT_BINARY_8 is last */
+#define N_IA_CSS_STREAM_FORMAT (IA_CSS_STREAM_FORMAT_BINARY_8+1)
+
+#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)
+
+#ifdef ISP2401
+#define DBG_EXT_CMD_TRACE_PNTS_DUMP (1 << 8)
+#define DBG_EXT_CMD_PUB_CFG_DUMP (1 << 9)
+#define DBG_EXT_CMD_GAC_REG_DUMP (1 << 10)
+#define DBG_EXT_CMD_GAC_ACB_REG_DUMP (1 << 11)
+#define DBG_EXT_CMD_FIFO_DUMP (1 << 12)
+#define DBG_EXT_CMD_QUEUE_DUMP (1 << 13)
+#define DBG_EXT_CMD_DMA_DUMP (1 << 14)
+#define DBG_EXT_CMD_MASK 0xAB0000CD
+
+#endif
+/*
+ * 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 ia_css_stream_format stream_format;
+} pg_inst = {true, 0, 0, 0, 0, N_IA_CSS_STREAM_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);
+}
+
+#if !defined(HRT_UNSCHED)
+static void debug_dump_long_array_formatted(
+	const sp_ID_t sp_id,
+	hrt_address stack_sp_addr,
+	unsigned stack_size)
+{
+	unsigned int i;
+	uint32_t val;
+	uint32_t addr = (uint32_t) stack_sp_addr;
+	uint32_t 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;
+	uint32_t stack_sizes[MAX_THREAD_NUM];
+	uint32_t 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);
+}
+#else
+/* Empty def for crun */
+void ia_css_debug_dump_sp_stack_info(void)
+{
+}
+#endif /* #if !HRT_UNSCHED */
+
+
+void ia_css_debug_set_dtrace_level(const unsigned int trace_level)
+{
+	ia_css_debug_trace_level = trace_level;
+	return;
+}
+
+unsigned int ia_css_debug_get_dtrace_level(void)
+{
+	return ia_css_debug_trace_level;
+}
+
+static const char *debug_stream_format2str(const enum ia_css_stream_format stream_format)
+{
+	switch (stream_format) {
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+		return "yuv420-8-legacy";
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+		return "yuv420-8";
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+		return "yuv420-10";
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+		return "yuv420-16";
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+		return "yuv422-8";
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+		return "yuv422-10";
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+		return "yuv422-16";
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+		return "rgb444";
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+		return "rgb555";
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+		return "rgb565";
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+		return "rgb666";
+	case IA_CSS_STREAM_FORMAT_RGB_888:
+		return "rgb888";
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+		return "raw6";
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+		return "raw7";
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+		return "raw8";
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+		return "raw10";
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+		return "raw12";
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+		return "raw14";
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+		return "raw16";
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+		return "binary8";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT1:
+		return "generic-short1";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT2:
+		return "generic-short2";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT3:
+		return "generic-short3";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT4:
+		return "generic-short4";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT5:
+		return "generic-short5";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT6:
+		return "generic-short6";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT7:
+		return "generic-short7";
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT8:
+		return "generic-short8";
+	case IA_CSS_STREAM_FORMAT_YUV420_8_SHIFT:
+		return "yuv420-8-shift";
+	case IA_CSS_STREAM_FORMAT_YUV420_10_SHIFT:
+		return "yuv420-10-shift";
+	case IA_CSS_STREAM_FORMAT_EMBEDDED:
+		return "embedded-8";
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+		return "user-def-8-type-1";
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+		return "user-def-8-type-2";
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+		return "user-def-8-type-3";
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+		return "user-def-8-type-4";
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+		return "user-def-8-type-5";
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+		return "user-def-8-type-6";
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+		return "user-def-8-type-7";
+	case IA_CSS_STREAM_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 != NULL);
+	assert(state != NULL);
+
+	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 != NULL);
+	assert(cell != NULL);
+
+	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 !defined(HAS_NO_INPUT_FORMATTER)
+		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);
+#endif
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[2] dma_FIFO stalled",
+				    stall.fifo2);
+#if defined(HAS_ISP_2400_MAMOIADA) || defined(HAS_ISP_2401_MAMOIADA) || defined(IS_ISP_2500_SYSTEM)
+
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[3] gdc0_FIFO stalled",
+				    stall.fifo3);
+#if !defined(IS_ISP_2500_SYSTEM)
+		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);
+#endif
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "[6] sp_FIFO stalled",
+				    stall.fifo6);
+#else
+#error "ia_css_debug: ISP cell must be one of {2400_MAMOIADA,, 2401_MAMOIADA, 2500_SKYCAM}"
+#endif
+		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);
+#if defined(HAS_ISP_2400_MAMOIADA) || defined(HAS_ISP_2401_MAMOIADA)
+		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);
+#endif
+	}
+	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) {
+#if defined(HAS_SP_2400) || defined(IS_ISP_2500_SYSTEM)
+#if !defined(HAS_NO_INPUT_SYSTEM)
+		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);
+#endif
+		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 !defined(HAS_NO_INPUT_FORMATTER)
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "if_prim_a_FIFO stalled", stall.fifo4);
+#endif
+		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 !defined(HAS_NO_INPUT_FORMATTER)
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n",
+				    "if_prim_b_FIFO stalled", stall.fifo7);
+#endif
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "gdc0_FIFO stalled",
+				    stall.fifo8);
+#if !defined(IS_ISP_2500_SYSTEM)
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "gdc1_FIFO stalled",
+				    stall.fifo9);
+#endif
+		ia_css_debug_dtrace(2, "\t%-32s: %d\n", "irq FIFO stalled",
+				    stall.fifoa);
+#else
+#error "ia_css_debug: SP cell must be one of {SP2400, SP2500}"
+#endif
+		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 != NULL);
+	assert(descr != NULL);
+
+	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;
+}
+
+#if !defined(HAS_NO_INPUT_FORMATTER) && defined(USE_INPUT_SYSTEM_VERSION_2)
+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");
+}
+
+static void debug_print_if_state(input_formatter_state_t *state, const char *id)
+{
+	unsigned int val;
+
+#if defined(HAS_INPUT_FORMATTER_VERSION_1)
+	const char *st_reset = (state->reset ? "Active" : "Not active");
+#endif
+	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 != NULL);
+	ia_css_debug_dtrace(2, "InputFormatter State (%s):\n", id);
+
+	ia_css_debug_dtrace(2, "\tConfiguration:\n");
+
+#if defined(HAS_INPUT_FORMATTER_VERSION_1)
+	ia_css_debug_dtrace(2, "\t\t%-32s: %s\n", "Software reset", st_reset);
+#endif
+	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);
+
+#if defined(HAS_INPUT_FORMATTER_VERSION_2)
+	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));
+#endif
+
+	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);
+	return;
+}
+
+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);
+}
+
+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 != NULL);
+	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 != NULL);
+	assert(descr != NULL);
+
+	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);
+	ia_css_debug_dtrace(2, "  is contiguous = %s\n",
+			    frame->contiguous ? "yes" : "no");
+	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 != NULL);
+	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 != NULL);
+	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"
+	};
+
+#if 1
+	/* 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"
+	};
+#else
+	/* Example SH_CSS_SP_DBG_NR_OF_TRACES==4 */
+	/* Adjust this to your trace case */
+	static char const *trace_name[SH_CSS_SP_DBG_NR_OF_TRACES] = {
+		"copy", "preview/video", "capture", "acceleration"
+	};
+#endif
+
+	/* 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 != NULL);
+
+	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 != NULL);
+
+	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(HAS_INPUT_FORMATTER_VERSION_2) && !defined(HAS_NO_INPUT_FORMATTER)
+static void debug_print_rx_mipi_port_state(mipi_port_state_t *state)
+{
+	int i;
+	unsigned int bits, infos;
+
+	assert(state != NULL);
+
+	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 != NULL);
+	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 != NULL);
+	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
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+void ia_css_debug_dump_rx_state(void)
+{
+#if defined(HAS_INPUT_FORMATTER_VERSION_2) && !defined(HAS_NO_INPUT_FORMATTER)
+	receiver_state_t state;
+
+	receiver_get_state(RX0_ID, &state);
+	debug_print_rx_state(&state);
+#endif
+}
+#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(USE_INPUT_SYSTEM_VERSION_2)
+static void debug_print_isys_capture_unit_state(capture_unit_state_t *state)
+{
+	assert(state != NULL);
+	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 != NULL);
+
+	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 != NULL);
+	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 != NULL);
+	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 */
+}
+
+void ia_css_debug_dump_isys_state(void)
+{
+	input_system_state_t state;
+
+	input_system_get_state(INPUT_SYSTEM0_ID, &state);
+	debug_print_isys_state(&state);
+
+	return;
+}
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+void ia_css_debug_dump_isys_state(void)
+{
+	/* Android compilation fails if made a local variable
+	stack size on android is limited to 2k and this structure
+	is around 3.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 input_system_state_t state;
+
+	input_system_get_state(INPUT_SYSTEM0_ID, &state);
+	input_system_dump_state(INPUT_SYSTEM0_ID, &state);
+}
+#endif
+
+void ia_css_debug_dump_debug_info(const char *context)
+{
+	if (context == NULL)
+		context = "No Context provided";
+
+	ia_css_debug_dtrace(2, "CSS Debug Info dump [Context = %s]\n", context);
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	ia_css_debug_dump_rx_state();
+#endif
+#if !defined(HAS_NO_INPUT_FORMATTER) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	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 defined(USE_INPUT_SYSTEM_VERSION_2)
+	ia_css_debug_dump_isys_state();
+
+	{
+		irq_controller_state_t 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);
+	}
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+	ia_css_debug_dump_isys_state();
+#endif
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	ia_css_debug_tagger_state();
+#endif
+	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);
+}
+
+#if !defined(IS_ISP_2500_SYSTEM)
+#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;
+}
+#endif
+
+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");
+#if defined(IS_ISP_2500_SYSTEM)
+	(void)enable;
+	(void)stream;
+#else
+
+	assert(stream != NULL);
+	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);
+	}
+#endif
+}
+
+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;
+	int32_t raw_copy_line_count;
+	static int32_t 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;
+	uint32_t curr_binary_id;
+	static uint32_t prev_binary_id = 0xFFFFFFFF;
+	static uint32_t 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)
+{
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	const struct ia_css_fw_info *fw;
+	int i;
+	unsigned int HIVE_ADDR_ia_css_isys_sp_error_cnt;
+	int32_t ia_css_sp_input_system_error_cnt[N_MIPI_PORT_ID + 1]; /* 3 Capture Units and 1 Acquire Unit. */
+
+	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]);
+	}
+#endif
+}
+
+/*
+
+void sh_css_init_ddr_debug_queue(void)
+{
+	hrt_vaddress ddr_debug_queue_addr =
+			mmgr_malloc(sizeof(debug_data_ddr_t));
+	const struct ia_css_fw_info *fw;
+	unsigned int HIVE_ADDR_debug_buffer_ddr_address;
+
+	fw = &sh_css_sp_fw;
+	HIVE_ADDR_debug_buffer_ddr_address =
+			fw->info.sp.debug_buffer_ddr_address;
+
+	(void)HIVE_ADDR_debug_buffer_ddr_address;
+
+	debug_buffer_ddr_init(ddr_debug_queue_addr);
+
+	sp_dmem_store_uint32(SP0_ID,
+		(unsigned int)sp_address_of(debug_buffer_ddr_address),
+		(uint32_t)(ddr_debug_queue_addr));
+}
+
+void sh_css_load_ddr_debug_queue(void)
+{
+	debug_synch_queue_ddr();
+}
+
+void ia_css_debug_dump_ddr_debug_queue(void)
+{
+	int i;
+	sh_css_load_ddr_debug_queue();
+	for (i = 0; i < DEBUG_BUF_SIZE; i++) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_VERBOSE,
+			"ddr_debug_queue[%d] = 0x%x\n",
+			i, debug_data_ptr->buf[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;
+}
+
+void dtrace_dot(const char *fmt, ...)
+{
+	va_list ap;
+
+	assert(fmt != NULL);
+	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;
+	int32_t 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;
+	int32_t 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;
+	int32_t 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(
+	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]; \"0x%08lx\" "
+		"[label = \"%s\\n%d(%d) x %d, %dbpp\\n%s\"];",
+		HOST_ADDRESS(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(
+			"\"0x%08lx\"->\"%s(pipe%d)\" "
+			"[label = %s_frame];",
+			HOST_ADDRESS(frame),
+			blob_name, id, frame_name);
+	} else {
+		dtrace_dot(
+			"\"%s(pipe%d)\"->\"0x%08lx\" "
+			"[label = %s_frame];",
+			blob_name, id,
+			HOST_ADDRESS(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_IA_CSS_STREAM_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_IA_CSS_STREAM_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 != NULL);
+	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";
+		strncpy_s(blob_name, sizeof(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 != NULL) {
+		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) {
+				/* It fits on one line, copy string and init */
+				/* other helper strings with empty string */
+				strcpy_s(enable_info,
+					sizeof(enable_info),
+					ei);
+			} else {
+				/* Too big for one line, find last comma */
+				p = ENABLE_LINE_MAX_LENGTH;
+				while (ei[p] != ',')
+					p--;
+				/* Last comma found, copy till that comma */
+				strncpy_s(enable_info1,
+					sizeof(enable_info1),
+					ei, p);
+				enable_info1[p] = '\0';
+
+				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 guarenteed dword aligned
+					 */
+					strncpy_s(enable_info2,
+						sizeof(enable_info2),
+						ei, l);
+					enable_info2[l] = '\0';
+					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--;
+					strncpy_s(enable_info2,
+						sizeof(enable_info2),
+						ei, p);
+					enable_info2[p] = '\0';
+					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 guarenteed dword aligned
+						*/
+						strcpy_s(enable_info3,
+							sizeof(enable_info3), ei);
+						enable_info3[l] = '\0';
+						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--;
+						strncpy_s(enable_info3,
+							sizeof(enable_info3),
+							ei, p);
+						enable_info3[p] = '\0';
+						ei += p+1;
+						strcpy_s(enable_info3,
+							sizeof(enable_info3), ei);
+						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);
+	}
+
+#ifndef ISP2401
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+#else
+	for (i = 0; i < NUM_TNR_FRAMES; i++) {
+#endif
+		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 != NULL);
+	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]; \"0x%08lx\" "
+		"[label = \"%s\\n%d(%d) x %d\\nRingbuffer\"];",
+		HOST_ADDRESS(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)\"->\"0x%08lx\" "
+		"[label = out_frame];",
+		"sp_raw_copy", 1, HOST_ADDRESS(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");
+#ifdef ISP2401
+	ia_css_debug_dump_resolution(&config->output_system_in_res,
+				     "output_system_in_res");
+#endif
+	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: 0x%x\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);
+		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
+#ifndef ISP2401
+static void debug_dump_one_trace(TRACE_CORE_ID proc_id)
+#else
+static void debug_dump_one_trace(enum TRACE_CORE_ID proc_id)
+#endif
+{
+#if defined(HAS_TRACER_V2)
+	uint32_t start_addr;
+	uint32_t start_addr_data;
+	uint32_t item_size;
+#ifndef ISP2401
+	uint32_t tmp;
+#else
+	uint8_t tid_val;
+	enum TRACE_DUMP_FORMAT dump_format;
+#endif
+	int i, j, max_trace_points, point_num, limit = -1;
+	/* using a static buffer here as the driver has issues allocating memory */
+	static uint32_t trace_read_buf[TRACE_BUFF_SIZE] = {0};
+#ifdef ISP2401
+	static struct trace_header_t header;
+	uint8_t *header_arr;
+#endif
+
+	/* 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;
+	}
+#ifndef ISP2401
+	tmp = ia_css_device_load_uint32(start_addr);
+	point_num = (tmp >> 16) & 0xFFFF;
+#endif
+
+#ifndef ISP2401
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, " ver %d %d points\n", tmp & 0xFF, point_num);
+	if ((tmp & 0xFF) != TRACER_VER) {
+#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);
+	if ((header.version & 0xFF) != TRACER_VER) {
+#endif
+		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;
+	}
+#ifdef 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");
+
+#endif
+	/* 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])
+		{
+#ifndef 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]);
+			}
+#endif
+			switch (dump_format)
+			{
+			case TRACE_DUMP_FORMAT_POINT:
+				ia_css_debug_dtrace(
+#ifndef ISP2401
+						IA_CSS_DEBUG_TRACE,	"\t\t%d %d:%d value - %d\n",
+						j, FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+#else
+						IA_CSS_DEBUG_TRACE,	"\t\t%d T%d %d:%d value - %x (%d)\n",
+						j,
+						tid_val,
+						FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+#endif
+						FIELD_MINOR_UNPACK(trace_read_buf[j]),
+#ifdef ISP2401
+						FIELD_VALUE_UNPACK(trace_read_buf[j]),
+#endif
+						FIELD_VALUE_UNPACK(trace_read_buf[j]));
+				break;
+#ifndef ISP2401
+			case TRACE_DUMP_FORMAT_VALUE24_HEX:
+#else
+			case TRACE_DUMP_FORMAT_POINT_NO_TID:
+#endif
+				ia_css_debug_dtrace(
+#ifndef ISP2401
+						IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %x H\n",
+#else
+						IA_CSS_DEBUG_TRACE,	"\t\t%d %d:%d value - %x (%d)\n",
+#endif
+						j,
+#ifndef ISP2401
+						FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+						FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+#else
+						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]));
+#endif
+				break;
+#ifndef ISP2401
+			case TRACE_DUMP_FORMAT_VALUE24_DEC:
+#else
+			case TRACE_DUMP_FORMAT_VALUE24:
+#endif
+				ia_css_debug_dtrace(
+#ifndef ISP2401
+						IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %d D\n",
+#else
+						IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, 24bit value %x (%d)\n",
+#endif
+						j,
+						FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+#ifdef ISP2401
+						FIELD_MAJOR_W_FMT_UNPACK(trace_read_buf[j]),
+						FIELD_VALUE_24_UNPACK(trace_read_buf[j]),
+#endif
+						FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+#ifdef ISP2401
+
+#endif
+			case TRACE_DUMP_FORMAT_VALUE24_TIMING:
+				ia_css_debug_dtrace(
+						IA_CSS_DEBUG_TRACE,	"\t\t%d, %d, timing %x\n",
+						j,
+#ifndef ISP2401
+						FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+#else
+						FIELD_MAJOR_W_FMT_UNPACK(trace_read_buf[j]),
+#endif
+						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,
+#ifndef ISP2401
+						FIELD_MAJOR_UNPACK(trace_read_buf[j]),
+#else
+						FIELD_MAJOR_W_FMT_UNPACK(trace_read_buf[j]),
+#endif
+						FIELD_VALUE_24_UNPACK(trace_read_buf[j]));
+				break;
+			default:
+				ia_css_debug_dtrace(
+						IA_CSS_DEBUG_TRACE,
+						"no such trace dump format %d",
+#ifndef ISP2401
+						FIELD_FORMAT_UNPACK(trace_read_buf[j]));
+#else
+						dump_format);
+#endif
+				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
+}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+/* 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);
+	}
+
+}
+#endif /* defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401) */
+
+#ifdef 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);
+	}
+}
+#endif
+
+#if defined(HRT_SCHED) || defined(SH_CSS_DEBUG_SPMEM_DUMP_SUPPORT)
+#include "spmem_dump.c"
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/interface/ia_css_event.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/interface/ia_css_event.h
new file mode 100644
index 000000000000..ab1d9bed9fd8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/interface/ia_css_event.h
@@ -0,0 +1,46 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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(
+	uint8_t	*in,
+	uint8_t	nr,
+	uint32_t	*out);
+
+void ia_css_event_decode(
+	uint32_t event,
+	uint8_t *payload);
+
+#endif /*_IA_CSS_EVENT_H*/
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/src/event.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/src/event.c
new file mode 100644
index 000000000000..2698c3e1adb0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/event/src/event.c
@@ -0,0 +1,126 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 "memory_access.h"
+
+#include "assert_support.h"
+#include "platform_support.h"	/* hrt_sleep() */
+
+#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(
+	uint8_t	*in,
+	uint8_t	nr,
+	uint32_t	*out)
+{
+	bool ret;
+	uint32_t nr_of_bits;
+	uint32_t i;
+	assert(in != NULL);
+	assert(out != NULL);
+	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(
+	uint32_t 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/atomisp2/css2400/runtime/eventq/interface/ia_css_eventq.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/eventq/interface/ia_css_eventq.h
new file mode 100644
index 000000000000..67eb8fdb33c5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/eventq/interface/ia_css_eventq.h
@@ -0,0 +1,69 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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,
+		uint8_t evt_id,
+		uint8_t evt_payload_0,
+		uint8_t evt_payload_1,
+		uint8_t evt_payload_2);
+#endif /* _IA_CSS_EVENTQ_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/eventq/src/eventq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/eventq/src/eventq.c
new file mode 100644
index 000000000000..56d6858890ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/eventq/src/eventq.c
@@ -0,0 +1,77 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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()
+				*/
+#include "platform_support.h" /* hrt_sleep() */
+
+int ia_css_eventq_recv(
+		ia_css_queue_t *eventq_handle,
+		uint8_t *payload)
+{
+	uint32_t 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,
+			uint8_t evt_id,
+			uint8_t evt_payload_0,
+			uint8_t evt_payload_1,
+			uint8_t evt_payload_2)
+{
+	uint8_t tmp[4];
+	uint32_t 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 (ENOBUFS != error) {
+			/* 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*/
+		hrt_sleep();
+	}
+	return error;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame.h
new file mode 100644
index 000000000000..c7e07b79f4e5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame.h
@@ -0,0 +1,180 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef __IA_CSS_FRAME_H__
+#define __IA_CSS_FRAME_H__
+
+#ifdef ISP2401
+#include <ia_css_types.h>
+#endif
+#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.
+ */
+enum ia_css_err 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.
+ */
+enum ia_css_err 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.
+ * @param[in]	contiguous	Allocate memory physically contiguously or not.
+ * @return	The error code.
+ *
+ * Allocate a frame using the given size in bytes.
+ * The frame structure is partially null initialized.
+ */
+enum ia_css_err ia_css_frame_allocate_with_buffer_size(
+	struct ia_css_frame **frame,
+	const unsigned int size_bytes,
+	const bool contiguous);
+
+/** @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
+ */
+void ia_css_dma_configure_from_info(
+	struct dma_port_config *config,
+	const struct ia_css_frame_info *info);
+
+#ifdef ISP2401
+/** @brief Finds the cropping resolution
+ * This function finds the maximum cropping resolution in an input image keeping
+ * the aspect ratio for the given output resolution.Calculates the coordinates
+ * for cropping from the center and returns the starting pixel location of the
+ * region in the input image. Also returns the dimension of the cropping
+ * resolution.
+ *
+ * @param
+ * @param[in]	in_res		Resolution of input image
+ * @param[in]	out_res		Resolution of output image
+ * @param[out]	crop_res	Crop resolution of input image
+ * @return	Returns IA_CSS_SUCCESS or IA_CSS_ERR_INVALID_ARGUMENTS on error
+ */
+enum ia_css_err
+ia_css_frame_find_crop_resolution(const struct ia_css_resolution *in_res,
+	const struct ia_css_resolution *out_res,
+	struct ia_css_resolution *crop_res);
+
+#endif
+#endif /* __IA_CSS_FRAME_H__ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame_comm.h
new file mode 100644
index 000000000000..a469e0afb2b5
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/interface/ia_css_frame_comm.h
@@ -0,0 +1,132 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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_types.h>	 /* hrt_vaddress */
+
+/*
+ * 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 {
+	uint16_t width;		/* width of valid data in pixels */
+	uint16_t 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;
+	uint16_t 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 {
+		hrt_vaddress 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/atomisp2/css2400/runtime/frame/src/frame.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/src/frame.c
new file mode 100644
index 000000000000..f1a943cf04c0
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/frame/src/frame.c
@@ -0,0 +1,1026 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 "memory_access.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_mipi_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_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 enum ia_css_err frame_allocate_buffer_data(struct ia_css_frame *frame);
+
+static enum ia_css_err 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,
+	bool contiguous);
+
+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 contiguous,
+	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 != NULL);
+	mmgr_clear(frame->data, frame->data_bytes);
+}
+
+enum ia_css_err ia_css_frame_allocate_from_info(struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	if (frame == NULL || info == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	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;
+}
+
+enum ia_css_err 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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	if (frame == NULL || width == 0 || height == 0)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+#ifndef ISP2401
+	  "ia_css_frame_allocate() enter: width=%d, height=%d, format=%d\n",
+	  width, height, format);
+#else
+	  "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);
+#endif
+
+	err = frame_allocate_with_data(frame, width, height, format,
+				       padded_width, raw_bit_depth, false);
+
+#ifndef ISP2401
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		      "ia_css_frame_allocate() leave: frame=%p\n", *frame);
+#else
+	if ((*frame != NULL) && err == IA_CSS_SUCCESS)
+		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);
+#endif
+
+	return err;
+}
+
+enum ia_css_err ia_css_frame_map(struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info,
+	const void *data,
+	uint16_t attribute,
+	void *context)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_frame *me;
+	assert(frame != NULL);
+
+	/* Create the frame structure */
+	err = ia_css_frame_create_from_info(&me, info);
+
+	if (err != IA_CSS_SUCCESS)
+		return err;
+
+	if (err == IA_CSS_SUCCESS) {
+		/* use mmgr_mmap to map */
+		me->data = (ia_css_ptr) mmgr_mmap(data,
+						  me->data_bytes,
+						  attribute, context);
+		if (me->data == mmgr_NULL)
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	};
+
+	if (err != IA_CSS_SUCCESS) {
+		sh_css_free(me);
+#ifndef ISP2401
+		return err;
+#else
+		me = NULL;
+#endif
+	}
+
+	*frame = me;
+
+	return err;
+}
+
+enum ia_css_err ia_css_frame_create_from_info(struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_frame *me;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"ia_css_frame_create_from_info() enter:\n");
+	if (frame == NULL || info == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			"ia_css_frame_create_from_info() leave:"
+			" invalid arguments\n");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	me = frame_create(info->res.width,
+		info->res.height,
+		info->format,
+		info->padded_width,
+		info->raw_bit_depth,
+		false,
+		false);
+	if (me == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			"ia_css_frame_create_from_info() leave:"
+			" frame create failed\n");
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+
+	err = ia_css_frame_init_planes(me);
+
+#ifndef ISP2401
+	if (err == IA_CSS_SUCCESS)
+		*frame = me;
+	else
+#else
+	if (err != IA_CSS_SUCCESS) {
+#endif
+		sh_css_free(me);
+#ifdef ISP2401
+		me = NULL;
+	}
+
+	*frame = me;
+#endif
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_frame_create_from_info() leave:\n");
+
+	return err;
+}
+
+enum ia_css_err ia_css_frame_set_data(struct ia_css_frame *frame,
+	const ia_css_ptr mapped_data,
+	size_t data_bytes)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"ia_css_frame_set_data() enter:\n");
+	if (frame == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			"ia_css_frame_set_data() leave: NULL frame\n");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	frame->data = mapped_data;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_frame_set_data() leave:\n");
+
+	return err;
+}
+
+enum ia_css_err ia_css_frame_allocate_contiguous(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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"ia_css_frame_allocate_contiguous() "
+#ifndef ISP2401
+		"enter: width=%d, height=%d, format=%d\n",
+		width, height, format);
+#else
+		"enter: width=%d, height=%d, format=%d, padded_width=%d, raw_bit_depth=%d\n",
+		width, height, format, padded_width, raw_bit_depth);
+#endif
+
+	err = frame_allocate_with_data(frame, width, height, format,
+			padded_width, raw_bit_depth, true);
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"ia_css_frame_allocate_contiguous() leave: frame=%p\n",
+		frame ? *frame : (void *)-1);
+
+	return err;
+}
+
+enum ia_css_err ia_css_frame_allocate_contiguous_from_info(
+	struct ia_css_frame **frame,
+	const struct ia_css_frame_info *info)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	assert(frame != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"ia_css_frame_allocate_contiguous_from_info() enter:\n");
+	err = ia_css_frame_allocate_contiguous(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_contiguous_from_info() leave:\n");
+	return err;
+}
+
+void ia_css_frame_free(struct ia_css_frame *frame)
+{
+	IA_CSS_ENTER_PRIVATE("frame = %p", frame);
+
+	if (frame != NULL) {
+		hmm_free(frame->data);
+		sh_css_free(frame);
+	}
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+/**************************************************************************
+**	Module public functions
+**************************************************************************/
+
+enum ia_css_err ia_css_frame_check_info(const struct ia_css_frame_info *info)
+{
+	assert(info != NULL);
+	if (info->res.width == 0 || info->res.height == 0)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err ia_css_frame_init_planes(struct ia_css_frame *frame)
+{
+	assert(frame != NULL);
+
+	switch (frame->info.format) {
+	case IA_CSS_FRAME_FORMAT_MIPI:
+		frame_init_mipi_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_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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	return IA_CSS_SUCCESS;
+}
+
+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 == NULL) {
+		IA_CSS_ERROR("NULL input parameter");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	if (min_padded_width > width)
+		align = min_padded_width;
+	else
+		align = 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 != NULL);
+	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 == NULL) {
+		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;
+		}
+	}
+}
+
+enum ia_css_err ia_css_frame_allocate_with_buffer_size(
+	struct ia_css_frame **frame,
+	const unsigned int buffer_size_bytes,
+	const bool contiguous)
+{
+	/* AM: Body coppied from frame_allocate_with_data(). */
+	enum ia_css_err err;
+	struct ia_css_frame *me = frame_create(0, 0,
+		IA_CSS_FRAME_FORMAT_NUM,/* Not valid format yet */
+		0, 0, contiguous, false);
+
+	if (me == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	/* Get the data size */
+	me->data_bytes = buffer_size_bytes;
+
+	err = frame_allocate_buffer_data(me);
+
+	if (err != IA_CSS_SUCCESS) {
+		sh_css_free(me);
+#ifndef ISP2401
+		return err;
+#else
+		me = NULL;
+#endif
+	}
+
+	*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;
+}
+
+void
+ia_css_dma_configure_from_info(
+	struct dma_port_config *config,
+	const struct ia_css_frame_info *info)
+{
+	unsigned is_raw_packed = info->format == IA_CSS_FRAME_FORMAT_RAW_PACKED;
+	unsigned bits_per_pixel = is_raw_packed ? info->raw_bit_depth : ia_css_elems_bytes_from_info(info)*8;
+	unsigned pix_per_ddrword = HIVE_ISP_DDR_WORD_BITS / bits_per_pixel;
+	unsigned words_per_line = CEIL_DIV(info->padded_width, pix_per_ddrword);
+	unsigned 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;
+	assert(config->width <= info->padded_width);
+}
+
+/**************************************************************************
+**	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 != NULL);
+
+	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_mipi_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->data_bytes = 8388608; /* 8*1024*1024 */
+	frame->valid = false;
+	frame->contiguous = true;
+	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 (IA_CSS_FRAME_FORMAT_NV12_TILEY == frame->info.format) {
+		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 enum ia_css_err 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 = mmgr_alloc_attr(frame->data_bytes,
+				      frame->contiguous ?
+				      MMGR_ATTRIBUTE_CONTIGUOUS :
+				      MMGR_ATTRIBUTE_DEFAULT);
+
+	if (frame->data == mmgr_NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err 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,
+	bool contiguous)
+{
+	enum ia_css_err err;
+	struct ia_css_frame *me = frame_create(width,
+		height,
+		format,
+		padded_width,
+		raw_bit_depth,
+		contiguous,
+		true);
+
+	if (me == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	err = ia_css_frame_init_planes(me);
+
+	if (err == IA_CSS_SUCCESS)
+		err = frame_allocate_buffer_data(me);
+
+	if (err != IA_CSS_SUCCESS) {
+		sh_css_free(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 contiguous,
+	bool valid)
+{
+	struct ia_css_frame *me = sh_css_malloc(sizeof(*me));
+
+	if (me == NULL)
+		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->contiguous = contiguous;
+	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 overwriten 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;
+}
+#ifdef ISP2401
+
+enum ia_css_err
+ia_css_frame_find_crop_resolution(const struct ia_css_resolution *in_res,
+	const struct ia_css_resolution *out_res,
+	struct ia_css_resolution *crop_res)
+{
+	uint32_t wd_even_ceil, ht_even_ceil;
+	uint32_t in_ratio, out_ratio;
+
+	if ((in_res == NULL) || (out_res == NULL) || (crop_res == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	IA_CSS_ENTER_PRIVATE("in(%ux%u) -> out(%ux%u)", in_res->width,
+		in_res->height, out_res->width, out_res->height);
+
+	if ((in_res->width == 0)
+		|| (in_res->height == 0)
+		|| (out_res->width == 0)
+		|| (out_res->height == 0))
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	if ((out_res->width > in_res->width) ||
+		 (out_res->height > in_res->height))
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* If aspect ratio (width/height) of out_res is higher than the aspect
+	 * ratio of the in_res, then we crop vertically, otherwise we crop
+	 * horizontally.
+	 */
+	in_ratio = in_res->width * out_res->height;
+	out_ratio = out_res->width * in_res->height;
+
+	if (in_ratio == out_ratio) {
+		crop_res->width = in_res->width;
+		crop_res->height = in_res->height;
+	} else if (out_ratio > in_ratio) {
+		crop_res->width = in_res->width;
+		crop_res->height = ROUND_DIV(out_res->height * crop_res->width,
+			out_res->width);
+	} else {
+		crop_res->height = in_res->height;
+		crop_res->width = ROUND_DIV(out_res->width * crop_res->height,
+			out_res->height);
+	}
+
+	/* Round new (cropped) width and height to an even number.
+	 * binarydesc_calculate_bds_factor is such that we should consider as
+	 * much of the input as possible. This is different only when we end up
+	 * with an odd number in the last step. So, we take the next even number
+	 * if it falls within the input, otherwise take the previous even no.
+	 */
+	wd_even_ceil = EVEN_CEIL(crop_res->width);
+	ht_even_ceil = EVEN_CEIL(crop_res->height);
+	if ((wd_even_ceil > in_res->width) || (ht_even_ceil > in_res->height)) {
+		crop_res->width = EVEN_FLOOR(crop_res->width);
+		crop_res->height = EVEN_FLOOR(crop_res->height);
+	} else {
+		crop_res->width = wd_even_ceil;
+		crop_res->height = ht_even_ceil;
+	}
+
+	IA_CSS_LEAVE_PRIVATE("in(%ux%u) -> out(%ux%u)", crop_res->width,
+		crop_res->height, out_res->width, out_res->height);
+	return IA_CSS_SUCCESS;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/ifmtr/interface/ia_css_ifmtr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/ifmtr/interface/ia_css_ifmtr.h
new file mode 100644
index 000000000000..d02bff1bbf46
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/ifmtr/interface/ia_css_ifmtr.h
@@ -0,0 +1,49 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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);
+
+enum ia_css_err 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/atomisp2/css2400/runtime/ifmtr/src/ifmtr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/ifmtr/src/ifmtr.c
new file mode 100644
index 000000000000..a7c6bba7e094
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/ifmtr/src/ifmtr.c
@@ -0,0 +1,568 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2
+
+#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 enum ia_css_err ifmtr_start_column(
+		const struct ia_css_stream_config *config,
+		unsigned int bin_in,
+		unsigned int *start_column);
+
+static enum ia_css_err 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 != NULL);
+	if ((IA_CSS_BAYER_ORDER_BGGR == config->input_config.bayer_order)
+	    || (IA_CSS_BAYER_ORDER_GBRG == config->input_config.bayer_order))
+		return 1;
+
+	return 0;
+}
+
+unsigned int ia_css_ifmtr_columns_needed_for_bayer_order(
+		const struct ia_css_stream_config *config)
+{
+	assert(config != NULL);
+	if ((IA_CSS_BAYER_ORDER_RGGB == config->input_config.bayer_order)
+	    || (IA_CSS_BAYER_ORDER_GBRG == config->input_config.bayer_order))
+		return 1;
+
+	return 0;
+}
+
+enum ia_css_err 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 ia_css_stream_format input_format;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	uint8_t if_config_index;
+
+	/* Determine which input formatter config set is targeted. */
+	/* Index is equal to the CSI-2 port used. */
+	enum ia_css_csi2_port 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 - IA_CSS_CSI2_PORT0);
+	} 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 != IA_CSS_SUCCESS)
+		return err;
+	err = ifmtr_start_column(config, cropped_width, &start_column);
+	if (err != IA_CSS_SUCCESS)
+		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 IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_RGB_444:
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_RAW_6:
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_RAW_14:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_BINARY_8:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT1:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT2:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT3:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT4:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT5:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT6:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT7:
+	case IA_CSS_STREAM_FORMAT_GENERIC_SHORT8:
+	case IA_CSS_STREAM_FORMAT_YUV420_8_SHIFT:
+	case IA_CSS_STREAM_FORMAT_YUV420_10_SHIFT:
+	case IA_CSS_STREAM_FORMAT_EMBEDDED:
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+	case IA_CSS_STREAM_FORMAT_USER_DEF8:
+		break;
+	}
+	if (width_a == 0)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 == IA_CSS_STREAM_FORMAT_YUV420_8)
+	    || (input_format == IA_CSS_STREAM_FORMAT_YUV420_10)
+	    || (input_format == IA_CSS_STREAM_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 == IA_CSS_STREAM_FORMAT_YUV420_8
+		    || input_format == IA_CSS_STREAM_FORMAT_YUV420_10
+		    || input_format == IA_CSS_STREAM_FORMAT_YUV420_16;
+		if_b_config.block_no_reqs =
+		    (config->mode != IA_CSS_INPUT_MODE_SENSOR);
+
+		if (SH_CSS_IF_CONFIG_NOT_NEEDED != if_config_index) {
+			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 (SH_CSS_IF_CONFIG_NOT_NEEDED != if_config_index) {
+			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 IA_CSS_SUCCESS;
+}
+
+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 <= (sizeof(block) / sizeof(block[0])));
+
+#if !defined(IS_ISP_2400_SYSTEM)
+#error "ifmtr_set_if_blocking_mode: ISP_SYSTEM must be one of {IS_ISP_2400_SYSTEM}"
+#endif
+
+	block[INPUT_FORMATTER0_ID] = (bool)config_a->block_no_reqs;
+	if (NULL != 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 enum ia_css_err 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* 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 IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* 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 IA_CSS_SUCCESS;
+}
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/inputfifo/interface/ia_css_inputfifo.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/inputfifo/interface/ia_css_inputfifo.h
new file mode 100644
index 000000000000..47d0f7e53f47
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/inputfifo/interface/ia_css_inputfifo.h
@@ -0,0 +1,69 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 ia_css_stream_format	input_format,
+	bool			two_ppc);
+
+void ia_css_inputfifo_start_frame(
+	unsigned int	ch_id,
+	enum ia_css_stream_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 ia_css_stream_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/atomisp2/css2400/runtime/inputfifo/src/inputfifo.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/inputfifo/src/inputfifo.c
new file mode 100644
index 000000000000..cf02970d4f59
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/inputfifo/src/inputfifo.c
@@ -0,0 +1,613 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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__
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "input_system.h"	/* MIPI_PREDICTOR_NONE,... */
+#endif
+
+#include "assert_support.h"
+
+/* System independent */
+#include "sh_css_internal.h"
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "ia_css_isys.h"
+#endif
+
+#define HBLANK_CYCLES (187)
+#define MARKER_CYCLES (6)
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include <hive_isp_css_streaming_to_mipi_types_hrt.h>
+#endif
+
+/* 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,
+};
+#if !defined(HAS_NO_INPUT_SYSTEM)
+static unsigned int inputfifo_curr_ch_id, inputfifo_curr_fmt_type;
+#endif
+struct inputfifo_instance {
+	unsigned int				ch_id;
+	enum ia_css_stream_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;
+};
+#if !defined(HAS_NO_INPUT_SYSTEM)
+/*
+ * 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 inputfifo_wrap_marker(
+/* STORAGE_CLASS_INLINE unsigned inputfifo_wrap_marker( */
+	unsigned 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);
+}
+
+STORAGE_CLASS_INLINE void
+_sh_css_fifo_snd(unsigned token)
+{
+	while (!can_event_send_token(STR2MIPI_EVENT_ID))
+		hrt_sleep();
+	event_send_token(STR2MIPI_EVENT_ID, token);
+	return;
+}
+
+static void inputfifo_send_data_a(
+/* STORAGE_CLASS_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(
+/* STORAGE_CLASS_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(
+/* STORAGE_CLASS_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)
+/* STORAGE_CLASS_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)
+/* STORAGE_CLASS_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)
+/* STORAGE_CLASS_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)
+/* STORAGE_CLASS_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;
+}
+
+
+
+#ifdef __ON__
+static void inputfifo_send_ch_id(
+/* STORAGE_CLASS_INLINE void inputfifo_send_ch_id( */
+	unsigned int ch_id)
+{
+	hrt_data	token;
+	inputfifo_curr_ch_id = ch_id & _HIVE_ISP_CH_ID_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_fmt_type(
+/* STORAGE_CLASS_INLINE void inputfifo_send_fmt_type( */
+	unsigned int fmt_type)
+{
+	hrt_data	token;
+	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;
+}
+#endif /*  __ON__ */
+
+
+
+static void inputfifo_send_ch_id_and_fmt_type(
+/* STORAGE_CLASS_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)
+/* STORAGE_CLASS_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(
+/* STORAGE_CLASS_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 != NULL);
+	assert((data2 != NULL) || (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 != NULL);
+	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 != NULL);
+	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 ia_css_stream_format input_format)
+{
+	enum inputfifo_mipi_data_type type;
+
+	type = inputfifo_mipi_data_type_regular;
+	if (input_format == IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY) {
+		type =
+			inputfifo_mipi_data_type_yuv420_legacy;
+	} else if (input_format == IA_CSS_STREAM_FORMAT_YUV420_8  ||
+		   input_format == IA_CSS_STREAM_FORMAT_YUV420_10 ||
+		   input_format == IA_CSS_STREAM_FORMAT_YUV420_16) {
+		type =
+			inputfifo_mipi_data_type_yuv420;
+	} else if (input_format >= IA_CSS_STREAM_FORMAT_RGB_444 &&
+		   input_format <= IA_CSS_STREAM_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 ia_css_stream_format input_format,
+	bool two_ppc)
+{
+	unsigned int fmt_type, hblank_cycles, marker_cycles;
+	enum inputfifo_mipi_data_type type;
+
+	assert(data != NULL);
+	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 ia_css_stream_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 != NULL);
+	assert((data2 != NULL) || (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 ia_css_stream_format	data_type,
+	const unsigned short	*data,
+	unsigned int	width)
+{
+	struct inputfifo_instance *s2mi;
+	unsigned int fmt_type;
+
+	assert(data != NULL);
+	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;
+}
+#endif /* #if !defined(HAS_NO_INPUT_SYSTEM) */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param.h
new file mode 100644
index 000000000000..285749885105
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param.h
@@ -0,0 +1,118 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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,
+	hrt_vaddress 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,
+	uint32_t 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 */
+enum ia_css_err
+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 */
+enum ia_css_err
+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/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param_types.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param_types.h
new file mode 100644
index 000000000000..8e651b80345a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/interface/ia_css_isp_param_types.h
@@ -0,0 +1,107 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t offset; /* Offset in isp_<mem>)parameters, etc. */
+	uint32_t 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 {
+	uint32_t 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];
+};
+
+#define IA_CSS_DEFAULT_ISP_MEM_PARAMS \
+		{ { { { 0, 0 } } } }
+
+#define IA_CSS_DEFAULT_ISP_CSS_PARAMS \
+		{ { { { 0, 0 } } } }
+
+#define IA_CSS_DEFAULT_ISP_ISP_PARAMS \
+		{ { { { 0, 0 } } } }
+
+#endif /* _IA_CSS_ISP_PARAM_TYPES_H_ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/src/isp_param.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/src/isp_param.c
new file mode 100644
index 000000000000..832d9e16edeb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isp_param/src/isp_param.c
@@ -0,0 +1,227 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "memory_access.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,
+	hrt_vaddress 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,
+	uint32_t 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 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;
+		}
+	}
+}
+
+enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned 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++) {
+			uint32_t 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 = sh_css_calloc(1, size);
+				if (!mem_params->params[pclass][mem].address) {
+					err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+					goto cleanup;
+				}
+				if (pclass != IA_CSS_PARAM_CLASS_PARAM) {
+					css_params->params[pclass][mem].address = mmgr_malloc(size);
+					if (!css_params->params[pclass][mem].address) {
+						err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+						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 mem, pclass;
+
+	for (mem = 0; mem < IA_CSS_NUM_MEMORIES; mem++) {
+		for (pclass = 0; pclass < IA_CSS_NUM_PARAM_CLASSES; pclass++) {
+			if (mem_params->params[pclass][mem].address)
+				sh_css_free(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 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]);
+	}
+}
+
+enum ia_css_err
+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 mem;
+
+	for (mem = 0; mem < N_IA_CSS_ISP_MEMORIES; mem++) {
+		size_t       size	  = host->params[pclass][mem].size;
+		hrt_vaddress 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 IA_CSS_ERR_INTERNAL_ERROR;
+		if (!size)
+			continue;
+		mmgr_store(ddr_mem_ptr, host_mem_ptr, size);
+	}
+	return IA_CSS_SUCCESS;
+}
+
+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/atomisp2/css2400/runtime/isys/interface/ia_css_isys.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/interface/ia_css_isys.h
new file mode 100644
index 000000000000..02bf908d94e6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/interface/ia_css_isys.h
@@ -0,0 +1,201 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 USE_INPUT_SYSTEM_VERSION_2401
+/**
+ * Virtual Input System. (Input System 2401)
+ */
+typedef input_system_cfg_t	ia_css_isys_descr_t;
+/** end of Virtual Input System */
+#endif
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+input_system_error_t ia_css_isys_init(void);
+void ia_css_isys_uninit(void);
+mipi_port_ID_t ia_css_isys_port_to_mipi_port(
+	enum ia_css_csi2_port api_port);
+#endif
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+
+/**
+ * @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			IA_CSS_SUCCESS if successful, IA_CSS_ERR_INTERNAL_ERROR if
+ *				there is already a stream registered with the same handle
+ */
+enum ia_css_err ia_css_isys_csi_rx_register_stream(
+	enum ia_css_csi2_port 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			IA_CSS_SUCCESS if successful, IA_CSS_ERR_INTERNAL_ERROR if
+ *				there is no stream registered with that handle
+ */
+enum ia_css_err ia_css_isys_csi_rx_unregister_stream(
+	enum ia_css_csi2_port port,
+	uint32_t isys_stream_id);
+
+enum ia_css_err ia_css_isys_convert_compressed_format(
+		struct ia_css_csi2_compression *comp,
+		struct input_system_cfg_s *cfg);
+unsigned int ia_css_csi2_calculate_input_system_alignment(
+	enum ia_css_stream_format fmt_type);
+#endif
+
+#if !defined(USE_INPUT_SYSTEM_VERSION_2401)
+/* 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(mipi_port_ID_t port);
+
+unsigned int ia_css_isys_rx_get_interrupt_reg(mipi_port_ID_t port);
+void ia_css_isys_rx_get_irq_info(mipi_port_ID_t port,
+				 unsigned int *irq_infos);
+void ia_css_isys_rx_clear_irq_info(mipi_port_ID_t port,
+				   unsigned int irq_infos);
+unsigned int ia_css_isys_rx_translate_irq_infos(unsigned int bits);
+
+#endif /* #if !defined(USE_INPUT_SYSTEM_VERSION_2401) */
+
+/** @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.
+ */
+enum ia_css_err ia_css_isys_convert_stream_format_to_mipi_format(
+		enum ia_css_stream_format input_format,
+		mipi_predictor_t compression,
+		unsigned int *fmt_type);
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+/**
+ * Virtual Input System. (Input System 2401)
+ */
+extern 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);
+
+extern void ia_css_isys_stream_destroy(
+		ia_css_isys_stream_h	isys_stream);
+
+extern 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);
+
+extern void ia_css_isys_csi_rx_lut_rmgr_init(void);
+
+extern void ia_css_isys_csi_rx_lut_rmgr_uninit(void);
+
+extern 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);
+
+extern 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);
+
+
+extern void ia_css_isys_ibuf_rmgr_init(void);
+
+extern void ia_css_isys_ibuf_rmgr_uninit(void);
+
+extern bool ia_css_isys_ibuf_rmgr_acquire(
+	uint32_t	size,
+	uint32_t	*start_addr);
+
+extern void ia_css_isys_ibuf_rmgr_release(
+	uint32_t	*start_addr);
+
+extern void ia_css_isys_dma_channel_rmgr_init(void);
+
+extern void ia_css_isys_dma_channel_rmgr_uninit(void);
+
+extern bool ia_css_isys_dma_channel_rmgr_acquire(
+	isys2401_dma_ID_t	dma_id,
+	isys2401_dma_channel	*channel);
+
+extern void ia_css_isys_dma_channel_rmgr_release(
+	isys2401_dma_ID_t	dma_id,
+	isys2401_dma_channel	*channel);
+
+extern void ia_css_isys_stream2mmio_sid_rmgr_init(void);
+
+extern void ia_css_isys_stream2mmio_sid_rmgr_uninit(void);
+
+extern bool ia_css_isys_stream2mmio_sid_rmgr_acquire(
+	stream2mmio_ID_t	stream2mmio,
+	stream2mmio_sid_ID_t	*sid);
+
+extern 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/atomisp2/css2400/runtime/isys/interface/ia_css_isys_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/interface/ia_css_isys_comm.h
new file mode 100644
index 000000000000..0c3434ad0613
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/interface/ia_css_isys_comm.h
@@ -0,0 +1,69 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef __IA_CSS_ISYS_COMM_H
+#define __IA_CSS_ISYS_COMM_H
+
+#include <type_support.h>
+#include <input_system.h>
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+#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(
+	uint32_t	sp_thread_id,
+	uint32_t	stream_id)
+{
+	return sp_thread_id * IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH + stream_id;
+}
+
+#endif  /* USE_INPUT_SYSTEM_VERSION_2401*/
+#endif  /*_IA_CSS_ISYS_COMM_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.c
new file mode 100644
index 000000000000..d1d4f79c00f1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.c
@@ -0,0 +1,179 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+
+#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;
+	uint32_t max_num_packets_of_type;
+	uint32_t num_active_of_type;
+	isys_csi_rx_rsrc_t *cur_rsrc = NULL;
+	uint16_t 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 != NULL);
+
+	if ((backend < N_CSI_RX_BACKEND_ID) && (entry != NULL)) {
+		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)
+{
+	uint32_t max_num_packets;
+	isys_csi_rx_rsrc_t *cur_rsrc = NULL;
+	uint32_t packet_entry = 0;
+
+	assert(backend < N_CSI_RX_BACKEND_ID);
+	assert(entry != NULL);
+	assert((packet_type >= CSI_MIPI_PACKET_TYPE_LONG) || (packet_type <= CSI_MIPI_PACKET_TYPE_SHORT));
+
+	if ((backend < N_CSI_RX_BACKEND_ID) && (entry != NULL)) {
+		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--;
+			}
+		}
+	}
+}
+
+enum ia_css_err ia_css_isys_csi_rx_register_stream(
+	enum ia_css_csi2_port port,
+	uint32_t isys_stream_id)
+{
+	enum ia_css_err retval = IA_CSS_ERR_INTERNAL_ERROR;
+
+	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 = IA_CSS_SUCCESS;
+		}
+	}
+	return retval;
+}
+
+enum ia_css_err ia_css_isys_csi_rx_unregister_stream(
+	enum ia_css_csi2_port port,
+	uint32_t isys_stream_id)
+{
+	enum ia_css_err retval = IA_CSS_ERR_INTERNAL_ERROR;
+
+	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 = IA_CSS_SUCCESS;
+		}
+	}
+	return retval;
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.h
new file mode 100644
index 000000000000..c27b0ab83c93
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/csi_rx_rmgr.h
@@ -0,0 +1,43 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t	active_table;
+	uint32_t        num_active;
+	uint16_t	num_long_packets;
+	uint16_t	num_short_packets;
+};
+
+#endif /* __CSI_RX_RMGR_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.c
new file mode 100644
index 000000000000..76d9142fd37e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.c
@@ -0,0 +1,141 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+
+#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(
+	uint32_t	size,
+	uint32_t	*start_addr)
+{
+	bool retval = false;
+	bool input_buffer_found = false;
+	uint32_t aligned_size;
+	ibuf_handle_t *handle = NULL;
+	uint16_t i;
+
+	assert(start_addr != NULL);
+	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)
+{
+	uint16_t i;
+	ibuf_handle_t *handle = NULL;
+
+	assert(start_addr != NULL);
+
+	for (i = 0; i < ibuf_rsrc.num_allocated; i++) {
+		handle = getHandle(i);
+		if ((handle->start_addr == *start_addr)
+		    && ( true == handle->active)) {
+			handle->active = false;
+			ibuf_rsrc.num_active--;
+			break;
+		}
+	}
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.h
new file mode 100644
index 000000000000..424cfe9f3b2a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/ibuf_ctrl_rmgr.h
@@ -0,0 +1,55 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t	start_addr;
+	uint32_t	size;
+	bool		active;
+};
+
+typedef struct ibuf_rsrc_s ibuf_rsrc_t;
+struct ibuf_rsrc_s {
+	uint32_t	free_start_addr;
+	uint32_t	free_size;
+	uint16_t	num_active;
+	uint16_t	num_allocated;
+	ibuf_handle_t	handles[MAX_IBUF_HANDLES];
+};
+
+#endif /* __IBUF_CTRL_RMGR_H_INCLUDED */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.c
new file mode 100644
index 000000000000..5032627342d9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.c
@@ -0,0 +1,103 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+
+#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 != NULL);
+
+	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 != NULL);
+
+	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/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.h
new file mode 100644
index 000000000000..b2c286537774
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_dma_rmgr.h
@@ -0,0 +1,41 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t active_table;
+	uint16_t num_active;
+};
+
+#endif /* __ISYS_DMA_RMGR_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_init.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_init.c
new file mode 100644
index 000000000000..239ef310bdeb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_init.c
@@ -0,0 +1,141 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "input_system.h"
+
+#ifdef HAS_INPUT_SYSTEM_VERSION_2
+#include "ia_css_isys.h"
+#include "platform_support.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+#include "isys_dma.h"		/* isys2401_dma_set_max_burst_size() */
+#include "isys_irq.h"
+#endif
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+input_system_error_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;
+	uint32_t acq_mem_region_size = 24;
+	uint32_t acq_nof_mem_regions = 2;
+	input_system_error_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(USE_INPUT_SYSTEM_VERSION_2401)
+input_system_error_t ia_css_isys_init(void)
+{
+	input_system_error_t error = INPUT_SYSTEM_ERR_NO_ERROR;
+
+	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 error;
+}
+#endif
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+void ia_css_isys_uninit(void)
+{
+}
+#elif defined(USE_INPUT_SYSTEM_VERSION_2401)
+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
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.c
new file mode 100644
index 000000000000..a93c7f44ff12
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.c
@@ -0,0 +1,105 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+
+#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 != NULL);
+
+	if ((stream2mmio < N_STREAM2MMIO_ID) && (sid != NULL)) {
+		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 != NULL);
+
+	if ((stream2mmio < N_STREAM2MMIO_ID) && (sid != NULL)) {
+		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/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.h
new file mode 100644
index 000000000000..4f63005b1071
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/isys_stream2mmio_rmgr.h
@@ -0,0 +1,41 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t	active_table;
+	uint16_t	num_active;
+};
+
+#endif /* __ISYS_STREAM2MMIO_RMGR_H_INCLUDED__ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/rx.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/rx.c
new file mode 100644
index 000000000000..46a157f64343
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/rx.c
@@ -0,0 +1,607 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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(USE_INPUT_SYSTEM_VERSION_2401)
+void ia_css_isys_rx_enable_all_interrupts(mipi_port_ID_t 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) |
+#if defined(HAS_RX_VERSION_2)
+	    (1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT) |
+#endif
+	    (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.
+ * */
+mipi_port_ID_t ia_css_isys_port_to_mipi_port(enum ia_css_csi2_port api_port)
+{
+	/* In this module the validity of the inptu variable should
+	 * have been checked already, so we do not check for erroneous
+	 * values. */
+	mipi_port_ID_t port = MIPI_PORT0_ID;
+
+	if (api_port == IA_CSS_CSI2_PORT1)
+		port = MIPI_PORT1_ID;
+	else if (api_port == IA_CSS_CSI2_PORT2)
+		port = MIPI_PORT2_ID;
+
+	return port;
+}
+
+unsigned int ia_css_isys_rx_get_interrupt_reg(mipi_port_ID_t 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(IA_CSS_CSI2_PORT1, irq_infos);
+}
+
+void ia_css_rx_port_get_irq_info(enum ia_css_csi2_port api_port,
+				 unsigned int *irq_infos)
+{
+	mipi_port_ID_t 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(mipi_port_ID_t port,
+				 unsigned int *irq_infos)
+{
+	unsigned int bits;
+
+	assert(irq_infos != NULL);
+	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 defined(HAS_RX_VERSION_2)
+	if (bits & (1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT))
+		infos |= IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT;
+#endif
+	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(IA_CSS_CSI2_PORT1, irq_infos);
+}
+
+void ia_css_rx_port_clear_irq_info(enum ia_css_csi2_port api_port, unsigned int irq_infos)
+{
+	mipi_port_ID_t 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(mipi_port_ID_t 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 defined(HAS_RX_VERSION_2)
+	if (irq_infos & IA_CSS_RX_IRQ_INFO_INIT_TIMEOUT)
+		bits |= 1U << _HRT_CSS_RECEIVER_IRQ_INIT_TIMEOUT_BIT;
+#endif
+	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(USE_INPUT_SYSTEM_VERSION_2401) */
+
+enum ia_css_err ia_css_isys_convert_stream_format_to_mipi_format(
+		enum ia_css_stream_format input_format,
+		mipi_predictor_t compression,
+		unsigned int *fmt_type)
+{
+	assert(fmt_type != NULL);
+	/*
+	 * 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 IA_CSS_STREAM_FORMAT_RAW_6:
+			*fmt_type = 6;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_7:
+			*fmt_type = 7;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_8:
+			*fmt_type = 8;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_10:
+			*fmt_type = 10;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_12:
+			*fmt_type = 12;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_14:
+			*fmt_type = 14;
+			break;
+		case IA_CSS_STREAM_FORMAT_RAW_16:
+			*fmt_type = 16;
+			break;
+		default:
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+		return IA_CSS_SUCCESS;
+	}
+	/*
+	 * 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 IA_CSS_STREAM_FORMAT_RGB_888:
+		*fmt_type = MIPI_FORMAT_RGB888;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+		*fmt_type = MIPI_FORMAT_RGB555;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+		*fmt_type = MIPI_FORMAT_RGB444;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+		*fmt_type = MIPI_FORMAT_RGB565;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+		*fmt_type = MIPI_FORMAT_RGB666;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+		*fmt_type = MIPI_FORMAT_RAW8;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+		*fmt_type = MIPI_FORMAT_RAW10;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+		*fmt_type = MIPI_FORMAT_RAW6;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+		*fmt_type = MIPI_FORMAT_RAW7;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+		*fmt_type = MIPI_FORMAT_RAW12;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+		*fmt_type = MIPI_FORMAT_RAW14;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+		*fmt_type = MIPI_FORMAT_YUV420_8;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+		*fmt_type = MIPI_FORMAT_YUV420_10;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+		*fmt_type = MIPI_FORMAT_YUV422_8;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+		*fmt_type = MIPI_FORMAT_YUV422_10;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+		*fmt_type = MIPI_FORMAT_YUV420_8_LEGACY;
+		break;
+	case IA_CSS_STREAM_FORMAT_EMBEDDED:
+		*fmt_type = MIPI_FORMAT_EMBEDDED;
+		break;
+#ifndef USE_INPUT_SYSTEM_VERSION_2401
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+		/* This is not specified by Arasan, so we use
+		 * 17 for now.
+		 */
+		*fmt_type = MIPI_FORMAT_RAW16;
+		break;
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+		*fmt_type = MIPI_FORMAT_BINARY_8;
+		break;
+#else
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+		*fmt_type = MIPI_FORMAT_CUSTOM0;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+		*fmt_type = MIPI_FORMAT_CUSTOM1;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+		*fmt_type = MIPI_FORMAT_CUSTOM2;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+		*fmt_type = MIPI_FORMAT_CUSTOM3;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+		*fmt_type = MIPI_FORMAT_CUSTOM4;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+		*fmt_type = MIPI_FORMAT_CUSTOM5;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+		*fmt_type = MIPI_FORMAT_CUSTOM6;
+		break;
+	case IA_CSS_STREAM_FORMAT_USER_DEF8:
+		*fmt_type = MIPI_FORMAT_CUSTOM7;
+		break;
+#endif
+
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+	default:
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+	return IA_CSS_SUCCESS;
+}
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+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;
+	default:
+		break;
+	}
+	return predictor;
+}
+enum ia_css_err ia_css_isys_convert_compressed_format(
+		struct ia_css_csi2_compression *comp,
+		struct input_system_cfg_s *cfg)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	assert(comp != NULL);
+	assert(cfg != NULL);
+
+	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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+			}
+		} 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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+			}
+		} else
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		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 ia_css_stream_format fmt_type)
+{
+	unsigned int memory_alignment_in_bytes = HIVE_ISP_DDR_WORD_BYTES;
+
+	switch (fmt_type) {
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+		memory_alignment_in_bytes = 2 * ISP_VEC_NELEMS;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_EMBEDDED:
+	default:
+		memory_alignment_in_bytes = HIVE_ISP_DDR_WORD_BYTES;
+		break;
+	}
+	return memory_alignment_in_bytes;
+}
+
+#endif
+
+#if !defined(USE_INPUT_SYSTEM_VERSION_2401)
+void ia_css_isys_rx_configure(const rx_cfg_t *config,
+			      const enum ia_css_input_mode input_mode)
+{
+#if defined(HAS_RX_VERSION_2)
+	bool port_enabled[N_MIPI_PORT_ID];
+	bool any_port_enabled = false;
+	mipi_port_ID_t port;
+
+	if ((config == NULL)
+		|| (config->mode >= N_RX_MODE)
+		|| (config->port >= N_MIPI_PORT_ID)) {
+		assert(0);
+		return;
+	}
+	for (port = (mipi_port_ID_t) 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);
+
+		port_enabled[port] = true;
+
+		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);
+#else
+#error "rx.c: RX version must be one of {RX_VERSION_2}"
+#endif
+
+	return;
+}
+
+void ia_css_isys_rx_disable(void)
+{
+	mipi_port_ID_t port;
+	for (port = (mipi_port_ID_t) 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(USE_INPUT_SYSTEM_VERSION_2401) */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/virtual_isys.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/virtual_isys.c
new file mode 100644
index 000000000000..0f1e8a2f6b10
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/virtual_isys.c
@@ -0,0 +1,898 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "system_global.h"
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+
+#include "ia_css_isys.h"
+#include "ia_css_debug.h"
+#include "math_support.h"
+#include "string_support.h"
+#include "virtual_isys.h"
+#include "isp.h"
+#include "sh_css_defs.h"
+
+/*************************************************
+ *
+ * Forwarded Declaration
+ *
+ *************************************************/
+#ifndef ISP2401
+
+#endif
+static bool create_input_system_channel(
+	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(
+	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,
+	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,
+	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(
+	int32_t bits_per_pixel,
+	int32_t pixels_per_line,
+	int32_t lines_per_frame,
+	int32_t align_in_bytes,
+	bool online,
+	ib_buffer_t *buf);
+
+static void release_ib_buffer(
+	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,
+	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,
+	input_system_cfg_t		*isys_cfg,
+	pixelgen_prbs_cfg_t		*cfg);
+
+static bool calculate_fe_cfg(
+	const 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 input_system_cfg_t	*isys_cfg,
+	bool				metadata,
+	csi_rx_backend_cfg_t		*cfg);
+
+static bool calculate_stream2mmio_cfg(
+	const 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 input_system_cfg_t	*isys_cfg,
+	ibuf_ctrl_cfg_t			*cfg);
+
+static bool calculate_isys2401_dma_cfg(
+	const input_system_channel_t	*channel,
+	const input_system_cfg_t	*isys_cfg,
+	isys2401_dma_cfg_t		*cfg);
+
+static bool calculate_isys2401_dma_port_cfg(
+	const 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(
+	int32_t bits_per_pixel,
+	int32_t 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 == NULL || isys_stream == NULL ||
+		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 == false)
+		return false;
+
+	rc = create_input_system_channel(isys_stream_descr, false, &(isys_stream->channel));
+	if (rc == false) {
+		destroy_input_system_input_port(&isys_stream->input_port);
+		return false;
+	}
+
+#ifdef ISP2401
+	/*
+	 * Early polling is required for timestamp accuracy in certain cause.
+	 * The ISYS HW polling is started on
+	 * ia_css_isys_stream_capture_indication() instead of
+	 * ia_css_pipeline_sp_wait_for_isys_stream_N() as isp processing of
+	 * capture takes longer than getting an ISYS frame
+	 */
+	isys_stream->polling_mode = isys_stream_descr->polling_mode;
+
+#endif
+	/* create metadata channel */
+	if (isys_stream_descr->metadata.enable) {
+		rc = create_input_system_channel(isys_stream_descr, true, &isys_stream->md_channel);
+		if (rc == false) {
+			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 == NULL		||
+		isys_stream_descr == NULL	||
+		isys_stream == NULL)
+		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 == false)
+		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 == false)
+			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 == false)
+		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(
+	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 == false)
+		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(
+	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,
+	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 == false)
+		return false;
+
+	rc = calculate_ibuf_ctrl_cfg(
+			channel,
+			input_port,
+			isys_cfg,
+			&(channel_cfg->ibuf_ctrl_cfg));
+	if (rc == false)
+		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 == false)
+		return false;
+
+	rc = calculate_isys2401_dma_port_cfg(
+			isys_cfg,
+			false,
+			metadata,
+			&(channel_cfg->dma_src_port_cfg));
+	if (rc == false)
+		return false;
+
+	rc = calculate_isys2401_dma_port_cfg(
+			isys_cfg,
+			isys_cfg->raw_packed,
+			metadata,
+			&(channel_cfg->dma_dest_port_cfg));
+	if (rc == false)
+		return false;
+
+	return true;
+}
+
+static bool calculate_input_system_input_port_cfg(
+	input_system_channel_t		*channel,
+	input_system_input_port_t	*input_port,
+	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(
+	int32_t bits_per_pixel,
+	int32_t pixels_per_line,
+	bool	raw_packed,
+	int32_t align_in_bytes)
+{
+	int32_t bytes_per_line;
+	int32_t pixels_per_word;
+	int32_t words_per_line;
+	int32_t 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(
+	int32_t bits_per_pixel,
+	int32_t pixels_per_line,
+	int32_t lines_per_frame,
+	int32_t align_in_bytes,
+	bool online,
+	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(
+	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,
+	input_system_cfg_t		*isys_cfg,
+	pixelgen_tpg_cfg_t		*cfg)
+{
+	(void)channel;
+	(void)input_port;
+
+	memcpy_s(
+		(void *)cfg,
+		sizeof(pixelgen_tpg_cfg_t),
+		(void *)(&(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,
+	input_system_cfg_t		*isys_cfg,
+	pixelgen_prbs_cfg_t		*cfg)
+{
+	(void)channel;
+	(void)input_port;
+
+	memcpy_s(
+		(void *)cfg,
+		sizeof(pixelgen_prbs_cfg_t),
+		(void *)(&(isys_cfg->prbs_port_attr)),
+		sizeof(pixelgen_prbs_cfg_t));
+	return true;
+}
+
+static bool calculate_fe_cfg(
+	const 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 input_system_cfg_t	*isys_cfg,
+	bool				metadata,
+	csi_rx_backend_cfg_t		*cfg)
+{
+
+	memcpy_s(
+		(void *)(&cfg->lut_entry),
+		sizeof(csi_rx_backend_lut_entry_t),
+		metadata ? (void *)(&input_port->metadata.backend_lut_entry) :
+			(void *)(&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 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 input_system_cfg_t	*isys_cfg,
+	ibuf_ctrl_cfg_t			*cfg)
+{
+	const int32_t bits_per_byte = 8;
+	int32_t bits_per_pixel;
+	int32_t bytes_per_pixel;
+	int32_t 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 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 input_system_cfg_t	*isys_cfg,
+	bool				raw_packed,
+	bool				metadata,
+	isys2401_dma_port_cfg_t		*cfg)
+{
+	int32_t bits_per_pixel;
+	int32_t pixels_per_line;
+	int32_t 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/atomisp2/css2400/runtime/isys/src/virtual_isys.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/virtual_isys.h
new file mode 100644
index 000000000000..66c7293c0a93
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/isys/src/virtual_isys.h
@@ -0,0 +1,41 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline.h
new file mode 100644
index 000000000000..90646f5f8885
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline.h
@@ -0,0 +1,308 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 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;
+	uint8_t pipe_num;
+	bool stop_requested;
+	struct ia_css_pipeline_stage *stages;
+	struct ia_css_pipeline_stage *current_stage;
+	unsigned 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 inout_port_config;
+	int num_execs;
+	bool acquire_isp_each_stage;
+	uint32_t pipe_qos_config;
+};
+
+#define DEFAULT_PIPELINE \
+{ \
+	IA_CSS_PIPE_ID_PREVIEW, /* pipe_id */ \
+	0,			/* pipe_num */ \
+	false,			/* stop_requested */ \
+	NULL,                   /* stages */ \
+	NULL,                   /* current_stage */ \
+	0,                      /* num_stages */ \
+	DEFAULT_FRAME,          /* in_frame */ \
+	{DEFAULT_FRAME},          /* out_frame */ \
+	{DEFAULT_FRAME},          /* vf_frame */ \
+	IA_CSS_FRAME_DELAY_1,   /* frame_delay */ \
+	0,                      /* inout_port_config */ \
+	-1,                     /* num_execs */ \
+	true,					/* acquire_isp_each_stage */\
+	QOS_INVALID             /* pipe_qos_config */\
+}
+
+/* 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 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                     IA_CSS_SUCCESS 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
+ */
+enum ia_css_err 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                     IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err 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                     IA_CSS_SUCCESS 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.
+*/
+enum ia_css_err 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                     IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err 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   IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err ia_css_pipeline_get_stage_from_fw(struct ia_css_pipeline *pipeline,
+			  uint32_t fw_handle,
+			  struct ia_css_pipeline_stage **stage);
+
+/** @brief Gets the Firmware handle correponding the stage num from the pipeline
+ *
+ * @param[in] pipeline
+ * @param[in] stage_num
+ * @param[out] fw_handle
+ *
+ * @return   IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err ia_css_pipeline_get_fw_from_stage(struct ia_css_pipeline *pipeline,
+			  uint32_t stage_num,
+			  uint32_t *fw_handle);
+
+/** @brief gets the output stage from the pipeline
+ *
+ * @param[in] pipeline
+ * @return                     IA_CSS_SUCCESS or error code upon error.
+ *
+ */
+enum ia_css_err 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(USE_INPUT_SYSTEM_VERSION_2401)
+/**
+ * @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/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline_common.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline_common.h
new file mode 100644
index 000000000000..a7e6edf41cdb
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/interface/ia_css_pipeline_common.h
@@ -0,0 +1,42 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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/atomisp2/css2400/runtime/pipeline/src/pipeline.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/src/pipeline.c
new file mode 100644
index 000000000000..95542fc82217
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/pipeline/src/pipeline.c
@@ -0,0 +1,806 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 "memory_access.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 enum ia_css_err 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();
+}
+
+enum ia_css_err 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 != NULL);
+	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 == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	pipeline_init_defaults(pipeline, pipe_id, pipe_num, dvs_frame_delay);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+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 != NULL);
+	IA_CSS_ENTER_PRIVATE("pipeline = %p", pipeline);
+
+	if (pipeline == NULL) {
+		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)
+{
+	uint8_t pipe_num = 0;
+	unsigned int thread_id;
+
+	assert(pipeline != NULL);
+	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,
+#ifndef ISP2401
+				IA_CSS_INPUT_MODE_MEMORY, NULL, NULL
+#else
+				IA_CSS_INPUT_MODE_MEMORY, NULL, NULL,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+				, (mipi_port_ID_t) 0
+#else
+				(mipi_port_ID_t) 0,
+#endif
+#endif
+#ifndef ISP2401
+				);
+#else
+				NULL, NULL);
+#endif
+	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 == NULL) || (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)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]);
+	}
+}
+
+enum ia_css_err ia_css_pipeline_request_stop(struct ia_css_pipeline *pipeline)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int thread_id;
+
+	assert(pipeline != NULL);
+
+	if (pipeline == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+	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 != NULL);
+	IA_CSS_ENTER_PRIVATE("pipeline = %p", pipeline);
+
+	if (pipeline == NULL) {
+		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      IA_CSS_SUCCESS 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.
+*/
+enum ia_css_err 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;
+	enum ia_css_err err;
+
+	/* other arguments can be NULL */
+	assert(pipeline != NULL);
+	assert(stage_desc != NULL);
+	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 IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	/* 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 IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	/* Create the new stage */
+	err = pipeline_stage_create(stage_desc, &new_stage);
+	if (err != IA_CSS_SUCCESS) {
+		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 IA_CSS_SUCCESS;
+}
+
+void ia_css_pipeline_finalize_stages(struct ia_css_pipeline *pipeline,
+			bool continuous)
+{
+	unsigned i = 0;
+	struct ia_css_pipeline_stage *stage;
+
+	assert(pipeline != NULL);
+	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);
+}
+
+enum ia_css_err 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 != NULL);
+	assert(stage != NULL);
+	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 IA_CSS_SUCCESS;
+		}
+	}
+	return IA_CSS_ERR_INTERNAL_ERROR;
+}
+
+enum ia_css_err ia_css_pipeline_get_stage_from_fw(struct ia_css_pipeline *pipeline,
+			  uint32_t fw_handle,
+			  struct ia_css_pipeline_stage **stage)
+{
+	struct ia_css_pipeline_stage *s;
+	assert(pipeline != NULL);
+	assert(stage != NULL);
+	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 IA_CSS_SUCCESS;
+		}
+	}
+	return IA_CSS_ERR_INTERNAL_ERROR;
+}
+
+enum ia_css_err ia_css_pipeline_get_fw_from_stage(struct ia_css_pipeline *pipeline,
+			  uint32_t 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 == NULL) || (fw_handle == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	for (s = pipeline->stages; s; s = s->next) {
+		if((s->stage_num == stage_num) && (s->firmware)) {
+			*fw_handle = s->firmware->handle;
+			return IA_CSS_SUCCESS;
+		}
+	}
+	return IA_CSS_ERR_INTERNAL_ERROR;
+}
+
+enum ia_css_err 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 != NULL);
+	assert(stage != NULL);
+	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 IA_CSS_SUCCESS;
+	/* 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(USE_INPUT_SYSTEM_VERSION_2401)
+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)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;
+	}
+	sh_css_free(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)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 != false);
+}
+
+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)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 enum ia_css_err pipeline_stage_create(
+	struct ia_css_pipeline_stage_desc *stage_desc,
+	struct ia_css_pipeline_stage **new_stage)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 = IA_CSS_ERR_INTERNAL_ERROR;
+		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 = sh_css_malloc(sizeof(*stage));
+	if (stage == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	memset(stage, 0, sizeof(*stage));
+
+	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 != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+				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 != NULL)
+		pipeline_stage_destroy(stage);
+	return err;
+}
+
+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)
+{
+	struct ia_css_frame init_frame = DEFAULT_FRAME;
+	unsigned int i;
+
+	pipeline->pipe_id = pipe_id;
+	pipeline->stages = NULL;
+	pipeline->stop_requested = false;
+	pipeline->current_stage = NULL;
+	pipeline->in_frame = init_frame;
+	for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+		pipeline->out_frame[i] = init_frame;
+		pipeline->vf_frame[i] = init_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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(pipeline != NULL);
+	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 == IA_CSS_SUCCESS)
+			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 == IA_CSS_SUCCESS)
+			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 == IA_CSS_SUCCESS)
+			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 == IA_CSS_SUCCESS)
+			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/atomisp2/css2400/runtime/queue/interface/ia_css_queue.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/interface/ia_css_queue.h
new file mode 100644
index 000000000000..e50a0f813753
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/interface/ia_css_queue.h
@@ -0,0 +1,192 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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.
+ *
+ */
+extern 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.
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern 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.
+ *
+ */
+extern int ia_css_queue_peek(
+		ia_css_queue_t *qhandle,
+		uint32_t 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.
+ *
+ */
+extern 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/atomisp2/css2400/runtime/queue/interface/ia_css_queue_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/interface/ia_css_queue_comm.h
new file mode 100644
index 000000000000..4ebaeb0c1847
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/interface/ia_css_queue_comm.h
@@ -0,0 +1,69 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t cb_desc_addr; /*Circbuf desc address for remote queues*/
+	uint32_t cb_elems_addr; /*Circbuf elements addr for remote queue*/
+	uint8_t location;    /* Cell location for queue */
+	uint8_t 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/atomisp2/css2400/runtime/queue/src/queue.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue.c
new file mode 100644
index 000000000000..606376fdf0ba
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue.c
@@ -0,0 +1,412 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 (NULL == 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;
+		uint32_t 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 || 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;
+		uint32_t 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 == NULL) || (is_full == NULL))
+		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;
+		uint32_t 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 == NULL) || (size == NULL))
+		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;
+		uint32_t 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 == NULL) || (size == NULL))
+		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;
+		uint32_t 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,
+		uint32_t offset,
+		uint32_t *element)
+{
+	uint32_t num_elems = 0;
+	int error = 0;
+
+	if ((qhandle == NULL) || (element == NULL))
+		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;
+		uint32_t 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 == NULL) || (is_empty == NULL))
+		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;
+		uint32_t 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 == NULL) || (size == NULL))
+		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;
+		uint32_t 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/atomisp2/css2400/runtime/queue/src/queue_access.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue_access.c
new file mode 100644
index 000000000000..946d4f2d2108
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue_access.c
@@ -0,0 +1,192 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "type_support.h"
+#include "queue_access.h"
+#include "ia_css_circbuf.h"
+#include "sp.h"
+#include "memory_access.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 == NULL || cb_desc == NULL)
+		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 (0 == cb_desc->size) {
+				/* 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 */
+		mmgr_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 ENOTSUP;
+	}
+
+	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 == NULL || cb_desc == NULL)
+		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 */
+		mmgr_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 ENOTSUP;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_item_load(
+		struct ia_css_queue *rdesc,
+		uint8_t position,
+		ia_css_circbuf_elem_t *item)
+{
+	if (rdesc == NULL || item == NULL)
+		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) {
+		mmgr_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 ENOTSUP;
+	}
+
+	return 0;
+}
+
+int ia_css_queue_item_store(
+		struct ia_css_queue *rdesc,
+		uint8_t position,
+		ia_css_circbuf_elem_t *item)
+{
+	if (rdesc == NULL || item == NULL)
+		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) {
+		mmgr_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 ENOTSUP;
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue_access.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue_access.h
new file mode 100644
index 000000000000..4775513f54cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/queue/src/queue_access.h
@@ -0,0 +1,101 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef __QUEUE_ACCESS_H
+#define __QUEUE_ACCESS_H
+
+#include <type_support.h>
+#include <ia_css_queue_comm.h>
+#include <ia_css_circbuf.h>
+#include <error_support.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 {
+	uint8_t type;        /* Specify remote/local type of access */
+	uint8_t location;    /* Cell location for queue */
+	uint8_t proc_id;     /* Processor id for queue access */
+	union {
+		ia_css_circbuf_t cb_local;
+		struct {
+			uint32_t cb_desc_addr; /*Circbuf desc address for remote queues*/
+			uint32_t cb_elems_addr; /*Circbuf elements addr for remote queue*/
+		}	remote;
+	} desc;
+};
+
+extern int ia_css_queue_load(
+		struct ia_css_queue *rdesc,
+		ia_css_circbuf_desc_t *cb_desc,
+		uint32_t ignore_desc_flags);
+
+extern int ia_css_queue_store(
+		struct ia_css_queue *rdesc,
+		ia_css_circbuf_desc_t *cb_desc,
+		uint32_t ignore_desc_flags);
+
+extern int ia_css_queue_item_load(
+		struct ia_css_queue *rdesc,
+		uint8_t position,
+		ia_css_circbuf_elem_t *item);
+
+extern int ia_css_queue_item_store(
+		struct ia_css_queue *rdesc,
+		uint8_t position,
+		ia_css_circbuf_elem_t *item);
+
+#endif /* __QUEUE_ACCESS_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr.h
new file mode 100644
index 000000000000..a0bb9f663ce6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr.h
@@ -0,0 +1,89 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef _IA_CSS_RMGR_H
+#define _IA_CSS_RMGR_H
+
+#include "storage_class.h"
+#include <ia_css_err.h>
+
+#ifndef __INLINE_RMGR__
+#define STORAGE_CLASS_RMGR_H STORAGE_CLASS_EXTERN
+#define STORAGE_CLASS_RMGR_C
+#else				/* __INLINE_RMGR__ */
+#define STORAGE_CLASS_RMGR_H STORAGE_CLASS_INLINE
+#define STORAGE_CLASS_RMGR_C STORAGE_CLASS_INLINE
+#endif				/* __INLINE_RMGR__ */
+
+/**
+ * @brief Initialize resource manager (host/common)
+ */
+enum ia_css_err 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/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr_vbuf.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr_vbuf.h
new file mode 100644
index 000000000000..90ac27cf02cf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/interface/ia_css_rmgr_vbuf.h
@@ -0,0 +1,115 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#ifndef _IA_CSS_RMGR_VBUF_H
+#define _IA_CSS_RMGR_VBUF_H
+
+#include "ia_css_rmgr.h"
+#include <type_support.h>
+#include <system_types.h>
+
+/**
+ * @brief Data structure for the resource handle (host, vbuf)
+ */
+struct ia_css_rmgr_vbuf_handle {
+	hrt_vaddress vptr;
+	uint8_t count;
+	uint32_t size;
+};
+
+/**
+ * @brief Data structure for the resource pool (host, vbuf)
+ */
+struct ia_css_rmgr_vbuf_pool {
+	uint8_t copy_on_write;
+	uint8_t recycle;
+	uint32_t size;
+	uint32_t 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 enum ia_css_err 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/atomisp2/css2400/runtime/rmgr/src/rmgr.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/src/rmgr.c
new file mode 100644
index 000000000000..efa9c140484f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/src/rmgr.c
@@ -0,0 +1,55 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "ia_css_rmgr.h"
+
+enum ia_css_err ia_css_rmgr_init(void)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	err = ia_css_rmgr_init_vbuf(vbuf_ref);
+	if (err == IA_CSS_SUCCESS)
+		err = ia_css_rmgr_init_vbuf(vbuf_write);
+	if (err == IA_CSS_SUCCESS)
+		err = ia_css_rmgr_init_vbuf(hmm_buffer_pool);
+	if (err != IA_CSS_SUCCESS)
+		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/atomisp2/css2400/runtime/rmgr/src/rmgr_vbuf.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/src/rmgr_vbuf.c
new file mode 100644
index 000000000000..fa92d8da8f1c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/rmgr/src/rmgr_vbuf.c
@@ -0,0 +1,330 @@
+/*
+ * 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"
+
+#include <type_support.h>
+#include <assert_support.h>
+#include <platform_support.h> /* memset */
+#include <memory_access.h>    /* mmmgr_malloc, mhmm_free */
+#include <ia_css_debug.h>
+
+/**
+ * @brief VBUF resource handles
+ */
+#define NUM_HANDLES 1000
+struct ia_css_rmgr_vbuf_handle handle_table[NUM_HANDLES];
+
+/**
+ * @brief VBUF resource pool - refpool
+ */
+struct ia_css_rmgr_vbuf_pool refpool = {
+	false,			/* copy_on_write */
+	false,			/* recycle */
+	0,			/* size */
+	0,			/* index */
+	NULL,			/* handles */
+};
+
+/**
+ * @brief VBUF resource pool - writepool
+ */
+struct ia_css_rmgr_vbuf_pool writepool = {
+	true,			/* copy_on_write */
+	false,			/* recycle */
+	0,			/* size */
+	0,			/* index */
+	NULL,			/* handles */
+};
+
+/**
+ * @brief VBUF resource pool - hmmbufferpool
+ */
+struct ia_css_rmgr_vbuf_pool hmmbufferpool = {
+	true,			/* copy_on_write */
+	true,			/* recycle */
+	32,			/* size */
+	0,			/* index */
+	NULL,			/* handles */
+};
+
+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 == NULL) || (*handle == NULL)) {
+		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 == NULL) {
+			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 == NULL) || ((*handle) == NULL) || (((*handle)->count) == 0)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "ia_css_rmgr_refcount_release_vbuf() invalid arguments!\n");
+		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
+ */
+enum ia_css_err ia_css_rmgr_init_vbuf(struct ia_css_rmgr_vbuf_pool *pool)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	size_t bytes_needed;
+	rmgr_refcount_init_vbuf();
+	assert(pool != NULL);
+	if (pool == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	/* 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 = sh_css_malloc(bytes_needed);
+		if (pool->handles != NULL)
+			memset(pool->handles, 0, bytes_needed);
+		else
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	} 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)
+{
+	uint32_t i;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_rmgr_uninit_vbuf()\n");
+	if (pool == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR, "ia_css_rmgr_uninit_vbuf(): NULL argument\n");
+		 return;
+	}
+	if (pool->handles != NULL) {
+		/* free the hmm buffers */
+		for (i = 0; i < pool->size; i++) {
+			if (pool->handles[i] != NULL) {
+				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 */
+		sh_css_free(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)
+{
+	uint32_t i;
+	bool succes = false;
+	assert(pool != NULL);
+	assert(pool->recycle);
+	assert(pool->handles != NULL);
+	assert(handle != NULL);
+	for (i = 0; i < pool->size; i++) {
+		if (pool->handles[i] == NULL) {
+			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)
+{
+	uint32_t i;
+	bool succes = false;
+	assert(pool != NULL);
+	assert(pool->recycle);
+	assert(pool->handles != NULL);
+	assert(handle != NULL);
+	assert(*handle != NULL);
+	for (i = 0; i < pool->size; i++) {
+		if ((pool->handles[i] != NULL) &&
+		    (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); */
+			succes = true;
+			break;
+		}
+	}
+}
+
+/**
+ * @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)
+{
+	struct ia_css_rmgr_vbuf_handle h;
+
+	if ((pool == NULL) || (handle == NULL) || (*handle == NULL)) {
+		IA_CSS_LOG("Invalid inputs");
+		return;
+	}
+
+	if (pool->copy_on_write) {
+		/* 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);
+			*handle = &h;
+		}
+		/* get new buffer for needed size */
+		if ((*handle)->vptr == 0x0) {
+			if (pool->recycle) {
+				/* try and pop from pool */
+				rmgr_pop_handle(pool, handle);
+			}
+			if ((*handle)->vptr == 0x0) {
+				/* we need to allocate */
+				(*handle)->vptr = mmgr_malloc((*handle)->size);
+			} else {
+				/* we popped a buffer */
+				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 == NULL) || (handle == NULL) || (*handle == NULL)) {
+		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/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl.h
new file mode 100644
index 000000000000..27e9eb1e2102
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl.h
@@ -0,0 +1,87 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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 {
+	uint32_t        ddr_data_offset;       /**<  posistion of data in DDR */
+	uint32_t        dmem_data_addr;        /**< data segment address in dmem */
+	uint32_t        dmem_bss_addr;         /**< bss segment address in dmem  */
+	uint32_t        data_size;             /**< data segment size            */
+	uint32_t        bss_size;              /**< bss segment size             */
+	uint32_t        spctrl_config_dmem_addr; /** <location of dmem_cfg  in SP dmem */
+	uint32_t        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 */
+	uint32_t         code_size;
+	char      *program_name;    /**< not used on hardware, only for simulation */
+} ia_css_spctrl_cfg;
+
+/* Get the code addr in DDR of SP */
+hrt_vaddress get_sp_code_addr(sp_ID_t  sp_id);
+
+/* ! Load firmware on to specfied SP
+*/
+enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
+			ia_css_spctrl_cfg *spctrl_cfg);
+
+#ifdef ISP2401
+/*! Setup registers for reloading FW */
+void sh_css_spctrl_reload_fw(sp_ID_t sp_id);
+
+#endif
+/*!  Unload/release any memory allocated to hold the firmware
+*/
+enum ia_css_err ia_css_spctrl_unload_fw(sp_ID_t sp_id);
+
+
+/*! Intilaize dmem_cfg in SP dmem  and  start SP program
+*/
+enum ia_css_err ia_css_spctrl_start(sp_ID_t sp_id);
+
+/*! stop spctrl
+*/
+enum ia_css_err 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/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl_comm.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl_comm.h
new file mode 100644
index 000000000000..3af2891efca7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/interface/ia_css_spctrl_comm.h
@@ -0,0 +1,61 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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  */
+	uint32_t        dmem_data_addr; /**< data segment address in dmem */
+	uint32_t        dmem_bss_addr;  /**< bss segment address in dmem  */
+	uint32_t        data_size;      /**< data segment size            */
+	uint32_t        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/atomisp2/css2400/runtime/spctrl/src/spctrl.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c
new file mode 100644
index 000000000000..b36d7b00ebe8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/spctrl/src/spctrl.c
@@ -0,0 +1,199 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#include "ia_css_types.h"
+#define __INLINE_SP__
+#include "sp.h"
+
+#include "memory_access.h"
+#include "assert_support.h"
+#include "ia_css_spctrl.h"
+#include "ia_css_debug.h"
+
+typedef struct {
+	struct ia_css_sp_init_dmem_cfg dmem_config;
+	uint32_t        spctrl_config_dmem_addr; /** location of dmem_cfg  in SP dmem */
+	uint32_t        spctrl_state_dmem_addr;
+	unsigned int    sp_entry;           /* entry function ptr on SP */
+	hrt_vaddress    code_addr;          /* sp firmware location in host mem-DDR*/
+	uint32_t        code_size;
+	char           *program_name;       /* used in case of PLATFORM_SIM */
+} spctrl_context_info;
+
+static spctrl_context_info spctrl_cofig_info[N_SP_ID];
+static bool spctrl_loaded[N_SP_ID] = {0};
+
+/* Load firmware */
+enum ia_css_err ia_css_spctrl_load_fw(sp_ID_t sp_id,
+				ia_css_spctrl_cfg *spctrl_cfg)
+{
+	hrt_vaddress code_addr = mmgr_NULL;
+	struct ia_css_sp_init_dmem_cfg *init_dmem_cfg;
+
+	if ((sp_id >= N_SP_ID) || (spctrl_cfg == 0))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	spctrl_cofig_info[sp_id].code_addr = mmgr_NULL;
+
+#if defined(HRT_UNSCHED)
+	(void)init_dmem_cfg;
+	code_addr = mmgr_malloc(1);
+	if (code_addr == mmgr_NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+#else
+	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 = mmgr_malloc(spctrl_cfg->code_size);
+	if (code_addr == mmgr_NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	mmgr_store(code_addr, spctrl_cfg->code, spctrl_cfg->code_size);
+
+	if (sizeof(hrt_vaddress) > sizeof(hrt_data)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "size of hrt_vaddress can not be greater than hrt_data\n");
+		hmm_free(code_addr);
+		code_addr = mmgr_NULL;
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	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 IA_CSS_ERR_INTERNAL_ERROR;
+	}
+#endif
+	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 IA_CSS_SUCCESS;
+}
+
+#ifdef 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;
+}
+#endif
+
+hrt_vaddress get_sp_code_addr(sp_ID_t  sp_id)
+{
+	return spctrl_cofig_info[sp_id].code_addr;
+}
+
+enum ia_css_err 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/*  freeup the resource */
+	if (spctrl_cofig_info[sp_id].code_addr)
+		hmm_free(spctrl_cofig_info[sp_id].code_addr);
+	spctrl_loaded[sp_id] = false;
+	return IA_CSS_SUCCESS;
+}
+
+/* Initialize dmem_cfg in SP dmem  and  start SP program*/
+enum ia_css_err 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* 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 IA_CSS_SUCCESS;
+}
+
+/* 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)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/atomisp2/css2400/runtime/tagger/interface/ia_css_tagger_common.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/tagger/interface/ia_css_tagger_common.h
new file mode 100644
index 000000000000..d0d74957358b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/tagger/interface/ia_css_tagger_common.h
@@ -0,0 +1,59 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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
+ */
+#if defined(HAS_SP_2400)
+#define MAX_CB_ELEMS_FOR_TAGGER 14
+#else
+#define MAX_CB_ELEMS_FOR_TAGGER 9
+#endif
+
+/**
+ * @brief Data structure for the tagger buffer element.
+ */
+typedef struct {
+	uint32_t frame;	/* the frame value stored in the element */
+	uint32_t param;	/* the param value stored in the element */
+	uint8_t mark;	/* the mark on the element */
+	uint8_t lock;	/* the lock on the element */
+	uint8_t 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/atomisp2/css2400/runtime/timer/src/timer.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/timer/src/timer.c
new file mode 100644
index 000000000000..49c69e60ca5c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/runtime/timer/src/timer.c
@@ -0,0 +1,48 @@
+#ifndef ISP2401
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+#else
+/**
+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.
+*/
+#endif
+
+#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"
+
+enum ia_css_err
+ia_css_timer_get_current_tick(
+	struct ia_css_clock_tick *curr_ts) {
+
+	assert(curr_ts !=  NULL);
+	if (curr_ts == NULL) {
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	curr_ts->ticks = (clock_value_t)gp_timer_read(GP_TIMER_SEL);
+	return IA_CSS_SUCCESS;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
new file mode 100644
index 000000000000..73c76583610a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css.c
@@ -0,0 +1,11364 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 "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 */
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "ia_css_isys.h"
+#endif
+#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"
+#ifdef USE_INPUT_SYSTEM_VERSION_2
+#include "ia_css_isys.h"
+#endif
+
+#include "memory_access.h"
+#include "tag.h"
+#include "assert_support.h"
+#include "math_support.h"
+#include "sw_event_global.h"			/* Event IDs.*/
+#if !defined(HAS_NO_INPUT_FORMATTER)
+#include "ia_css_ifmtr.h"
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "input_system.h"
+#endif
+#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 "platform_support.h" /* hrt_sleep(), inline */
+#include "ia_css_inputfifo.h"
+#define WITH_PC_MONITORING  0
+
+#define SH_CSS_VIDEO_BUFFER_ALIGNMENT 0
+
+#if WITH_PC_MONITORING
+#define MULTIPLE_SAMPLES 1
+#define NOF_SAMPLES      60
+#include "linux/kthread.h"
+#include "linux/sched.h"
+#include "linux/delay.h"
+#include "sh_css_metrics.h"
+static int thread_alive;
+#endif /* WITH_PC_MONITORING */
+
+#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"
+
+#if defined(HAS_BL)
+#include "support/bootloader/interface/ia_css_blctrl.h"
+#endif
+#if defined(HAS_RES_MGR)
+#include "components/acc_cluster/gen/host/acc_cluster.host.h"
+#endif
+
+/* Name of the sp program: should not be built-in */
+#define SP_PROG_NAME "sp"
+#if defined(HAS_BL)
+#define BL_PROG_NAME "bootloader"
+#endif
+/* 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 upto 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))
+
+#define DEFAULT_PLANES { {0, 0, 0, 0} }
+
+struct sh_css my_css;
+
+int (*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
+};
+
+/* a stream seed, to save and restore the stream data.
+   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;                /* pointer to restore the original handle */
+	struct ia_css_stream		*stream;                      /* handle, used as ID too.*/
+	struct ia_css_stream_config	stream_config;				/* stream config struct */
+	int				num_pipes;
+	struct ia_css_pipe		*pipes[IA_CSS_PIPE_ID_NUM];			/* pipe handles */
+	struct ia_css_pipe		**orig_pipes[IA_CSS_PIPE_ID_NUM];	/* pointer to restore original handle */
+	struct ia_css_pipe_config	pipe_config[IA_CSS_PIPE_ID_NUM];	/* pipe config structs */
+};
+
+#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;
+	uint32_t		       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 (1 << HIVE_GPIO_STROBE_TRIGGER_PIN)
+
+static bool fw_explicitly_loaded = false;
+
+/**
+ * Local prototypes
+ */
+
+static enum ia_css_err
+allocate_delay_frames(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+sh_css_pipe_start(struct ia_css_stream *stream);
+
+#ifdef ISP2401
+/**
+ * @brief Stop all "ia_css_pipe" instances in the target
+ * "ia_css_stream" instance.
+ *
+ * @param[in] stream	Point to the target "ia_css_stream" instance.
+ *
+ * @return
+ * - IA_CSS_SUCCESS, if the "stop" requests have been sucessfully sent out.
+ * - CSS error code, otherwise.
+ *
+ *
+ * NOTE
+ * This API sends the "stop" requests to the "ia_css_pipe"
+ * instances in the same "ia_css_stream" instance. It will
+ * return without waiting for all "ia_css_pipe" instatnces
+ * being stopped.
+ */
+static enum ia_css_err
+sh_css_pipes_stop(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.
+ */
+static bool
+sh_css_pipes_have_stopped(struct ia_css_stream *stream);
+
+static enum ia_css_err
+ia_css_pipe_check_format(struct ia_css_pipe *pipe, enum ia_css_frame_format format);
+
+static enum ia_css_err
+check_pipe_resolutions(const struct ia_css_pipe *pipe);
+
+#endif
+
+static enum ia_css_err
+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);
+
+#ifndef ISP2401
+static void
+sh_css_mmu_set_page_table_base_index(hrt_data base_index);
+
+#endif
+static enum ia_css_err 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 enum ia_css_err 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 enum ia_css_err
+sh_css_pipe_load_binaries(struct ia_css_pipe *pipe);
+
+static
+enum ia_css_err sh_css_pipe_get_viewfinder_frame_info(
+	struct ia_css_pipe *pipe,
+	struct ia_css_frame_info *info,
+	unsigned int idx);
+
+static enum ia_css_err
+sh_css_pipe_get_output_frame_info(struct ia_css_pipe *pipe,
+				  struct ia_css_frame_info *info,
+				  unsigned int idx);
+
+static enum ia_css_err
+capture_start(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+video_start(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+preview_start(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+yuvpp_start(struct ia_css_pipe *pipe);
+
+static bool copy_on_sp(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+init_vf_frameinfo_defaults(struct ia_css_pipe *pipe,
+	struct ia_css_frame *vf_frame, unsigned int idx);
+
+static enum ia_css_err
+init_in_frameinfo_memory_defaults(struct ia_css_pipe *pipe,
+	struct ia_css_frame *frame, enum ia_css_frame_format format);
+
+static enum ia_css_err
+init_out_frameinfo_defaults(struct ia_css_pipe *pipe,
+	struct ia_css_frame *out_frame, unsigned int idx);
+
+static enum ia_css_err
+sh_css_pipeline_add_acc_stage(struct ia_css_pipeline *pipeline,
+			      const void *acc_fw);
+
+static enum ia_css_err
+alloc_continuous_frames(
+	struct ia_css_pipe *pipe, bool init_time);
+
+static void
+pipe_global_init(void);
+
+static enum ia_css_err
+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 enum ia_css_err
+create_host_pipeline_structure(struct ia_css_stream *stream);
+
+static enum ia_css_err
+create_host_pipeline(struct ia_css_stream *stream);
+
+static enum ia_css_err
+create_host_preview_pipeline(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+create_host_video_pipeline(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+create_host_copy_pipeline(struct ia_css_pipe *pipe,
+    unsigned max_input_width,
+    struct ia_css_frame *out_frame);
+
+static enum ia_css_err
+create_host_isyscopy_capture_pipeline(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+create_host_capture_pipeline(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+create_host_yuvpp_pipeline(struct ia_css_pipe *pipe);
+
+static enum ia_css_err
+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);
+
+#ifndef ISP2401
+static bool
+sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+#else
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+#endif
+			enum ia_css_buffer_type type,
+			hrt_address kernel_ptr);
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_validate(hrt_vaddress 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);
+
+
+#if CONFIG_ON_FRAME_ENQUEUE()
+static enum ia_css_err set_config_on_frame_enqueue(struct ia_css_frame_info *info, struct frame_data_wrapper *frame);
+#endif
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+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
+
+#ifdef ISP2401
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+static enum ia_css_err
+aspect_ratio_crop_init(struct ia_css_stream *curr_stream,
+		struct ia_css_pipe *pipes[],
+		bool *do_crop_status);
+
+static bool
+aspect_ratio_crop_check(bool enabled, struct ia_css_pipe *curr_pipe);
+
+static enum ia_css_err
+aspect_ratio_crop(struct ia_css_pipe *curr_pipe,
+		struct ia_css_resolution *effective_res);
+#endif
+
+#endif
+static void
+sh_css_pipe_free_shading_table(struct ia_css_pipe *pipe)
+{
+	assert(pipe != NULL);
+	if (pipe == NULL) {
+		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
+};
+
+#define array_length(array) (sizeof(array)/sizeof(array[0]))
+
+/* Verify whether the selected output format is can be produced
+ * by the copy binary given the stream format.
+ * */
+static enum ia_css_err
+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 != NULL);
+	assert(pipe->stream != NULL);
+
+	switch (pipe->stream->config.input_config.format) {
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+		for (i=0; i<array_length(yuv420_copy_formats) && !found; i++)
+			found = (out_fmt == yuv420_copy_formats[i]);
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_YUV420_16);
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+		for (i=0; i<array_length(yuv422_copy_formats) && !found; i++)
+			found = (out_fmt == yuv422_copy_formats[i]);
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_YUV422_16 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_YUV420_16);
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RGBA888 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_RGB565);
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_FORMAT_RGB_888:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RGBA888 ||
+			 out_fmt == IA_CSS_FRAME_FORMAT_YUV420);
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_RAW) ||
+			(out_fmt == IA_CSS_FRAME_FORMAT_RAW_PACKED);
+		break;
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+		found = (out_fmt == IA_CSS_FRAME_FORMAT_BINARY_8);
+		break;
+	default:
+		break;
+	}
+	if (!found)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	return IA_CSS_SUCCESS;
+}
+
+unsigned int
+ia_css_stream_input_format_bits_per_pixel(struct ia_css_stream *stream)
+{
+	int bpp = 0;
+
+	if (stream != NULL)
+		bpp = ia_css_util_input_format_bpp(stream->config.input_config.format,
+						stream->config.pixels_per_clock == 2);
+
+	return bpp;
+}
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+static enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(stream != NULL);
+	assert(pipe != NULL);
+
+	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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			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 defined(IS_ISP_2400_SYSTEM)
+		if (pipe->stream->config.mode == IA_CSS_INPUT_MODE_TPG) {
+			/* TODO: move define to proper file in tools */
+			#define GP_ISEL_TPG_MODE 0x90058
+			ia_css_device_store_uint32(GP_ISEL_TPG_MODE, 0);
+		}
+#endif
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+		"sh_css_config_input_network() leave:\n");
+	return IA_CSS_SUCCESS;
+}
+#elif !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+static unsigned int csi2_protocol_calculate_max_subpixels_per_line(
+		enum ia_css_stream_format	format,
+		unsigned int			pixels_per_line)
+{
+	unsigned int rval;
+
+	switch (format) {
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_RGB_444:
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_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 IA_CSS_STREAM_FORMAT_RAW_6:
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+	case IA_CSS_STREAM_FORMAT_USER_DEF8:
+		/*
+		 * The frame format layout is shown below.
+		 *
+		 *		Line	0:	Pixel Pixel ... Pixel
+		 *		Line	1:	Pixel Pixel ... Pixel
+		 *		Line	2:	Pixel Pixel ... Pixel
+		 *		Line	3:	Pixel Pixel ... Pixel
+		 *		...
+		 *		Line (n-2):	Pixel Pixel ... Pixel
+		 *		Line (n-1):	Pixel 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 == IA_CSS_CSI2_PORT0) {
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_CSI_PORT0_ID;
+		} else if (stream_cfg->source.port.port == IA_CSS_CSI2_PORT1) {
+			isys_stream_descr->input_port_id = INPUT_SYSTEM_CSI_PORT1_ID;
+		} else if (stream_cfg->source.port.port == IA_CSS_CSI2_PORT2) {
+			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:
+	{
+		enum ia_css_err 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 != IA_CSS_SUCCESS)
+			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 USE_INPUT_SYSTEM_VERSION_2401
+		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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+				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 USE_INPUT_SYSTEM_VERSION_2401
+			/* 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 ia_css_stream_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 = IA_CSS_STREAM_FORMAT_RAW_10;
+		}
+		else if (stream_cfg->source.port.compression.uncompressed_bits_per_pixel ==
+			UNCOMPRESSED_BITS_PER_PIXEL_12) {
+				fmt_type = IA_CSS_STREAM_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;
+	/*
+	 * Early polling is required for timestamp accuracy in certain case.
+	 * The ISYS HW polling is started on
+	 * ia_css_isys_stream_capture_indication() instead of
+	 * ia_css_pipeline_sp_wait_for_isys_stream_N() as isp processing of
+	 * capture takes longer than getting an ISYS frame
+	 *
+	 * Only 2401 relevant ??
+	 */
+	isys_stream_descr->polling_mode
+		= early_polling ? INPUT_SYSTEM_POLL_ON_CAPTURE_REQUEST
+			: INPUT_SYSTEM_POLL_ON_WAIT_FOR_FRAME;
+	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 enum ia_css_err
+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;
+	uint32_t isys_stream_id;
+	bool early_polling = false;
+
+	assert(stream != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+		"sh_css_config_input_network() enter 0x%p:\n", stream);
+
+	if (stream->config.continuous == true) {
+		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 (stream->last_pipe->config.mode == IA_CSS_PIPE_MODE_CAPTURE) {
+			/*
+			 * We need to poll the ISYS HW in capture_indication itself
+			 * for "non-continous" capture usecase for getting accurate
+			 * isys frame capture timestamps.
+			 * This is because the capturepipe propcessing takes longer
+			 * to execute than the input system frame capture.
+			 * 2401 specific
+			 */
+			early_polling = true;
+		}
+	}
+
+	assert(pipe != NULL);
+	if (pipe == NULL)
+		return IA_CSS_ERR_INTERNAL_ERROR;
+
+	if (pipe->pipeline.stages != NULL)
+		if (pipe->pipeline.stages->binary != NULL)
+			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 != true)
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	/* 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 != true)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+
+		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 != true)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+
+		/* 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 != true) {
+			ia_css_isys_stream_destroy(&(sp_pipeline_input_terminal->context.virtual_input_system_stream[i]));
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+		"sh_css_config_input_network() leave:\n");
+
+	return IA_CSS_SUCCESS;
+}
+
+static inline struct ia_css_pipe *stream_get_last_pipe(
+		struct ia_css_stream *stream)
+{
+	struct ia_css_pipe *last_pipe = NULL;
+	if (stream != NULL)
+		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 != NULL) &&
+	    (last_pipe != NULL) &&
+	    (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 enum ia_css_err stream_csi_rx_helper(
+	struct ia_css_stream *stream,
+	enum ia_css_err (*func)(enum ia_css_csi2_port, uint32_t))
+{
+	enum ia_css_err retval = IA_CSS_ERR_INTERNAL_ERROR;
+	uint32_t sp_thread_id, stream_id;
+	bool rc;
+	struct ia_css_pipe *target_pipe = NULL;
+
+	if ((stream == NULL) || (stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR))
+		goto exit;
+
+	target_pipe = stream_get_target_pipe(stream);
+
+	if (target_pipe == NULL)
+		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) {
+			uint32_t 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 == IA_CSS_SUCCESS) &&
+		 (stream_id < IA_CSS_STREAM_MAX_ISYS_STREAM_PER_CH));
+
+exit:
+	return retval;
+}
+
+static inline enum ia_css_err 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 enum ia_css_err stream_unregister_with_csi_rx(
+	struct ia_css_stream *stream)
+{
+	return stream_csi_rx_helper(stream, ia_css_isys_csi_rx_unregister_stream);
+}
+#endif
+
+#if WITH_PC_MONITORING
+static struct task_struct *my_kthread;    /* Handle for the monitoring thread */
+static int sh_binary_running;         /* Enable sampling in the thread */
+
+static void print_pc_histo(char *core_name, struct sh_css_pc_histogram *hist)
+{
+	unsigned i;
+	unsigned cnt_run = 0;
+	unsigned cnt_stall = 0;
+
+	if (hist == NULL)
+		return;
+
+	sh_css_print("%s histogram length = %d\n", core_name, hist->length);
+	sh_css_print("%s PC\trun\tstall\n", core_name);
+
+	for (i = 0; i < hist->length; i++) {
+		if ((hist->run[i] == 0) && (hist->run[i] == hist->stall[i]))
+			continue;
+		sh_css_print("%s %d\t%d\t%d\n",
+				core_name, i, hist->run[i], hist->stall[i]);
+		cnt_run += hist->run[i];
+		cnt_stall += hist->stall[i];
+	}
+
+	sh_css_print(" Statistics for %s, cnt_run = %d, cnt_stall = %d, "
+	       "hist->length = %d\n",
+			core_name, cnt_run, cnt_stall, hist->length);
+}
+
+static void print_pc_histogram(void)
+{
+	struct ia_css_binary_metrics *metrics;
+
+	for (metrics = sh_css_metrics.binary_metrics;
+	     metrics;
+	     metrics = metrics->next) {
+		if (metrics->mode == IA_CSS_BINARY_MODE_PREVIEW ||
+		    metrics->mode == IA_CSS_BINARY_MODE_VF_PP) {
+			sh_css_print("pc_histogram for binary %d is SKIPPED\n",
+				metrics->id);
+			continue;
+		}
+
+		sh_css_print(" pc_histogram for binary %d\n", metrics->id);
+		print_pc_histo("  ISP", &metrics->isp_histogram);
+		print_pc_histo("  SP",   &metrics->sp_histogram);
+		sh_css_print("print_pc_histogram() done for binay->id = %d, "
+			     "done.\n", metrics->id);
+	}
+
+	sh_css_print("PC_MONITORING:print_pc_histogram() -- DONE\n");
+}
+
+static int pc_monitoring(void *data)
+{
+	int i = 0;
+
+	(void)data;
+	while (true) {
+		if (sh_binary_running) {
+			sh_css_metrics_sample_pcs();
+#if MULTIPLE_SAMPLES
+			for (i = 0; i < NOF_SAMPLES; i++)
+				sh_css_metrics_sample_pcs();
+#endif
+		}
+		usleep_range(10, 50);
+	}
+	return 0;
+}
+
+static void spying_thread_create(void)
+{
+	my_kthread = kthread_run(pc_monitoring, NULL, "sh_pc_monitor");
+	sh_css_metrics_enable_pc_histogram(1);
+}
+
+static void input_frame_info(struct ia_css_frame_info frame_info)
+{
+	sh_css_print("SH_CSS:input_frame_info() -- frame->info.res.width = %d, "
+	       "frame->info.res.height = %d, format = %d\n",
+			frame_info.res.width, frame_info.res.height, frame_info.format);
+}
+#endif /* WITH_PC_MONITORING */
+
+static void
+start_binary(struct ia_css_pipe *pipe,
+	     struct ia_css_binary *binary)
+{
+	struct ia_css_stream *stream;
+
+	assert(pipe != NULL);
+	/* Acceleration uses firmware, the binary thus can be NULL */
+	/* assert(binary != NULL); */
+
+	(void)binary;
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	stream = pipe->stream;
+#else
+	(void)pipe;
+	(void)stream;
+#endif
+
+	if (binary)
+		sh_css_metrics_start_binary(&binary->metrics);
+
+#if WITH_PC_MONITORING
+	sh_css_print("PC_MONITORING: %s() -- binary id = %d , "
+		     "enable_dvs_envelope = %d\n",
+		     __func__, binary->info->sp.id,
+		     binary->info->sp.enable.dvs_envelope);
+	input_frame_info(binary->in_frame_info);
+
+	if (binary && binary->info->sp.pipeline.mode == IA_CSS_BINARY_MODE_VIDEO)
+		sh_binary_running = true;
+#endif
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	if (stream->reconfigure_css_rx) {
+		ia_css_isys_rx_configure(&pipe->stream->csi_rx_config,
+					 pipe->stream->config.mode);
+		stream->reconfigure_css_rx = false;
+	}
+#endif
+}
+
+/* start the copy function on the SP */
+static enum ia_css_err
+start_copy_on_sp(struct ia_css_pipe *pipe,
+		 struct ia_css_frame *out_frame)
+{
+
+	(void)out_frame;
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+
+	if ((pipe == NULL) || (pipe->stream == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	if (pipe->stream->reconfigure_css_rx)
+		ia_css_isys_rx_disable();
+#endif
+
+	if (pipe->stream->config.input_config.format != IA_CSS_STREAM_FORMAT_BINARY_8)
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	sh_css_sp_start_binary_copy(ia_css_pipe_get_pipe_num(pipe), out_frame, pipe->stream->config.pixels_per_clock == 2);
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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 IA_CSS_SUCCESS;
+}
+
+void sh_css_binary_args_reset(struct sh_css_binary_args *args)
+{
+	unsigned int i;
+
+#ifndef ISP2401
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++)
+#else
+	for (i = 0; i < NUM_TNR_FRAMES; i++)
+#endif
+		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)
+{
+#if defined(HAS_NO_INPUT_SYSTEM)
+	(void)input_mode;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("me = %p, copy_ovrd = %d, input_mode = %d",
+			     me, copy_ovrd, input_mode);
+
+	assert(me != NULL); /* 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,
+#ifndef ISP2401
+				&me->stream->info.metadata_info
+#else
+				&me->stream->info.metadata_info,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+				, (input_mode==IA_CSS_INPUT_MODE_MEMORY)?
+#else
+				(input_mode == IA_CSS_INPUT_MODE_MEMORY) ?
+#endif
+					(mipi_port_ID_t)0 :
+#ifndef ISP2401
+					me->stream->config.source.port.port
+#else
+					me->stream->config.source.port.port,
+#endif
+#endif
+#ifndef ISP2401
+				);
+#else
+				&me->config.internal_frame_origin_bqs_on_sctbl,
+				me->stream->isp_params_configs);
+#endif
+
+	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 != NULL);
+
+	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] != NULL);
+		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");
+}
+
+#ifndef ISP2401
+static void
+enable_interrupts(enum ia_css_irq_type irq_type)
+{
+#ifdef USE_INPUT_SYSTEM_VERSION_2
+	mipi_port_ID_t 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(
+			(virq_id_t)(IRQ_SW_CHANNEL0_ID + IRQ_SW_CHANNEL_OFFSET),
+			true);
+	/* Enable SW interrupt 1, this is used to signal PSYS events */
+	cnd_virq_enable_channel(
+			(virq_id_t)(IRQ_SW_CHANNEL1_ID + IRQ_SW_CHANNEL_OFFSET),
+			true);
+#if !defined(HAS_IRQ_MAP_VERSION_2)
+	/* IRQ_SW_CHANNEL2_ID does not exist on 240x systems */
+	cnd_virq_enable_channel(
+			(virq_id_t)(IRQ_SW_CHANNEL2_ID + IRQ_SW_CHANNEL_OFFSET),
+			true);
+	virq_clear_all();
+#endif
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2
+	for (port = 0; port < N_MIPI_PORT_ID; port++)
+		ia_css_isys_rx_enable_all_interrupts(port);
+#endif
+
+	IA_CSS_LEAVE_PRIVATE("");
+}
+
+#endif
+#if defined(HAS_BL)
+static bool sh_css_setup_blctrl_config(const struct ia_css_fw_info *fw,
+							const char *program,
+							ia_css_blctrl_cfg  *blctrl_cfg)
+{
+	if((fw == NULL)||(blctrl_cfg == NULL))
+		return false;
+	blctrl_cfg->bl_entry = 0;
+	blctrl_cfg->program_name = (char *)(program);
+
+#if !defined(HRT_UNSCHED)
+	blctrl_cfg->ddr_data_offset =  fw->blob.data_source;
+	blctrl_cfg->dmem_data_addr = fw->blob.data_target;
+	blctrl_cfg->dmem_bss_addr = fw->blob.bss_target;
+	blctrl_cfg->data_size = fw->blob.data_size ;
+	blctrl_cfg->bss_size = fw->blob.bss_size;
+
+	blctrl_cfg->blctrl_state_dmem_addr = fw->info.bl.sw_state;
+	blctrl_cfg->blctrl_dma_cmd_list = fw->info.bl.dma_cmd_list;
+	blctrl_cfg->blctrl_nr_of_dma_cmds = fw->info.bl.num_dma_cmds;
+
+	blctrl_cfg->code_size = fw->blob.size;
+	blctrl_cfg->code      = fw->blob.code;
+	blctrl_cfg->bl_entry  = fw->info.bl.bl_entry; /* entry function ptr on Bootloader */
+#endif
+	return true;
+}
+#endif
+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 == NULL)||(spctrl_cfg == NULL))
+		return false;
+	spctrl_cfg->sp_entry = 0;
+	spctrl_cfg->program_name = (char *)(program);
+
+#if !defined(HRT_UNSCHED)
+	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 */
+#endif
+	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;
+}
+
+bool
+ia_css_check_firmware_version(const struct ia_css_fw  *fw)
+{
+	bool retval = false;
+
+	if (fw != NULL) {
+		retval = sh_css_check_firmware_version(fw->data);
+	}
+	return retval;
+}
+
+enum ia_css_err
+ia_css_load_firmware(const struct ia_css_env *env,
+	    const struct ia_css_fw  *fw)
+{
+	enum ia_css_err err;
+
+	if (env == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	if (fw == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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(fw->data, fw->bytes);
+	if (err == IA_CSS_SUCCESS) {
+		err = ia_css_binary_init_infos();
+		if (err == IA_CSS_SUCCESS)
+			fw_explicitly_loaded = true;
+	}
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_load_firmware() leave \n");
+	return err;
+}
+
+enum ia_css_err
+ia_css_init(const struct ia_css_env *env,
+	    const struct ia_css_fw  *fw,
+	    uint32_t                 mmu_l1_base,
+	    enum ia_css_irq_type     irq_type)
+{
+	enum ia_css_err err;
+	ia_css_spctrl_cfg spctrl_cfg;
+#if defined(HAS_BL)
+	ia_css_blctrl_cfg blctrl_cfg;
+#endif
+
+	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 == NULL && !fw_explicitly_loaded)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	if (env == NULL)
+	    return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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);
+#ifndef ISP2401
+	my_css_save.mmu_base = mmu_l1_base;
+#else
+	ia_css_save_mmu_base_addr(mmu_l1_base);
+#endif
+
+	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_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#ifndef ISP2401
+	IA_CSS_LOG("init: %d", my_css_save_initialized);
+#else
+	ia_css_save_restore_data_init();
+#endif
+
+#ifndef ISP2401
+	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);
+	}
+#endif
+	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;
+#ifndef ISP2401
+	my_css_save.irq_type = irq_type;
+#else
+	ia_css_save_irq_type(irq_type);
+#endif
+	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_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	err = sh_css_params_init();
+	if (err != IA_CSS_SUCCESS) {
+		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(fw->data, fw->bytes);
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR(err);
+			return err;
+		}
+		err = ia_css_binary_init_infos();
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR(err);
+			return err;
+		}
+		fw_explicitly_loaded = false;
+#ifndef ISP2401
+		my_css_save.loaded_fw = (struct ia_css_fw *)fw;
+#endif
+	}
+	if(!sh_css_setup_spctrl_config(&sh_css_sp_fw,SP_PROG_NAME,&spctrl_cfg))
+		return IA_CSS_ERR_INTERNAL_ERROR;
+
+	err = ia_css_spctrl_load_fw(SP0_ID, &spctrl_cfg);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#if defined(HAS_BL)
+	if (!sh_css_setup_blctrl_config(&sh_css_bl_fw, BL_PROG_NAME, &blctrl_cfg))
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	err = ia_css_blctrl_load_fw(&blctrl_cfg);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#ifdef ISP2401
+	err = ia_css_blctrl_add_target_fw_info(&sh_css_sp_fw, IA_CSS_SP0,
+					 get_sp_code_addr(SP0_ID));
+
+#endif
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif /* HAS_BL */
+
+#if WITH_PC_MONITORING
+	if (!thread_alive) {
+		thread_alive++;
+		sh_css_print("PC_MONITORING: %s() -- create thread DISABLED\n",
+			     __func__);
+		spying_thread_create();
+	}
+#endif
+	if (!sh_css_hrt_system_is_idle()) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_SYSTEM_NOT_IDLE);
+		return IA_CSS_ERR_SYSTEM_NOT_IDLE;
+	}
+	/* 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(HAS_INPUT_SYSTEM_VERSION_2) && defined(HAS_INPUT_SYSTEM_VERSION_2401)
+#if	defined(USE_INPUT_SYSTEM_VERSION_2)
+	gp_device_reg_store(GP_DEVICE0_ID, _REG_GP_SWITCH_ISYS2401_ADDR, 0);
+#elif defined (USE_INPUT_SYSTEM_VERSION_2401)
+	gp_device_reg_store(GP_DEVICE0_ID, _REG_GP_SWITCH_ISYS2401_ADDR, 1);
+#endif
+#endif
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	dma_set_max_burst_size(DMA0_ID, HIVE_DMA_BUS_DDR_CONN,
+			       ISP_DMA_MAX_BURST_LENGTH);
+
+	if(ia_css_isys_init() != INPUT_SYSTEM_ERR_NO_ERROR)
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+#endif
+
+	sh_css_params_map_and_store_default_gdc_lut();
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+enum ia_css_err ia_css_suspend(void)
+{
+	int i;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_suspend() enter\n");
+	my_css_save.mode = sh_css_mode_suspend;
+	for(i=0;i<MAX_ACTIVE_STREAMS;i++)
+		if (my_css_save.stream_seeds[i].stream != NULL)
+		{
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "==*> unloading seed %d (%p)\n", i, my_css_save.stream_seeds[i].stream);
+			ia_css_stream_unload(my_css_save.stream_seeds[i].stream);
+		}
+	my_css_save.mode = sh_css_mode_working;
+	ia_css_stop_sp();
+	ia_css_uninit();
+	for(i=0;i<MAX_ACTIVE_STREAMS;i++)
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "==*> after 1: seed %d (%p)\n", i, my_css_save.stream_seeds[i].stream);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_suspend() leave\n");
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_resume(void)
+{
+	int i, j;
+	enum ia_css_err err;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_resume() enter: void\n");
+
+	err = ia_css_init(&(my_css_save.driver_env), my_css_save.loaded_fw, my_css_save.mmu_base, my_css_save.irq_type);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	err = ia_css_start_sp();
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	my_css_save.mode = sh_css_mode_resume;
+	for(i=0;i<MAX_ACTIVE_STREAMS;i++)
+	{
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "==*> seed stream %p\n", my_css_save.stream_seeds[i].stream);
+		if (my_css_save.stream_seeds[i].stream != NULL)
+		{
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "==*> loading seed %d\n", i);
+			err = ia_css_stream_load(my_css_save.stream_seeds[i].stream);
+			if (err != IA_CSS_SUCCESS)
+			{
+				if (i)
+					for(j=0;j<i;j++)
+						ia_css_stream_unload(my_css_save.stream_seeds[j].stream);
+				return err;
+			}
+			err = ia_css_stream_start(my_css_save.stream_seeds[i].stream);
+			if (err != IA_CSS_SUCCESS)
+			{
+				for(j=0;j<=i;j++)
+				{
+					ia_css_stream_stop(my_css_save.stream_seeds[j].stream);
+					ia_css_stream_unload(my_css_save.stream_seeds[j].stream);
+				}
+				return err;
+			}
+			*my_css_save.stream_seeds[i].orig_stream = my_css_save.stream_seeds[i].stream;
+			for(j=0;j<my_css_save.stream_seeds[i].num_pipes;j++)
+				*(my_css_save.stream_seeds[i].orig_pipes[j]) = my_css_save.stream_seeds[i].pipes[j];
+		}
+	}
+	my_css_save.mode = sh_css_mode_working;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_resume() leave: return_void\n");
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_enable_isys_event_queue(bool enable)
+{
+	if (sh_css_sp_is_running())
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	sh_css_sp_enable_isys_event_queue(enable);
+	return IA_CSS_SUCCESS;
+}
+
+void *sh_css_malloc(size_t size)
+{
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_malloc() enter: size=%d\n",size);
+	/* FIXME: This first test can probably go away */
+	if (size == 0)
+		return NULL;
+	if (size > PAGE_SIZE)
+		return vmalloc(size);
+	return kmalloc(size, GFP_KERNEL);
+}
+
+void *sh_css_calloc(size_t N, size_t size)
+{
+	void *p;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_calloc() enter: N=%d, size=%d\n",N,size);
+
+	/* FIXME: this test can probably go away */
+	if (size > 0) {
+		p = sh_css_malloc(N*size);
+		if (p)
+			memset(p, 0, size);
+	}
+	return NULL;
+}
+
+void sh_css_free(void *ptr)
+{
+	if (is_vmalloc_addr(ptr))
+		vfree(ptr);
+	else
+		kfree(ptr);
+}
+
+/* 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 != NULL) && (my_css.flush != NULL))
+		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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum ia_css_pipe_id pipe_id;
+
+	assert(stream != NULL);
+	IA_CSS_ENTER_PRIVATE("stream = %p, map = %p", stream, map);
+
+	if (stream == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int copy_pipe_delay = 0,
+		     capture_pipe_delay = 0;
+
+	assert(stream != NULL);
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+
+	if (stream == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	main_pipe	= stream->last_pipe;
+	assert(main_pipe != NULL);
+	if (main_pipe == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if ((IA_CSS_SUCCESS == err) && copy_pipe) {
+		err = ia_css_pipeline_create(&copy_pipe->pipeline,
+								copy_pipe->mode,
+								copy_pipe->pipe_num,
+								copy_pipe_delay);
+	}
+
+	if ((IA_CSS_SUCCESS == err) && capture_pipe) {
+		err = ia_css_pipeline_create(&capture_pipe->pipeline,
+								capture_pipe->mode,
+								capture_pipe->pipe_num,
+								capture_pipe_delay);
+	}
+
+	if ((IA_CSS_SUCCESS == 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 && IA_CSS_SUCCESS == 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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned max_input_width = 0;
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+	if (stream == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 != IA_CSS_SUCCESS)
+				goto ERR;
+		}
+
+	}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+	/* 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 != IA_CSS_SUCCESS)
+			goto ERR;
+	}
+#elif defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			goto ERR;
+
+		break;
+
+	case IA_CSS_PIPE_ID_ACC:
+		err = create_host_acc_pipeline(main_pipe);
+		if (err != IA_CSS_SUCCESS)
+			goto ERR;
+
+		break;
+	default:
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	if (err != IA_CSS_SUCCESS)
+		goto ERR;
+
+	if(copy_pipe) {
+		err = create_host_copy_pipeline(copy_pipe, max_input_width,
+					  main_pipe->continuous_frames[0]);
+		if (err != IA_CSS_SUCCESS)
+			goto ERR;
+	}
+
+	if(capture_pipe) {
+		err = create_host_capture_pipeline(capture_pipe);
+		if (err != IA_CSS_SUCCESS)
+			goto ERR;
+	}
+
+	if (acc_pipe) {
+		err = create_host_acc_pipeline(acc_pipe);
+		if (err != IA_CSS_SUCCESS)
+			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 && IA_CSS_SUCCESS == 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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+			}
+			if (err != IA_CSS_SUCCESS)
+				goto ERR;
+		}
+	}
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static enum ia_css_err
+init_pipe_defaults(enum ia_css_pipe_mode mode,
+	       struct ia_css_pipe *pipe,
+	       bool copy_pipe)
+{
+	static struct ia_css_pipe default_pipe = IA_CSS_DEFAULT_PIPE;
+	static struct ia_css_preview_settings prev  = IA_CSS_DEFAULT_PREVIEW_SETTINGS;
+	static struct ia_css_capture_settings capt  = IA_CSS_DEFAULT_CAPTURE_SETTINGS;
+	static struct ia_css_video_settings   video = IA_CSS_DEFAULT_VIDEO_SETTINGS;
+	static struct ia_css_yuvpp_settings   yuvpp = IA_CSS_DEFAULT_YUVPP_SETTINGS;
+
+	if (pipe == NULL) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* Initialize pipe to pre-defined defaults */
+	*pipe = 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;
+		pipe->pipe_settings.preview = prev;
+		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;
+		}
+		pipe->pipe_settings.capture = capt;
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		pipe->mode = IA_CSS_PIPE_ID_VIDEO;
+		pipe->pipe_settings.video = 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;
+		pipe->pipe_settings.yuvpp = yuvpp;
+		break;
+	default:
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	return IA_CSS_SUCCESS;
+}
+
+static void
+pipe_global_init(void)
+{
+	uint8_t i;
+
+	my_css.pipe_counter = 0;
+	for (i = 0; i < IA_CSS_PIPELINE_NUM_MAX; i++) {
+		my_css.all_pipes[i] = NULL;
+	}
+}
+
+static enum ia_css_err
+pipe_generate_pipe_num(const struct ia_css_pipe *pipe, unsigned int *pipe_number)
+{
+	const uint8_t INVALID_PIPE_NUM = (uint8_t)~(0);
+	uint8_t pipe_num = INVALID_PIPE_NUM;
+	uint8_t i;
+
+	if (pipe == NULL) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* 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] == NULL) {
+			/*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 IA_CSS_ERR_RESOURCE_EXHAUSTED;
+	}
+
+	my_css.pipe_counter++;
+
+	IA_CSS_LOG("pipe_num (%d)", pipe_num);
+
+	*pipe_number = pipe_num;
+	return IA_CSS_SUCCESS;
+}
+
+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 enum ia_css_err
+create_pipe(enum ia_css_pipe_mode mode,
+	    struct ia_css_pipe **pipe,
+	    bool copy_pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_pipe *me;
+
+	if (pipe == NULL) {
+		IA_CSS_ERROR("NULL pipe parameter");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	me = kmalloc(sizeof(*me), GFP_KERNEL);
+	if (!me)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	err = init_pipe_defaults(mode, me, copy_pipe);
+	if (err != IA_CSS_SUCCESS) {
+		kfree(me);
+		return err;
+	}
+
+	err = pipe_generate_pipe_num(me, &(me->pipe_num));
+	if (err != IA_CSS_SUCCESS) {
+		kfree(me);
+		return err;
+	}
+
+	*pipe = me;
+	return IA_CSS_SUCCESS;
+}
+
+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;
+
+	assert(pipe != NULL);
+	if (pipe == NULL) {
+		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);
+	}
+}
+
+enum ia_css_err
+ia_css_pipe_destroy(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	IA_CSS_ENTER("pipe = %p", pipe);
+
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (pipe->stream != NULL) {
+		IA_CSS_LOG("ia_css_stream_destroy not called!");
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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_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;
+	}
+
+#ifndef ISP2401
+	if (pipe->scaler_pp_lut != mmgr_NULL) {
+		hmm_free(pipe->scaler_pp_lut);
+		pipe->scaler_pp_lut = mmgr_NULL;
+	}
+#else
+	sh_css_params_free_gdc_lut(pipe->scaler_pp_lut);
+	pipe->scaler_pp_lut = mmgr_NULL;
+#endif
+
+	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");
+#if WITH_PC_MONITORING
+	sh_css_print("PC_MONITORING: %s() -- started\n", __func__);
+	print_pc_histogram();
+#endif
+
+	sh_css_params_free_default_gdc_lut();
+
+
+	/* TODO: JB: implement decent check and handling of freeing mipi frames */
+	//assert(ref_count_mipi_allocation == 0); //mipi frames are not freed
+	/* cleanup generic data */
+	sh_css_params_uninit();
+	ia_css_refcount_uninit();
+
+	ia_css_rmgr_uninit();
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+	/* needed for reprogramming the inputformatter after power cycle of css */
+	ifmtr_set_if_blocking_mode_reset = true;
+#endif
+
+	if (fw_explicitly_loaded == false) {
+		ia_css_unload_firmware();
+	}
+	ia_css_spctrl_unload_fw(SP0_ID);
+	sh_css_sp_set_sp_running(false);
+#if defined(HAS_BL)
+	ia_css_blctrl_unload_fw();
+#endif
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	/* check and free any remaining mipi frames */
+	free_mipi_frames(NULL);
+#endif
+
+	sh_css_sp_reset_global_vars();
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	ia_css_isys_uninit();
+#endif
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_uninit() leave: return_void\n");
+}
+
+#ifndef ISP2401
+/* Deprecated, this is an HRT backend function (memory_access.h) */
+static void
+sh_css_mmu_set_page_table_base_index(hrt_data base_index)
+{
+	int i;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "sh_css_mmu_set_page_table_base_index() enter: base_index=0x%08x\n",base_index);
+	my_css.page_table_base_index = base_index;
+	for (i = 0; i < (int)N_MMU_ID; i++) {
+		mmu_ID_t mmu_id = (mmu_ID_t)i;
+		mmu_set_page_table_base_index(mmu_id, base_index);
+		mmu_invalidate_cache(mmu_id);
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE, "sh_css_mmu_set_page_table_base_index() leave: return_void\n");
+}
+
+#endif
+#if defined(HAS_IRQ_MAP_VERSION_2)
+enum ia_css_err ia_css_irq_translate(
+	unsigned int *irq_infos)
+{
+	virq_id_t	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 IA_CSS_ERR_INTERNAL_ERROR;
+
+#if WITH_PC_MONITORING
+		sh_css_print("PC_MONITORING: %s() irq = %d, "
+			     "sh_binary_running set to 0\n", __func__, irq);
+		sh_binary_running = 0 ;
+#endif
+
+		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;
+#if !defined(HAS_NO_INPUT_SYSTEM)
+		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;
+#endif
+#if !defined(HAS_NO_INPUT_FORMATTER)
+		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=%p\n", infos);
+
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err ia_css_irq_enable(
+	enum ia_css_irq_info info,
+	bool enable)
+{
+	virq_id_t	irq = N_virq_id;
+	IA_CSS_ENTER("info=%d, enable=%d", info, enable);
+
+	switch (info) {
+#if !defined(HAS_NO_INPUT_FORMATTER)
+	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;
+#endif
+#if !defined(HAS_NO_INPUT_FORMATTER)
+	case IA_CSS_IRQ_INFO_IF_ERROR:
+		irq = virq_ifmt0_id;
+		break;
+#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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	cnd_virq_enable_channel(irq, enable);
+
+	IA_CSS_LEAVE_ERR(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+#else
+#error "sh_css.c: IRQ MAP must be one of \
+	{IRQ_MAP_VERSION_2}"
+#endif
+
+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 enum ia_css_err 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;
+	enum ia_css_err err;
+	struct ia_css_binary_descr copy_descr;
+
+	/* next_binary can be NULL */
+	assert(pipe != NULL);
+	assert(copy_binary != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+		"load_copy_binary() enter:\n");
+
+	if (next_binary != NULL) {
+		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 : NULL/*TODO: &copy_vf_info*/);
+	err = ia_css_binary_find(&copy_descr, copy_binary);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	copy_binary->left_padding = left_padding;
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+alloc_continuous_frames(
+	struct ia_css_pipe *pipe, bool init_time)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_frame_info ref_info;
+	enum ia_css_pipe_id pipe_id;
+	bool continuous;
+	unsigned int i, idx;
+	unsigned int num_frames;
+	struct ia_css_pipe *capture_pipe = NULL;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, init_time = %d", pipe, init_time);
+
+	if ((pipe == NULL) || (pipe->stream == NULL)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+	/* 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 !defined(HAS_NO_PACKED_RAW_PIXELS)
+	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
+#endif
+	{
+		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;
+		capture_pipe = pipe->pipe_settings.preview.capture_pipe;
+	} else if (pipe_id == IA_CSS_PIPE_ID_VIDEO) {
+		pipe->pipe_settings.video.video_binary.in_frame_info.format = ref_info.format;
+		capture_pipe = pipe->pipe_settings.video.capture_pipe;
+	} else {
+		/* should not happen */
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	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_SUCCESS) {
+				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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_alloc_continuous_frame_remain(struct ia_css_stream *stream)
+{
+	if (stream == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	return alloc_continuous_frames(stream->continuous_pipe, false);
+}
+
+static enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool continuous, need_vf_pp = false;
+	bool need_isp_copy_binary = false;
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	bool sensor = false;
+#endif
+	/* preview only have 1 output pin now */
+	struct ia_css_frame_info *pipe_out_info = &pipe->output_info[0];
+#ifdef ISP2401
+	struct ia_css_preview_settings *mycs  = &pipe->pipe_settings.preview;
+
+#endif
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+	assert(pipe->mode == IA_CSS_PIPE_ID_PREVIEW);
+
+	online = pipe->stream->config.online;
+	continuous = pipe->stream->config.continuous;
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	sensor = pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR;
+#endif
+
+#ifndef ISP2401
+	if (pipe->pipe_settings.preview.preview_binary.info)
+#else
+	if (mycs->preview_binary.info)
+#endif
+		return IA_CSS_SUCCESS;
+
+	err = ia_css_util_check_input(&pipe->stream->config, false, false);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+	err = ia_css_frame_check_info(pipe_out_info);
+	if (err != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+		return err;
+	err = ia_css_binary_find(&preview_descr,
+#ifndef ISP2401
+				 &pipe->pipe_settings.preview.preview_binary);
+#else
+				 &mycs->preview_binary);
+#endif
+	if (err != IA_CSS_SUCCESS)
+		return err;
+
+#ifdef ISP2401
+	/* 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;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+		"load_preview_binaries() num_invalid_frames=%d dvs_frame_delay=%d\n",
+		pipe->num_invalid_frames, pipe->dvs_frame_delay);
+
+#endif
+	/* The vf_pp binary is needed when (further) YUV downscaling is required */
+#ifndef ISP2401
+	need_vf_pp |= pipe->pipe_settings.preview.preview_binary.out_frame_info[0].res.width != pipe_out_info->res.width;
+	need_vf_pp |= pipe->pipe_settings.preview.preview_binary.out_frame_info[0].res.height != pipe_out_info->res.height;
+#else
+	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;
+#endif
+
+	/* 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 &&
+#ifndef ISP2401
+		(pipe->pipe_settings.preview.preview_binary.out_frame_info[0].format != IA_CSS_FRAME_FORMAT_YUV_LINE)) {
+#else
+		(mycs->preview_binary.out_frame_info[0].format != IA_CSS_FRAME_FORMAT_YUV_LINE)) {
+#endif
+
+		/* 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 != IA_CSS_SUCCESS)
+			return err;
+		err = ia_css_binary_find(&preview_descr,
+#ifndef ISP2401
+				&pipe->pipe_settings.preview.preview_binary);
+#else
+				&mycs->preview_binary);
+#endif
+		if (err != IA_CSS_SUCCESS)
+			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,
+#ifndef ISP2401
+			&pipe->pipe_settings.preview.preview_binary.out_frame_info[0],
+#else
+			&mycs->preview_binary.out_frame_info[0],
+#endif
+			pipe_out_info);
+		err = ia_css_binary_find(&vf_pp_descr,
+#ifndef ISP2401
+				 &pipe->pipe_settings.preview.vf_pp_binary);
+#else
+				 &mycs->vf_pp_binary);
+#endif
+		if (err != IA_CSS_SUCCESS)
+			return err;
+	}
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	/* 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
+#ifndef ISP2401
+	need_isp_copy_binary = !online && !continuous;
+#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.
+	 */
+	need_isp_copy_binary = !online && !continuous && !(pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY);
+#endif
+#endif
+
+	/* Copy */
+	if (need_isp_copy_binary) {
+		err = load_copy_binary(pipe,
+#ifndef ISP2401
+				       &pipe->pipe_settings.preview.copy_binary,
+				       &pipe->pipe_settings.preview.preview_binary);
+#else
+				       &mycs->copy_binary,
+				       &mycs->preview_binary);
+#endif
+		if (err != IA_CSS_SUCCESS)
+			return err;
+	}
+
+	if (pipe->shading_table) {
+		ia_css_shading_table_free(pipe->shading_table);
+		pipe->shading_table = NULL;
+	}
+
+	return IA_CSS_SUCCESS;
+}
+
+static void
+ia_css_binary_unload(struct ia_css_binary *binary)
+{
+	ia_css_binary_destroy_isp_parameters(binary);
+}
+
+static enum ia_css_err
+unload_preview_binaries(struct ia_css_pipe *pipe)
+{
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((pipe == NULL) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+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 enum ia_css_err 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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 != IA_CSS_SUCCESS)
+			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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	if (in_frame == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	if (vf_pp_binary == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	if (vf_pp_stage == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 != IA_CSS_SUCCESS)
+			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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 != NULL);
+	assert(pipe != NULL);
+	assert(me != NULL);
+	assert(yuv_scaler_binary != NULL);
+	assert(pre_vf_pp_stage != NULL);
+	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 != IA_CSS_SUCCESS)
+		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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 != NULL);
+	assert(pipe != NULL);
+	assert(me != NULL);
+	assert(capture_pp_binary != NULL);
+	assert(capture_pp_stage != NULL);
+	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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+		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 enum ia_css_err
+init_vf_frameinfo_defaults(struct ia_css_pipe *pipe,
+	struct ia_css_frame *vf_frame, unsigned int idx)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(vf_frame != NULL);
+
+	sh_css_pipe_get_viewfinder_frame_info(pipe, &vf_frame->info, idx);
+	vf_frame->contiguous = false;
+	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 USE_INPUT_SYSTEM_VERSION_2401
+static unsigned int
+get_crop_lines_for_bayer_order (
+		const struct ia_css_stream_config *config)
+{
+	assert(config != NULL);
+	if ((IA_CSS_BAYER_ORDER_BGGR == config->input_config.bayer_order)
+	    || (IA_CSS_BAYER_ORDER_GBRG == config->input_config.bayer_order))
+		return 1;
+
+	return 0;
+}
+
+static unsigned int
+get_crop_columns_for_bayer_order (
+		const struct ia_css_stream_config *config)
+{
+	assert(config != NULL);
+	if ((IA_CSS_BAYER_ORDER_RGGB == config->input_config.bayer_order)
+	    || (IA_CSS_BAYER_ORDER_GBRG == config->input_config.bayer_order))
+		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 != NULL);
+	assert(pipe->stream != NULL);
+	assert(in_frame != NULL);
+
+	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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(frame != NULL);
+	in_frame = frame;
+
+	in_frame->info.format = format;
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	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->contiguous = false;
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	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 enum ia_css_err
+init_out_frameinfo_defaults(struct ia_css_pipe *pipe,
+	struct ia_css_frame *out_frame, unsigned int idx)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int thread_id;
+	enum sh_css_queue_id queue_id;
+
+	assert(out_frame != NULL);
+
+	sh_css_pipe_get_output_frame_info(pipe, &out_frame->info, idx);
+	out_frame->contiguous = false;
+	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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool need_copy   = false;
+	bool need_vf_pp  = false;
+	bool need_yuv_pp = false;
+	unsigned num_output_pins;
+	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 == NULL) || (pipe->stream == NULL) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 != IA_CSS_SUCCESS)
+			goto ERR;
+	}
+
+	out_frame->data = 0;
+	err = init_out_frameinfo_defaults(pipe, out_frame, 0);
+	if (err != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			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;
+	num_output_pins = video_binary->info->num_output_pins;
+
+	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 != NULL && copy_binary->info != NULL);
+	need_vf_pp  = (vf_pp_binary != NULL && vf_pp_binary->info != NULL);
+	need_yuv_pp = (yuv_scaler_binary != NULL && yuv_scaler_binary->info != NULL);
+
+	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 != IA_CSS_SUCCESS)
+			goto ERR;
+		in_frame = me->stages->args.out_frame[0];
+	} else if (pipe->stream->config.continuous) {
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+		/* When continous 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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			goto ERR;
+	}
+	if (video_stage) {
+		int frm;
+#ifndef ISP2401
+		for (frm = 0; frm < NUM_VIDEO_TNR_FRAMES; frm++) {
+#else
+		for (frm = 0; frm < NUM_TNR_FRAMES; frm++) {
+#endif
+			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 != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+			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++) {
+			if (is_output_stage[i] == true) {
+				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_SUCCESS) {
+				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 enum ia_css_err
+create_host_acc_pipeline(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned int i;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->stream == NULL)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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;
+
+{
+	const struct ia_css_fw_info *fw = pipe->vf_stage;
+	for (i = 0; fw; fw = fw->next){
+		err = sh_css_pipeline_add_acc_stage(&pipe->pipeline, fw);
+		if (err != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	bool sensor = false;
+	bool buffered_sensor = false;
+	bool online = false;
+	bool continuous = false;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->stream == NULL) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 Continous 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 != IA_CSS_SUCCESS)
+			goto 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_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			goto ERR;
+		in_frame = me->stages->args.out_frame[0];
+#ifndef ISP2401
+	} else {
+#else
+	} else if (pipe->stream->config.continuous) {
+#endif
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+		/* 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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			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 enum ia_css_err
+preview_start(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me ;
+	struct ia_css_binary *copy_binary, *preview_binary, *vf_pp_binary = NULL;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->stream == NULL) || (pipe->mode != IA_CSS_PIPE_ID_PREVIEW)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	me = &pipe->pipeline;
+
+	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;
+
+	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;
+
+	sh_css_metrics_start_frame();
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	/* multi stream video needs mipi buffers */
+	err = send_mipi_frames(pipe);
+	if (err != IA_CSS_SUCCESS)
+		goto ERR;
+#endif
+	send_raw_frames(pipe);
+
+	{
+		unsigned int thread_id;
+
+		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,
+#ifndef ISP2401
+			&pipe->stream->info.metadata_info
+#else
+			&pipe->stream->info.metadata_info,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+			, pipe->stream->config.source.port.port
+#else
+			pipe->stream->config.source.port.port,
+#endif
+#endif
+#ifndef ISP2401
+			);
+#else
+			&pipe->config.internal_frame_origin_bqs_on_sctbl,
+			pipe->stream->isp_params_configs);
+#endif
+
+		/* 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,
+#ifndef ISP2401
+			&pipe->stream->info.metadata_info
+#else
+			&pipe->stream->info.metadata_info,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+			, (mipi_port_ID_t)0
+#else
+			(mipi_port_ID_t)0,
+#endif
+#endif
+#ifndef ISP2401
+			);
+#else
+			&capture_pipe->config.internal_frame_origin_bqs_on_sctbl,
+			capture_pipe->stream->isp_params_configs);
+#endif
+	}
+
+	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,
+#ifndef ISP2401
+			NULL
+#else
+			NULL,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+			, (mipi_port_ID_t) 0
+#else
+			(mipi_port_ID_t) 0,
+#endif
+#endif
+#ifndef ISP2401
+			);
+#else
+			&pipe->config.internal_frame_origin_bqs_on_sctbl,
+			pipe->stream->isp_params_configs);
+#endif
+	}
+
+	start_pipe(pipe, copy_ovrd, preview_pipe_input_mode);
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+ERR:
+#endif
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+enum ia_css_err
+ia_css_pipe_enqueue_buffer(struct ia_css_pipe *pipe,
+			   const struct ia_css_buffer *buffer)
+{
+	enum ia_css_err return_err = IA_CSS_SUCCESS;
+	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 == NULL) || (buffer == NULL)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	buf_type = buffer->type;
+	/* following code will be enabled when IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME
+	   is removed */
+#if 0
+	if (buf_type == IA_CSS_BUFFER_TYPE_OUTPUT_FRAME) {
+		bool found_pipe = false;
+		for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+			if ((buffer->data.frame->info.res.width == pipe->output_info[i].res.width) &&
+				(buffer->data.frame->info.res.height == pipe->output_info[i].res.height)) {
+				buf_type += i;
+				found_pipe = true;
+				break;
+			}
+		}
+		if (!found_pipe)
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	if (buf_type == IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
+		bool found_pipe = false;
+		for (i = 0; i < IA_CSS_PIPE_MAX_OUTPUT_STAGE; i++) {
+			if ((buffer->data.frame->info.res.width == pipe->vf_output_info[i].res.width) &&
+				(buffer->data.frame->info.res.height == pipe->vf_output_info[i].res.height)) {
+				buf_type += i;
+				found_pipe = true;
+				break;
+			}
+		}
+		if (!found_pipe)
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+#endif
+	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(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	ret_err = ia_css_query_internal_queue_id(buf_type, thread_id, &queue_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if ((queue_id <= SH_CSS_INVALID_QUEUE_ID) || (queue_id >= SH_CSS_MAX_NUM_QUEUES)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LOG("SP is not running!");
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+		/* SP is not running. The queues are not valid */
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+
+	pipeline = &pipe->pipeline;
+
+	assert(pipeline != NULL ||
+	       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 == NULL) {
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+		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 == NULL) {
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+		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 == NULL) {
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+		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 == NULL) {
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+		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);
+
+
+#if CONFIG_ON_FRAME_ENQUEUE()
+		return_err = set_config_on_frame_enqueue(
+				&buffer->data.frame->info,
+				&ddr_buffer.payload.frame);
+		if (IA_CSS_SUCCESS != return_err) {
+			IA_CSS_LEAVE_ERR(return_err);
+			return return_err;
+		}
+#endif
+	}
+
+	/* 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);
+
+	assert(h_vbuf != NULL);
+	assert(h_vbuf->vptr != 0x0);
+
+	if ((h_vbuf == NULL) || (h_vbuf->vptr == 0x0)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	mmgr_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 == NULL) {
+			ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &h_vbuf);
+			IA_CSS_LOG("pipeline is empty!");
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+
+		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 defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+		if ((return_err == IA_CSS_SUCCESS) && (IA_CSS_BUFFER_TYPE_OUTPUT_FRAME == buf_type)) {
+			IA_CSS_LOG("pfp: enqueued OF %d to q %d thread %d",
+				ddr_buffer.payload.frame.frame_data,
+				queue_id, thread_id);
+		}
+#endif
+
+	}
+
+	if (return_err == IA_CSS_SUCCESS) {
+#ifndef ISP2401
+		bool found_record = false;
+		found_record = sh_css_hmm_buffer_record_acquire(
+#else
+		struct sh_css_hmm_buffer_record *hmm_buffer_record = NULL;
+
+		hmm_buffer_record = sh_css_hmm_buffer_record_acquire(
+#endif
+					h_vbuf, buf_type,
+					HOST_ADDRESS(ddr_buffer.kernel_ptr));
+#ifndef ISP2401
+		if (found_record == true) {
+#else
+		if (hmm_buffer_record) {
+#endif
+			IA_CSS_LOG("send vbuf=0x%x", h_vbuf);
+		} else {
+			return_err = IA_CSS_ERR_INTERNAL_ERROR;
+			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 == IA_CSS_SUCCESS) {
+		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(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+			return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+		}
+		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.
+	 */
+enum ia_css_err
+ia_css_pipe_dequeue_buffer(struct ia_css_pipe *pipe,
+			   struct ia_css_buffer *buffer)
+{
+	enum ia_css_err return_err;
+	enum sh_css_queue_id queue_id;
+	hrt_vaddress ddr_buffer_addr = (hrt_vaddress)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 == NULL) || (buffer == NULL)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	ret_err = ia_css_query_internal_queue_id(buf_type, thread_id, &queue_id);
+	if (!ret_err) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if ((queue_id <= SH_CSS_INVALID_QUEUE_ID) || (queue_id >= SH_CSS_MAX_NUM_QUEUES)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		IA_CSS_LOG("SP is not running!");
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+		/* SP is not running. The queues are not valid */
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	return_err = ia_css_bufq_dequeue_buffer(queue_id,
+		(uint32_t *)&ddr_buffer_addr);
+
+	if (return_err == IA_CSS_SUCCESS) {
+		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 != NULL) {
+			/* valid hmm_buffer_record found. Save the kernel_ptr
+			 * for validation after performing mmgr_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%p) buf_type(%d)",
+				 ddr_buffer_addr, buf_type);
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+
+		mmgr_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%p)", kernel_ptr);
+			IA_CSS_ERROR("actual: (0x%p)", HOST_ADDRESS(ddr_buffer.kernel_ptr));
+			IA_CSS_ERROR("buf_type: %d\n", buf_type);
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+
+		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 == true))
+				{
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+					/* 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_SUCCESS) {
+						IA_CSS_LOG("free_mipi_frames() failed");
+						IA_CSS_LEAVE_ERR(return_err);
+						return return_err;
+					}
+#endif
+					pipe->stop_requested = false;
+				}
+			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 USE_INPUT_SYSTEM_VERSION_2401
+					frame->planes.binary.size = frame->data_bytes;
+#else
+					frame->planes.binary.size =
+					    sh_css_sp_get_binary_copy_size();
+#endif
+				}
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+				if (IA_CSS_BUFFER_TYPE_OUTPUT_FRAME == buf_type) {
+					IA_CSS_LOG("pfp: dequeued OF %d with config id %d thread %d",
+						frame->data, frame->isp_config_id, thread_id);
+				}
+#endif
+
+				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 = IA_CSS_ERR_INTERNAL_ERROR;
+				break;
+			}
+		}
+	}
+
+	/*
+	 * Tell the SP which queues are not full,
+	 * by sending the software event.
+	 */
+	if (return_err == IA_CSS_SUCCESS){
+		if (!sh_css_sp_is_running()) {
+			IA_CSS_LOG("SP is not running!");
+			IA_CSS_LEAVE_ERR(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+			/* SP is not running. The queues are not valid */
+			return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+		}
+		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. */
+};
+
+enum ia_css_err
+ia_css_dequeue_event(struct ia_css_event *event)
+{
+	return ia_css_dequeue_psys_event(event);
+}
+
+enum ia_css_err
+ia_css_dequeue_psys_event(struct ia_css_event *event)
+{
+	enum ia_css_pipe_id pipe_id = 0;
+	uint8_t payload[4] = {0,0,0,0};
+	enum ia_css_err 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 == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* dequeue the event (if any) from the psys event queue */
+	ret_err = ia_css_bufq_dequeue_psys_event(payload);
+	if (ret_err != IA_CSS_SUCCESS)
+		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 = IA_CSS_CSI2_PORT0;
+	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 */
+		uint32_t 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 != IA_CSS_SUCCESS) {
+			/* 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];
+		}
+		/* 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 recieved, 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. */
+		else {
+			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 ia_css_csi2_port)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 IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+
+		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. */
+			uint32_t 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_SUCCESS) {
+				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 IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_dequeue_isys_event(struct ia_css_event *event)
+{
+	uint8_t payload[4] = {0, 0, 0, 0};
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	/* We skip the IA_CSS_ENTER logging call
+	 * to avoid flooding the logs when the host application
+	 * uses polling. */
+	if (event == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	err = ia_css_bufq_dequeue_isys_event(payload);
+	if (err != IA_CSS_SUCCESS)
+		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 != NULL);
+	assert(pipe->stream != NULL);
+
+	start_pipe(pipe, SH_CSS_PIPE_CONFIG_OVRD_NO_OVRD,
+			pipe->stream->config.mode);
+}
+
+static enum ia_css_err
+sh_css_pipe_start(struct ia_css_stream *stream)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	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 == NULL) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	pipe = stream->last_pipe;
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	pipe_id = pipe->mode;
+
+	if(stream->started == true) {
+		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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	/* 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 && IA_CSS_SUCCESS == 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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+			}
+		}
+	}
+	if (err != IA_CSS_SUCCESS) {
+		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_SUCCESS) {
+			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(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+		/* SP is not running. The queues are not valid */
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+	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 == NULL) {
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+		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 == NULL) {
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+		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;
+}
+
+#ifndef ISP2401
+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)
+#else
+/**
+ * @brief Stop all "ia_css_pipe" instances in the target
+ * "ia_css_stream" instance.
+ *
+ * Refer to "Local prototypes" for more info.
+ */
+static enum ia_css_err
+sh_css_pipes_stop(struct ia_css_stream *stream)
+#endif
+{
+#ifndef ISP2401
+	struct ia_css_pipe *pipe;
+	bool continuous;
+#else
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_pipe *main_pipe;
+	enum ia_css_pipe_id main_pipe_id;
+	int i;
+#endif
+
+#ifndef ISP2401
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "sh_css_continuous_is_enabled() enter: pipe_num=%d\n", pipe_num);
+#else
+	assert(stream != NULL);
+	if (stream == NULL) {
+		IA_CSS_LOG("stream does NOT exist!");
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+		goto ERR;
+	}
+#endif
+
+#ifndef ISP2401
+	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;
+}
+#else
+	main_pipe = stream->last_pipe;
+	assert(main_pipe != NULL);
+	if (main_pipe == NULL) {
+		IA_CSS_LOG("main_pipe does NOT exist!");
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+		goto ERR;
+	}
+#endif
+
+#ifndef ISP2401
+enum ia_css_err
+ia_css_stream_get_max_buffer_depth(struct ia_css_stream *stream, int *buffer_depth)
+{
+	if (buffer_depth == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	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 IA_CSS_SUCCESS;
+}
+#else
+	main_pipe_id = main_pipe->mode;
+	IA_CSS_ENTER_PRIVATE("main_pipe_id=%d", main_pipe_id);
+#endif
+
+#ifndef ISP2401
+enum ia_css_err
+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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	/* ok, value allowed */
+	stream->config.target_num_cont_raw_buf = buffer_depth;
+	/* TODO: check what to regarding initialization */
+	return IA_CSS_SUCCESS;
+}
+#else
+	/**
+	 * Stop all "ia_css_pipe" instances in this target
+	 * "ia_css_stream" instance.
+	 */
+	for (i = 0; i < stream->num_pipes; i++) {
+		/* send the "stop" request to the "ia_css_pipe" instance */
+		IA_CSS_LOG("Send the stop-request to the pipe: pipe_id=%d",
+				stream->pipes[i]->pipeline.pipe_id);
+		err = ia_css_pipeline_request_stop(&stream->pipes[i]->pipeline);
+#endif
+
+#ifndef ISP2401
+enum ia_css_err
+ia_css_stream_get_buffer_depth(struct ia_css_stream *stream, int *buffer_depth)
+{
+	if (buffer_depth == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_get_buffer_depth() enter: void\n");
+#else
+		/*
+		 * Exit this loop if "ia_css_pipeline_request_stop()"
+		 * returns the error code.
+		 *
+		 * The error code would be generated in the following
+		 * two cases:
+		 * (1) The Scalar Processor has already been stopped.
+		 * (2) The "Host->SP" event queue is full.
+		 *
+		 * As the convention of using CSS API 2.0/2.1, such CSS
+		 * error code would be propogated from the CSS-internal
+		 * API returned value to the CSS API returned value. Then
+		 * the CSS driver should capture these error code and
+		 * handle it in the driver exception handling mechanism.
+		 */
+		if (err != IA_CSS_SUCCESS) {
+			goto ERR;
+		}
+	}
+
+	/**
+	 * In the CSS firmware use scenario "Continuous Preview"
+	 * as well as "Continuous Video", the "ia_css_pipe" instance
+	 * "Copy Pipe" is activated. This "Copy Pipe" is private to
+	 * the CSS firmware so that it is not listed in the target
+	 * "ia_css_stream" instance.
+	 *
+	 * We need to stop this "Copy Pipe", as well.
+	 */
+	if (main_pipe->stream->config.continuous) {
+		struct ia_css_pipe *copy_pipe = NULL;
+
+		/* get the reference to "Copy Pipe" */
+		if (main_pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			copy_pipe = main_pipe->pipe_settings.preview.copy_pipe;
+		else if (main_pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			copy_pipe = main_pipe->pipe_settings.video.copy_pipe;
+
+		/* return the error code if "Copy Pipe" does NOT exist */
+		assert(copy_pipe != NULL);
+		if (copy_pipe == NULL) {
+			IA_CSS_LOG("Copy Pipe does NOT exist!");
+			err = IA_CSS_ERR_INTERNAL_ERROR;
+			goto ERR;
+		}
+
+		/* send the "stop" request to "Copy Pipe" */
+		IA_CSS_LOG("Send the stop-request to the pipe: pipe_id=%d",
+				copy_pipe->pipeline.pipe_id);
+		err = ia_css_pipeline_request_stop(&copy_pipe->pipeline);
+	}
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+/**
+ * @brief Check if all "ia_css_pipe" instances in the target
+ * "ia_css_stream" instance have stopped.
+ *
+ * Refer to "Local prototypes" for more info.
+ */
+static bool
+sh_css_pipes_have_stopped(struct ia_css_stream *stream)
+{
+	bool rval = true;
+
+	struct ia_css_pipe *main_pipe;
+	enum ia_css_pipe_id main_pipe_id;
+
+	int i;
+
+	assert(stream != NULL);
+	if (stream == NULL) {
+		IA_CSS_LOG("stream does NOT exist!");
+		rval = false;
+		goto RET;
+	}
+
+	main_pipe = stream->last_pipe;
+	assert(main_pipe != NULL);
+
+	if (main_pipe == NULL) {
+		IA_CSS_LOG("main_pipe does NOT exist!");
+		rval = false;
+		goto RET;
+	}
+
+	main_pipe_id = main_pipe->mode;
+	IA_CSS_ENTER_PRIVATE("main_pipe_id=%d", main_pipe_id);
+
+	/**
+	 * Check if every "ia_css_pipe" instance in this target
+	 * "ia_css_stream" instance has stopped.
+	 */
+	for (i = 0; i < stream->num_pipes; i++) {
+		rval = rval && ia_css_pipeline_has_stopped(&stream->pipes[i]->pipeline);
+		IA_CSS_LOG("Pipe has stopped: pipe_id=%d, stopped=%d",
+				stream->pipes[i]->pipeline.pipe_id,
+				rval);
+	}
+
+	/**
+	 * In the CSS firmware use scenario "Continuous Preview"
+	 * as well as "Continuous Video", the "ia_css_pipe" instance
+	 * "Copy Pipe" is activated. This "Copy Pipe" is private to
+	 * the CSS firmware so that it is not listed in the target
+	 * "ia_css_stream" instance.
+	 *
+	 * We need to check if this "Copy Pipe" has stopped, as well.
+	 */
+	if (main_pipe->stream->config.continuous) {
+		struct ia_css_pipe *copy_pipe = NULL;
+
+		/* get the reference to "Copy Pipe" */
+		if (main_pipe_id == IA_CSS_PIPE_ID_PREVIEW)
+			copy_pipe = main_pipe->pipe_settings.preview.copy_pipe;
+		else if (main_pipe_id == IA_CSS_PIPE_ID_VIDEO)
+			copy_pipe = main_pipe->pipe_settings.video.copy_pipe;
+
+		/* return if "Copy Pipe" does NOT exist */
+		assert(copy_pipe != NULL);
+		if (copy_pipe == NULL) {
+			IA_CSS_LOG("Copy Pipe does NOT exist!");
+
+			rval = false;
+			goto RET;
+		}
+
+		/* check if "Copy Pipe" has stopped or not */
+		rval = rval && ia_css_pipeline_has_stopped(&copy_pipe->pipeline);
+		IA_CSS_LOG("Pipe has stopped: pipe_id=%d, stopped=%d",
+				copy_pipe->pipeline.pipe_id,
+				rval);
+	}
+
+RET:
+	IA_CSS_LEAVE_PRIVATE("rval=%d", rval);
+	return rval;
+}
+
+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;
+}
+
+enum ia_css_err
+ia_css_stream_get_max_buffer_depth(struct ia_css_stream *stream, int *buffer_depth)
+{
+	if (buffer_depth == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	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 IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	/* ok, value allowed */
+	stream->config.target_num_cont_raw_buf = buffer_depth;
+	/* TODO: check what to regarding initialization */
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+ia_css_stream_get_buffer_depth(struct ia_css_stream *stream, int *buffer_depth)
+{
+	if (buffer_depth == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_get_buffer_depth() enter: void\n");
+#endif
+	(void)stream;
+	*buffer_depth = stream->config.target_num_cont_raw_buf;
+	return IA_CSS_SUCCESS;
+}
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+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 enum ia_css_err 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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, width = %d, height = %d, paddaed width = %d, format = %d, idx = %d",
+			     pipe, width, height, padded_width, format, idx);
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	err = ia_css_util_check_res(width, height);
+	if (err != IA_CSS_SUCCESS) {
+		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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+sh_css_pipe_get_shading_info(struct ia_css_pipe *pipe,
+#ifndef ISP2401
+			     struct ia_css_shading_info *info)
+#else
+			     struct ia_css_shading_info *shading_info,
+			     struct ia_css_pipe_config *pipe_config)
+#endif
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe != NULL);
+#ifndef ISP2401
+	assert(info != NULL);
+#else
+	assert(shading_info != NULL);
+	assert(pipe_config != NULL);
+#endif
+	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,
+#ifndef ISP2401
+			info);
+#else
+			shading_info, pipe_config);
+#endif
+		/* 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 IA_CSS_SUCCESS.
+		 */
+#ifndef ISP2401
+		memset(info, 0, sizeof(*info));
+#else
+		memset(shading_info, 0, sizeof(*shading_info));
+#endif
+	}
+	return err;
+}
+
+static enum ia_css_err
+sh_css_pipe_get_grid_info(struct ia_css_pipe *pipe,
+			  struct ia_css_grid_info *info)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_binary *binary = NULL;
+
+	assert(pipe != NULL);
+	assert(info != NULL);
+
+	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 != IA_CSS_SUCCESS)
+			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.dvs_grid_info, 0,
+			   sizeof(info->dvs_grid.dvs_grid_info));
+		memset(&info->dvs_grid.dvs_stat_grid_info, 0,
+			   sizeof(info->dvs_grid.dvs_stat_grid_info));
+	}
+
+	if (binary != NULL) {
+		/* 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;
+	}
+
+#if defined(HAS_VAMEM_VERSION_2)
+	info->vamem_type = IA_CSS_VAMEM_TYPE_2;
+#elif defined(HAS_VAMEM_VERSION_1)
+	info->vamem_type = IA_CSS_VAMEM_TYPE_1;
+#else
+#error "Unknown VAMEM version"
+#endif
+
+ERR:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+#ifdef ISP2401
+/**
+ * @brief Check if a format is supported by the pipe.
+ *
+ */
+static enum ia_css_err
+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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_SUCCESS);
+		return IA_CSS_SUCCESS;
+	}
+}
+#endif
+
+static enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool continuous = pipe->stream->config.continuous;
+	unsigned int i;
+	unsigned 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 != NULL);
+	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 IA_CSS_SUCCESS;
+
+	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 != NULL);
+
+	/*
+	 * 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 != IA_CSS_SUCCESS)
+		return err;
+	/* cannot have online video and input_mode memory */
+	if (online && pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	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_SUCCESS)
+			return err;
+	} else {
+		err = ia_css_frame_check_info(pipe_out_info);
+		if (err != IA_CSS_SUCCESS)
+			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
+			= IA_CSS_DEFAULT_CAS_BINARY_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 != IA_CSS_SUCCESS)
+			return err;
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kzalloc(cas_scaler_descr.num_stage *
+			sizeof(struct ia_css_binary), GFP_KERNEL);
+		if (mycs->yuv_scaler_binary == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			return err;
+		}
+		mycs->is_output_stage = kzalloc(cas_scaler_descr.num_stage
+					* sizeof(bool), GFP_KERNEL);
+		if (mycs->is_output_stage == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			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_SUCCESS) {
+				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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS) {
+			if (video_vf_info) {
+				/* This will do another video binary lookup later for YUV_LINE format*/
+				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 additiona 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 != IA_CSS_SUCCESS)
+				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(USE_INPUT_SYSTEM_VERSION_2401)
+	/* 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 != IA_CSS_SUCCESS)
+			return err;
+	}
+#else
+	(void)continuous;
+#endif
+
+#if !defined(HAS_OUTPUT_SYSTEM)
+	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 == true);
+			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 != IA_CSS_SUCCESS)
+			return err;
+	}
+#endif
+
+	err = allocate_delay_frames(pipe);
+
+	if (err != IA_CSS_SUCCESS)
+		return err;
+
+	if (mycs->video_binary.info->sp.enable.block_output) {
+#ifdef ISP2401
+		unsigned int tnr_width;
+		unsigned int tnr_height;
+#endif
+		tnr_info = mycs->video_binary.out_frame_info[0];
+#ifdef ISP2401
+
+		/* Select resolution for TNR. If
+		 * output_system_in_resolution(GDC_out_resolution) is
+		 * being used, then select that as it will also be in resolution for
+		 * TNR. At present, it only make sense for Skycam */
+		if (pipe->config.output_system_in_res.width && pipe->config.output_system_in_res.height) {
+			tnr_width = pipe->config.output_system_in_res.width;
+			tnr_height = pipe->config.output_system_in_res.height;
+		} else {
+			tnr_width = tnr_info.res.width;
+			tnr_height = tnr_info.res.height;
+		}
+
+		/* Make tnr reference buffers output block width(in pix) align */
+		tnr_info.res.width  =
+			CEIL_MUL(tnr_width,
+			 (mycs->video_binary.info->sp.block.block_width * ISP_NWAY));
+		tnr_info.padded_width = tnr_info.res.width;
+
+#endif
+		/* Make tnr reference buffers output block height align */
+		tnr_info.res.height =
+#ifndef ISP2401
+			CEIL_MUL(tnr_info.res.height,
+#else
+			CEIL_MUL(tnr_height,
+#endif
+			 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;
+
+#ifndef ISP2401
+	for (i = 0; i < NUM_VIDEO_TNR_FRAMES; i++) {
+#else
+	for (i = 0; i < NUM_TNR_FRAMES; i++) {
+#endif
+		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 != IA_CSS_SUCCESS)
+			return err;
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+unload_video_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((pipe == NULL) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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);
+#ifndef ISP2401
+	ia_css_binary_unload(&pipe->pipe_settings.video.vf_pp_binary);
+#endif
+
+	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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err video_start(struct ia_css_pipe *pipe)
+{
+	struct ia_css_binary *copy_binary;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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;
+
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->mode != IA_CSS_PIPE_ID_VIDEO)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	video_pipe_input_mode = pipe->stream->config.mode;
+
+	copy_pipe    = pipe->pipe_settings.video.copy_pipe;
+	capture_pipe = pipe->pipe_settings.video.capture_pipe;
+
+	copy_binary  = &pipe->pipe_settings.video.copy_binary;
+
+	sh_css_metrics_start_frame();
+
+	/* multi stream video needs mipi buffers */
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	err = send_mipi_frames(pipe);
+	if (err != IA_CSS_SUCCESS)
+		return err;
+#endif
+
+	send_raw_frames(pipe);
+	{
+		unsigned int thread_id;
+
+		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,
+#ifndef ISP2401
+			&pipe->stream->info.metadata_info
+#else
+			&pipe->stream->info.metadata_info,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+			, pipe->stream->config.source.port.port
+#else
+			pipe->stream->config.source.port.port,
+#endif
+#endif
+#ifndef ISP2401
+			);
+#else
+			&copy_pipe->config.internal_frame_origin_bqs_on_sctbl,
+			copy_pipe->stream->isp_params_configs);
+#endif
+
+		/* 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,
+#ifndef ISP2401
+			&pipe->stream->info.metadata_info
+#else
+			&pipe->stream->info.metadata_info,
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#ifndef ISP2401
+			, (mipi_port_ID_t)0
+#else
+			(mipi_port_ID_t)0,
+#endif
+#endif
+#ifndef ISP2401
+			);
+#else
+			&capture_pipe->config.internal_frame_origin_bqs_on_sctbl,
+			capture_pipe->stream->isp_params_configs);
+#endif
+	}
+
+	start_pipe(pipe, copy_ovrd, video_pipe_input_mode);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static
+enum ia_css_err sh_css_pipe_get_viewfinder_frame_info(
+	struct ia_css_pipe *pipe,
+	struct ia_css_frame_info *info,
+	unsigned int idx)
+{
+	assert(pipe != NULL);
+	assert(info != NULL);
+
+/* 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 IA_CSS_ERR_MODE_HAS_NO_VIEWFINDER;
+	/* 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 IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	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 == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+
+	err = ia_css_util_check_res(width, height);
+	if (err != IA_CSS_SUCCESS) {
+	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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err load_copy_binaries(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(pipe != NULL);
+	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 IA_CSS_SUCCESS;
+
+	err = ia_css_frame_check_info(&pipe->output_info[0]);
+	if (err != IA_CSS_SUCCESS)
+		goto ERR;
+
+	err = verify_copy_out_frame_format(pipe);
+	if (err != IA_CSS_SUCCESS)
+		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 != NULL);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE);
+#ifdef ISP2401
+
+	/* ldc and capture_pp are not supported in the same pipeline */
+	if (need_capt_ldc(pipe) == true)
+		return false;
+#endif
+	/* 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 != NULL);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE);
+	return (pipe->extra_config.enable_dvs_6axis) ? true:false;
+}
+
+static enum ia_css_err set_num_primary_stages(unsigned int *num, enum ia_css_pipe_version version)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	if (num == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+		break;
+	}
+
+	return err;
+}
+
+static enum ia_css_err load_primary_binaries(
+	struct ia_css_pipe *pipe)
+{
+	bool online = false;
+	bool memory = false;
+	bool continuous = false;
+	bool need_pp = false;
+	bool need_isp_copy_binary = false;
+	bool need_ldc = false;
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	bool sensor = false;
+#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,
+#ifndef ISP2401
+				 *pipe_vf_out_info, *capt_pp_in_info,
+				 capt_ldc_out_info;
+#else
+				 *pipe_vf_out_info;
+#endif
+#if defined(HAS_RES_MGR)
+	struct ia_css_frame_info bds_out_info;
+#endif
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_capture_settings *mycs;
+	unsigned int i;
+	bool need_extra_yuv_scaler = false;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+	assert(pipe->mode == IA_CSS_PIPE_ID_CAPTURE || pipe->mode == IA_CSS_PIPE_ID_COPY);
+
+	online = pipe->stream->config.online;
+	memory = pipe->stream->config.mode == IA_CSS_INPUT_MODE_MEMORY;
+	continuous = pipe->stream->config.continuous;
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	sensor = (pipe->stream->config.mode == IA_CSS_INPUT_MODE_SENSOR);
+#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 IA_CSS_SUCCESS;
+
+	err = set_num_primary_stages(&mycs->num_primary_stage, pipe->config.isp_pipe_version);
+	if (err != IA_CSS_SUCCESS) {
+		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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+	else{
+		err = ia_css_frame_check_info(pipe_out_info);
+		if (err != IA_CSS_SUCCESS) {
+			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);
+
+/*
+ * 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.
+ * */
+	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
+			= IA_CSS_DEFAULT_CAS_BINARY_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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kzalloc(cas_scaler_descr.num_stage *
+			sizeof(struct ia_css_binary), GFP_KERNEL);
+		if (mycs->yuv_scaler_binary == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		mycs->is_output_stage = kzalloc(cas_scaler_descr.num_stage *
+			sizeof(bool), GFP_KERNEL);
+		if (mycs->is_output_stage == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			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]);
+#if defined(HAS_RES_MGR)
+			bds_out_info.res = pipe->config.bayer_ds_out_res;
+			yuv_scaler_descr.bds_out_info = &bds_out_info;
+#endif
+			err = ia_css_binary_find(&yuv_scaler_descr,
+						&mycs->yuv_scaler_binary[i]);
+			if (err != IA_CSS_SUCCESS) {
+				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);
+#ifdef ISP2401
+	/* ldc and capt_pp are not supported in the same pipeline */
+	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_pp_out_info);
+#endif
+
+#ifdef ISP2401
+		err = ia_css_binary_find(&capt_ldc_descr,
+					&mycs->capture_ldc_binary);
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	} else if (need_pp) {
+#endif
+	/* we build up the pipeline starting at the end */
+	/* Capture post-processing */
+#ifndef ISP2401
+	if (need_pp) {
+#endif
+		struct ia_css_binary_descr capture_pp_descr;
+#ifndef ISP2401
+		capt_pp_in_info = need_ldc ? &capt_ldc_out_info : &prim_out_info;
+#endif
+
+		ia_css_pipe_get_capturepp_binarydesc(pipe,
+#ifndef ISP2401
+			&capture_pp_descr, capt_pp_in_info,
+#else
+			&capture_pp_descr, &prim_out_info,
+#endif
+			&capt_pp_out_info, &vf_info);
+#if defined(HAS_RES_MGR)
+			bds_out_info.res = pipe->config.bayer_ds_out_res;
+			capture_pp_descr.bds_out_info = &bds_out_info;
+#endif
+		err = ia_css_binary_find(&capture_pp_descr,
+					&mycs->capture_pp_binary);
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+#ifndef ISP2401
+
+		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_SUCCESS) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+#endif
+	} else {
+		prim_out_info = *pipe_out_info;
+	}
+
+	/* Primary */
+	{
+		struct ia_css_binary_descr prim_descr[MAX_NUM_PRIMARY_STAGES];
+
+		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);
+#if defined(HAS_RES_MGR)
+			bds_out_info.res = pipe->config.bayer_ds_out_res;
+			prim_descr[i].bds_out_info = &bds_out_info;
+#endif
+			err = ia_css_binary_find(&prim_descr[i], &mycs->primary_binary[i]);
+			if (err != IA_CSS_SUCCESS) {
+				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);
+#if defined(HAS_RES_MGR)
+		bds_out_info.res = pipe->config.bayer_ds_out_res;
+		vf_pp_descr.bds_out_info = &bds_out_info;
+#endif
+		err = ia_css_binary_find(&vf_pp_descr, &mycs->vf_pp_binary);
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+	err = allocate_delay_frames(pipe);
+
+	if (err != IA_CSS_SUCCESS)
+		return err;
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	/* 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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum ia_css_pipe_id mode = IA_CSS_PIPE_ID_VIDEO;
+	struct ia_css_frame **delay_frames = NULL;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (pipe == NULL) {
+		IA_CSS_ERROR("Invalid args - pipe %x", pipe);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	}
+
+	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 != IA_CSS_SUCCESS)
+			return err;
+	}
+	IA_CSS_LEAVE_PRIVATE("");
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	assert(pipe != NULL);
+	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 IA_CSS_SUCCESS;
+	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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			return err;
+	}
+
+	/* Copy */
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 enum ia_css_err load_bayer_isp_binaries(
+	struct ia_css_pipe *pipe)
+{
+	struct ia_css_frame_info pre_isp_in_info, *pipe_out_info;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_binary_descr pre_de_descr;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	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 IA_CSS_SUCCESS;
+
+	err = ia_css_frame_check_info(pipe_out_info);
+	if (err != IA_CSS_SUCCESS)
+		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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	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 IA_CSS_SUCCESS;
+	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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			return err;
+	}
+
+	/* Copy */
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 != NULL);
+	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 == IA_CSS_STREAM_FORMAT_BINARY_8) ||
+		(pipe->config.mode == IA_CSS_PIPE_MODE_COPY));
+
+	return rval;
+}
+
+static enum ia_css_err load_capture_binaries(
+	struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool must_be_raw;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	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(IA_CSS_SUCCESS);
+		return IA_CSS_SUCCESS;
+	}
+
+	/* 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_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (copy_on_sp(pipe) &&
+	    pipe->stream->config.input_config.format == IA_CSS_STREAM_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(IA_CSS_SUCCESS);
+		return IA_CSS_SUCCESS;
+	}
+
+	switch (pipe->config.default_capture_config.mode) {
+	case IA_CSS_CAPTURE_MODE_RAW:
+		err = load_copy_binaries(pipe);
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+	  if (err == IA_CSS_SUCCESS)
+		  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_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static enum ia_css_err
+unload_capture_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((pipe == NULL) || ((pipe->mode != IA_CSS_PIPE_ID_CAPTURE) && (pipe->mode != IA_CSS_PIPE_ID_COPY))) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+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 != NULL);
+	assert(pipe->mode == IA_CSS_PIPE_ID_YUVPP);
+
+	/* TODO: make generic function */
+	need_format_conversion =
+		((pipe->stream->config.input_config.format == IA_CSS_STREAM_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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_frame_info tmp_in_info = IA_CSS_BINARY_DEFAULT_FRAME_INFO;
+
+	unsigned max_scale_factor_per_stage = MAX_PREFERRED_YUV_DS_PER_STEP;
+
+	assert(cas_scaler_in_info != NULL);
+	assert(cas_scaler_out_info != NULL);
+
+	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 == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->internal_out_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->internal_out_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->out_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->out_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->vf_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->vf_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->is_output_stage = kmalloc(descr->num_stage * sizeof(bool), GFP_KERNEL);
+	if (descr->is_output_stage == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		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 != NULL) {
+				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 enum ia_css_err 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	unsigned 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] != NULL) {
+			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(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->in_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->internal_out_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->internal_out_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->out_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->out_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->vf_info = kmalloc(descr->num_stage * sizeof(struct ia_css_frame_info), GFP_KERNEL);
+	if (descr->vf_info == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		goto ERR;
+	}
+	descr->is_output_stage = kmalloc(descr->num_stage * sizeof(bool), GFP_KERNEL);
+	if (descr->is_output_stage == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		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] != NULL);
+
+		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] != NULL) {
+				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 enum ia_css_err
+load_yuvpp_binaries(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 = IA_CSS_DEFAULT_CAS_BINARY_DESCR;
+	unsigned int i, j;
+	bool need_isp_copy_binary = false;
+
+	IA_CSS_ENTER_PRIVATE("");
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+	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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+			goto ERR;
+		mycs->num_output = cas_scaler_descr.num_output_stage;
+		mycs->num_yuv_scaler = cas_scaler_descr.num_stage;
+		mycs->yuv_scaler_binary = kzalloc(cas_scaler_descr.num_stage *
+			sizeof(struct ia_css_binary), GFP_KERNEL);
+		if (mycs->yuv_scaler_binary == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			goto ERR;
+		}
+		mycs->is_output_stage = kzalloc(cas_scaler_descr.num_stage *
+			sizeof(bool), GFP_KERNEL);
+		if (mycs->is_output_stage == NULL) {
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			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 != IA_CSS_SUCCESS)
+				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(USE_INPUT_SYSTEM_VERSION_2401)
+	/*
+	 * NOTES
+	 * - Why does the "yuvpp" pipe needs "isp_copy_binary" (i.e. ISP Copy) when
+	 *   its input is "IA_CSS_STREAM_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 == IA_CSS_STREAM_FORMAT_YUV422_8);
+#else  /* !USE_INPUT_SYSTEM_VERSION_2401 */
+	need_isp_copy_binary = true;
+#endif /*  USE_INPUT_SYSTEM_VERSION_2401 */
+
+	if (need_isp_copy_binary) {
+		err = load_copy_binary(pipe,
+				       &mycs->copy_binary,
+				       next_binary);
+
+		if (err != IA_CSS_SUCCESS)
+			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 = kzalloc(mycs->num_vf_pp * sizeof(struct ia_css_binary),
+						GFP_KERNEL);
+	if (mycs->vf_pp_binary == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		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 != IA_CSS_SUCCESS)
+					goto ERR;
+			}
+		}
+	}
+
+	if (err != IA_CSS_SUCCESS)
+		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 enum ia_css_err
+unload_yuvpp_binaries(struct ia_css_pipe *pipe)
+{
+	unsigned int i;
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if ((pipe == NULL) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err yuvpp_start(struct ia_css_pipe *pipe)
+{
+	struct ia_css_binary *copy_binary;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum sh_css_pipe_config_override copy_ovrd;
+	enum ia_css_input_mode yuvpp_pipe_input_mode;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	yuvpp_pipe_input_mode = pipe->stream->config.mode;
+
+	copy_binary  = &pipe->pipe_settings.yuvpp.copy_binary;
+
+	sh_css_metrics_start_frame();
+
+	/* multi stream video needs mipi buffers */
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && ( defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401) )
+	err = send_mipi_frames(pipe);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+#endif
+
+	{
+		unsigned int thread_id;
+
+		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 enum ia_css_err
+sh_css_pipe_unload_binaries(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	/* 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(IA_CSS_SUCCESS);
+		return IA_CSS_SUCCESS;
+	}
+
+	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 enum ia_css_err
+sh_css_pipe_load_binaries(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(pipe != NULL);
+	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 = IA_CSS_ERR_INTERNAL_ERROR;
+		break;
+	}
+	if (err != IA_CSS_SUCCESS) {
+		if (sh_css_pipe_unload_binaries(pipe) != IA_CSS_SUCCESS) {
+			/* currently css does not support multiple error returns in a single function,
+			 * using IA_CSS_ERR_INTERNAL_ERROR in this case */
+			err = IA_CSS_ERR_INTERNAL_ERROR;
+		}
+	}
+	return err;
+}
+
+static enum ia_css_err
+create_host_yuvpp_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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_vf_pp_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 USE_INPUT_SYSTEM_VERSION_2401
+	bool sensor = false;
+	bool buffered_sensor = false;
+	bool online = false;
+	bool continuous = false;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if ((pipe == NULL) || (pipe->stream == NULL) || (pipe->mode != IA_CSS_PIPE_ID_YUVPP)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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_vf_pp_stage   = pipe->pipe_settings.yuvpp.num_vf_pp;
+	num_output_stage   = pipe->pipe_settings.yuvpp.num_output;
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 Continous 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 == IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY) {
+			in_frame_format = IA_CSS_FRAME_FORMAT_CSI_MIPI_LEGACY_YUV420_8;
+		} else if (pipe->stream->config.input_config.format == IA_CSS_STREAM_FORMAT_YUV422_8) {
+			/*
+			 * When the sensor output frame format is "IA_CSS_STREAM_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_SUCCESS) {
+			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_SUCCESS) {
+				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_SUCCESS) {
+				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 USE_INPUT_SYSTEM_VERSION_2401
+		/* 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_SUCCESS) {
+			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] == true) {
+				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_SUCCESS) {
+				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] == true) {
+				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_SUCCESS) {
+						IA_CSS_LEAVE_ERR_PRIVATE(err);
+						return err;
+					}
+				}
+				j++;
+			}
+		}
+	} else if (copy_stage != NULL) {
+		if (vf_frame[0] != NULL && 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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+	ia_css_pipeline_finalize_stages(&pipe->pipeline, pipe->stream->config.continuous);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_SUCCESS);
+
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+create_host_copy_pipeline(struct ia_css_pipe *pipe,
+    unsigned max_input_width,
+    struct ia_css_frame *out_frame)
+{
+	struct ia_css_pipeline *me;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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->contiguous = false;
+	out_frame->flash_state = IA_CSS_FRAME_FLASH_STATE_NONE;
+
+	if (copy_on_sp(pipe) &&
+	    pipe->stream->config.input_config.format == IA_CSS_STREAM_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 enum ia_css_err
+create_host_isyscopy_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me = &pipe->pipeline;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 != IA_CSS_SUCCESS)
+		return err;
+	out_frame->contiguous = false;
+	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 != IA_CSS_SUCCESS)
+		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 enum ia_css_err
+create_host_regular_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	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 != NULL);
+	assert(pipe->stream != NULL);
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	/* 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 Continous 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_SUCCESS) {
+			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_SUCCESS) {
+		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(IA_CSS_ERR_INTERNAL_ERROR);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+	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 != NULL && yuv_scaler_binary->info != NULL);
+	need_ldc = (capture_ldc_binary != NULL && capture_ldc_binary->info != NULL);
+
+	if (pipe->pipe_settings.capture.copy_binary.info) {
+		if (raw) {
+			ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+			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_SUCCESS) {
+			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) {
+		unsigned int frm;
+		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_SUCCESS) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				return err;
+			}
+		}
+		(void)frm;
+		/* 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_SUCCESS) {
+			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_SUCCESS) {
+			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_SUCCESS) {
+			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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+#ifndef ISP2401
+	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,
+#else
+	/* ldc and capture_pp not supported in same pipeline */
+	if (need_ldc && current_stage) {
+		in_frame = current_stage->args.out_frame[0];
+		ia_css_pipe_util_set_output_frames(out_frames, 0, out_frame);
+		ia_css_pipe_get_generic_stage_desc(&stage_desc, capture_ldc_binary,
+			out_frames, in_frame, NULL);
+		err = ia_css_pipeline_create_and_add_stage(me,
+			&stage_desc,
+			NULL);
+	} else if (need_pp && current_stage) {
+		in_frame = current_stage->args.out_frame[0];
+		err = add_capture_pp_stage(pipe, me, in_frame, need_yuv_pp ? NULL : out_frame,
+#endif
+					   capture_pp_binary,
+					   &current_stage);
+		if (err != IA_CSS_SUCCESS) {
+			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] == true)
+				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_SUCCESS) {
+				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_SUCCESS) {
+			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 IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+create_host_capture_pipeline(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	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_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+
+	return err;
+}
+
+static enum ia_css_err capture_start(
+	struct ia_css_pipe *pipe)
+{
+	struct ia_css_pipeline *me;
+
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum sh_css_pipe_config_override copy_ovrd;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p", pipe);
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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(USE_INPUT_SYSTEM_VERSION_2)
+	/* old isys: need to send_mipi_frames() in all pipe modes */
+	err = send_mipi_frames(pipe);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+#elif defined(USE_INPUT_SYSTEM_VERSION_2401)
+	if (pipe->config.mode != IA_CSS_PIPE_MODE_COPY) {
+		err = send_mipi_frames(pipe);
+		if (err != IA_CSS_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+	}
+
+#endif
+
+	{
+		unsigned int thread_id;
+
+		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(HAS_NO_INPUT_SYSTEM) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	/*
+	 * 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 enum ia_css_err
+sh_css_pipe_get_output_frame_info(struct ia_css_pipe *pipe,
+				  struct ia_css_frame_info *info,
+				  unsigned int idx)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(pipe != NULL);
+	assert(info != NULL);
+
+	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 == IA_CSS_STREAM_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 err;
+}
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+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 != NULL);
+
+	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 != NULL);
+
+	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 != NULL);
+
+	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 ia_css_stream_format format,
+		const unsigned short *data,
+		unsigned int width)
+{
+	assert(stream != NULL);
+	if (data == NULL || 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 != NULL);
+
+	ia_css_inputfifo_end_frame(stream->config.channel_id);
+}
+#endif
+
+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 == NULL) {
+		IA_CSS_ERROR("NULL fw_info");
+		IA_CSS_LEAVE_PRIVATE("");
+		return;
+	}
+	while (*l)
+		l = &(*l)->next;
+	*l = firmware;
+	/*firmware->next = NULL;*/ /* when multiple acc extensions are loaded, 'next' can be not 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() */
+}
+
+#if !defined(HRT_UNSCHED)
+static enum ia_css_err
+upload_isp_code(struct ia_css_fw_info *firmware)
+{
+	hrt_vaddress binary;
+
+	if (firmware == NULL) {
+		IA_CSS_ERROR("NULL input parameter");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	binary = firmware->info.isp.xmem_addr;
+
+	if (!binary) {
+		unsigned 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 IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	return IA_CSS_SUCCESS;
+}
+#endif
+
+static enum ia_css_err
+acc_load_extension(struct ia_css_fw_info *firmware)
+{
+#if !defined(HRT_UNSCHED)
+	enum ia_css_err err;
+	struct ia_css_fw_info *hd = firmware;
+	while (hd){
+		err = upload_isp_code(hd);
+		if (err != IA_CSS_SUCCESS)
+			return err;
+		hd = hd->next;
+	}
+#endif
+
+	if (firmware == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	firmware->loaded = true;
+	return IA_CSS_SUCCESS;
+}
+
+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 == NULL) /* 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 enum ia_css_err
+ia_css_pipe_load_extension(struct ia_css_pipe *pipe,
+			   struct ia_css_fw_info *firmware)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("fw = %p pipe = %p", firmware, pipe);
+
+	if ((firmware == NULL) || (pipe == NULL)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (firmware->info.isp.type == IA_CSS_ACC_OUTPUT) {
+		if (&pipe->output_stage != NULL)
+			append_firmware(&pipe->output_stage, firmware);
+		else {
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+	}
+	else if (firmware->info.isp.type == IA_CSS_ACC_VIEWFINDER) {
+		if (&pipe->vf_stage != NULL)
+			append_firmware(&pipe->vf_stage, firmware);
+		else {
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		}
+	}
+	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 == NULL) || (pipe == NULL)) {
+		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 != NULL);
+
+	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 enum ia_css_err
+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 */
+	enum ia_css_err	err = IA_CSS_SUCCESS;
+	if (fw->loaded == false)
+		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 == IA_CSS_SUCCESS) {
+		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.
+ */
+enum ia_css_err ia_css_stream_capture_frame(struct ia_css_stream *stream,
+				unsigned int exp_id)
+{
+	struct sh_css_tag_descr tag_descr;
+	uint32_t encoded_tag_descr;
+	enum ia_css_err err;
+
+	assert(stream != NULL);
+	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(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (!sh_css_sp_is_running()) {
+		/* SP is not running. The queues are not valid */
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+		return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* 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.
+ */
+enum ia_css_err 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;
+	enum ia_css_err return_err;
+
+	if (stream == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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",
+		IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* 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 IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	}
+
+	/* 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 != NULL);
+	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;
+#ifndef ISP2401
+	unsigned int HIVE_ADDR_sp_stop_copy_preview;
+#endif
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int o = offsetof(struct host_sp_communication, host2sp_command)
+				/ sizeof(int);
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	unsigned int i;
+#endif
+
+	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;
+#ifndef ISP2401
+	HIVE_ADDR_sp_stop_copy_preview = fw->info.sp.stop_copy_preview;
+#endif
+	HIVE_ADDR_host_sp_com = fw->info.sp.host_sp_com;
+
+	(void)HIVE_ADDR_ia_css_ispctrl_sp_isp_started; /* Suppres warnings in CRUN */
+
+	(void)HIVE_ADDR_sp_sleep_mode;
+	(void)HIVE_ADDR_ia_css_dmaproxy_sp_invalidate_tlb;
+#ifndef ISP2401
+	(void)HIVE_ADDR_sp_stop_copy_preview;
+#endif
+	(void)HIVE_ADDR_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));
+#ifndef ISP2401
+	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));
+#endif
+	store_sp_array_uint(host_sp_com, o, host2sp_cmd_ready);
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	for (i = 0; i < N_CSI_PORTS; i++) {
+		sh_css_update_host2sp_num_mipi_frames
+			(my_css.num_mipi_frames[i]);
+	}
+#endif
+
+	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
+ */
+void ia_css_pipe_config_defaults(struct ia_css_pipe_config *pipe_config)
+{
+	struct ia_css_pipe_config def_config = DEFAULT_PIPE_CONFIG;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_pipe_config_defaults()\n");
+	*pipe_config = def_config;
+}
+
+void
+ia_css_pipe_extra_config_defaults(struct ia_css_pipe_extra_config *extra_config)
+{
+	if (extra_config == NULL) {
+		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 != NULL);
+	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 enum ia_css_err
+ia_css_acc_pipe_create(struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	if (pipe == NULL) {
+		IA_CSS_ERROR("NULL input parameter");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* There is not meaning for num_execs = 0 semantically. Run atleast 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;
+}
+
+enum ia_css_err
+ia_css_pipe_create(const struct ia_css_pipe_config *config,
+		   struct ia_css_pipe **pipe)
+{
+#ifndef ISP2401
+	if (config == NULL)
+#else
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	IA_CSS_ENTER_PRIVATE("config = %p, pipe = %p", config, pipe);
+
+	if (config == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+#endif
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+#ifndef ISP2401
+	if (pipe == NULL)
+#else
+	}
+	if (pipe == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+#endif
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+#ifndef ISP2401
+	return ia_css_pipe_create_extra(config, NULL, pipe);
+#else
+	}
+
+	err = ia_css_pipe_create_extra(config, NULL, pipe);
+
+	if(err == IA_CSS_SUCCESS) {
+		IA_CSS_LOG("pipe created successfuly = %p", *pipe);
+	}
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+
+	return err;
+#endif
+}
+
+enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+	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(IA_CSS_ERR_RESOURCE_EXHAUSTED);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if ((pipe == NULL) || (config == NULL)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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_SUCCESS) {
+		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(IA_CSS_SUCCESS);
+			return IA_CSS_SUCCESS;
+		}
+		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. */
+	if (internal_pipe->extra_config.enable_raw_binning &&
+		 internal_pipe->config.bayer_ds_out_res.width) {
+		/* fill some code here, if no code is needed, please remove it during integration */
+	}
+
+	/* 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_SUCCESS) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				sh_css_free(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_SUCCESS) {
+				IA_CSS_LEAVE_ERR_PRIVATE(err);
+				sh_css_free(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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			sh_css_free(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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+
+enum ia_css_err
+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");
+	assert(pipe_info != NULL);
+	if (pipe_info == NULL) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+			"ia_css_pipe_get_info: pipe_info cannot be NULL\n");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	if (pipe == NULL || pipe->stream == NULL) {
+		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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	/* 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 IA_CSS_SUCCESS;
+}
+
+bool ia_css_pipe_has_dvs_stats(struct ia_css_pipe_info *pipe_info)
+{
+	unsigned int i;
+
+	if (pipe_info != NULL) {
+		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;
+}
+
+#ifdef ISP2401
+enum ia_css_err
+ia_css_pipe_override_frame_format(struct ia_css_pipe *pipe,
+				int pin_index,
+				enum ia_css_frame_format new_format)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("pipe = %p, pin_index = %d, new_formats = %d", pipe, pin_index, new_format);
+
+	if (NULL == pipe) {
+		IA_CSS_ERROR("pipe is not set");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (0 != pin_index && 1 != pin_index) {
+		IA_CSS_ERROR("pin index is not valid");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+	if (IA_CSS_FRAME_FORMAT_NV12_TILEY != new_format) {
+		IA_CSS_ERROR("new format is not valid");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	} else {
+		err = ia_css_pipe_check_format(pipe, new_format);
+		if (IA_CSS_SUCCESS == 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;
+}
+
+#endif
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+/* Configuration of INPUT_SYSTEM_VERSION_2401 is done on SP */
+static enum ia_css_err
+ia_css_stream_configure_rx(struct ia_css_stream *stream)
+{
+	struct ia_css_input_port *config;
+	assert(stream != NULL);
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	if (config->port > IA_CSS_CSI2_PORT2)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	stream->csi_rx_config.is_two_ppc = (stream->config.pixels_per_clock == 2);
+	stream->reconfigure_css_rx = true;
+	return IA_CSS_SUCCESS;
+}
+#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 i;
+	assert(pipes != NULL);
+	for (i = 0; i < num_pipes; i++) {
+		assert(pipes[i] != NULL);
+		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 enum ia_css_err
+ia_css_acc_stream_create(struct ia_css_stream *stream)
+{
+	int i;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(stream != NULL);
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+
+	if (stream == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	for (i = 0;  i < stream->num_pipes; i++) {
+		struct ia_css_pipe *pipe = stream->pipes[i];
+		assert(pipe != NULL);
+		if (pipe == NULL) {
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+
+		pipe->stream = stream;
+	}
+
+	/* Map SP threads before doing anything. */
+	err = map_sp_threads(stream, true);
+	if (err != IA_CSS_SUCCESS) {
+		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 != NULL);
+		ia_css_pipe_map_queue(pipe, true);
+	}
+
+	err = create_host_pipeline_structure(stream);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	stream->started = false;
+
+
+	IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_SUCCESS);
+
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_SUCCESS;
+}
+
+#ifdef ISP2401
+static enum ia_css_err check_pipe_resolutions(const struct ia_css_pipe *pipe)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (!pipe || !pipe->stream) {
+		IA_CSS_ERROR("null arguments");
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+		goto EXIT;
+	}
+
+	if (ia_css_util_check_res(pipe->config.input_effective_res.width,
+				pipe->config.input_effective_res.height) != IA_CSS_SUCCESS) {
+		IA_CSS_ERROR("effective resolution not supported");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		goto EXIT;
+	}
+	if (!ia_css_util_resolution_is_zero(pipe->stream->config.input_config.input_res)) {
+		if (!ia_css_util_res_leq(pipe->config.input_effective_res,
+						pipe->stream->config.input_config.input_res)) {
+			IA_CSS_ERROR("effective resolution is larger than input resolution");
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+			goto EXIT;
+		}
+	}
+	if (!ia_css_util_resolution_is_even(pipe->config.output_info[0].res)) {
+		IA_CSS_ERROR("output resolution must be even");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		goto EXIT;
+	}
+	if (!ia_css_util_resolution_is_even(pipe->config.vf_output_info[0].res)) {
+		IA_CSS_ERROR("VF resolution must be even");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		goto EXIT;
+	}
+EXIT:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+#endif
+
+enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+	struct ia_css_metadata_info md_info;
+#ifndef ISP2401
+	struct ia_css_resolution effective_res;
+#else
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	bool aspect_ratio_crop_enabled = false;
+#endif
+#endif
+
+	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 == NULL ||
+		pipes == NULL) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+	/* We don't support metadata for JPEG stream, since they both use str2mem */
+	if (stream_config->input_config.format == IA_CSS_STREAM_FORMAT_BINARY_8 &&
+	    stream_config->metadata_config.resolution.height > 0) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	if (stream_config->online && stream_config->pack_raw_pixels) {
+		IA_CSS_LOG("online and pack raw is invalid on input system 2401");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	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 USE_INPUT_SYSTEM_VERSION_2401
+	if (!stream_config->online)
+#endif
+	{
+		unsigned int port = (unsigned int) stream_config->source.port.port;
+		if (port >= N_MIPI_PORT_ID) {
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+			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 = IA_CSS_ERR_INTERNAL_ERROR;
+			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 = IA_CSS_ERR_INTERNAL_ERROR;
+			IA_CSS_LEAVE_ERR(err);
+			return err;
+		}
+
+	}
+#endif
+
+	/* Currently we only supported metadata up to a certain size. */
+	err = metadata_info_init(&stream_config->metadata_config, &md_info);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	/* allocate the stream instance */
+	curr_stream = kmalloc(sizeof(struct ia_css_stream), GFP_KERNEL);
+	if (curr_stream == NULL) {
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+	/* default all to 0 */
+	memset(curr_stream, 0, sizeof(struct ia_css_stream));
+	curr_stream->info.metadata_info = md_info;
+
+	/* allocate pipes */
+	curr_stream->num_pipes = num_pipes;
+	curr_stream->pipes = kzalloc(num_pipes * sizeof(struct ia_css_pipe *), GFP_KERNEL);
+	if (curr_stream->pipes == NULL) {
+		curr_stream->num_pipes = 0;
+		kfree(curr_stream);
+		curr_stream = NULL;
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		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(USE_INPUT_SYSTEM_VERSION_2401) && defined(CSI2P_DISABLE_ISYS2401_ONLINE_MODE)
+	if (stream_config->mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR &&
+		stream_config->online)
+		curr_stream->config.online = false;
+#endif
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	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(USE_INPUT_SYSTEM_VERSION_2)
+		ia_css_stream_configure_rx(curr_stream);
+#endif
+		break;
+	case IA_CSS_INPUT_MODE_TPG:
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+		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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+		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");
+	}
+
+#ifdef ISP2401
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	err = aspect_ratio_crop_init(curr_stream,
+				pipes,
+				&aspect_ratio_crop_enabled);
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+#endif
+
+#endif
+	for (i = 0; i < num_pipes; i++) {
+#ifdef ISP2401
+		struct ia_css_resolution effective_res;
+#endif
+		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;
+#ifdef ISP2401
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+			/* The aspect ratio cropping is currently only
+			 * supported on the new input system. */
+			if (aspect_ratio_crop_check(aspect_ratio_crop_enabled, curr_pipe)) {
+
+				struct ia_css_resolution crop_res;
+
+				err = aspect_ratio_crop(curr_pipe, &crop_res);
+				if (err == IA_CSS_SUCCESS) {
+					effective_res = crop_res;
+				} else {
+					/* in case of error fallback to default
+					 * effective resolution from driver. */
+					IA_CSS_LOG("aspect_ratio_crop() failed with err(%d)", err);
+				}
+			}
+#endif
+#endif
+			curr_pipe->config.input_effective_res = effective_res;
+		}
+		IA_CSS_LOG("effective_res=%dx%d",
+					effective_res.width,
+					effective_res.height);
+	}
+
+#ifdef ISP2401
+	for (i = 0; i < num_pipes; i++) {
+		if (pipes[i]->config.mode != IA_CSS_PIPE_MODE_ACC &&
+			pipes[i]->config.mode != IA_CSS_PIPE_MODE_COPY) {
+			err = check_pipe_resolutions(pipes[i]);
+			if (err != IA_CSS_SUCCESS) {
+				goto ERR;
+			}
+		}
+	}
+
+#endif
+	err = ia_css_stream_isp_parameters_init(curr_stream);
+	if (err != IA_CSS_SUCCESS)
+		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;
+#ifndef ISP2401
+			curr_stream->stop_copy_preview = my_css.stop_copy_preview;
+#endif
+		}
+
+		/* 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 == true)
+			curr_stream->cont_capt = false; /* preview + QoS case will not need cont_capt switch */
+		if (curr_stream->cont_capt == true) {
+			capture_pipe = find_pipe(pipes, num_pipes,
+						IA_CSS_PIPE_MODE_CAPTURE, false);
+			if (capture_pipe == NULL) {
+				err = IA_CSS_ERR_INTERNAL_ERROR;
+				goto ERR;
+			}
+		}
+		/* We do not support preview and video pipe at the same time */
+		if (preview_pipe && video_pipe) {
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+			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 != IA_CSS_SUCCESS)
+				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 == true)) {
+			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 != IA_CSS_SUCCESS)
+				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 == true)) {
+			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;
+#ifndef ISP2401
+		/* take over effective info */
+
+		effective_res = curr_pipe->config.input_effective_res;
+#endif
+
+#ifndef ISP2401
+		err = ia_css_util_check_res(
+					effective_res.width,
+					effective_res.height);
+		if (err != IA_CSS_SUCCESS)
+			goto ERR;
+#endif
+		/* 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 != IA_CSS_SUCCESS)
+			goto ERR;
+
+#if defined(HAS_RES_MGR)
+		/* update acc configuration - striping info is ready */
+		err = ia_css_update_cfg_stripe_info(curr_pipe);
+		if (err != IA_CSS_SUCCESS)
+			goto ERR;
+#endif
+
+		/* 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 != IA_CSS_SUCCESS)
+				goto ERR;
+		}
+#ifdef ISP2401
+		pipe_info->output_system_in_res_info = curr_pipe->config.output_system_in_res;
+#endif
+		if (!spcopyonly){
+			err = sh_css_pipe_get_shading_info(curr_pipe,
+#ifndef ISP2401
+						&pipe_info->shading_info);
+#else
+					&pipe_info->shading_info, &curr_pipe->config);
+#endif
+			if (err != IA_CSS_SUCCESS)
+				goto ERR;
+			err = sh_css_pipe_get_grid_info(curr_pipe,
+						&pipe_info->grid_info);
+			if (err != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+					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_SUCCESS) {
+		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_SUCCESS) {
+		IA_CSS_LOG("create_host_pipeline_structure: return_err=%d", err);
+		goto ERR;
+	}
+
+	/* assign curr_stream */
+	*stream = curr_stream;
+
+ERR:
+#ifndef ISP2401
+	if (err == IA_CSS_SUCCESS)
+	{
+		/* 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 == NULL)
+			{
+				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
+	if (err == IA_CSS_SUCCESS) {
+		err = ia_css_save_stream(curr_stream);
+#endif
+	} else {
+		ia_css_stream_destroy(curr_stream);
+	}
+#ifndef ISP2401
+	IA_CSS_LEAVE("return_err=%d mode=%d", err, my_css_save.mode);
+#else
+	IA_CSS_LEAVE("return_err=%d", err);
+#endif
+	return err;
+}
+
+enum ia_css_err
+ia_css_stream_destroy(struct ia_css_stream *stream)
+{
+	int i;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+#ifdef ISP2401
+	enum ia_css_err err1 = IA_CSS_SUCCESS;
+	enum ia_css_err err2 = IA_CSS_SUCCESS;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("stream = %p", stream);
+	if (stream == NULL) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	ia_css_stream_isp_parameters_uninit(stream);
+
+	if ((stream->last_pipe != NULL) &&
+		ia_css_pipeline_is_mapped(stream->last_pipe->pipe_num)) {
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+		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 != NULL);
+			if (entry != NULL) {
+				/* get the SP thread id */
+				if (ia_css_pipeline_get_sp_thread_id(
+					ia_css_pipe_get_pipe_num(entry), &sp_thread_id) != true)
+					return IA_CSS_ERR_INTERNAL_ERROR;
+				/* 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);
+				}
+			}
+		}
+#ifndef ISP2401
+		if (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+#else
+		if (stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR ||
+			stream->config.mode == IA_CSS_INPUT_MODE_TPG ||
+			stream->config.mode == IA_CSS_INPUT_MODE_PRBS) {
+#endif
+			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 != NULL)
+					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 != NULL);
+			ia_css_pipe_map_queue(curr_pipe, false);
+		}
+
+		err = map_sp_threads(stream, false);
+		if (err != IA_CSS_SUCCESS) {
+			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 != NULL);
+		if (entry != NULL) {
+			/* 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;
+#ifndef ISP2401
+	/* 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;
+			}
+#else
+	err2 = ia_css_save_restore_remove_stream(stream);
+
+	err1 = (err != IA_CSS_SUCCESS) ? err : err2;
+#endif
+	kfree(stream);
+#ifndef ISP2401
+	IA_CSS_LEAVE_ERR(err);
+#else
+	IA_CSS_LEAVE_ERR(err1);
+#endif
+
+#ifndef ISP2401
+	return err;
+#else
+	return err1;
+#endif
+}
+
+enum ia_css_err
+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 != NULL);
+	assert(stream_info != NULL);
+
+	*stream_info = stream->info;
+	return IA_CSS_SUCCESS;
+}
+
+/*
+ * Rebuild a stream, including allocating structs, setting configuration and
+ * building the required pipes.
+ * The data is taken from the css_save struct updated upon stream creation.
+ * The stream handle is used to identify the correct entry in the css_save struct
+ */
+enum ia_css_err
+ia_css_stream_load(struct ia_css_stream *stream)
+{
+#ifndef ISP2401
+	int i;
+	enum ia_css_err err;
+	assert(stream != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"ia_css_stream_load() enter, \n");
+	for(i=0;i<MAX_ACTIVE_STREAMS;i++)
+		if (my_css_save.stream_seeds[i].stream == stream)
+		{
+			int j;
+			for(j=0;j<my_css_save.stream_seeds[i].num_pipes;j++)
+				if ((err = ia_css_pipe_create(&(my_css_save.stream_seeds[i].pipe_config[j]), &my_css_save.stream_seeds[i].pipes[j])) != IA_CSS_SUCCESS)
+				{
+					if (j)
+					{
+						int k;
+						for(k=0;k<j;k++)
+							ia_css_pipe_destroy(my_css_save.stream_seeds[i].pipes[k]);
+					}
+					return err;
+				}
+			err = ia_css_stream_create(&(my_css_save.stream_seeds[i].stream_config), my_css_save.stream_seeds[i].num_pipes,
+						    my_css_save.stream_seeds[i].pipes, &(my_css_save.stream_seeds[i].stream));
+		    if (err != IA_CSS_SUCCESS)
+			{
+				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]);
+				return err;
+			}
+			break;
+		}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"ia_css_stream_load() exit, \n");
+	return IA_CSS_SUCCESS;
+#else
+	/* TODO remove function - DEPRECATED */
+	(void)stream;
+	return IA_CSS_ERR_NOT_SUPPORTED;
+#endif
+}
+
+enum ia_css_err
+ia_css_stream_start(struct ia_css_stream *stream)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	IA_CSS_ENTER("stream = %p", stream);
+	if ((stream == NULL) || (stream->last_pipe == NULL)) {
+		IA_CSS_LEAVE_ERR(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	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_SUCCESS) {
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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
+#endif
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	/* 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 !defined(HAS_NO_INPUT_SYSTEM)
+	if (stream->config.mode != IA_CSS_INPUT_MODE_MEMORY) {
+		err = sh_css_config_input_network(stream);
+		if (err != IA_CSS_SUCCESS)
+			return err;
+	}
+#endif /* !HAS_NO_INPUT_SYSTEM */
+
+	err = sh_css_pipe_start(stream);
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+enum ia_css_err
+ia_css_stream_stop(struct ia_css_stream *stream)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_stop() enter/exit\n");
+	assert(stream != NULL);
+	assert(stream->last_pipe != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "ia_css_stream_stop: stopping %d\n",
+		stream->last_pipe->mode);
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	/* 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
+#ifndef ISP2401
+	err = ia_css_pipeline_request_stop(&stream->last_pipe->pipeline);
+#else
+
+	err = sh_css_pipes_stop(stream);
+#endif
+	if (err != IA_CSS_SUCCESS)
+		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 != NULL);
+
+#ifndef ISP2401
+	stopped = ia_css_pipeline_has_stopped(&stream->last_pipe->pipeline);
+#else
+	stopped = sh_css_pipes_have_stopped(stream);
+#endif
+
+	return stopped;
+}
+
+#ifndef ISP2401
+/*
+ * 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
+ */
+enum ia_css_err
+ia_css_stream_unload(struct ia_css_stream *stream)
+{
+	int i;
+	assert(stream != NULL);
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,	"ia_css_stream_unload() enter, \n");
+	/* some checks */
+	assert (stream != NULL);
+	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 IA_CSS_SUCCESS;
+}
+
+#endif
+enum ia_css_err
+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 != NULL)
+		*pipe_id = pipe->mode;
+	else
+		*pipe_id = IA_CSS_PIPE_ID_COPY;
+
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_stream_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 != NULL);
+
+	pipe = stream->pipes[0];
+
+	if (stream->num_pipes == 2) {
+		assert(stream->pipes[1] != NULL);
+		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 != NULL);
+
+	pipe = stream->pipes[0];
+
+	if (stream->num_pipes == 2) {
+		assert(stream->pipes[1] != NULL);
+		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;
+}
+
+
+enum ia_css_err
+ia_css_stream_set_output_padded_width(struct ia_css_stream *stream, unsigned int output_padded_width)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	struct ia_css_pipe *pipe;
+
+	assert(stream != NULL);
+
+	pipe = stream->last_pipe;
+
+	assert(pipe != NULL);
+
+	/* 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 err;
+}
+
+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 != NULL);
+
+	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 != NULL);
+
+	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 != NULL);
+
+	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 != NULL);
+
+	return (struct ia_css_pipeline*)&pipe->pipeline;
+}
+
+unsigned int
+ia_css_pipe_get_pipe_num(const struct ia_css_pipe *pipe)
+{
+	assert(pipe != NULL);
+
+	/* 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 != NULL);
+
+	return (unsigned int)pipe->config.isp_pipe_version;
+}
+
+#if defined(HAS_BL)
+#define BL_START_TIMEOUT_US 30000000
+static enum ia_css_err
+ia_css_start_bl(void)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned long timeout;
+
+	IA_CSS_ENTER("");
+	sh_css_start_bl();
+	/* waiting for the Bootloader to complete execution */
+	timeout = BL_START_TIMEOUT_US;
+	while((ia_css_blctrl_get_state() == BOOTLOADER_BUSY) && timeout) {
+		timeout--;
+		hrt_sleep();
+	}
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE,
+			    "Bootloader state %d\n", ia_css_blctrl_get_state());
+	if (timeout == 0) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "Bootloader Execution Timeout\n");
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+	}
+	if (ia_css_blctrl_get_state() != BOOTLOADER_OK) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				    "Bootloader Execution Failed\n");
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+	}
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+#endif
+
+#define SP_START_TIMEOUT_US 30000000
+
+enum ia_css_err
+ia_css_start_sp(void)
+{
+	unsigned long timeout;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("");
+#if defined(HAS_BL)
+	/* Starting bootloader before Sp0 and Sp1
+	 * and not exposing CSS API */
+	err = ia_css_start_bl();
+	if (err != IA_CSS_SUCCESS) {
+		IA_CSS_LEAVE("Bootloader fails");
+		return err;
+	}
+#endif
+	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--;
+		hrt_sleep();
+	}
+	if (timeout == 0) {
+		IA_CSS_ERROR("timeout during SP initialization");
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	/* 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();
+
+#ifdef ISP2401
+	if (ia_css_is_system_mode_suspend_or_resume() == false) { /* skip in suspend/resume flow */
+		ia_css_set_system_mode(IA_CSS_SYS_MODE_WORKING);
+	}
+#endif
+	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
+
+enum ia_css_err
+ia_css_stop_sp(void)
+{
+	unsigned long timeout;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("void");
+
+	if (!sh_css_sp_is_running()) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		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 */
+#ifndef ISP2401
+	sh_css_write_host2sp_command(host2sp_cmd_terminate);
+#else
+	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);
+	}
+#endif
+	sh_css_sp_set_sp_running(false);
+
+	timeout = SP_SHUTDOWN_TIMEOUT_US;
+	while (!ia_css_spctrl_is_idle(SP0_ID) && timeout) {
+		timeout--;
+		hrt_sleep();
+	}
+	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--;
+		hrt_sleep();
+	}
+	if (timeout == 0) {
+		IA_CSS_WARNING("ISP is not idle");
+		ia_css_debug_dump_sp_sw_debug_info();
+	}
+
+	sh_css_hmm_buffer_record_uninit();
+
+#ifndef ISP2401
+	/* clear pending param sets from refcount */
+	sh_css_param_clear_param_sets();
+#else
+	if (ia_css_is_system_mode_suspend_or_resume() == false) { /* skip in suspend/resume flow */
+		/* clear pending param sets from refcount */
+		sh_css_param_clear_param_sets();
+		ia_css_set_system_mode(IA_CSS_SYS_MODE_INIT);  /* System is initialized but not 'running' */
+	}
+#endif
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+enum ia_css_err
+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 == NULL) {
+		ia_css_debug_dtrace(
+			IA_CSS_DEBUG_TRACE,
+			"sh_css_update_continuous_frames() leave: invalid stream, return_void\n");
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 IA_CSS_SUCCESS;
+}
+
+void ia_css_pipe_map_queue(struct ia_css_pipe *pipe, bool map)
+{
+	unsigned int thread_id;
+	enum ia_css_pipe_id pipe_id;
+	unsigned int pipe_num;
+	bool need_input_queue;
+
+	IA_CSS_ENTER("");
+	assert(pipe != NULL);
+
+	pipe_id = pipe->mode;
+	pipe_num = pipe->pipe_num;
+
+	ia_css_pipeline_get_sp_thread_id(pipe_num, &thread_id);
+
+#if defined(HAS_NO_INPUT_SYSTEM) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+		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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+		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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+		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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+	} 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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+	} 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);
+#if defined SH_CSS_ENABLE_METADATA
+		ia_css_queue_map(thread_id, IA_CSS_BUFFER_TYPE_METADATA, map);
+#endif
+	}
+	IA_CSS_LEAVE("");
+}
+
+#if CONFIG_ON_FRAME_ENQUEUE()
+static enum ia_css_err set_config_on_frame_enqueue(struct ia_css_frame_info *info, struct frame_data_wrapper *frame)
+{
+	frame->config_on_frame_enqueue.padded_width = 0;
+
+	/* currently we support configuration on frame enqueue only on YUV formats */
+	/* on other formats the padded_width is zeroed for no configuration override */
+	switch (info->format) {
+	case IA_CSS_FRAME_FORMAT_YUV420:
+	case IA_CSS_FRAME_FORMAT_NV12:
+		if (info->padded_width > info->res.width)
+		{
+			frame->config_on_frame_enqueue.padded_width = info->padded_width;
+		}
+		else if ((info->padded_width < info->res.width) && (info->padded_width > 0))
+		{
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+		/* nothing to do if width == padded width or padded width is zeroed (the same) */
+		break;
+	default:
+		break;
+	}
+
+	return IA_CSS_SUCCESS;
+}
+#endif
+
+enum ia_css_err
+ia_css_unlock_raw_frame(struct ia_css_stream *stream, uint32_t exp_id)
+{
+	enum ia_css_err ret;
+
+	IA_CSS_ENTER("");
+
+	/* Only continuous streams have a tagger to which we can send the
+	 * unlock message. */
+	if (stream == NULL || !stream->config.continuous) {
+		IA_CSS_ERROR("invalid stream pointer");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if (exp_id > IA_CSS_ISYS_MAX_EXPOSURE_ID ||
+	    exp_id < IA_CSS_ISYS_MIN_EXPOSURE_ID) {
+		IA_CSS_ERROR("invalid expsure ID: %d\n", exp_id);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* 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.
+ */
+enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("");
+
+	/* Parameter Check */
+	if (pipe == NULL || pipe->stream == NULL) {
+		IA_CSS_ERROR("Invalid Pipe.");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!(pipe->config.acc_extension)) {
+		IA_CSS_ERROR("Invalid Pipe(No Extension Firmware)");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!sh_css_sp_is_running()) {
+		IA_CSS_ERROR("Leaving: queue unavailable.");
+		err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	} 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 == IA_CSS_SUCCESS) {
+			/* 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 == true) ? 1 : 0);
+			if (err == IA_CSS_SUCCESS) {
+				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.
+ */
+enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("");
+
+	/* Parameter Check */
+	if (pipe == NULL || pipe->stream == NULL) {
+		IA_CSS_ERROR("Invalid Pipe.");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!(pipe->config.acc_extension)) {
+		IA_CSS_ERROR("Invalid Pipe (No Extension Firmware).");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!sh_css_sp_is_running()) {
+		IA_CSS_ERROR("Leaving: queue unavailable.");
+		err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	} 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 == IA_CSS_SUCCESS) {
+			/* 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;
+}
+
+#ifdef ISP2401
+enum ia_css_err
+ia_css_pipe_update_qos_ext_mapped_arg(struct ia_css_pipe *pipe, uint32_t fw_handle,
+	struct ia_css_isp_param_css_segments *css_seg, struct ia_css_isp_param_isp_segments *isp_seg)
+{
+	unsigned int HIVE_ADDR_sp_group;
+	static struct sh_css_sp_group sp_group;
+	static struct sh_css_sp_stage sp_stage;
+	static struct sh_css_isp_stage isp_stage;
+	const struct ia_css_fw_info *fw;
+	unsigned int thread_id;
+	struct ia_css_pipeline_stage *stage;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	int stage_num = 0;
+	enum ia_css_isp_memories mem;
+	bool enabled;
+
+	IA_CSS_ENTER("");
+
+	fw = &sh_css_sp_fw;
+
+	/* Parameter Check */
+	if (pipe == NULL || pipe->stream == NULL) {
+		IA_CSS_ERROR("Invalid Pipe.");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!(pipe->config.acc_extension)) {
+		IA_CSS_ERROR("Invalid Pipe (No Extension Firmware).");
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+	} else if (!sh_css_sp_is_running()) {
+		IA_CSS_ERROR("Leaving: queue unavailable.");
+		err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+	} else {
+		/* Query the thread_id 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 == IA_CSS_SUCCESS) {
+			/* Get the Extension State */
+			enabled = (SH_CSS_QOS_STAGE_IS_ENABLED(&(sh_css_sp_group.pipe[thread_id]), stage->stage_num)) ? true : false;
+			/* Update mapped arg only when extension stage is not enabled */
+			if (enabled) {
+				IA_CSS_ERROR("Leaving: cannot update when stage is enabled.");
+				err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+			} else {
+				stage_num = stage->stage_num;
+
+				HIVE_ADDR_sp_group = fw->info.sp.group;
+				sp_dmem_load(SP0_ID,
+					(unsigned int)sp_address_of(sp_group),
+					&sp_group, sizeof(struct sh_css_sp_group));
+				mmgr_load(sp_group.pipe[thread_id].sp_stage_addr[stage_num],
+					&sp_stage, sizeof(struct sh_css_sp_stage));
+
+				mmgr_load(sp_stage.isp_stage_addr,
+					&isp_stage, sizeof(struct sh_css_isp_stage));
+
+				for (mem = 0; mem < N_IA_CSS_ISP_MEMORIES; mem++) {
+					isp_stage.mem_initializers.params[IA_CSS_PARAM_CLASS_PARAM][mem].address =
+						css_seg->params[IA_CSS_PARAM_CLASS_PARAM][mem].address;
+					isp_stage.mem_initializers.params[IA_CSS_PARAM_CLASS_PARAM][mem].size =
+						css_seg->params[IA_CSS_PARAM_CLASS_PARAM][mem].size;
+					isp_stage.binary_info.mem_initializers.params[IA_CSS_PARAM_CLASS_PARAM][mem].address =
+						isp_seg->params[IA_CSS_PARAM_CLASS_PARAM][mem].address;
+					isp_stage.binary_info.mem_initializers.params[IA_CSS_PARAM_CLASS_PARAM][mem].size =
+						isp_seg->params[IA_CSS_PARAM_CLASS_PARAM][mem].size;
+				}
+
+				mmgr_store(sp_stage.isp_stage_addr,
+					&isp_stage, sizeof(struct sh_css_isp_stage));
+			}
+		}
+	}
+	IA_CSS_LEAVE("err:%d handle:%u", err, fw_handle);
+	return err;
+}
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+static enum ia_css_err
+aspect_ratio_crop_init(struct ia_css_stream *curr_stream,
+		struct ia_css_pipe *pipes[],
+		bool *do_crop_status)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	int i;
+	struct ia_css_pipe *curr_pipe;
+	uint32_t pipe_mask = 0;
+
+	if ((curr_stream == NULL) ||
+	    (curr_stream->num_pipes == 0) ||
+	    (pipes == NULL) ||
+	    (do_crop_status == NULL)) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	for (i = 0; i < curr_stream->num_pipes; i++) {
+		curr_pipe = pipes[i];
+		pipe_mask |= (1 << curr_pipe->config.mode);
+	}
+
+	*do_crop_status =
+		(((pipe_mask & (1 << IA_CSS_PIPE_MODE_PREVIEW)) ||
+		  (pipe_mask & (1 << IA_CSS_PIPE_MODE_VIDEO))) &&
+		  (pipe_mask & (1 << IA_CSS_PIPE_MODE_CAPTURE)) &&
+		  curr_stream->config.continuous);
+	return IA_CSS_SUCCESS;
+}
+
+static bool
+aspect_ratio_crop_check(bool enabled, struct ia_css_pipe *curr_pipe)
+{
+	bool status = false;
+
+	if ((curr_pipe != NULL) && enabled) {
+		if ((curr_pipe->config.mode == IA_CSS_PIPE_MODE_PREVIEW) ||
+		    (curr_pipe->config.mode == IA_CSS_PIPE_MODE_VIDEO) ||
+		    (curr_pipe->config.mode == IA_CSS_PIPE_MODE_CAPTURE))
+			status = true;
+	}
+
+	return status;
+}
+
+static enum ia_css_err
+aspect_ratio_crop(struct ia_css_pipe *curr_pipe,
+		struct ia_css_resolution *effective_res)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_resolution crop_res;
+	struct ia_css_resolution *in_res = NULL;
+	struct ia_css_resolution *out_res = NULL;
+	bool use_bds_output_info = false;
+	bool use_vf_pp_in_res = false;
+	bool use_capt_pp_in_res = false;
+
+	if ((curr_pipe == NULL) ||
+	    (effective_res == NULL)) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	if ((curr_pipe->config.mode != IA_CSS_PIPE_MODE_PREVIEW) &&
+	    (curr_pipe->config.mode != IA_CSS_PIPE_MODE_VIDEO) &&
+	    (curr_pipe->config.mode != IA_CSS_PIPE_MODE_CAPTURE)) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR(err);
+		return err;
+	}
+
+	use_bds_output_info =
+		((curr_pipe->bds_output_info.res.width != 0) &&
+		 (curr_pipe->bds_output_info.res.height != 0));
+
+	use_vf_pp_in_res =
+		((curr_pipe->config.vf_pp_in_res.width != 0) &&
+		 (curr_pipe->config.vf_pp_in_res.height != 0));
+
+	use_capt_pp_in_res =
+		((curr_pipe->config.capt_pp_in_res.width != 0) &&
+		 (curr_pipe->config.capt_pp_in_res.height != 0));
+
+	in_res = &curr_pipe->stream->config.input_config.effective_res;
+	out_res = &curr_pipe->output_info[0].res;
+
+	switch (curr_pipe->config.mode) {
+	case IA_CSS_PIPE_MODE_PREVIEW:
+		if (use_bds_output_info)
+			out_res = &curr_pipe->bds_output_info.res;
+		else if (use_vf_pp_in_res)
+			out_res = &curr_pipe->config.vf_pp_in_res;
+		break;
+	case IA_CSS_PIPE_MODE_VIDEO:
+		if (use_bds_output_info)
+			out_res = &curr_pipe->bds_output_info.res;
+		break;
+	case IA_CSS_PIPE_MODE_CAPTURE:
+		if (use_capt_pp_in_res)
+			out_res = &curr_pipe->config.capt_pp_in_res;
+		break;
+	case IA_CSS_PIPE_MODE_ACC:
+	case IA_CSS_PIPE_MODE_COPY:
+	case IA_CSS_PIPE_MODE_YUVPP:
+	default:
+		IA_CSS_ERROR("aspect ratio cropping invalid args: mode[%d]\n",
+			curr_pipe->config.mode);
+		assert(0);
+		break;
+	}
+
+	err = ia_css_frame_find_crop_resolution(in_res, out_res, &crop_res);
+	if (err == IA_CSS_SUCCESS) {
+		*effective_res = crop_res;
+	} else {
+		/* in case of error fallback to default
+		 * effective resolution from driver. */
+		IA_CSS_LOG("ia_css_frame_find_crop_resolution() failed with err(%d)", err);
+	}
+	return err;
+}
+#endif
+
+#endif
+static void
+sh_css_hmm_buffer_record_init(void)
+{
+	int i;
+
+#ifndef ISP2401
+	for (i = 0; i < MAX_HMM_BUFFER_NUM; i++) {
+		sh_css_hmm_buffer_record_reset(&hmm_buffer_record[i]);
+#else
+	if (ia_css_is_system_mode_suspend_or_resume() == false) { /* skip in suspend/resume flow */
+		for (i = 0; i < MAX_HMM_BUFFER_NUM; i++) {
+			sh_css_hmm_buffer_record_reset(&hmm_buffer_record[i]);
+		}
+#endif
+	}
+}
+
+static void
+sh_css_hmm_buffer_record_uninit(void)
+{
+	int i;
+	struct sh_css_hmm_buffer_record *buffer_record = NULL;
+
+#ifndef ISP2401
+	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 != NULL)
+				ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &buffer_record->h_vbuf);
+			sh_css_hmm_buffer_record_reset(buffer_record);
+#else
+	if (ia_css_is_system_mode_suspend_or_resume() == false) { /* skip in suspend/resume flow */
+		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 != NULL)
+					ia_css_rmgr_rel_vbuf(hmm_buffer_pool, &buffer_record->h_vbuf);
+				sh_css_hmm_buffer_record_reset(buffer_record);
+			}
+			buffer_record++;
+#endif
+		}
+#ifndef ISP2401
+		buffer_record++;
+#endif
+	}
+}
+
+static void
+sh_css_hmm_buffer_record_reset(struct sh_css_hmm_buffer_record *buffer_record)
+{
+	assert(buffer_record != NULL);
+	buffer_record->in_use = false;
+	buffer_record->type = IA_CSS_BUFFER_TYPE_INVALID;
+	buffer_record->h_vbuf = NULL;
+	buffer_record->kernel_ptr = 0;
+}
+
+#ifndef ISP2401
+static bool
+sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+#else
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_acquire(struct ia_css_rmgr_vbuf_handle *h_vbuf,
+#endif
+			enum ia_css_buffer_type type,
+			hrt_address kernel_ptr)
+{
+	int i;
+	struct sh_css_hmm_buffer_record *buffer_record = NULL;
+#ifndef ISP2401
+	bool found_record = false;
+#else
+	struct sh_css_hmm_buffer_record *out_buffer_record = NULL;
+#endif
+
+	assert(h_vbuf != NULL);
+	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 == false) {
+			buffer_record->in_use = true;
+			buffer_record->type = type;
+			buffer_record->h_vbuf = h_vbuf;
+			buffer_record->kernel_ptr = kernel_ptr;
+#ifndef ISP2401
+			found_record = true;
+#else
+			out_buffer_record = buffer_record;
+#endif
+			break;
+		}
+		buffer_record++;
+	}
+
+#ifndef ISP2401
+	return found_record;
+#else
+	return out_buffer_record;
+#endif
+}
+
+static struct sh_css_hmm_buffer_record
+*sh_css_hmm_buffer_record_validate(hrt_vaddress 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 == true) &&
+		    (buffer_record->type == type) &&
+		    (buffer_record->h_vbuf != NULL) &&
+		    (buffer_record->h_vbuf->vptr == ddr_buffer_addr)) {
+			found_record = true;
+			break;
+		}
+		buffer_record++;
+	}
+
+	if (found_record == true)
+		return buffer_record;
+	else
+		return NULL;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_defs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_defs.h
new file mode 100644
index 000000000000..4072c564f911
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_defs.h
@@ -0,0 +1,410 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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              (1 << 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 */
+#if ISP_VEC_NELEMS == 16
+#define SH_CSS_MAX_LEFT_CROPPING          0
+#define SH_CSS_MAX_TOP_CROPPING           0
+#else
+#define SH_CSS_MAX_LEFT_CROPPING          12
+#define SH_CSS_MAX_TOP_CROPPING           12
+#endif
+
+#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 */
+#if ISP_VEC_NELEMS == 16
+#define SH_CSS_MAX_SENSOR_WIDTH           2560
+#define SH_CSS_MAX_SENSOR_HEIGHT          1920
+#else
+#define SH_CSS_MAX_SENSOR_WIDTH           4608
+#define SH_CSS_MAX_SENSOR_HEIGHT          3450
+#endif
+
+/* 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
+
+#if defined(IS_ISP_2400_SYSTEM)
+/* MAX width and height set to the same to allow for rotated
+ * resolutions. */
+#define SH_CSS_MAX_VF_WIDTH               1920
+#define SH_CSS_MAX_VF_HEIGHT              1920
+#else
+#define SH_CSS_MAX_VF_WIDTH               1280
+#define SH_CSS_MAX_VF_HEIGHT              960
+#endif
+/*
+#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)
+
+#ifndef ISP2401
+#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)
+#else
+/* TODO: I will move macros of "*_SCTBL_*" to SC kernel.
+   "+ 2" should be "+ SH_CSS_SCTBL_CENTERING_MARGIN + SH_CSS_SCTBL_LAST_GRID_COUNT". (michie, Sep/23/2014) */
+#define SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR   (SH_CSS_MAX_BQ_GRID_WIDTH + 2)
+#define SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR   (SH_CSS_MAX_BQ_GRID_HEIGHT + 2)
+#endif
+#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
+
+#ifndef ISP2401
+/* The video binary supports a delay of 1 or 2 */
+#define MAX_DVS_FRAME_DELAY		2
+/* We always need one additional frame because the video binary
+ * 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
+
+#define NUM_TNR_FRAMES			2	/* FIXME */
+
+
+#define MAX_NUM_DELAY_FRAMES		MAX_NUM_VIDEO_DELAY_FRAMES
+
+#else
+/* Video mode specific DVS define */
+/* The video binary supports a delay of 1 or 2 frames */
+#define VIDEO_FRAME_DELAY		2
+/* +1 because DVS reads the previous and writes the current frame concurrently */
+#define MAX_NUM_VIDEO_DELAY_FRAMES	(VIDEO_FRAME_DELAY + 1)
+
+/* Preview mode specific DVS define. */
+/* In preview we only need GDC functionality (and not the DVS functionality) */
+/* The minimum number of DVS frames you need is 2, one were GDC reads from and another where GDC writes into */
+#define NUM_PREVIEW_DVS_FRAMES		(2)
+
+/* TNR is no longer exclusive to video, SkyCam preview has TNR too (same kernel as video).
+ * All uses the generic define NUM_TNR_FRAMES. The define NUM_VIDEO_TNR_FRAMES has been deprecated.
+ *
+ * Notes
+ * 1) The value depends on the used TNR kernel and is not something that depends on the mode
+ *    and it is not something you just could choice.
+ * 2) For the luma only pipeline a version that supports two different sets of TNR reference frames
+ * is being used.
+ *.
+ */
+#define NUM_VALID_TNR_REF_FRAMES		(1) /* At least one valid TNR reference frame is required */
+#define NUM_TNR_FRAMES_PER_REF_BUF_SET		(2)
+
+/* In luma-only mode alternate illuminated frames are supported, that requires two double buffers */
+#ifdef ENABLE_LUMA_ONLY
+#define NUM_TNR_REF_BUF_SETS	(2)
+#else
+#define NUM_TNR_REF_BUF_SETS	(1)
+#endif
+
+#define NUM_TNR_FRAMES		(NUM_TNR_FRAMES_PER_REF_BUF_SET * NUM_TNR_REF_BUF_SETS)
+
+#define MAX_NUM_DELAY_FRAMES	MAX(MAX_NUM_VIDEO_DELAY_FRAMES, NUM_PREVIEW_DVS_FRAMES)
+
+#endif
+
+/* 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)
+
+#ifndef ISP2401
+#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)
+
+#endif
+/* *****************************************************************
+ * 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 */
+#ifdef CONFIG_CSI2_PLUS
+#define _ISP_EXTRA_PADDING_VECS 2
+#else
+#define _ISP_EXTRA_PADDING_VECS 0
+#endif /* CONFIG_CSI2_PLUS */
+
+#endif /* _SH_CSS_DEFS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_dvs_info.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_dvs_info.h
new file mode 100644
index 000000000000..23044aad654f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_dvs_info.h
@@ -0,0 +1,36 @@
+/**
+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/atomisp2/css2400/sh_css_firmware.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
new file mode 100644
index 000000000000..34cc56f0b471
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.c
@@ -0,0 +1,342 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 <linux/vmalloc.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 "memory_access.h"
+#include "assert_support.h"
+#include "string_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;
+};
+
+/* Warning: same order as SH_CSS_BINARY_ID_* */
+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  */
+#ifndef ISP2401
+static const char *release_version = STR(irci_stable_candrpv_0415_20150521_0458);
+#else
+static const char *release_version = STR(irci_ecr-master_20150911_0724);
+#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;
+#if defined(HAS_BL)
+struct ia_css_fw_info     sh_css_bl_fw;
+#endif /* HAS_BL */
+struct ia_css_blob_descr *sh_css_blob_info; /* Only ISP blob info (no SP) */
+unsigned		  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 enum ia_css_err
+setup_binary(struct ia_css_fw_info *fw, const char *fw_data, struct ia_css_fw_info *sh_css_fw, unsigned binary_id)
+{
+	const char *blob_data;
+
+	if ((fw == NULL) || (fw_data == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	blob_data = fw_data + fw->blob.offset;
+
+	*sh_css_fw = *fw;
+
+#if defined(HRT_UNSCHED)
+	sh_css_fw->blob.code = vmalloc(1);
+#else
+	sh_css_fw->blob.code = vmalloc(fw->blob.size);
+#endif
+
+	if (sh_css_fw->blob.code == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	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 IA_CSS_SUCCESS;
+}
+enum ia_css_err
+sh_css_load_blob_info(const char *fw, const struct ia_css_fw_info *bi, struct ia_css_blob_descr *bd, unsigned index)
+{
+	const char *name;
+	const unsigned char *blob;
+
+	if ((fw == NULL) || (bd == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	/* Special case: only one binary in fw */
+	if (bi == NULL) 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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	if ((bi->blob.offset % (1UL<<(ISP_PMEM_WIDTH_LOG2-3))) != 0)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	bd->blob = blob;
+	bd->header = *bi;
+
+	if ((bi->type == ia_css_isp_firmware) || (bi->type == ia_css_sp_firmware)
+#if defined(HAS_BL)
+	|| (bi->type == ia_css_bootloader_firmware)
+#endif /* HAS_BL */
+	)
+	{
+		char *namebuffer;
+		int namelength = (int)strlen(name);
+
+		namebuffer = (char *) kmalloc(namelength + 1, GFP_KERNEL);
+		if (namebuffer == NULL)
+			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+		memcpy(namebuffer, name, namelength + 1);
+
+		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 = (char *)kmalloc(paramstruct_size + configstruct_size + statestruct_size,
+							GFP_KERNEL);
+		if (parambuf == NULL)
+			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+		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 IA_CSS_SUCCESS;
+}
+
+bool
+sh_css_check_firmware_version(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) {
+		return false;
+	} else {
+		/* firmware version matches */
+		return true;
+	}
+}
+
+enum ia_css_err
+sh_css_load_firmware(const char *fw_data,
+		     unsigned int fw_size)
+{
+	unsigned i;
+	struct ia_css_fw_info *binaries;
+	struct sh_css_fw_bi_file_h *file_header;
+	bool valid_firmware = false;
+
+	firmware_header = (struct firmware_header *)fw_data;
+	file_header = &firmware_header->file_header;
+	binaries = &firmware_header->binary_header;
+	strncpy(FW_rel_ver_name, file_header->version, min(sizeof(FW_rel_ver_name), sizeof(file_header->version)) - 1);
+	valid_firmware = sh_css_check_firmware_version(fw_data);
+	if (!valid_firmware) {
+#if !defined(HRT_RTL)
+		IA_CSS_ERROR("CSS code version (%s) and firmware version (%s) mismatch!",
+				file_header->version, release_version);
+		return IA_CSS_ERR_VERSION_MISMATCH;
+#endif
+	} 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 IA_CSS_ERR_INTERNAL_ERROR;
+
+	if (file_header->h_size != sizeof(struct sh_css_fw_bi_file_h))
+		return IA_CSS_ERR_INTERNAL_ERROR;
+
+	sh_css_num_binaries = file_header->binary_nr;
+	/* Only allocate memory for ISP blob info */
+	if (sh_css_num_binaries > (NUM_OF_SPS + NUM_OF_BLS)) {
+		sh_css_blob_info = kmalloc(
+					(sh_css_num_binaries - (NUM_OF_SPS + NUM_OF_BLS)) *
+					sizeof(*sh_css_blob_info), GFP_KERNEL);
+		if (sh_css_blob_info == NULL)
+			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	} else {
+		sh_css_blob_info = NULL;
+	}
+
+	fw_minibuffer = kzalloc(sh_css_num_binaries * sizeof(struct fw_param), GFP_KERNEL);
+	if (fw_minibuffer == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	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;
+		enum ia_css_err err;
+
+		err = sh_css_load_blob_info(fw_data, bi, &bd, i);
+
+		if (err != IA_CSS_SUCCESS)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+
+		if (bi->blob.offset + bi->blob.size > fw_size)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+
+		if (bi->type == ia_css_sp_firmware) {
+			if (i != SP_FIRMWARE)
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			err = setup_binary(bi, fw_data, &sh_css_sp_fw, i);
+			if (err != IA_CSS_SUCCESS)
+				return err;
+#if defined(HAS_BL)
+		} else if (bi->type == ia_css_bootloader_firmware) {
+			if (i != BOOTLOADER_FIRMWARE)
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			err = setup_binary(bi, fw_data, &sh_css_bl_fw, i);
+			if (err != IA_CSS_SUCCESS)
+				return err;
+			IA_CSS_LOG("Bootloader binary recognized\n");
+#endif
+		} else {
+			/* All subsequent binaries (including bootloaders) (i>NUM_OF_SPS+NUM_OF_BLS) are ISP firmware */
+			if (i < (NUM_OF_SPS + NUM_OF_BLS))
+				return IA_CSS_ERR_INTERNAL_ERROR;
+
+			if (bi->type != ia_css_isp_firmware)
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			if (sh_css_blob_info == NULL) /* cannot happen but KW does not see this */
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			sh_css_blob_info[i-(NUM_OF_SPS + NUM_OF_BLS)] = bd;
+		}
+	}
+
+	return IA_CSS_SUCCESS;
+}
+
+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++) {
+			if (fw_minibuffer[i].name)
+				kfree((void *)fw_minibuffer[i].name);
+			if (fw_minibuffer[i].buffer)
+				vfree((void *)fw_minibuffer[i].buffer);
+		}
+		kfree(fw_minibuffer);
+		fw_minibuffer = NULL;
+	}
+
+	memset(&sh_css_sp_fw, 0, sizeof(sh_css_sp_fw));
+	if (sh_css_blob_info) {
+		kfree(sh_css_blob_info);
+		sh_css_blob_info = NULL;
+	}
+	sh_css_num_binaries = 0;
+}
+
+hrt_vaddress
+sh_css_load_blob(const unsigned char *blob, unsigned size)
+{
+	hrt_vaddress target_addr = mmgr_malloc(size);
+	/* this will allocate memory aligned to a DDR word boundary which
+	   is required for the CSS DMA to read the instructions. */
+
+	assert(blob != NULL);
+	if (target_addr) 
+		mmgr_store(target_addr, blob, size);
+	return target_addr;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.h
new file mode 100644
index 000000000000..588aabde8a86
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_firmware.h
@@ -0,0 +1,54 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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;
+#if defined(HAS_BL)
+extern struct ia_css_fw_info     sh_css_bl_fw;
+#endif /* HAS_BL */
+extern struct ia_css_blob_descr *sh_css_blob_info;
+extern unsigned			 sh_css_num_binaries;
+
+char
+*sh_css_get_fw_version(void);
+
+bool
+sh_css_check_firmware_version(const char *fw_data);
+
+enum ia_css_err
+sh_css_load_firmware(const char *fw_data,
+		     unsigned int fw_size);
+
+void sh_css_unload_firmware(void);
+
+hrt_vaddress sh_css_load_blob(const unsigned char *blob, unsigned size);
+
+enum ia_css_err
+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/atomisp2/css2400/sh_css_frac.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_frac.h
new file mode 100644
index 000000000000..1d1771d71f3c
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_frac.h
@@ -0,0 +1,40 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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)(sISP_REG_BIT-1))
+#define sSHIFT				    (16-sISP_REG_BIT)
+#define uSHIFT				    ((unsigned)(16-uISP_REG_BIT))
+#define sFRACTION_BITS_FITTING(a) (a-sSHIFT)
+#define uFRACTION_BITS_FITTING(a) ((unsigned)(a-uSHIFT))
+#define sISP_VAL_MIN		      (-(1<<uISP_REG_BIT))
+#define sISP_VAL_MAX		      ((1<<uISP_REG_BIT)-1)
+#define uISP_VAL_MIN		      ((unsigned)0)
+#define uISP_VAL_MAX		      ((unsigned)((1<<uISP_REG_BIT)-1))
+
+/* a:fraction bits for 16bit precision, b:fraction bits for ISP precision */
+#define sDIGIT_FITTING(v, a, b) \
+	min(max((((v)>>sSHIFT) >> max(sFRACTION_BITS_FITTING(a)-(b), 0)), \
+	  sISP_VAL_MIN), sISP_VAL_MAX)
+#define uDIGIT_FITTING(v, a, b) \
+	min((unsigned)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/atomisp2/css2400/sh_css_host_data.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_host_data.c
new file mode 100644
index 000000000000..348183a221a8
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_host_data.c
@@ -0,0 +1,42 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 = sh_css_malloc(size);
+	if (!me->address) {
+		kfree(me);
+		return NULL;
+	}
+	return me;
+}
+
+void ia_css_host_data_free(struct ia_css_host_data *me)
+{
+	if (me) {
+		sh_css_free(me->address);
+		me->address = NULL;
+		kfree(me);
+	}
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.c
new file mode 100644
index 000000000000..0bfebced63af
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.c
@@ -0,0 +1,84 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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;
+}
+
+enum ia_css_err sh_css_hrt_sp_wait(void)
+{
+#if defined(HAS_IRQ_MAP_VERSION_2)
+	irq_sw_channel_id_t	irq_id = IRQ_SW_CHANNEL0_ID;
+#else
+	irq_sw_channel_id_t	irq_id = IRQ_SW_CHANNEL2_ID;
+#endif
+	/*
+	 * 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)) {
+		hrt_sleep();
+	}
+
+return IA_CSS_SUCCESS;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.h
new file mode 100644
index 000000000000..fd23ed1848ec
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_hrt.h
@@ -0,0 +1,34 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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);
+
+enum ia_css_err 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/atomisp2/css2400/sh_css_internal.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h
new file mode 100644
index 000000000000..e2b6f06ed099
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_internal.h
@@ -0,0 +1,1096 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 <stdarg.h>
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+#include "input_formatter.h"
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "input_system.h"
+#endif
+
+#include "ia_css_types.h"
+#include "ia_css_acc_types.h"
+#include "ia_css_buffer.h"
+
+#include "ia_css_binary.h"
+#if !defined(__ISP)
+#include "sh_css_firmware.h" /* not needed/desired on SP/ISP */
+#endif
+#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 */
+#if defined(HAS_SP_2400)
+#define NUM_CONTINUOUS_FRAMES	15
+#else
+#define NUM_CONTINUOUS_FRAMES	10
+#endif
+#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
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+#define SH_CSS_ENABLE_METADATA
+#endif
+
+#if defined(SH_CSS_ENABLE_METADATA) && !defined(USE_INPUT_SYSTEM_VERSION_2401)
+#define SH_CSS_ENABLE_METADATA_THREAD
+#endif
+
+
+ /*
+  * 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(SH_CSS_ENABLE_METADATA_THREAD)
+#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
+
+#ifdef __DISABLE_UNUSED_THREAD__
+	#define SH_CSS_MAX_SP_THREADS			0
+#else
+	#define SH_CSS_MAX_SP_THREADS		5
+#endif
+
+#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
+
+#if defined(HAS_BL)
+#define NUM_OF_BLS 1
+#else
+#define NUM_OF_BLS 0
+#endif
+
+/* Enum for order of Binaries */
+enum sh_css_order_binaries {
+	SP_FIRMWARE = 0,
+#if defined(HAS_BL)
+	BOOTLOADER_FIRMWARE,
+#endif
+	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 hrt_vaddress-es
+   Otherwise this will cause a fail in the function ref_sh_css_ddr_address_map
+ */
+struct sh_css_ddr_address_map {
+	hrt_vaddress isp_param;
+	hrt_vaddress isp_mem_param[SH_CSS_MAX_STAGES][IA_CSS_NUM_MEMORIES];
+	hrt_vaddress macc_tbl;
+	hrt_vaddress fpn_tbl;
+	hrt_vaddress sc_tbl;
+	hrt_vaddress tetra_r_x;
+	hrt_vaddress tetra_r_y;
+	hrt_vaddress tetra_gr_x;
+	hrt_vaddress tetra_gr_y;
+	hrt_vaddress tetra_gb_x;
+	hrt_vaddress tetra_gb_y;
+	hrt_vaddress tetra_b_x;
+	hrt_vaddress tetra_b_y;
+	hrt_vaddress tetra_ratb_x;
+	hrt_vaddress tetra_ratb_y;
+	hrt_vaddress tetra_batr_x;
+	hrt_vaddress tetra_batr_y;
+	hrt_vaddress 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 */
+	uint32_t                       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 */
+	struct ia_css_frame *delay_frames[MAX_NUM_VIDEO_DELAY_FRAMES];   /* reference input frame */
+#ifndef ISP2401
+	struct ia_css_frame *tnr_frames[NUM_VIDEO_TNR_FRAMES];   /* tnr frames */
+#else
+	struct ia_css_frame *tnr_frames[NUM_TNR_FRAMES];   /* tnr frames */
+#endif
+	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             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 {
+	uint16_t frame;
+	uint16_t line;
+	uint16_t pixel_distance;
+	uint16_t mipi_used_dword;
+	uint16_t sp_index;
+};
+
+struct sh_css_sp_debug_state {
+	uint16_t if_start_line;
+	uint16_t if_start_column;
+	uint16_t if_cropped_height;
+	uint16_t 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
+
+#if 1
+/* Example of just one global trace */
+#define SH_CSS_SP_DBG_NR_OF_TRACES	(1)
+#define SH_CSS_SP_DBG_TRACE_DEPTH	(40)
+#else
+/* E.g. if you like seperate traces for 4 threads */
+#define SH_CSS_SP_DBG_NR_OF_TRACES	(4)
+#define SH_CSS_SP_DBG_TRACE_DEPTH	(10)
+#endif
+
+#define SH_CSS_SP_DBG_TRACE_FILE_ID_BIT_POS (13)
+
+struct sh_css_sp_debug_trace {
+	uint16_t time_stamp;
+	uint16_t location;	/* bit 15..13 = file_id, 12..0 = line nr. */
+	uint32_t 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];
+	uint16_t index_last[SH_CSS_SP_DBG_NR_OF_TRACES];
+	uint8_t 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-reqeust 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.
+	 */
+	uint32_t dma_sw_reg;
+};
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+/* SP input formatter configuration.*/
+struct sh_css_sp_input_formatter_set {
+	uint32_t				stream_format;
+	input_formatter_cfg_t	config_a;
+	input_formatter_cfg_t	config_b;
+};
+#endif
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#define IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT (3)
+#endif
+
+/* SP configuration information */
+struct sh_css_sp_config {
+	uint8_t			no_isp_sync; /* Signal host immediately after start */
+	uint8_t			enable_raw_pool_locking; /**< Enable Raw Buffer Locking for HALv3 Support */
+	uint8_t			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(HAS_NO_INPUT_FORMATTER)
+	struct {
+		uint8_t					a_changed;
+		uint8_t					b_changed;
+		uint8_t					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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+	sync_generator_cfg_t	sync_gen;
+	tpg_cfg_t		tpg;
+	prbs_cfg_t		prbs;
+	input_system_cfg_t	input_circuit;
+	uint8_t			input_circuit_cfg_changed;
+	uint32_t		mipi_sizes_for_check[N_CSI_PORTS][IA_CSS_MIPI_SIZE_CHECK_MAX_NOF_ENTRIES_PER_PORT];
+#endif
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	uint8_t                 enable_isys_event_queue;
+#endif
+	uint8_t			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	(1 << 0)
+#define SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS_MASK \
+	((SH_CSS_PIPE_CONFIG_SAMPLE_PARAMS << SH_CSS_MAX_SP_THREADS)-1)
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+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 {
+	uint32_t	active[N_INPUT_SYSTEM_CSI_PORT];	/**< registered streams */
+	uint32_t	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 {
+	uint32_t	pipe_id;	/* the pipe ID */
+	uint32_t	pipe_num;	/* the dynamic pipe number */
+	uint32_t	thread_id;	/* the sp thread ID */
+	uint32_t	pipe_config;	/* the pipe config */
+	uint32_t	pipe_qos_config;	/* Bitmap of multiple QOS extension fw state.
+						(0xFFFFFFFF) indicates non QOS pipe.*/
+	uint32_t	inout_port_config;
+	uint32_t	required_bds_factor;
+	uint32_t	dvs_frame_delay;
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	uint32_t	input_system_mode;	/* enum ia_css_input_mode */
+	uint32_t	port_id;	/* port_id for input system */
+#endif
+	uint32_t	num_stages;		/* the pipe config */
+	uint32_t	running;	/* needed for pipe termination */
+	hrt_vaddress	sp_stage_addr[SH_CSS_MAX_STAGES];
+	hrt_vaddress	scaler_pp_lut; /* Early bound LUT */
+	uint32_t	dummy; /* stage ptr is only used on sp but lives in
+				  this struct; needs cleanup */
+	int32_t num_execs; /* number of times to run if this is
+			      an acceleration pipe. */
+#if defined(SH_CSS_ENABLE_METADATA)
+	struct {
+		uint32_t        format;   /* Metadata format in hrt format */
+		uint32_t        width;    /* Width of a line */
+		uint32_t        height;   /* Number of lines */
+		uint32_t        stride;   /* Stride (in bytes) per line */
+		uint32_t        size;     /* Total size (in bytes) */
+		hrt_vaddress    cont_buf; /* Address of continuous buffer */
+	} metadata;
+#endif
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+	uint32_t	output_frame_queue_id;
+#endif
+	union {
+		struct {
+			uint32_t	bytes_available;
+		} bin;
+		struct {
+			uint32_t	height;
+			uint32_t	width;
+			uint32_t	padded_width;
+			uint32_t	max_input_width;
+			uint32_t	raw_bit_depth;
+		} raw;
+	} copy;
+#ifdef ISP2401
+
+	/* Parameters passed to Shading Correction kernel. */
+	struct {
+		uint32_t internal_frame_origin_x_bqs_on_sctbl; /* Origin X (bqs) of internal frame on shading table */
+		uint32_t internal_frame_origin_y_bqs_on_sctbl; /* Origin Y (bqs) of internal frame on shading table */
+	} shading;
+#endif
+};
+
+/*
+ * 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 addres 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 seperately
+ */
+#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;
+#if defined SH_CSS_ENABLE_METADATA
+	struct ia_css_buffer_sp metadata_buf;
+#endif
+};
+
+/* Information for a single pipeline stage for an ISP */
+struct sh_css_isp_stage {
+	/*
+	 * For compatability 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 compatability 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
+	 */
+	uint8_t			num; /* Stage number */
+	uint8_t			isp_online;
+	uint8_t			isp_copy_vf;
+	uint8_t			isp_copy_output;
+	uint8_t			sp_enable_xnr;
+	uint8_t			isp_deci_log_factor;
+	uint8_t			isp_vf_downscale_bits;
+	uint8_t			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
+ */
+	uint8_t			program_input_circuit;
+/* enum ia_css_pipeline_stage_sp_func	func; */
+	uint8_t			func;
+	/* The type of the pipe-stage */
+	/* enum sh_css_stage_type	stage_type; */
+	uint8_t			stage_type;
+	uint8_t			num_stripes;
+	uint8_t			isp_pipe_version;
+	struct {
+		uint8_t		vf_output;
+		uint8_t		s3a;
+		uint8_t		sdis;
+		uint8_t		dvs_stats;
+		uint8_t		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;
+	hrt_vaddress			isp_stage_addr;
+	hrt_vaddress			xmem_bin_addr;
+	hrt_vaddress			xmem_map_addr;
+
+	uint16_t		top_cropping;
+	uint16_t		row_stripes_height;
+	uint16_t		row_stripes_overlap_lines;
+	uint8_t			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 transfered 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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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 */
+	hrt_vaddress			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];
+};
+
+#define CONFIG_ON_FRAME_ENQUEUE() 0
+
+/**
+ * @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
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+/* 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)
+#else
+#define  IA_CSS_NUM_ELEMS_SP2HOST_ISYS_EVENT_QUEUE 0
+#define  IA_CSS_NUM_ELEMS_HOST2SP_ISYS_EVENT_QUEUE 0
+#define  IA_CSS_NUM_ELEMS_HOST2SP_TAG_CMD_QUEUE  0
+#endif
+
+#if defined(HAS_SP_2400)
+#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 */
+#else
+#define  IA_CSS_NUM_ELEMS_HOST2SP_PSYS_EVENT_QUEUE    6
+#define  IA_CSS_NUM_ELEMS_SP2HOST_BUFFER_QUEUE        6
+#define  IA_CSS_NUM_ELEMS_SP2HOST_PSYS_EVENT_QUEUE    6
+#endif
+
+struct sh_css_hmm_buffer {
+	union {
+		struct ia_css_isp_3a_statistics  s3a;
+		struct ia_css_isp_dvs_statistics dis;
+		hrt_vaddress skc_dvs_statistics;
+		hrt_vaddress lace_stat;
+		struct ia_css_metadata	metadata;
+		struct frame_data_wrapper {
+			hrt_vaddress	frame_data;
+			uint32_t	flashed;
+			uint32_t	exp_id;
+			uint32_t	isp_parameters_id; /**< Unique ID to track which config was
+								actually applied to a particular frame */
+#if CONFIG_ON_FRAME_ENQUEUE()
+			struct sh_css_config_on_frame_enqueue config_on_frame_enqueue;
+#endif
+		} frame;
+		hrt_vaddress 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.
+	 */
+#if !defined(__ISP)
+	CSS_ALIGN(uint64_t cookie_ptr, 8); /* TODO: check if this alignment is needed */
+	uint64_t kernel_ptr;
+#else
+	CSS_ALIGN(struct { uint32_t a[2]; } cookie_ptr, 8); /* TODO: check if this alignment is needed */
+	struct { uint32_t a[2]; } kernel_ptr;
+#endif
+	struct ia_css_time_meas timing_data;
+	clock_value_t isys_eof_clock_tick;
+};
+#if CONFIG_ON_FRAME_ENQUEUE()
+#define SIZE_OF_FRAME_STRUCT						\
+	(SIZE_OF_HRT_VADDRESS +						\
+	(3 * sizeof(uint32_t)) +					\
+	sizeof(uint32_t))
+#else
+#define SIZE_OF_FRAME_STRUCT						\
+	(SIZE_OF_HRT_VADDRESS +						\
+	(3 * sizeof(uint32_t)))
+#endif
+
+#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,
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	sh_css_sp2host_isys_event_queue,
+	sh_css_host2sp_isys_event_queue,
+	sh_css_host2sp_tag_cmd_queue,
+#endif
+};
+
+struct sh_css_event_irq_mask {
+	uint16_t or_mask;
+	uint16_t 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 dependant and thus non-portable
+	 */
+	uint32_t 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.
+	 */
+	hrt_vaddress host2sp_offline_frames[NUM_CONTINUOUS_FRAMES];
+	hrt_vaddress host2sp_offline_metadata[NUM_CONTINUOUS_FRAMES];
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	hrt_vaddress host2sp_mipi_frames[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	hrt_vaddress host2sp_mipi_metadata[N_CSI_PORTS][NUM_MIPI_FRAMES_PER_STREAM];
+	uint32_t host2sp_num_mipi_frames[N_CSI_PORTS];
+#endif
+	uint32_t host2sp_cont_avail_num_raw_frames;
+	uint32_t host2sp_cont_extra_num_raw_frames;
+	uint32_t host2sp_cont_target_num_raw_frames;
+	struct sh_css_event_irq_mask host2sp_event_irq_mask[NR_OF_PIPELINES];
+
+};
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+#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))
+#else
+#define SIZE_OF_HOST_SP_COMMUNICATION_STRUCT				\
+	(sizeof(uint32_t) +						\
+	(NUM_CONTINUOUS_FRAMES * SIZE_OF_HRT_VADDRESS * 2) +		\
+	(3 * sizeof(uint32_t)) +						\
+	(NR_OF_PIPELINES * SIZE_OF_SH_CSS_EVENT_IRQ_MASK_STRUCT))
+#endif
+
+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];
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	/*
+	 * 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];
+#endif
+};
+
+#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)))
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#define IA_CSS_NUM_CIRCBUF_DESCS 5
+#else
+#ifndef ISP2401
+#define IA_CSS_NUM_CIRCBUF_DESCS 3
+#else
+#define IA_CSS_NUM_CIRCBUF_DESCS 2
+#endif
+#endif
+
+#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 (*sh_css_printf)(const char *fmt, va_list args);
+
+STORAGE_CLASS_INLINE void
+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);
+	}
+}
+
+STORAGE_CLASS_INLINE void
+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.
+*/
+#if !defined(__ISP)
+hrt_vaddress
+sh_css_params_ddr_address_map(void);
+
+enum ia_css_err
+sh_css_params_init(void);
+
+void
+sh_css_params_uninit(void);
+
+void *sh_css_malloc(size_t size);
+
+void *sh_css_calloc(size_t N, size_t size);
+
+void sh_css_free(void *ptr);
+
+/* 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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+
+unsigned int
+sh_css_get_mipi_sizes_for_check(const unsigned int port, const unsigned int idx);
+
+#endif
+
+hrt_vaddress
+sh_css_store_sp_group_to_ddr(void);
+
+hrt_vaddress
+sh_css_store_sp_stage_to_ddr(unsigned pipe, unsigned stage);
+
+hrt_vaddress
+sh_css_store_isp_stage_to_ddr(unsigned pipe, unsigned 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 USE_INPUT_SYSTEM_VERSION_2401
+void
+ia_css_get_crop_offsets(
+		struct ia_css_pipe *pipe,
+		struct ia_css_frame_info *in_frame);
+#endif
+#endif /* !defined(__ISP) */
+
+#endif /* _SH_CSS_INTERNAL_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c
new file mode 100644
index 000000000000..37e954aea36f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_irq.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_irq.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_legacy.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_legacy.h
new file mode 100644
index 000000000000..e12789236bb9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_legacy.h
@@ -0,0 +1,88 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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,
+#ifndef ISP2401
+	IA_CSS_PIPE_ID_ACC,
+	IA_CSS_PIPE_ID_NUM
+#else
+	IA_CSS_PIPE_ID_ACC
+#endif
+};
+#ifdef ISP2401
+#define IA_CSS_PIPE_ID_NUM (IA_CSS_PIPE_ID_ACC+1)
+#endif
+
+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;
+};
+
+#define DEFAULT_PIPE_EXTRA_CONFIG \
+{ \
+	false,				/* enable_raw_binning */ \
+	false,				/* enable_yuv_ds */ \
+	false,				/* enable_high_speed */ \
+	false,				/* enable_dvs_6axis */ \
+	false,				/* enable_reduced_pipe */ \
+	false,				/* enable_fractional_ds */ \
+	false,				/* disable_vf_pp */ \
+}
+
+enum ia_css_err
+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);
+
+enum ia_css_err
+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. */
+enum ia_css_err
+sh_css_set_black_frame(struct ia_css_stream *stream,
+			const struct ia_css_frame *raw_black_frame);
+
+#ifndef ISP2401
+void
+sh_css_enable_cont_capt(bool enable, bool stop_copy_preview);
+
+#endif
+#endif /* _SH_CSS_LEGACY_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metadata.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metadata.c
new file mode 100644
index 000000000000..ebdf84d4a138
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metadata.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_metadata.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metrics.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metrics.c
new file mode 100644
index 000000000000..48e5542b3a43
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metrics.c
@@ -0,0 +1,176 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 i;
+
+	assert(histogram != NULL);
+
+	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 length)
+{
+	assert(histogram != NULL);
+
+	if (histogram->length)
+		return;
+	if (histogram->run)
+		return;
+	histogram->run = sh_css_malloc(length * sizeof(*histogram->run));
+	if (!histogram->run)
+		return;
+	histogram->stall = sh_css_malloc(length * sizeof(*histogram->stall));
+	if (!histogram->stall)
+		return;
+	histogram->msink = sh_css_malloc(length * sizeof(*histogram->msink));
+	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 != NULL);
+	assert(*l != NULL);
+	assert(metrics != NULL);
+
+	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 != NULL);
+
+	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 SUSPEND
+	unsigned int sc = 0;
+	unsigned int stopped_sc = 0;
+	unsigned int resume_sc = 0;
+#endif
+
+
+#if MULTIPLE_PCS
+	int i;
+	unsigned int pc_tab[NOF_PCS];
+
+	for (i = 0; i < NOF_PCS; i++)
+		pc_tab[i] = 0;
+#endif
+
+	if (!pc_histogram_enabled)
+		return;
+
+	if (isp_histogram) {
+#if SUSPEND
+		/* STOP the ISP */
+		isp_ctrl_store(ISP0_ID, ISP_SC_REG, STOP_MASK);
+#endif
+		msink = isp_ctrl_load(ISP0_ID, ISP_CTRL_SINK_REG);
+#if MULTIPLE_PCS
+		for (i = 0; i < NOF_PCS; i++)
+			pc_tab[i] = isp_ctrl_load(ISP0_ID, ISP_PC_REG);
+#else
+		pc = isp_ctrl_load(ISP0_ID, ISP_PC_REG);
+#endif
+
+#if SUSPEND
+		/* RESUME the ISP */
+		isp_ctrl_store(ISP0_ID, ISP_SC_REG, RESUME_MASK);
+#endif
+		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/atomisp2/css2400/sh_css_metrics.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metrics.h
new file mode 100644
index 000000000000..40840ea318ab
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_metrics.h
@@ -0,0 +1,76 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 length;
+	unsigned *run;
+	unsigned *stall;
+	unsigned *msink;
+};
+
+#if !defined(__USE_DESIGNATED_INITIALISERS__)
+#define DEFAULT_PC_HISTOGRAM \
+{ \
+	0, \
+	NULL, \
+	NULL, \
+	NULL \
+}
+#endif
+
+struct sh_css_binary_metrics {
+	unsigned mode;
+	unsigned id;
+	struct sh_css_pc_histogram isp_histogram;
+	struct sh_css_pc_histogram sp_histogram;
+	struct sh_css_binary_metrics *next;
+};
+
+#if !defined(__USE_DESIGNATED_INITIALISERS__)
+#define DEFAULT_BINARY_METRICS \
+{ \
+	0, \
+	0, \
+	DEFAULT_PC_HISTOGRAM, \
+	DEFAULT_PC_HISTOGRAM, \
+	NULL \
+}
+#endif
+
+struct ia_css_frame_metrics {
+	unsigned 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/atomisp2/css2400/sh_css_mipi.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c
new file mode 100644
index 000000000000..7e3893c6c08a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.c
@@ -0,0 +1,749 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 */
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+static uint32_t ref_count_mipi_allocation[N_CSI_PORTS]; /* Initialized in mipi_init */
+#endif
+
+enum ia_css_err
+ia_css_mipi_frame_specify(const unsigned int size_mem_words,
+				const bool contiguous)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	my_css.size_mem_words = size_mem_words;
+	(void)contiguous;
+
+	return err;
+}
+
+#ifdef ISP2401
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+/*
+ * Check if a source port or TPG/PRBS ID is valid
+ */
+static bool ia_css_mipi_is_source_port_valid(struct ia_css_pipe *pipe,
+						unsigned int *pport)
+{
+	bool ret = true;
+	unsigned int port = 0;
+	unsigned int max_ports = 0;
+
+	switch (pipe->stream->config.mode) {
+	case IA_CSS_INPUT_MODE_BUFFERED_SENSOR:
+		port = (unsigned int) pipe->stream->config.source.port.port;
+		max_ports = N_CSI_PORTS;
+		break;
+	case IA_CSS_INPUT_MODE_TPG:
+		port = (unsigned int) pipe->stream->config.source.tpg.id;
+		max_ports = N_CSS_TPG_IDS;
+		break;
+	case IA_CSS_INPUT_MODE_PRBS:
+		port = (unsigned int) pipe->stream->config.source.prbs.id;
+		max_ports = N_CSS_PRBS_IDS;
+		break;
+	default:
+		assert(false);
+		ret = false;
+		break;
+	}
+
+	if (ret) {
+		assert(port < max_ports);
+
+		if (port >= max_ports)
+			ret = false;
+	}
+
+	*pport = port;
+
+	return ret;
+}
+#endif
+
+#endif
+/* 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
+ */
+enum ia_css_err
+ia_css_mipi_frame_calculate_size(const unsigned int width,
+				const unsigned int height,
+				const enum ia_css_stream_format format,
+				const bool hasSOLandEOL,
+				const unsigned int embedded_data_size_words,
+				unsigned int *size_mem_words)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	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(USE_INPUT_SYSTEM_VERSION_2401)
+	/* 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 IA_CSS_STREAM_FORMAT_RAW_6:		/* 4p, 3B, 24bits */
+		bits_per_pixel = 6;	break;
+	case IA_CSS_STREAM_FORMAT_RAW_7:		/* 8p, 7B, 56bits */
+		bits_per_pixel = 7;		break;
+	case IA_CSS_STREAM_FORMAT_RAW_8:		/* 1p, 1B, 8bits */
+	case IA_CSS_STREAM_FORMAT_BINARY_8:		/*  8bits, TODO: check. */
+	case IA_CSS_STREAM_FORMAT_YUV420_8:		/* odd 2p, 2B, 16bits, even 2p, 4B, 32bits */
+		bits_per_pixel = 8;		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_10:		/* odd 4p, 5B, 40bits, even 4p, 10B, 80bits */
+	case IA_CSS_STREAM_FORMAT_RAW_10:		/* 4p, 5B, 40bits */
+#if !defined(HAS_NO_PACKED_RAW_PIXELS)
+		/* 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;
+#else
+		bits_per_pixel = 16;
+#endif
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:	/* 2p, 3B, 24bits */
+	case IA_CSS_STREAM_FORMAT_RAW_12:		/* 2p, 3B, 24bits */
+		bits_per_pixel = 12;	break;
+	case IA_CSS_STREAM_FORMAT_RAW_14:		/* 4p, 7B, 56bits */
+		bits_per_pixel = 14;	break;
+	case IA_CSS_STREAM_FORMAT_RGB_444:		/* 1p, 2B, 16bits */
+	case IA_CSS_STREAM_FORMAT_RGB_555:		/* 1p, 2B, 16bits */
+	case IA_CSS_STREAM_FORMAT_RGB_565:		/* 1p, 2B, 16bits */
+	case IA_CSS_STREAM_FORMAT_YUV422_8:		/* 2p, 4B, 32bits */
+		bits_per_pixel = 16;	break;
+	case IA_CSS_STREAM_FORMAT_RGB_666:		/* 4p, 9B, 72bits */
+		bits_per_pixel = 18;	break;
+	case IA_CSS_STREAM_FORMAT_YUV422_10:		/* 2p, 5B, 40bits */
+		bits_per_pixel = 20;	break;
+	case IA_CSS_STREAM_FORMAT_RGB_888:		/* 1p, 3B, 24bits */
+		bits_per_pixel = 24;	break;
+
+	case IA_CSS_STREAM_FORMAT_YUV420_16:		/* Not supported */
+	case IA_CSS_STREAM_FORMAT_YUV422_16:		/* Not supported */
+	case IA_CSS_STREAM_FORMAT_RAW_16:		/* TODO: not specified in MIPI SPEC, check */
+	default:
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 == IA_CSS_STREAM_FORMAT_YUV420_8
+			|| format == IA_CSS_STREAM_FORMAT_YUV420_10
+			|| format == IA_CSS_STREAM_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;
+}
+
+#if !defined(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+enum ia_css_err
+ia_css_mipi_frame_enable_check_on_size(const enum ia_css_csi2_port port,
+				const unsigned int	size_mem_words)
+{
+	uint32_t idx;
+
+	enum ia_css_err err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+
+	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 = IA_CSS_SUCCESS;
+	}
+
+	return err;
+}
+#endif
+
+void
+mipi_init(void)
+{
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	unsigned int i;
+
+	for (i = 0; i < N_CSI_PORTS; i++)
+		ref_count_mipi_allocation[i] = 0;
+#endif
+}
+
+enum ia_css_err
+calculate_mipi_buff_size(
+		struct ia_css_stream_config *stream_cfg,
+		unsigned int *size_mem_words)
+{
+#if !defined(USE_INPUT_SYSTEM_VERSION_2401)
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+	(void)stream_cfg;
+	(void)size_mem_words;
+#else
+	unsigned int width;
+	unsigned int height;
+	enum ia_css_stream_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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	/**
+#ifndef ISP2401
+	 * zhengjie.lu@intel.com
+	 *
+#endif
+	 * 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 */
+
+	/**
+#ifndef ISP2401
+	 * zhengjie.lu@intel.com
+	 *
+#endif
+	 * 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 == IA_CSS_STREAM_FORMAT_RAW_10 && pack_raw_pixels) ? bits_per_pixel : 16;
+	if (bits_per_pixel == 0)
+		return IA_CSS_ERR_INTERNAL_ERROR;
+
+	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 == IA_CSS_STREAM_FORMAT_YUV420_8
+		|| format == IA_CSS_STREAM_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);
+#endif
+	return err;
+}
+
+enum ia_css_err
+allocate_mipi_frames(struct ia_css_pipe *pipe, struct ia_css_stream_info *info)
+{
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+#ifndef ISP2401
+	unsigned int port;
+#else
+	unsigned int port = 0;
+#endif
+	struct ia_css_frame_info mipi_intermediate_info;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+		"allocate_mipi_frames(%p) enter:\n", pipe);
+
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+	if ((pipe == NULL) || (pipe->stream == NULL)) {
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			"allocate_mipi_frames(%p) exit: pipe or stream is null.\n",
+			pipe);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	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 IA_CSS_SUCCESS;
+	}
+
+#endif
+#ifndef ISP2401
+	if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+#else
+	if (!(pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR ||
+		pipe->stream->config.mode == IA_CSS_INPUT_MODE_TPG ||
+		pipe->stream->config.mode == IA_CSS_INPUT_MODE_PRBS)) {
+#endif
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			"allocate_mipi_frames(%p) exit: no buffers needed for pipe mode.\n",
+			pipe);
+		return IA_CSS_SUCCESS; /* AM TODO: Check  */
+	}
+
+#ifndef ISP2401
+	port = (unsigned int) pipe->stream->config.source.port.port;
+	assert(port < N_CSI_PORTS);
+	if (port >= N_CSI_PORTS) {
+#else
+	if (!ia_css_mipi_is_source_port_valid(pipe, &port)) {
+#endif
+		ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+			"allocate_mipi_frames(%p) exit: error: port is not correct (port=%d).\n",
+			pipe, port);
+		return IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+	err = calculate_mipi_buff_size(
+			&(pipe->stream->config),
+			&(my_css.mipi_frame_size[port]));
+#endif
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+	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 IA_CSS_SUCCESS;
+	}
+#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.
+	 */
+	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 IA_CSS_SUCCESS;
+	}
+#endif
+
+	ref_count_mipi_allocation[port]++;
+
+	/* TODO: Cleaning needed. */
+	/* This code needs to modified to allocate the MIPI frames in the correct normal way
+	  with an allocate from info, by justin */
+	mipi_intermediate_info = pipe->pipe_settings.video.video_binary.internal_frame_info;
+	mipi_intermediate_info.res.width = 0;
+	mipi_intermediate_info.res.height = 0;
+	/* To indicate it is not (yet) valid format. */
+	mipi_intermediate_info.format = IA_CSS_FRAME_FORMAT_NUM;
+	mipi_intermediate_info.padded_width = 0;
+	mipi_intermediate_info.raw_bit_depth = 0;
+
+	/* 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 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,
+					false);
+				if (err != IA_CSS_SUCCESS) {
+					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_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+						"allocate_mipi_frames(%p, %d) exit: error: allocation failed.\n",
+						pipe, port);
+					return err;
+				}
+			}
+			if (info->metadata_info.size > 0) {
+				/* free previous metadata buffer */
+				if (my_css.mipi_metadata[port][i] != NULL) {
+					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] == NULL) {
+						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;
+#else
+	(void)pipe;
+	(void)info;
+	return IA_CSS_SUCCESS;
+#endif
+}
+
+enum ia_css_err
+free_mipi_frames(struct ia_css_pipe *pipe)
+{
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+#ifndef ISP2401
+	unsigned int port;
+#else
+	unsigned int port = 0;
+#endif
+	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 != NULL) {
+		assert(pipe->stream != NULL);
+		if ((pipe == NULL) || (pipe->stream == NULL)) {
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				"free_mipi_frames(%p) exit: error: pipe or stream is null.\n",
+				pipe);
+			return IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+
+#ifndef ISP2401
+		if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+#else
+		if (!(pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR ||
+			pipe->stream->config.mode == IA_CSS_INPUT_MODE_TPG ||
+			pipe->stream->config.mode == IA_CSS_INPUT_MODE_PRBS)) {
+#endif
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+				"free_mipi_frames(%p) exit: error: wrong mode.\n",
+				pipe);
+			return err;
+		}
+
+#ifndef ISP2401
+		port = (unsigned int) pipe->stream->config.source.port.port;
+		assert(port < N_CSI_PORTS);
+		if (port >= N_CSI_PORTS) {
+#else
+		if (!ia_css_mipi_is_source_port_valid(pipe, &port)) {
+#endif
+			ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+#ifndef ISP2401
+				"free_mipi_frames(%p, %d) exit: error: pipe port is not correct.\n",
+#else
+				"free_mipi_frames(%p) exit: error: pipe port is not correct (port=%d).\n",
+#endif
+				pipe, port);
+			return err;
+		}
+#ifdef ISP2401
+
+#endif
+		if (ref_count_mipi_allocation[port] > 0) {
+#if defined(USE_INPUT_SYSTEM_VERSION_2)
+			assert(ref_count_mipi_allocation[port] == 1);
+			if (ref_count_mipi_allocation[port] != 1) {
+				ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE_PRIVATE,
+					"free_mipi_frames(%p) exit: error: wrong ref_count (ref_count=%d).\n",
+					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] != NULL) {
+						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] != NULL) {
+						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(USE_INPUT_SYSTEM_VERSION_2401)
+			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] != NULL) {
+					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] != NULL) {
+					ia_css_metadata_free(my_css.mipi_metadata[port][i]);
+					my_css.mipi_metadata[port][i] = NULL;
+				}
+			}
+			ref_count_mipi_allocation[port] = 0;
+		}
+	}
+#else
+	(void)pipe;
+#endif
+	return IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+send_mipi_frames(struct ia_css_pipe *pipe)
+{
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	enum ia_css_err err = IA_CSS_ERR_INTERNAL_ERROR;
+	unsigned int i;
+#ifndef ISP2401
+	unsigned int port;
+#else
+	unsigned int port = 0;
+#endif
+
+	IA_CSS_ENTER_PRIVATE("pipe=%d", pipe);
+
+	assert(pipe != NULL);
+	assert(pipe->stream != NULL);
+	if (pipe == NULL || pipe->stream == NULL) {
+		IA_CSS_ERROR("pipe or stream is null");
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	/* multi stream video needs mipi buffers */
+	/* nothing to be done in other cases. */
+#ifndef ISP2401
+	if (pipe->stream->config.mode != IA_CSS_INPUT_MODE_BUFFERED_SENSOR) {
+#else
+	if (!(pipe->stream->config.mode == IA_CSS_INPUT_MODE_BUFFERED_SENSOR ||
+		pipe->stream->config.mode == IA_CSS_INPUT_MODE_TPG ||
+		pipe->stream->config.mode == IA_CSS_INPUT_MODE_PRBS)) {
+#endif
+		IA_CSS_LOG("nothing to be done for this mode");
+		return IA_CSS_SUCCESS;
+		/* TODO: AM: maybe this should be returning an error. */
+	}
+
+#ifndef ISP2401
+	port = (unsigned int) pipe->stream->config.source.port.port;
+	assert(port < N_CSI_PORTS);
+	if (port >= N_CSI_PORTS) {
+		IA_CSS_ERROR("invalid port specified (%d)", port);
+#else
+	if (!ia_css_mipi_is_source_port_valid(pipe, &port)) {
+		IA_CSS_ERROR("send_mipi_frames(%p) exit: invalid port specified (port=%d).\n", pipe, port);
+#endif
+		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(IA_CSS_SUCCESS);
+#else
+	(void)pipe;
+#endif
+	return IA_CSS_SUCCESS;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.h
new file mode 100644
index 000000000000..990f678422fd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mipi.h
@@ -0,0 +1,49 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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);
+
+enum ia_css_err
+allocate_mipi_frames(struct ia_css_pipe *pipe, struct ia_css_stream_info *info);
+
+enum ia_css_err
+free_mipi_frames(struct ia_css_pipe *pipe);
+
+enum ia_css_err
+send_mipi_frames(struct ia_css_pipe *pipe);
+
+/**
+ * @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
+ */
+enum ia_css_err
+calculate_mipi_buff_size(
+		struct ia_css_stream_config *stream_cfg,
+		unsigned int *size_mem_words);
+
+#endif /* __SH_CSS_MIPI_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c
new file mode 100644
index 000000000000..6de8472f1b07
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_mmu.c
@@ -0,0 +1,62 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#ifdef ISP2401
+#include "ia_css_mmu_private.h"
+#endif
+#include <ia_css_debug.h>
+#include "sh_css_sp.h"
+#include "sh_css_firmware.h"
+#include "sp.h"
+#ifdef ISP2401
+#include "mmu_device.h"
+#endif
+
+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");
+}
+#ifdef ISP2401
+
+/* Deprecated, this is an HRT backend function (memory_access.h) */
+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("");
+}
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_morph.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_morph.c
new file mode 100644
index 000000000000..1f4fa25b1e79
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_morph.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_morph.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_dvs.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_dvs.c
new file mode 100644
index 000000000000..57dd5e7988c9
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_dvs.c
@@ -0,0 +1,267 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#include "memory_access.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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_dvs_6axis_config  *dvs_config = NULL;
+
+	dvs_config = (struct ia_css_dvs_6axis_config *)sh_css_malloc(sizeof(struct ia_css_dvs_6axis_config));
+	if (dvs_config == NULL)	{
+		IA_CSS_ERROR("out of memory");
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+	else
+	{	/*Initialize new struct with latest config settings*/
+		if (NULL != 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 (NULL != 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 = (uint32_t *)sh_css_malloc(width_y * height_y * sizeof(uint32_t));
+		if (dvs_config->xcoords_y == NULL) {
+			IA_CSS_ERROR("out of memory");
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			goto exit;
+		}
+
+		dvs_config->ycoords_y = (uint32_t *)sh_css_malloc(width_y * height_y * sizeof(uint32_t));
+		if (dvs_config->ycoords_y == NULL) {
+			IA_CSS_ERROR("out of memory");
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			goto exit;
+		}
+
+		/* Generate UV buffers  */
+		IA_CSS_LOG("UV W %d H %d", width_uv, height_uv);
+
+		dvs_config->xcoords_uv = (uint32_t *)sh_css_malloc(width_uv * height_uv * sizeof(uint32_t));
+		if (dvs_config->xcoords_uv == NULL) {
+			IA_CSS_ERROR("out of memory");
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			goto exit;
+		}
+
+		dvs_config->ycoords_uv = (uint32_t *)sh_css_malloc(width_uv * height_uv * sizeof(uint32_t));
+		if (dvs_config->ycoords_uv == NULL) {
+			IA_CSS_ERROR("out of memory");
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		}
+exit:
+		if (err != IA_CSS_SUCCESS) {
+			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 != NULL);
+	assert(dvs_offset != NULL);
+
+	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(NULL != 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 != NULL);
+	assert(*dvs_6axis_config != NULL);
+
+	if ((dvs_6axis_config != NULL) && (*dvs_6axis_config != NULL))
+	{
+		IA_CSS_ENTER_PRIVATE("dvs_6axis_config %p", (*dvs_6axis_config));
+		if ((*dvs_6axis_config)->xcoords_y != NULL)
+		{
+			sh_css_free((*dvs_6axis_config)->xcoords_y);
+			(*dvs_6axis_config)->xcoords_y = NULL;
+		}
+
+		if ((*dvs_6axis_config)->ycoords_y != NULL)
+		{
+			sh_css_free((*dvs_6axis_config)->ycoords_y);
+			(*dvs_6axis_config)->ycoords_y = NULL;
+		}
+
+		/* Free up UV buffers */
+		if ((*dvs_6axis_config)->xcoords_uv != NULL)
+		{
+			sh_css_free((*dvs_6axis_config)->xcoords_uv);
+			(*dvs_6axis_config)->xcoords_uv = NULL;
+		}
+
+		if ((*dvs_6axis_config)->ycoords_uv != NULL)
+		{
+			sh_css_free((*dvs_6axis_config)->ycoords_uv);
+			(*dvs_6axis_config)->ycoords_uv = NULL;
+		}
+
+		IA_CSS_LEAVE_PRIVATE("dvs_6axis_config %p", (*dvs_6axis_config));
+		sh_css_free(*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 != NULL);
+	assert(dvs_config_dst != NULL);
+	assert(dvs_config_src->xcoords_y != NULL);
+	assert(dvs_config_src->xcoords_uv != NULL);
+	assert(dvs_config_src->ycoords_y != NULL);
+	assert(dvs_config_src->ycoords_uv != NULL);
+	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 (DVS_STATISTICS == type)
+	{
+		ia_css_get_dvs_statistics(host_stats->p_dvs_statistics_host,
+			isp_stats->p_dvs_statistics_isp);
+	} else if (DVS2_STATISTICS == type)
+	{
+		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/atomisp2/css2400/sh_css_param_dvs.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_dvs.h
new file mode 100644
index 000000000000..79b563dc78ee
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_dvs.h
@@ -0,0 +1,86 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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>
+#ifdef ISP2401
+#include <sh_css_dvs_info.h>
+#endif
+#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*/
+
+#ifndef ISP2401
+/* 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))
+#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
+#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)
+#ifndef ISP2401
+#define DVS_INPUT_BYTES_PER_PIXEL (1)
+#endif
+#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))
+
+#ifndef ISP2401
+/* 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
+
+#endif
+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/atomisp2/css2400/sh_css_param_shading.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_shading.c
new file mode 100644
index 000000000000..eaf60e7b2dac
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_shading.c
@@ -0,0 +1,419 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL);
+	assert(out_table != NULL);
+
+	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,
+#ifndef ISP2401
+	const struct ia_css_binary *binary)
+#else
+	unsigned int table_width,
+	unsigned int table_height)
+#endif
+{
+	/* initialize table with ones, shift becomes zero */
+#ifndef ISP2401
+	unsigned int i, j, table_width, table_height;
+#else
+	unsigned int i, j;
+#endif
+	struct ia_css_shading_table *result;
+
+	assert(target_table != NULL);
+#ifndef ISP2401
+	assert(binary != NULL);
+#endif
+
+#ifndef ISP2401
+	table_width  = binary->sctbl_width_per_color;
+	table_height = binary->sctbl_height;
+#endif
+	result = ia_css_shading_table_alloc(table_width, table_height);
+	if (result == NULL) {
+		*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,
+		     left_padding,
+		     top_padding,
+		     padded_width,
+		     left_cropping,
+		     i;
+	unsigned int bds_numerator, bds_denominator;
+	int right_padding;
+
+	struct ia_css_shading_table *result;
+
+	assert(target_table != NULL);
+	assert(binary != NULL);
+
+	if (!in_table) {
+#ifndef ISP2401
+		sh_css_params_shading_id_table_generate(target_table, binary);
+#else
+		sh_css_params_shading_id_table_generate(target_table,
+			binary->sctbl_legacy_width_per_color, binary->sctbl_legacy_height);
+#endif
+		return;
+	}
+
+	padded_width = binary->in_frame_info.padded_width;
+	/* We use the ISP input resolution for the shading table because
+	   shading correction is performed in the bayer domain (before bayer
+	   down scaling). */
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+	padded_width = CEIL_MUL(binary->effective_in_frame_res.width + 2*ISP_VEC_NELEMS,
+					2*ISP_VEC_NELEMS);
+#endif
+	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;
+
+#if !defined(USE_WINDOWS_BINNING_FACTOR)
+	/* @deprecated{This part of the code will be replaced by the code
+	 * in the #else section below to make the calculation same across
+	 * all platforms.
+	 * Android and Windows platforms interpret the binning_factor parameter
+	 * differently. In Android, the binning factor is expressed in the form
+	 * 2^N * 2^N, whereas in Windows platform, the binning factor is N*N}
+	 */
+
+	/* 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;
+#else
+	input_width   *= sensor_binning;
+	input_height  *= sensor_binning;
+	left_padding  *= sensor_binning;
+	right_padding *= sensor_binning;
+	top_padding   *= sensor_binning;
+#endif /*USE_WINDOWS_BINNING_FACTOR*/
+
+	/* 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);
+
+#ifndef ISP2401
+	table_width  = binary->sctbl_width_per_color;
+	table_height = binary->sctbl_height;
+#else
+	/* 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_legacy_width_per_color;
+	table_height = binary->sctbl_legacy_height;
+#endif
+
+	result = ia_css_shading_table_alloc(table_width, table_height);
+	if (result == NULL) {
+		*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 == NULL) {
+		IA_CSS_ERROR("out of memory");
+		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] =
+		    sh_css_malloc(width * height * sizeof(*me->data[0]));
+		if (me->data[i] == NULL) {
+			unsigned int j;
+			for (j = 0; j < i; j++) {
+				sh_css_free(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 == NULL)
+		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]) {
+			sh_css_free(table->data[i]);
+			table->data[i] = NULL;
+		}
+	}
+	kfree(table);
+
+	IA_CSS_LEAVE("");
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_shading.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_shading.h
new file mode 100644
index 000000000000..e87863b7c8cc
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_param_shading.h
@@ -0,0 +1,39 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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,
+#ifndef ISP2401
+	const struct ia_css_binary *binary);
+#else
+	unsigned int table_width,
+	unsigned int table_height);
+#endif
+
+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/atomisp2/css2400/sh_css_params.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
new file mode 100644
index 000000000000..561f4a7236f7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.c
@@ -0,0 +1,5268 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+#if !defined(HAS_NO_HMEM)
+#ifndef __INLINE_HMEM__
+#define __INLINE_HMEM__
+#endif
+#include "hmem.h"
+#endif /* !defined(HAS_NO_HMEM) */
+#define IA_CSS_INCLUDE_PARAMETERS
+#define IA_CSS_INCLUDE_ACC_PARAMETERS
+
+#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 "memory_access.h"
+#if 0   /* FIXME */
+#include "memory_realloc.h"
+#endif
+#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"
+#if 0
+#include "ia_css_system_ctrl.h"
+#endif
+
+/* 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"
+
+#if defined(HAS_OUTPUT_SYSTEM)
+#include <components/output_system/sc_output_system_1.0/host/output_system.host.h>
+#endif
+
+#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)
+	 
+#ifndef ISP2401
+
+#define SCTBL_BYTES(binary) \
+	(sizeof(unsigned short) * (binary)->sctbl_height * \
+	 (binary)->sctbl_aligned_width_per_color * IA_CSS_SC_NUM_COLORS)
+
+#else
+
+#define SCTBL_BYTES(binary) \
+	(sizeof(unsigned short) * max((binary)->sctbl_height, (binary)->sctbl_legacy_height) * \
+			/* height should be the larger height between new api and legacy api */ \
+	 (binary)->sctbl_aligned_width_per_color * IA_CSS_SC_NUM_COLORS)
+
+#endif
+
+#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 hrt_vaddress sp_ddr_ptrs;
+
+/* sp group address on DDR */
+static hrt_vaddress xmem_sp_group_ptrs;
+
+static hrt_vaddress xmem_sp_stage_ptrs[IA_CSS_PIPE_ID_NUM]
+						[SH_CSS_MAX_STAGES];
+static hrt_vaddress xmem_isp_stage_ptrs[IA_CSS_PIPE_ID_NUM]
+						[SH_CSS_MAX_STAGES];
+
+static hrt_vaddress 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.
+ */
+#if defined(HAS_GDC_VERSION_2)
+#if defined(CONFIG_CSI2_PLUS)
+/*
+ * Coefficients from
+ * Css_Mizuchi/regressions/20140424_0930/all/applications/common/gdc_v2_common/lut.h
+ */
+
+static const int zoom_table[4][HRT_GDC_N] = {
+	{	   0,    0,    0,    0,    0,    0,    0,    0,
+		   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,   -3,   -3,   -4,
+		  -4,   -4,   -4,   -4,   -5,   -5,   -5,   -5,
+		  -5,   -5,   -6,   -6,   -6,   -6,   -7,   -7,
+		  -7,   -7,   -7,   -8,   -8,   -8,   -8,   -9,
+		  -9,   -9,   -9,  -10,  -10,  -10,  -10,  -11,
+		 -11,  -11,  -12,  -12,  -12,  -12,  -13,  -13,
+		 -13,  -14,  -14,  -14,  -15,  -15,  -15,  -15,
+		 -16,  -16,  -16,  -17,  -17,  -17,  -18,  -18,
+		 -18,  -19,  -19,  -20,  -20,  -20,  -21,  -21,
+		 -21,  -22,  -22,  -22,  -23,  -23,  -24,  -24,
+		 -24,  -25,  -25,  -25,  -26,  -26,  -27,  -27,
+		 -28,  -28,  -28,  -29,  -29,  -30,  -30,  -30,
+		 -31,  -31,  -32,  -32,  -33,  -33,  -33,  -34,
+		 -34,  -35,  -35,  -36,  -36,  -37,  -37,  -37,
+		 -38,  -38,  -39,  -39,  -40,  -40,  -41,  -41,
+		 -42,  -42,  -43,  -43,  -44,  -44,  -45,  -45,
+		 -46,  -46,  -47,  -47,  -48,  -48,  -49,  -49,
+		 -50,  -50,  -51,  -51,  -52,  -52,  -53,  -53,
+		 -54,  -54,  -55,  -55,  -56,  -56,  -57,  -57,
+		 -58,  -59,  -59,  -60,  -60,  -61,  -61,  -62,
+		 -62,  -63,  -63,  -64,  -65,  -65,  -66,  -66,
+		 -67,  -67,  -68,  -69,  -69,  -70,  -70,  -71,
+		 -71,  -72,  -73,  -73,  -74,  -74,  -75,  -75,
+		 -76,  -77,  -77,  -78,  -78,  -79,  -80,  -80,
+		 -81,  -81,  -82,  -83,  -83,  -84,  -84,  -85,
+		 -86,  -86,  -87,  -87,  -88,  -89,  -89,  -90,
+		 -91,  -91,  -92,  -92,  -93,  -94,  -94,  -95,
+		 -96,  -96,  -97,  -97,  -98,  -99,  -99, -100,
+		-101, -101, -102, -102, -103, -104, -104, -105,
+		-106, -106, -107, -108, -108, -109, -109, -110,
+		-111, -111, -112, -113, -113, -114, -115, -115,
+		-116, -117, -117, -118, -119, -119, -120, -121,
+		-121, -122, -122, -123, -124, -124, -125, -126,
+		-126, -127, -128, -128, -129, -130, -130, -131,
+		-132, -132, -133, -134, -134, -135, -136, -136,
+		-137, -138, -138, -139, -140, -140, -141, -142,
+		-142, -143, -144, -144, -145, -146, -146, -147,
+		-148, -148, -149, -150, -150, -151, -152, -152,
+		-153, -154, -154, -155, -156, -156, -157, -158,
+		-158, -159, -160, -160, -161, -162, -162, -163,
+		-164, -164, -165, -166, -166, -167, -168, -168,
+		-169, -170, -170, -171, -172, -172, -173, -174,
+		-174, -175, -176, -176, -177, -178, -178, -179,
+		-180, -180, -181, -181, -182, -183, -183, -184,
+		-185, -185, -186, -187, -187, -188, -189, -189,
+		-190, -191, -191, -192, -193, -193, -194, -194,
+		-195, -196, -196, -197, -198, -198, -199, -200,
+		-200, -201, -201, -202, -203, -203, -204, -205,
+		-205, -206, -206, -207, -208, -208, -209, -210,
+		-210, -211, -211, -212, -213, -213, -214, -215,
+		-215, -216, -216, -217, -218, -218, -219, -219,
+		-220, -221, -221, -222, -222, -223, -224, -224,
+		-225, -225, -226, -227, -227, -228, -228, -229,
+		-229, -230, -231, -231, -232, -232, -233, -233,
+		-234, -235, -235, -236, -236, -237, -237, -238,
+		-239, -239, -240, -240, -241, -241, -242, -242,
+		-243, -244, -244, -245, -245, -246, -246, -247,
+		-247, -248, -248, -249, -249, -250, -250, -251,
+		-251, -252, -252, -253, -253, -254, -254, -255,
+		-256, -256, -256, -257, -257, -258, -258, -259,
+		-259, -260, -260, -261, -261, -262, -262, -263,
+		-263, -264, -264, -265, -265, -266, -266, -266,
+		-267, -267, -268, -268, -269, -269, -270, -270,
+		-270, -271, -271, -272, -272, -273, -273, -273,
+		-274, -274, -275, -275, -275, -276, -276, -277,
+		-277, -277, -278, -278, -279, -279, -279, -280,
+		-280, -280, -281, -281, -282, -282, -282, -283,
+		-283, -283, -284, -284, -284, -285, -285, -285,
+		-286, -286, -286, -287, -287, -287, -288, -288,
+		-288, -289, -289, -289, -289, -290, -290, -290,
+		-291, -291, -291, -291, -292, -292, -292, -293,
+		-293, -293, -293, -294, -294, -294, -294, -295,
+		-295, -295, -295, -295, -296, -296, -296, -296,
+		-297, -297, -297, -297, -297, -298, -298, -298,
+		-298, -298, -299, -299, -299, -299, -299, -299,
+		-300, -300, -300, -300, -300, -300, -300, -301,
+		-301, -301, -301, -301, -301, -301, -301, -301,
+		-302, -302, -302, -302, -302, -302, -302, -302,
+		-302, -302, -302, -302, -302, -303, -303, -303,
+		-303, -303, -303, -303, -303, -303, -303, -303,
+		-303, -303, -303, -303, -303, -303, -303, -303,
+		-303, -303, -303, -303, -303, -303, -303, -303,
+		-303, -303, -302, -302, -302, -302, -302, -302,
+		-302, -302, -302, -302, -302, -302, -301, -301,
+		-301, -301, -301, -301, -301, -301, -300, -300,
+		-300, -300, -300, -300, -299, -299, -299, -299,
+		-299, -299, -298, -298, -298, -298, -298, -297,
+		-297, -297, -297, -296, -296, -296, -296, -295,
+		-295, -295, -295, -294, -294, -294, -293, -293,
+		-293, -293, -292, -292, -292, -291, -291, -291,
+		-290, -290, -290, -289, -289, -289, -288, -288,
+		-288, -287, -287, -286, -286, -286, -285, -285,
+		-284, -284, -284, -283, -283, -282, -282, -281,
+		-281, -280, -280, -279, -279, -279, -278, -278,
+		-277, -277, -276, -276, -275, -275, -274, -273,
+		-273, -272, -272, -271, -271, -270, -270, -269,
+		-268, -268, -267, -267, -266, -266, -265, -264,
+		-264, -263, -262, -262, -261, -260, -260, -259,
+		-259, -258, -257, -256, -256, -255, -254, -254,
+		-253, -252, -252, -251, -250, -249, -249, -248,
+		-247, -246, -246, -245, -244, -243, -242, -242,
+		-241, -240, -239, -238, -238, -237, -236, -235,
+		-234, -233, -233, -232, -231, -230, -229, -228,
+		-227, -226, -226, -225, -224, -223, -222, -221,
+		-220, -219, -218, -217, -216, -215, -214, -213,
+		-212, -211, -210, -209, -208, -207, -206, -205,
+		-204, -203, -202, -201, -200, -199, -198, -197,
+		-196, -194, -193, -192, -191, -190, -189, -188,
+		-187, -185, -184, -183, -182, -181, -180, -178,
+		-177, -176, -175, -174, -172, -171, -170, -169,
+		-167, -166, -165, -164, -162, -161, -160, -158,
+		-157, -156, -155, -153, -152, -151, -149, -148,
+		-147, -145, -144, -142, -141, -140, -138, -137,
+		-135, -134, -133, -131, -130, -128, -127, -125,
+		-124, -122, -121, -120, -118, -117, -115, -114,
+		-112, -110, -109, -107, -106, -104, -103, -101,
+		-100,  -98,  -96,  -95,  -93,  -92,  -90,  -88,
+		 -87,  -85,  -83,  -82,  -80,  -78,  -77,  -75,
+		 -73,  -72,  -70,  -68,  -67,  -65,  -63,  -61,
+		 -60,  -58,  -56,  -54,  -52,  -51,  -49,  -47,
+		 -45,  -43,  -42,  -40,  -38,  -36,  -34,  -32,
+		 -31,  -29,  -27,  -25,  -23,  -21,  -19,  -17,
+		 -15,  -13,  -11,   -9,   -7,   -5,   -3,   -1
+	},
+	{	   0,    2,    4,    6,    8,   10,   12,   14,
+		  16,   18,   20,   22,   25,   27,   29,   31,
+		  33,   36,   38,   40,   43,   45,   47,   50,
+		  52,   54,   57,   59,   61,   64,   66,   69,
+		  71,   74,   76,   79,   81,   84,   86,   89,
+		  92,   94,   97,   99,  102,  105,  107,  110,
+		 113,  116,  118,  121,  124,  127,  129,  132,
+		 135,  138,  141,  144,  146,  149,  152,  155,
+		 158,  161,  164,  167,  170,  173,  176,  179,
+		 182,  185,  188,  191,  194,  197,  200,  203,
+		 207,  210,  213,  216,  219,  222,  226,  229,
+		 232,  235,  239,  242,  245,  248,  252,  255,
+		 258,  262,  265,  269,  272,  275,  279,  282,
+		 286,  289,  292,  296,  299,  303,  306,  310,
+		 313,  317,  321,  324,  328,  331,  335,  338,
+		 342,  346,  349,  353,  357,  360,  364,  368,
+		 372,  375,  379,  383,  386,  390,  394,  398,
+		 402,  405,  409,  413,  417,  421,  425,  429,
+		 432,  436,  440,  444,  448,  452,  456,  460,
+		 464,  468,  472,  476,  480,  484,  488,  492,
+		 496,  500,  504,  508,  512,  516,  521,  525,
+		 529,  533,  537,  541,  546,  550,  554,  558,
+		 562,  567,  571,  575,  579,  584,  588,  592,
+		 596,  601,  605,  609,  614,  618,  622,  627,
+		 631,  635,  640,  644,  649,  653,  657,  662,
+		 666,  671,  675,  680,  684,  689,  693,  698,
+		 702,  707,  711,  716,  720,  725,  729,  734,
+		 738,  743,  747,  752,  757,  761,  766,  771,
+		 775,  780,  784,  789,  794,  798,  803,  808,
+		 813,  817,  822,  827,  831,  836,  841,  846,
+		 850,  855,  860,  865,  870,  874,  879,  884,
+		 889,  894,  898,  903,  908,  913,  918,  923,
+		 928,  932,  937,  942,  947,  952,  957,  962,
+		 967,  972,  977,  982,  986,  991,  996, 1001,
+		1006, 1011, 1016, 1021, 1026, 1031, 1036, 1041,
+		1046, 1051, 1056, 1062, 1067, 1072, 1077, 1082,
+		1087, 1092, 1097, 1102, 1107, 1112, 1117, 1122,
+		1128, 1133, 1138, 1143, 1148, 1153, 1158, 1164,
+		1169, 1174, 1179, 1184, 1189, 1195, 1200, 1205,
+		1210, 1215, 1221, 1226, 1231, 1236, 1242, 1247,
+		1252, 1257, 1262, 1268, 1273, 1278, 1284, 1289,
+		1294, 1299, 1305, 1310, 1315, 1321, 1326, 1331,
+		1336, 1342, 1347, 1352, 1358, 1363, 1368, 1374,
+		1379, 1384, 1390, 1395, 1400, 1406, 1411, 1417,
+		1422, 1427, 1433, 1438, 1443, 1449, 1454, 1460,
+		1465, 1470, 1476, 1481, 1487, 1492, 1497, 1503,
+		1508, 1514, 1519, 1525, 1530, 1535, 1541, 1546,
+		1552, 1557, 1563, 1568, 1574, 1579, 1585, 1590,
+		1596, 1601, 1606, 1612, 1617, 1623, 1628, 1634,
+		1639, 1645, 1650, 1656, 1661, 1667, 1672, 1678,
+		1683, 1689, 1694, 1700, 1705, 1711, 1716, 1722,
+		1727, 1733, 1738, 1744, 1749, 1755, 1761, 1766,
+		1772, 1777, 1783, 1788, 1794, 1799, 1805, 1810,
+		1816, 1821, 1827, 1832, 1838, 1844, 1849, 1855,
+		1860, 1866, 1871, 1877, 1882, 1888, 1893, 1899,
+		1905, 1910, 1916, 1921, 1927, 1932, 1938, 1943,
+		1949, 1955, 1960, 1966, 1971, 1977, 1982, 1988,
+		1993, 1999, 2005, 2010, 2016, 2021, 2027, 2032,
+		2038, 2043, 2049, 2055, 2060, 2066, 2071, 2077,
+		2082, 2088, 2093, 2099, 2105, 2110, 2116, 2121,
+		2127, 2132, 2138, 2143, 2149, 2154, 2160, 2165,
+		2171, 2177, 2182, 2188, 2193, 2199, 2204, 2210,
+		2215, 2221, 2226, 2232, 2237, 2243, 2248, 2254,
+		2259, 2265, 2270, 2276, 2281, 2287, 2292, 2298,
+		2304, 2309, 2314, 2320, 2325, 2331, 2336, 2342,
+		2347, 2353, 2358, 2364, 2369, 2375, 2380, 2386,
+		2391, 2397, 2402, 2408, 2413, 2419, 2424, 2429,
+		2435, 2440, 2446, 2451, 2457, 2462, 2467, 2473,
+		2478, 2484, 2489, 2495, 2500, 2505, 2511, 2516,
+		2522, 2527, 2532, 2538, 2543, 2549, 2554, 2559,
+		2565, 2570, 2575, 2581, 2586, 2591, 2597, 2602,
+		2607, 2613, 2618, 2623, 2629, 2634, 2639, 2645,
+		2650, 2655, 2661, 2666, 2671, 2676, 2682, 2687,
+		2692, 2698, 2703, 2708, 2713, 2719, 2724, 2729,
+		2734, 2740, 2745, 2750, 2755, 2760, 2766, 2771,
+		2776, 2781, 2786, 2792, 2797, 2802, 2807, 2812,
+		2817, 2823, 2828, 2833, 2838, 2843, 2848, 2853,
+		2859, 2864, 2869, 2874, 2879, 2884, 2889, 2894,
+		2899, 2904, 2909, 2914, 2919, 2924, 2930, 2935,
+		2940, 2945, 2950, 2955, 2960, 2965, 2970, 2975,
+		2980, 2984, 2989, 2994, 2999, 3004, 3009, 3014,
+		3019, 3024, 3029, 3034, 3039, 3044, 3048, 3053,
+		3058, 3063, 3068, 3073, 3078, 3082, 3087, 3092,
+		3097, 3102, 3106, 3111, 3116, 3121, 3126, 3130,
+		3135, 3140, 3145, 3149, 3154, 3159, 3163, 3168,
+		3173, 3177, 3182, 3187, 3191, 3196, 3201, 3205,
+		3210, 3215, 3219, 3224, 3228, 3233, 3238, 3242,
+		3247, 3251, 3256, 3260, 3265, 3269, 3274, 3279,
+		3283, 3287, 3292, 3296, 3301, 3305, 3310, 3314,
+		3319, 3323, 3327, 3332, 3336, 3341, 3345, 3349,
+		3354, 3358, 3362, 3367, 3371, 3375, 3380, 3384,
+		3388, 3393, 3397, 3401, 3405, 3410, 3414, 3418,
+		3422, 3426, 3431, 3435, 3439, 3443, 3447, 3451,
+		3455, 3460, 3464, 3468, 3472, 3476, 3480, 3484,
+		3488, 3492, 3496, 3500, 3504, 3508, 3512, 3516,
+		3520, 3524, 3528, 3532, 3536, 3540, 3544, 3548,
+		3552, 3555, 3559, 3563, 3567, 3571, 3575, 3578,
+		3582, 3586, 3590, 3593, 3597, 3601, 3605, 3608,
+		3612, 3616, 3619, 3623, 3627, 3630, 3634, 3638,
+		3641, 3645, 3649, 3652, 3656, 3659, 3663, 3666,
+		3670, 3673, 3677, 3680, 3684, 3687, 3691, 3694,
+		3698, 3701, 3704, 3708, 3711, 3714, 3718, 3721,
+		3724, 3728, 3731, 3734, 3738, 3741, 3744, 3747,
+		3751, 3754, 3757, 3760, 3763, 3767, 3770, 3773,
+		3776, 3779, 3782, 3785, 3788, 3791, 3794, 3798,
+		3801, 3804, 3807, 3809, 3812, 3815, 3818, 3821,
+		3824, 3827, 3830, 3833, 3836, 3839, 3841, 3844,
+		3847, 3850, 3853, 3855, 3858, 3861, 3864, 3866,
+		3869, 3872, 3874, 3877, 3880, 3882, 3885, 3887,
+		3890, 3893, 3895, 3898, 3900, 3903, 3905, 3908,
+		3910, 3913, 3915, 3917, 3920, 3922, 3925, 3927,
+		3929, 3932, 3934, 3936, 3939, 3941, 3943, 3945,
+		3948, 3950, 3952, 3954, 3956, 3958, 3961, 3963,
+		3965, 3967, 3969, 3971, 3973, 3975, 3977, 3979,
+		3981, 3983, 3985, 3987, 3989, 3991, 3993, 3994,
+		3996, 3998, 4000, 4002, 4004, 4005, 4007, 4009,
+		4011, 4012, 4014, 4016, 4017, 4019, 4021, 4022,
+		4024, 4025, 4027, 4028, 4030, 4031, 4033, 4034,
+		4036, 4037, 4039, 4040, 4042, 4043, 4044, 4046,
+		4047, 4048, 4050, 4051, 4052, 4053, 4055, 4056,
+		4057, 4058, 4059, 4060, 4062, 4063, 4064, 4065,
+		4066, 4067, 4068, 4069, 4070, 4071, 4072, 4073,
+		4074, 4075, 4075, 4076, 4077, 4078, 4079, 4079,
+		4080, 4081, 4082, 4082, 4083, 4084, 4084, 4085,
+		4086, 4086, 4087, 4087, 4088, 4088, 4089, 4089,
+		4090, 4090, 4091, 4091, 4092, 4092, 4092, 4093,
+		4093, 4093, 4094, 4094, 4094, 4094, 4095, 4095,
+		4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095
+	},
+	{	4096, 4095, 4095, 4095, 4095, 4095, 4095, 4095,
+		4095, 4095, 4095, 4094, 4094, 4094, 4094, 4093,
+		4093, 4093, 4092, 4092, 4092, 4091, 4091, 4090,
+		4090, 4089, 4089, 4088, 4088, 4087, 4087, 4086,
+		4086, 4085, 4084, 4084, 4083, 4082, 4082, 4081,
+		4080, 4079, 4079, 4078, 4077, 4076, 4075, 4075,
+		4074, 4073, 4072, 4071, 4070, 4069, 4068, 4067,
+		4066, 4065, 4064, 4063, 4062, 4060, 4059, 4058,
+		4057, 4056, 4055, 4053, 4052, 4051, 4050, 4048,
+		4047, 4046, 4044, 4043, 4042, 4040, 4039, 4037,
+		4036, 4034, 4033, 4031, 4030, 4028, 4027, 4025,
+		4024, 4022, 4021, 4019, 4017, 4016, 4014, 4012,
+		4011, 4009, 4007, 4005, 4004, 4002, 4000, 3998,
+		3996, 3994, 3993, 3991, 3989, 3987, 3985, 3983,
+		3981, 3979, 3977, 3975, 3973, 3971, 3969, 3967,
+		3965, 3963, 3961, 3958, 3956, 3954, 3952, 3950,
+		3948, 3945, 3943, 3941, 3939, 3936, 3934, 3932,
+		3929, 3927, 3925, 3922, 3920, 3917, 3915, 3913,
+		3910, 3908, 3905, 3903, 3900, 3898, 3895, 3893,
+		3890, 3887, 3885, 3882, 3880, 3877, 3874, 3872,
+		3869, 3866, 3864, 3861, 3858, 3855, 3853, 3850,
+		3847, 3844, 3841, 3839, 3836, 3833, 3830, 3827,
+		3824, 3821, 3818, 3815, 3812, 3809, 3807, 3804,
+		3801, 3798, 3794, 3791, 3788, 3785, 3782, 3779,
+		3776, 3773, 3770, 3767, 3763, 3760, 3757, 3754,
+		3751, 3747, 3744, 3741, 3738, 3734, 3731, 3728,
+		3724, 3721, 3718, 3714, 3711, 3708, 3704, 3701,
+		3698, 3694, 3691, 3687, 3684, 3680, 3677, 3673,
+		3670, 3666, 3663, 3659, 3656, 3652, 3649, 3645,
+		3641, 3638, 3634, 3630, 3627, 3623, 3619, 3616,
+		3612, 3608, 3605, 3601, 3597, 3593, 3590, 3586,
+		3582, 3578, 3575, 3571, 3567, 3563, 3559, 3555,
+		3552, 3548, 3544, 3540, 3536, 3532, 3528, 3524,
+		3520, 3516, 3512, 3508, 3504, 3500, 3496, 3492,
+		3488, 3484, 3480, 3476, 3472, 3468, 3464, 3460,
+		3455, 3451, 3447, 3443, 3439, 3435, 3431, 3426,
+		3422, 3418, 3414, 3410, 3405, 3401, 3397, 3393,
+		3388, 3384, 3380, 3375, 3371, 3367, 3362, 3358,
+		3354, 3349, 3345, 3341, 3336, 3332, 3327, 3323,
+		3319, 3314, 3310, 3305, 3301, 3296, 3292, 3287,
+		3283, 3279, 3274, 3269, 3265, 3260, 3256, 3251,
+		3247, 3242, 3238, 3233, 3228, 3224, 3219, 3215,
+		3210, 3205, 3201, 3196, 3191, 3187, 3182, 3177,
+		3173, 3168, 3163, 3159, 3154, 3149, 3145, 3140,
+		3135, 3130, 3126, 3121, 3116, 3111, 3106, 3102,
+		3097, 3092, 3087, 3082, 3078, 3073, 3068, 3063,
+		3058, 3053, 3048, 3044, 3039, 3034, 3029, 3024,
+		3019, 3014, 3009, 3004, 2999, 2994, 2989, 2984,
+		2980, 2975, 2970, 2965, 2960, 2955, 2950, 2945,
+		2940, 2935, 2930, 2924, 2919, 2914, 2909, 2904,
+		2899, 2894, 2889, 2884, 2879, 2874, 2869, 2864,
+		2859, 2853, 2848, 2843, 2838, 2833, 2828, 2823,
+		2817, 2812, 2807, 2802, 2797, 2792, 2786, 2781,
+		2776, 2771, 2766, 2760, 2755, 2750, 2745, 2740,
+		2734, 2729, 2724, 2719, 2713, 2708, 2703, 2698,
+		2692, 2687, 2682, 2676, 2671, 2666, 2661, 2655,
+		2650, 2645, 2639, 2634, 2629, 2623, 2618, 2613,
+		2607, 2602, 2597, 2591, 2586, 2581, 2575, 2570,
+		2565, 2559, 2554, 2549, 2543, 2538, 2532, 2527,
+		2522, 2516, 2511, 2505, 2500, 2495, 2489, 2484,
+		2478, 2473, 2467, 2462, 2457, 2451, 2446, 2440,
+		2435, 2429, 2424, 2419, 2413, 2408, 2402, 2397,
+		2391, 2386, 2380, 2375, 2369, 2364, 2358, 2353,
+		2347, 2342, 2336, 2331, 2325, 2320, 2314, 2309,
+		2304, 2298, 2292, 2287, 2281, 2276, 2270, 2265,
+		2259, 2254, 2248, 2243, 2237, 2232, 2226, 2221,
+		2215, 2210, 2204, 2199, 2193, 2188, 2182, 2177,
+		2171, 2165, 2160, 2154, 2149, 2143, 2138, 2132,
+		2127, 2121, 2116, 2110, 2105, 2099, 2093, 2088,
+		2082, 2077, 2071, 2066, 2060, 2055, 2049, 2043,
+		2038, 2032, 2027, 2021, 2016, 2010, 2005, 1999,
+		1993, 1988, 1982, 1977, 1971, 1966, 1960, 1955,
+		1949, 1943, 1938, 1932, 1927, 1921, 1916, 1910,
+		1905, 1899, 1893, 1888, 1882, 1877, 1871, 1866,
+		1860, 1855, 1849, 1844, 1838, 1832, 1827, 1821,
+		1816, 1810, 1805, 1799, 1794, 1788, 1783, 1777,
+		1772, 1766, 1761, 1755, 1749, 1744, 1738, 1733,
+		1727, 1722, 1716, 1711, 1705, 1700, 1694, 1689,
+		1683, 1678, 1672, 1667, 1661, 1656, 1650, 1645,
+		1639, 1634, 1628, 1623, 1617, 1612, 1606, 1601,
+		1596, 1590, 1585, 1579, 1574, 1568, 1563, 1557,
+		1552, 1546, 1541, 1535, 1530, 1525, 1519, 1514,
+		1508, 1503, 1497, 1492, 1487, 1481, 1476, 1470,
+		1465, 1460, 1454, 1449, 1443, 1438, 1433, 1427,
+		1422, 1417, 1411, 1406, 1400, 1395, 1390, 1384,
+		1379, 1374, 1368, 1363, 1358, 1352, 1347, 1342,
+		1336, 1331, 1326, 1321, 1315, 1310, 1305, 1299,
+		1294, 1289, 1284, 1278, 1273, 1268, 1262, 1257,
+		1252, 1247, 1242, 1236, 1231, 1226, 1221, 1215,
+		1210, 1205, 1200, 1195, 1189, 1184, 1179, 1174,
+		1169, 1164, 1158, 1153, 1148, 1143, 1138, 1133,
+		1128, 1122, 1117, 1112, 1107, 1102, 1097, 1092,
+		1087, 1082, 1077, 1072, 1067, 1062, 1056, 1051,
+		1046, 1041, 1036, 1031, 1026, 1021, 1016, 1011,
+		1006, 1001,  996,  991,  986,  982,  977,  972,
+		 967,  962,  957,  952,  947,  942,  937,  932,
+		 928,  923,  918,  913,  908,  903,  898,  894,
+		 889,  884,  879,  874,  870,  865,  860,  855,
+		 850,  846,  841,  836,  831,  827,  822,  817,
+		 813,  808,  803,  798,  794,  789,  784,  780,
+		 775,  771,  766,  761,  757,  752,  747,  743,
+		 738,  734,  729,  725,  720,  716,  711,  707,
+		 702,  698,  693,  689,  684,  680,  675,  671,
+		 666,  662,  657,  653,  649,  644,  640,  635,
+		 631,  627,  622,  618,  614,  609,  605,  601,
+		 596,  592,  588,  584,  579,  575,  571,  567,
+		 562,  558,  554,  550,  546,  541,  537,  533,
+		 529,  525,  521,  516,  512,  508,  504,  500,
+		 496,  492,  488,  484,  480,  476,  472,  468,
+		 464,  460,  456,  452,  448,  444,  440,  436,
+		 432,  429,  425,  421,  417,  413,  409,  405,
+		 402,  398,  394,  390,  386,  383,  379,  375,
+		 372,  368,  364,  360,  357,  353,  349,  346,
+		 342,  338,  335,  331,  328,  324,  321,  317,
+		 313,  310,  306,  303,  299,  296,  292,  289,
+		 286,  282,  279,  275,  272,  269,  265,  262,
+		 258,  255,  252,  248,  245,  242,  239,  235,
+		 232,  229,  226,  222,  219,  216,  213,  210,
+		 207,  203,  200,  197,  194,  191,  188,  185,
+		 182,  179,  176,  173,  170,  167,  164,  161,
+		 158,  155,  152,  149,  146,  144,  141,  138,
+		 135,  132,  129,  127,  124,  121,  118,  116,
+		 113,  110,  107,  105,  102,   99,   97,   94,
+		  92,   89,   86,   84,   81,   79,   76,   74,
+		  71,   69,   66,   64,   61,   59,   57,   54,
+		  52,   50,   47,   45,   43,   40,   38,   36,
+		  33,   31,   29,   27,   25,   22,   20,   18,
+		  16,   14,   12,   10,    8,    6,    4,    2
+	},
+	{	   0,   -1,   -3,   -5,   -7,   -9,  -11,  -13,
+		 -15,  -17,  -19,  -20,  -23,  -25,  -27,  -28,
+		 -30,  -33,  -34,  -36,  -39,  -40,  -42,  -43,
+		 -45,  -46,  -49,  -50,  -52,  -54,  -56,  -58,
+		 -60,  -61,  -62,  -65,  -66,  -68,  -70,  -72,
+		 -73,  -74,  -77,  -78,  -80,  -82,  -83,  -85,
+		 -87,  -89,  -90,  -92,  -93,  -95,  -96,  -98,
+		-100, -102, -103, -105, -106, -107, -108, -110,
+		-112, -114, -116, -116, -118, -120, -122, -122,
+		-124, -126, -127, -128, -130, -131, -133, -133,
+		-136, -137, -138, -139, -141, -142, -144, -145,
+		-147, -147, -150, -151, -151, -153, -155, -156,
+		-157, -159, -160, -161, -163, -164, -165, -166,
+		-168, -168, -170, -171, -172, -174, -174, -176,
+		-177, -178, -180, -181, -182, -183, -184, -185,
+		-187, -188, -189, -190, -191, -192, -193, -195,
+		-196, -196, -198, -199, -200, -200, -202, -204,
+		-204, -205, -206, -207, -208, -209, -211, -212,
+		-212, -213, -214, -215, -216, -217, -218, -220,
+		-220, -221, -222, -223, -224, -225, -225, -227,
+		-227, -228, -229, -230, -230, -231, -233, -234,
+		-234, -235, -235, -237, -238, -239, -239, -240,
+		-240, -242, -242, -243, -243, -245, -246, -247,
+		-247, -249, -248, -249, -250, -251, -251, -253,
+		-253, -253, -255, -255, -256, -256, -257, -258,
+		-259, -259, -260, -261, -261, -262, -262, -264,
+		-263, -265, -265, -265, -266, -267, -267, -268,
+		-269, -269, -269, -270, -271, -271, -272, -273,
+		-273, -273, -274, -274, -276, -275, -276, -277,
+		-277, -278, -278, -278, -279, -279, -280, -281,
+		-280, -281, -282, -283, -283, -282, -284, -284,
+		-284, -285, -285, -286, -286, -286, -287, -287,
+		-288, -288, -288, -289, -289, -289, -290, -290,
+		-290, -291, -291, -292, -291, -291, -292, -292,
+		-292, -293, -293, -293, -294, -294, -295, -295,
+		-294, -295, -295, -296, -297, -297, -297, -297,
+		-297, -297, -298, -298, -297, -298, -298, -298,
+		-299, -299, -300, -299, -299, -300, -299, -300,
+		-301, -300, -300, -301, -300, -301, -301, -301,
+		-301, -301, -302, -301, -302, -301, -302, -302,
+		-302, -302, -302, -302, -302, -302, -303, -302,
+		-303, -302, -303, -303, -302, -303, -303, -303,
+		-302, -303, -303, -302, -303, -303, -302, -303,
+		-303, -302, -303, -303, -302, -303, -303, -303,
+		-303, -302, -303, -303, -302, -302, -302, -303,
+		-302, -302, -302, -301, -303, -302, -301, -302,
+		-301, -301, -301, -302, -301, -301, -301, -300,
+		-301, -300, -300, -300, -300, -299, -300, -299,
+		-300, -300, -299, -300, -299, -299, -299, -299,
+		-298, -299, -298, -297, -297, -297, -296, -297,
+		-296, -296, -296, -296, -295, -296, -295, -296,
+		-295, -294, -294, -294, -293, -294, -294, -293,
+		-293, -292, -293, -292, -292, -292, -291, -290,
+		-291, -290, -291, -289, -289, -290, -289, -289,
+		-288, -288, -288, -288, -286, -287, -286, -286,
+		-286, -285, -286, -284, -284, -284, -284, -283,
+		-283, -283, -282, -282, -282, -281, -280, -281,
+		-279, -280, -280, -278, -279, -278, -278, -277,
+		-278, -276, -276, -277, -275, -276, -274, -275,
+		-274, -273, -273, -272, -273, -272, -272, -271,
+		-270, -270, -269, -269, -269, -268, -268, -267,
+		-267, -266, -266, -266, -265, -265, -264, -264,
+		-263, -263, -262, -262, -261, -261, -260, -260,
+		-259, -259, -258, -258, -257, -257, -256, -256,
+		-256, -255, -254, -254, -253, -253, -252, -252,
+		-251, -251, -250, -250, -249, -249, -248, -248,
+		-247, -247, -246, -246, -245, -245, -244, -244,
+		-243, -242, -242, -241, -241, -240, -239, -239,
+		-239, -238, -238, -237, -237, -235, -235, -235,
+		-234, -234, -232, -233, -232, -232, -231, -229,
+		-230, -229, -228, -228, -227, -226, -227, -225,
+		-224, -225, -223, -223, -222, -222, -221, -221,
+		-220, -219, -219, -218, -218, -216, -217, -216,
+		-215, -215, -214, -213, -212, -213, -211, -211,
+		-210, -210, -209, -209, -208, -206, -207, -206,
+		-205, -204, -204, -204, -203, -202, -202, -200,
+		-200, -200, -200, -198, -197, -197, -196, -195,
+		-195, -195, -194, -194, -192, -192, -191, -191,
+		-189, -189, -188, -188, -187, -186, -186, -186,
+		-185, -185, -183, -183, -182, -182, -181, -181,
+		-180, -178, -178, -177, -177, -176, -176, -174,
+		-174, -173, -173, -172, -172, -172, -170, -170,
+		-168, -168, -167, -167, -167, -165, -165, -164,
+		-164, -164, -162, -162, -161, -160, -160, -158,
+		-158, -158, -157, -156, -155, -155, -154, -153,
+		-153, -152, -151, -151, -150, -149, -149, -148,
+		-147, -147, -146, -146, -144, -144, -144, -142,
+		-142, -141, -142, -140, -140, -139, -138, -138,
+		-137, -136, -136, -134, -134, -133, -134, -132,
+		-132, -131, -130, -130, -128, -128, -128, -127,
+		-127, -126, -124, -124, -124, -123, -123, -122,
+		-121, -120, -120, -119, -118, -118, -117, -117,
+		-116, -115, -115, -115, -114, -113, -111, -111,
+		-110, -110, -109, -109, -108, -107, -107, -106,
+		-105, -104, -104, -103, -102, -103, -102, -101,
+		-101, -100,  -99,  -99,  -98,  -97,  -97,  -96,
+		 -96,  -95,  -94,  -94,  -93,  -92,  -92,  -91,
+		 -91,  -90,  -89,  -88,  -88,  -88,  -87,  -86,
+		 -85,  -86,  -84,  -84,  -83,  -82,  -82,  -81,
+		 -81,  -80,  -80,  -78,  -79,  -77,  -77,  -77,
+		 -76,  -76,  -75,  -74,  -74,  -73,  -72,  -72,
+		 -72,  -71,  -70,  -70,  -69,  -68,  -68,  -68,
+		 -66,  -67,  -66,  -65,  -65,  -65,  -63,  -63,
+		 -62,  -62,  -61,  -61,  -60,  -60,  -60,  -58,
+		 -58,  -58,  -56,  -56,  -56,  -55,  -54,  -55,
+		 -54,  -54,  -53,  -52,  -51,  -51,  -51,  -50,
+		 -49,  -49,  -49,  -49,  -48,  -47,  -46,  -46,
+		 -46,  -46,  -45,  -43,  -43,  -43,  -43,  -42,
+		 -42,  -42,  -40,  -40,  -40,  -39,  -39,  -38,
+		 -38,  -38,  -37,  -37,  -36,  -36,  -35,  -35,
+		 -34,  -35,  -34,  -33,  -33,  -32,  -32,  -31,
+		 -31,  -31,  -30,  -29,  -29,  -29,  -28,  -27,
+		 -28,  -28,  -27,  -26,  -26,  -25,  -25,  -25,
+		 -24,  -24,  -24,  -23,  -23,  -22,  -22,  -22,
+		 -21,  -21,  -20,  -20,  -20,  -20,  -19,  -18,
+		 -19,  -18,  -18,  -17,  -18,  -17,  -16,  -17,
+		 -16,  -15,  -15,  -15,  -14,  -14,  -15,  -13,
+		 -13,  -13,  -13,  -12,  -12,  -11,  -12,  -11,
+		 -12,  -10,  -10,  -10,  -10,  -10,   -9,  -10,
+		  -9,   -9,   -9,   -8,   -8,   -7,   -8,   -7,
+		  -7,   -7,   -6,   -6,   -6,   -7,   -6,   -6,
+		  -5,   -5,   -5,   -5,   -5,   -4,   -4,   -5,
+		  -4,   -4,   -3,   -3,   -3,   -3,   -3,   -2,
+		  -3,   -2,   -2,   -2,   -1,   -2,   -1,   -2,
+		  -1,   -1,   -1,   -1,   -1,    0,   -1,    0,
+		  -1,   -1,    0,    0,   -1,    0,    0,   -1,
+		   1,    1,    0,    0,    0,    1,    0,    0,
+		   0,    0,    0,    0,    0,    0,    0,    0
+	}
+};
+#else   /* defined(CONFIG_CSI2_PLUS) */
+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
+	}
+};
+#endif
+#else
+#error "sh_css_params.c: GDC version must be \
+	one of {GDC_VERSION_2}"
+#endif
+
+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 enum ia_css_err
+ref_sh_css_ddr_address_map(
+		struct sh_css_ddr_address_map *map,
+		struct sh_css_ddr_address_map *out);
+
+static enum ia_css_err
+write_ia_css_isp_parameter_set_info_to_ddr(
+	struct ia_css_isp_parameter_set_info *me,
+	hrt_vaddress *out);
+
+static enum ia_css_err
+free_ia_css_isp_parameter_set_info(hrt_vaddress ptr);
+
+static enum ia_css_err
+sh_css_params_write_to_ddr_internal(
+		struct ia_css_pipe *pipe,
+		unsigned 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 enum ia_css_err
+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 enum ia_css_err
+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 enum ia_css_err
+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);
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+static enum ia_css_err
+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);
+#endif
+
+static enum ia_css_err
+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);
+
+hrt_vaddress
+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;
+	uint32_t *isp_format_data_ptr;
+
+	assert(params != NULL);
+
+	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 enum ia_css_err
+store_fpntbl(struct ia_css_isp_parameters *params, hrt_vaddress ptr)
+{
+	struct ia_css_host_data *isp_data;
+
+	assert(params != NULL);
+	assert(ptr != mmgr_NULL);
+
+	isp_data = convert_allocate_fpntbl(params);
+	if (!isp_data) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+	ia_css_params_store_ia_css_host_data(ptr, isp_data);
+
+	ia_css_host_data_free(isp_data);
+	return IA_CSS_SUCCESS;
+}
+
+static void
+convert_raw_to_fpn(struct ia_css_isp_parameters *params)
+{
+	int maxval = 0;
+	unsigned int i;
+
+	assert(params != NULL);
+
+	/* 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 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 enum ia_css_err
+sh_css_select_dp_10bpp_config(const struct ia_css_pipe *pipe, bool *is_dp_10bpp) {
+
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	/* 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 == NULL) || (is_dp_10bpp == NULL)) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INTERNAL_ERROR);
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+	} 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 (IA_CSS_SUCCESS == binarydesc_calculate_bds_factor(
+					pipe->config.input_effective_res,
+					pipe->config.bayer_ds_out_res,
+					&required_bds_factor)) {
+					if (SH_CSS_BDS_FACTOR_1_00 != required_bds_factor) {
+						/*we use 10bpp BDS configuration*/
+						*is_dp_10bpp = true;
+					}
+				}
+			}
+		}
+	}
+
+	return err;
+}
+
+enum ia_css_err
+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;
+	hrt_vaddress ptr;
+
+	assert(stream != NULL);
+	assert(raw_black_frame != NULL);
+
+	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)) {
+		sh_css_free(params->fpn_config.data);
+		params->fpn_config.data = NULL;
+	}
+	if (params->fpn_config.data == NULL) {
+		params->fpn_config.data = sh_css_malloc(height * width * sizeof(short));
+		if (!params->fpn_config.data) {
+			IA_CSS_ERROR("out of memory");
+			IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		}
+		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) {
+				mmgr_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) {
+				mmgr_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(IA_CSS_SUCCESS);
+
+	return IA_CSS_SUCCESS;
+}
+
+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 != NULL);
+
+	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_update_shading_table_status(struct ia_css_pipe *pipe,
+			struct ia_css_isp_parameters *params)
+{
+	if (params && pipe && (pipe->pipe_num != params->sc_table_last_pipe_num)) {
+		params->sc_table_dirty = true;
+		params->sc_table_last_pipe_num = pipe->pipe_num;
+	}
+}
+
+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 == NULL)
+		return;
+	assert(stream != NULL);
+
+	if (!table->enable)
+		table = NULL;
+
+	if ((table != params->sc_table) || params->sc_table_dirty) {
+		params->sc_table = table;
+		params->sc_table_changed = true;
+		params->sc_table_dirty = false;
+		/* 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(
+	hrt_vaddress ddr_addr,
+	struct ia_css_host_data *data)
+{
+	assert(data != NULL);
+	assert(data->address != NULL);
+	assert(ddr_addr != mmgr_NULL);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	mmgr_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, row_padding;
+	unsigned int sctbl_size;
+	short int    *ptr;
+
+	assert(binary != NULL);
+	assert(shading_table != NULL);
+
+	IA_CSS_ENTER_PRIVATE("");
+
+	if (shading_table == NULL) {
+		IA_CSS_LEAVE_PRIVATE("void");
+		return NULL;
+	}
+
+	aligned_width = binary->sctbl_aligned_width_per_color;
+	row_padding = aligned_width - shading_table->width;
+	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;
+}
+
+enum ia_css_err ia_css_params_store_sctbl(
+	const struct ia_css_pipeline_stage *stage,
+	hrt_vaddress 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 == NULL) {
+		IA_CSS_LEAVE_PRIVATE("void");
+		return IA_CSS_SUCCESS;
+	}
+
+	isp_sc_tbl = ia_css_params_alloc_convert_sctbl(stage, sc_config);
+	if (!isp_sc_tbl) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+	/* 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 IA_CSS_SUCCESS;
+}
+
+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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_resolution pipe_in_res;
+	pipe_in_res.width = 0;
+	pipe_in_res.height = 0;
+
+	assert(params != NULL);
+
+	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,
+				IA_CSS_STREAM_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 != IA_CSS_SUCCESS)
+			    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 == NULL)
+		return;
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	*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 == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	*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 == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	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 == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	*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 == NULL)
+		return;
+
+	IA_CSS_ENTER("");
+
+
+
+	for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
+		if (me->coordinates_x[i]) {
+			sh_css_free(me->coordinates_x[i]);
+			me->coordinates_x[i] = NULL;
+		}
+		if (me->coordinates_y[i]) {
+			sh_css_free(me->coordinates_y[i]);
+			me->coordinates_y[i] = NULL;
+		}
+	}
+
+	sh_css_free(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 = sh_css_malloc(sizeof(*me));
+	if (me == NULL) {
+		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] =
+		    sh_css_malloc(height * width *
+				  sizeof(*me->coordinates_x[i]));
+		me->coordinates_y[i] =
+		    sh_css_malloc(height * width *
+				  sizeof(*me->coordinates_y[i]));
+
+		if ((me->coordinates_x[i] == NULL) ||
+			(me->coordinates_y[i] == NULL)) {
+			ia_css_morph_table_free(me);
+			me = NULL;
+			return me;
+		}
+	}
+	me->width = width;
+	me->height = height;
+	IA_CSS_LEAVE("");
+	return me;
+
+}
+
+
+static enum ia_css_err 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 != NULL);
+	assert(binary != NULL);
+
+	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 == NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	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(IA_CSS_SUCCESS);
+
+	return IA_CSS_SUCCESS;
+}
+
+static void
+sh_css_set_morph_table(struct ia_css_isp_parameters *params,
+			const struct ia_css_morph_table *table)
+{
+	if (table == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("table=%p", table);
+
+	assert(params != NULL);
+	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);
+	}
+#if !defined(HAS_NO_HMEM)
+	IA_CSS_LOG("3A: HMEM");
+	ia_css_s3a_hmem_decode(host_stats, isp_stats->hmem_stats);
+#endif
+
+	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) {
+			sh_css_free(me->data_ptr);
+			me->data_ptr = NULL;
+			me->data_allocated = false;
+		}
+		sh_css_free(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 = sh_css_malloc(sizeof(*me));
+	if (!me) {
+		IA_CSS_LEAVE("cannot allocate memory");
+		goto err;
+	}
+
+	me->data_ptr = data_ptr;
+	me->data_allocated = data_ptr == NULL;
+	if (!data_ptr) {
+		me->data_ptr = sh_css_malloc(isp_stats->size);
+		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:
+	if (me)
+		sh_css_free(me);
+	return NULL;
+
+}
+
+enum ia_css_err
+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;
+	enum ia_css_err ret = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("host_stats=%p, isp_stats=%p", host_stats, isp_stats);
+
+	assert(host_stats != NULL);
+	assert(isp_stats != NULL);
+
+	map = ia_css_isp_3a_statistics_map_allocate(isp_stats, NULL);
+	if (map) {
+		mmgr_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 = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+
+	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 != NULL);
+
+	/* 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;
+#ifdef ISP2401
+	assert(pipe != NULL);
+	assert(pipe->mode < IA_CSS_PIPE_ID_NUM);
+
+	if (pipe->mode < IA_CSS_PIPE_ID_NUM) {
+		params->pipe_dp_config[pipe->mode].gr = params->wb_config.gr;
+		params->pipe_dp_config[pipe->mode].r  = params->wb_config.r;
+		params->pipe_dp_config[pipe->mode].b  = params->wb_config.b;
+		params->pipe_dp_config[pipe->mode].gb = params->wb_config.gb;
+	}
+#endif
+}
+
+#ifdef ISP2401
+static void
+sh_css_set_dp_config(const struct ia_css_pipe *pipe,
+			struct ia_css_isp_parameters *params,
+			const struct ia_css_dp_config *config)
+{
+	if (config == NULL)
+		return;
+
+	assert(params != NULL);
+	assert(pipe != NULL);
+	assert(pipe->mode < IA_CSS_PIPE_ID_NUM);
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+	ia_css_dp_debug_dtrace(config, IA_CSS_DEBUG_TRACE_PRIVATE);
+	if (pipe->mode < IA_CSS_PIPE_ID_NUM) {
+		params->pipe_dp_config[pipe->mode] = *config;
+		params->pipe_dpc_config_changed[pipe->mode] = true;
+	}
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+#endif
+
+static void
+sh_css_get_dp_config(const struct ia_css_pipe *pipe,
+			const struct ia_css_isp_parameters *params,
+			struct ia_css_dp_config *config)
+{
+	if (config == NULL)
+		return;
+
+	assert(params != NULL);
+	assert(pipe != NULL);
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	*config = params->pipe_dp_config[pipe->mode];
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void
+sh_css_set_nr_config(struct ia_css_isp_parameters *params,
+			const struct ia_css_nr_config *config)
+{
+	if (config == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+
+	IA_CSS_ENTER_PRIVATE("config=%p", config);
+
+	assert(params != NULL);
+	*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 == NULL)
+		return;
+	assert(params != NULL);
+	assert(pipe != NULL);
+	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);
+
+#if !defined(HAS_NO_DVS_6AXIS_CONFIG_UPDATE)
+	params->pipe_dvs_6axis_config_changed[pipe->mode] = true;
+#endif
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+	assert(pipe != NULL);
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+		return;
+	assert(params != NULL);
+
+	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 == NULL)
+	{
+		IA_CSS_ERROR("pipe=%p", NULL);
+		return NULL;
+	}
+	return pipe->config.p_isp_config;
+}
+
+enum ia_css_err
+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);
+}
+
+enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	if ((stream == NULL) || (config == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	IA_CSS_ENTER("stream=%p, config=%p, pipe=%p", stream, config, pipe);
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+	if (config->output_frame)
+		err = sh_css_set_per_frame_isp_config_on_pipe(stream, config, pipe);
+	else
+#endif
+		err = sh_css_set_global_isp_config_on_pipe(stream->pipes[0], config, pipe);
+
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+enum ia_css_err
+ia_css_pipe_set_isp_config(struct ia_css_pipe *pipe,
+	struct ia_css_isp_config *config)
+{
+	struct ia_css_pipe *pipe_in = pipe;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	IA_CSS_ENTER("pipe=%p", pipe);
+
+	if ((pipe == NULL) || (pipe->stream == NULL))
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	ia_css_debug_dtrace(IA_CSS_DEBUG_TRACE, "config=%p\n", config);
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+	if (config->output_frame)
+		err = sh_css_set_per_frame_isp_config_on_pipe(pipe->stream, config, pipe);
+	else
+#endif
+		err = sh_css_set_global_isp_config_on_pipe(pipe, config, pipe_in);
+	IA_CSS_LEAVE_ERR(err);
+	return err;
+}
+
+static enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum ia_css_err err1 = IA_CSS_SUCCESS;
+	enum ia_css_err err2 = IA_CSS_SUCCESS;
+
+	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 != IA_CSS_SUCCESS ) ? err1 : ((err2 != IA_CSS_SUCCESS) ? err2 : err);
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+static enum ia_css_err
+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 i;
+	bool per_frame_config_created = false;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	enum ia_css_err err1 = IA_CSS_SUCCESS;
+	enum ia_css_err err2 = IA_CSS_SUCCESS;
+	enum ia_css_err err3 = IA_CSS_SUCCESS;
+
+	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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+		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 != IA_CSS_SUCCESS)
+			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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 != IA_CSS_SUCCESS) ? err1 :
+		(err2 != IA_CSS_SUCCESS) ? err2 :
+		(err3 != IA_CSS_SUCCESS) ? err3 : err;
+exit:
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+#endif
+
+static enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool is_dp_10bpp = true;
+	assert(pipe != NULL);
+
+	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_update_shading_table_status(pipe_in, params);
+	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 != NULL);
+	params->dvs2_coef_table_changed = (config->dvs2_coefs != NULL);
+
+	params->output_frame = config->output_frame;
+	params->isp_parameters_id = config->isp_config_id;
+#ifdef ISP2401
+	/* Currently we do not offer CSS interface to set different
+	 * configurations for DPC, i.e. depending on DPC being enabled
+	 * before (NORM+OBC) or after. The folllowing code to set the
+	 * DPC configuration should be updated when this interface is made
+	 * available */
+	sh_css_set_dp_config(pipe, params, config->dp_config);
+	ia_css_set_param_exceptions(pipe, params);
+#endif
+
+	if (IA_CSS_SUCCESS ==
+		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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+		}
+	} else {
+		err = IA_CSS_ERR_INTERNAL_ERROR;
+		goto exit;
+	}
+
+#ifndef ISP2401
+	ia_css_set_param_exceptions(pipe, params);
+#endif
+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 != NULL);
+
+	IA_CSS_ENTER("config=%p", config);
+
+	params = pipe->stream->isp_params_configs;
+	assert(params != NULL);
+
+	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_dp_config(pipe, params, config->dp_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");
+}
+
+#ifndef ISP2401
+/*
+ * coding style says the return of "mmgr_NULL" is the error signal
+ *
+ * Deprecated: Implement mmgr_realloc()
+ */
+static bool realloc_isp_css_mm_buf(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err,
+	uint16_t mmgr_attribute)
+{
+	int32_t id;
+
+	*err = IA_CSS_SUCCESS;
+	/* 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, mmgr_alloc_attr(needed_size,
+							mmgr_attribute));
+
+	if (!*curr_buf) {
+		*err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		*curr_size = 0;
+	} else {
+		*curr_size = needed_size;
+	}
+	IA_CSS_LEAVE_PRIVATE("true");
+	return true;
+}
+
+static bool reallocate_buffer(
+	hrt_vaddress *curr_buf,
+	size_t *curr_size,
+	size_t needed_size,
+	bool force,
+	enum ia_css_err *err)
+{
+	bool ret;
+	uint16_t	mmgr_attribute = MMGR_ATTRIBUTE_DEFAULT;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	ret = realloc_isp_css_mm_buf(curr_buf,
+		curr_size, needed_size, force, err, mmgr_attribute);
+
+	IA_CSS_LEAVE_PRIVATE("ret=%d", ret);
+	return ret;
+}
+
+#endif
+
+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 != NULL);
+
+	/* MW: Does "grid->enable" also control the histogram output ?? */
+	if (!grid->enable)
+		return NULL;
+
+	me = sh_css_calloc(1, sizeof(*me));
+	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;
+	}
+#if !defined(HAS_NO_HMEM)
+	me->hmem_size = sizeof_hmem(HMEM0_ID);
+#endif
+
+	/* 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 = mmgr_malloc(me->size);
+	if (me->data_ptr == mmgr_NULL) {
+		sh_css_free(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 != NULL) {
+		hmm_free(me->data_ptr);
+		sh_css_free(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 = sh_css_malloc(sizeof(*md));
+	if (md == NULL)
+		goto error;
+
+	md->info = *metadata_info;
+	md->exp_id = 0;
+	md->address = mmgr_malloc(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 != NULL) {
+		/* 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);
+		sh_css_free(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 != NULL) {
+		for (i = 0; i < num_bufs; i++)
+			ia_css_metadata_free(bufs[i]);
+	}
+}
+
+unsigned g_param_buffer_dequeue_count = 0;
+unsigned g_param_buffer_enqueue_count = 0;
+
+enum ia_css_err
+ia_css_stream_isp_parameters_init(struct ia_css_stream *stream)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	unsigned 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 != NULL);
+	IA_CSS_ENTER_PRIVATE("void");
+
+	if (stream == NULL) {
+		IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_INVALID_ARGUMENTS);
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	/* 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 != IA_CSS_SUCCESS)
+		 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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	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 enum ia_css_err
+sh_css_create_isp_params(struct ia_css_stream *stream,
+			 struct ia_css_isp_parameters **isp_params_out)
+{
+	bool succ = true;
+	unsigned i;
+	struct sh_css_ddr_address_map *ddr_ptrs;
+	struct sh_css_ddr_address_map_size *ddr_ptrs_size;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	size_t params_size;
+	struct ia_css_isp_parameters *params =
+				sh_css_malloc(sizeof(struct ia_css_isp_parameters));
+
+	if (!params)
+	{
+		*isp_params_out = NULL;
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		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,
+					mmgr_malloc(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,
+					mmgr_malloc(sizeof(struct ia_css_macc_table)));
+	succ &= (ddr_ptrs->macc_tbl != mmgr_NULL);
+
+	*isp_params_out = params;
+	return err;
+}
+
+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 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);
+#ifndef ISP2401
+		ia_css_set_param_exceptions(pipe_in, params);
+#else
+
+		for (i = 0; i < stream->num_pipes; i++) {
+			if (IA_CSS_SUCCESS == 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. */
+				if(is_dp_10bpp) {
+					sh_css_set_dp_config(stream->pipes[i], params, &default_dp_10bpp_config);
+				} else {
+					sh_css_set_dp_config(stream->pipes[i], params, &default_dp_config);
+				}
+			} else {
+				retval = false;
+				goto exit;
+			}
+
+			ia_css_set_param_exceptions(stream->pipes[i], params);
+		}
+
+#endif
+		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;
+		params->sc_table_dirty = false;
+		params->sc_table_last_pipe_num = 0;
+
+		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;
+#ifdef ISP2401
+		ia_css_tnr3_set_default_config(&params->tnr3_config);
+#endif
+	}
+	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 (IA_CSS_SUCCESS ==
+				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. */
+#ifndef ISP2401
+				retval = !is_dp_10bpp;
+#else
+				if (is_dp_10bpp) {
+					retval = false;
+				}
+			} else {
+				retval = false;
+				goto exit;
+			}
+			if (stream->pipes[i]->mode < IA_CSS_PIPE_ID_NUM) {
+				sh_css_set_dp_config(stream->pipes[i], params,
+					&stream_params->pipe_dp_config[stream->pipes[i]->mode]);
+				ia_css_set_param_exceptions(stream->pipes[i], params);
+#endif
+			} else {
+				retval = false;
+				goto exit;
+			}
+		}
+
+#ifndef ISP2401
+		ia_css_set_param_exceptions(pipe_in, params);
+
+#endif
+		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_update_shading_table_status(pipe_in, params);
+			sh_css_set_shading_table(stream, params, stream_params->sc_table);
+		}
+		else {
+			params->sc_table = NULL;
+			params->sc_table_changed = true;
+			params->sc_table_dirty = false;
+			params->sc_table_last_pipe_num = 0;
+		}
+
+		/* 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;
+}
+
+enum ia_css_err
+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,
+					    mmgr_calloc(1,
+					    sizeof(struct sh_css_sp_stage)));
+			xmem_isp_stage_ptrs[p][i] =
+					ia_css_refcount_increment(-1,
+					    mmgr_calloc(1,
+					    sizeof(struct sh_css_isp_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(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+				return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+			}
+		}
+	}
+
+	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, mmgr_calloc(1,
+		CEIL_MUL(sizeof(struct sh_css_ddr_address_map),
+			 HIVE_ISP_DDR_WORD_BYTES)));
+	xmem_sp_group_ptrs = ia_css_refcount_increment(-1, mmgr_calloc(1,
+		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(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+	IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+static void host_lut_store(const void *lut)
+{
+	unsigned i;
+
+	for (i = 0; i < N_GDC_ID; i++)
+		gdc_lut_store((gdc_ID_t)i, (const int (*)[HRT_GDC_N]) lut);
+}
+
+/* Note that allocation is in ipu address space. */
+inline hrt_vaddress sh_css_params_alloc_gdc_lut(void)
+{
+	return mmgr_malloc(sizeof(zoom_table));
+}
+
+inline void sh_css_params_free_gdc_lut(hrt_vaddress addr)
+{
+	if (addr != mmgr_NULL)
+		hmm_free(addr);
+}
+
+enum ia_css_err ia_css_pipe_set_bci_scaler_lut(struct ia_css_pipe *pipe,
+	const void *lut)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+#ifndef ISP2401
+	bool store = true;
+#else
+	bool stream_started = false;
+#endif
+	IA_CSS_ENTER("pipe=%p lut=%p", pipe, lut);
+
+	if (lut == NULL || pipe == NULL) {
+		err = IA_CSS_ERR_INVALID_ARGUMENTS;
+		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");
+#ifndef ISP2401
+		store = false;
+#else
+		stream_started = true;
+#endif
+		err = IA_CSS_ERR_NOT_SUPPORTED;
+	}
+
+	/* Free any existing tables. */
+#ifndef ISP2401
+	if (pipe->scaler_pp_lut != mmgr_NULL) {
+		hmm_free(pipe->scaler_pp_lut);
+		pipe->scaler_pp_lut = mmgr_NULL;
+	}
+#else
+	sh_css_params_free_gdc_lut(pipe->scaler_pp_lut);
+	pipe->scaler_pp_lut = mmgr_NULL;
+#endif
+
+#ifndef ISP2401
+	if (store) {
+		pipe->scaler_pp_lut = mmgr_malloc(sizeof(zoom_table));
+#else
+	if (!stream_started) {
+		pipe->scaler_pp_lut = sh_css_params_alloc_gdc_lut();
+#endif
+		if (pipe->scaler_pp_lut == mmgr_NULL) {
+#ifndef ISP2401
+			IA_CSS_LEAVE("lut(%p) err=%d", pipe->scaler_pp_lut, err);
+			return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+#else
+			ia_css_debug_dtrace(IA_CSS_DEBUG_ERROR,
+				"unable to allocate scaler_pp_lut\n");
+			err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+		} else {
+			gdc_lut_convert_to_isp_format((const int(*)[HRT_GDC_N])lut,
+				interleaved_lut_temp);
+			mmgr_store(pipe->scaler_pp_lut,
+				(int *)interleaved_lut_temp,
+				sizeof(zoom_table));
+#endif
+		}
+#ifndef ISP2401
+
+		gdc_lut_convert_to_isp_format((const int(*)[HRT_GDC_N])lut, interleaved_lut_temp);
+		mmgr_store(pipe->scaler_pp_lut, (int *)interleaved_lut_temp,
+			sizeof(zoom_table));
+#endif
+	}
+
+	IA_CSS_LEAVE("lut(%p) err=%d", pipe->scaler_pp_lut, err);
+	return err;
+}
+
+/* if pipe is NULL, returns default lut addr. */
+hrt_vaddress sh_css_pipe_get_pp_gdc_lut(const struct ia_css_pipe *pipe)
+{
+	assert(pipe != NULL);
+
+	if (pipe->scaler_pp_lut != mmgr_NULL)
+		return pipe->scaler_pp_lut;
+	else
+		return sh_css_params_get_default_gdc_lut();
+}
+
+enum ia_css_err sh_css_params_map_and_store_default_gdc_lut(void)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	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);
+
+#ifndef ISP2401
+	default_gdc_lut = mmgr_malloc(sizeof(zoom_table));
+#else
+	default_gdc_lut = sh_css_params_alloc_gdc_lut();
+#endif
+	if (default_gdc_lut == mmgr_NULL)
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	gdc_lut_convert_to_isp_format((const int(*)[HRT_GDC_N])zoom_table,
+		interleaved_lut_temp);
+	mmgr_store(default_gdc_lut, (int *)interleaved_lut_temp,
+		sizeof(zoom_table));
+
+	IA_CSS_LEAVE_PRIVATE("lut(%p) err=%d", default_gdc_lut, err);
+	return err;
+}
+
+void sh_css_params_free_default_gdc_lut(void)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+#ifndef ISP2401
+	if (default_gdc_lut != mmgr_NULL) {
+		hmm_free(default_gdc_lut);
+		default_gdc_lut = mmgr_NULL;
+	}
+#else
+	sh_css_params_free_gdc_lut(default_gdc_lut);
+	default_gdc_lut = mmgr_NULL;
+#endif
+
+	IA_CSS_LEAVE_PRIVATE("void");
+
+}
+
+hrt_vaddress sh_css_params_get_default_gdc_lut(void)
+{
+	return default_gdc_lut;
+}
+
+static void free_param_set_callback(
+	hrt_vaddress ptr)
+{
+	IA_CSS_ENTER_PRIVATE("void");
+
+	free_ia_css_isp_parameter_set_info(ptr);
+
+	IA_CSS_LEAVE_PRIVATE("void");
+}
+
+static void free_buffer_callback(
+	hrt_vaddress 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;
+
+	hrt_vaddress *addrs = (hrt_vaddress *)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 == NULL) {
+		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) {
+		sh_css_free(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;
+		}
+	}
+
+	sh_css_free(params);
+	if (per_frame_params)
+		sh_css_free(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 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;
+	uint16_t *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(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		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 enum ia_css_err
+store_morph_plane(
+	unsigned short *data,
+	unsigned int width,
+	unsigned int height,
+	hrt_vaddress 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(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+		return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+	}
+	ia_css_params_store_ia_css_host_data(dest, isp_data);
+
+	ia_css_host_data_free(isp_data);
+	return IA_CSS_SUCCESS;
+}
+
+static void sh_css_update_isp_params_to_ddr(
+	struct ia_css_isp_parameters *params,
+	hrt_vaddress ddr_ptr)
+{
+	size_t size = sizeof(params->uds);
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(params != NULL);
+
+	mmgr_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,
+	hrt_vaddress 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);
+	mmgr_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;
+	hrt_vaddress 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 = (hrt_vaddress)0;
+		/* clean-up old copy */
+		while (IA_CSS_SUCCESS == ia_css_bufq_dequeue_buffer(param_queue_ids[i], (uint32_t *)&cpy)) {
+			/* 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 = (hrt_vaddress)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 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);
+	}
+}
+
+enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	hrt_vaddress 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 != NULL);
+
+	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;
+
+		(void)stage;
+		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);
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+		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);
+#else
+		ia_css_query_internal_queue_id(IA_CSS_BUFFER_TYPE_PARAMETER_SET, thread_id, &queue_id);
+#endif
+		if (!sh_css_sp_is_running()) {
+			/* SP is not running. The queues are not valid */
+			err = IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+			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 dependant
+		 * 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 != IA_CSS_SUCCESS)
+			    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 %x", 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 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 != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+				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 != IA_CSS_SUCCESS)
+			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 != IA_CSS_SUCCESS)
+			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 (IA_CSS_SUCCESS != err) {
+			free_ia_css_isp_parameter_set_info(cpy);
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+			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);
+#endif
+			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);
+#ifdef ISP2401
+			ia_css_save_latest_paramset_ptr(pipe, cpy);
+#endif
+			/*
+			 * 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(IA_CSS_ERR_RESOURCE_NOT_AVAILABLE);
+				return IA_CSS_ERR_RESOURCE_NOT_AVAILABLE;
+			}
+			ia_css_bufq_enqueue_psys_event(
+					IA_CSS_PSYS_SW_EVENT_BUFFER_ENQUEUED,
+					(uint8_t)thread_id,
+					(uint8_t)queue_id,
+					0);
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+			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);
+#endif
+		}
+		/* 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 enum ia_css_err
+sh_css_params_write_to_ddr_internal(
+	struct ia_css_pipe *pipe,
+	unsigned 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)
+{
+	enum ia_css_err err;
+	const struct ia_css_binary *binary;
+
+	unsigned stage_num;
+	unsigned 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 != NULL);
+	assert(ddr_map != NULL);
+	assert(ddr_map_size != NULL);
+	assert(stage != NULL);
+
+	binary = stage->binary;
+	assert(binary != NULL);
+
+
+	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_SUCCESS) {
+			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_SUCCESS) {
+					IA_CSS_LEAVE_ERR_PRIVATE(err);
+					return err;
+				}
+			}
+		}
+	}
+
+	if (binary->info->sp.enable.sc) {
+		uint32_t enable_conv = params->
+			shading_settings.enable_shading_table_conversion;
+
+		buff_realloced = reallocate_buffer(&ddr_map->sc_tbl,
+			&ddr_map_size->sc_tbl,
+			(size_t)(SCTBL_BYTES(binary)),
+			params->sc_table_changed,
+			&err);
+		if (err != IA_CSS_SUCCESS) {
+			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_SUCCESS) {
+						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;
+					}
+#ifndef ISP2401
+					sh_css_params_shading_id_table_generate(&params->sc_config, binary);
+#else
+					sh_css_params_shading_id_table_generate(&params->sc_config,
+						binary->sctbl_width_per_color, binary->sctbl_height);
+#endif
+					if (params->sc_config == NULL) {
+						IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+						return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+					}
+
+					/* store the shading table to ddr */
+					err = ia_css_params_store_sctbl(stage, ddr_map->sc_tbl, params->sc_config);
+					if (err != IA_CSS_SUCCESS) {
+						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 == NULL) {
+					IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+					return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+				}
+
+				/* store the shading table to ddr */
+				err = ia_css_params_store_sctbl(stage, ddr_map->sc_tbl, params->sc_config);
+				if (err != IA_CSS_SUCCESS) {
+					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 ------ */
+			}
+		}
+	}
+#ifdef ISP2401
+	/* DPC configuration is made pipe specific to allow flexibility in positioning of the
+	 * DPC kernel. The code below sets the pipe specific configuration to
+	 * individual binaries. */
+	if (params->pipe_dpc_config_changed[pipe_id] && binary->info->sp.enable.dpc) {
+		unsigned size   = stage->binary->info->mem_offsets.offsets.param->dmem.dp.size;
+
+		unsigned offset = stage->binary->info->mem_offsets.offsets.param->dmem.dp.offset;
+		if (size) {
+			ia_css_dp_encode((struct sh_css_isp_dp_params *)
+				&binary->mem_params.params[IA_CSS_PARAM_CLASS_PARAM][IA_CSS_ISP_DMEM].address[offset],
+				&params->pipe_dp_config[pipe_id], size);
+#endif
+
+#ifdef ISP2401
+			params->isp_params_changed = true;
+			params->isp_mem_params_changed[pipe_id][stage->stage_num][IA_CSS_ISP_DMEM] = true;
+		}
+	}
+#endif
+	if (params->config_changed[IA_CSS_MACC_ID] && binary->info->sp.enable.macc) {
+		unsigned int i, j, idx;
+		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_SUCCESS) {
+			IA_CSS_LEAVE_ERR_PRIVATE(err);
+			return err;
+		}
+		mmgr_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_SUCCESS) {
+			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] == NULL) {
+
+#ifndef ISP2401
+				struct ia_css_resolution dvs_offset;
+				dvs_offset.width  =
+#else
+				struct ia_css_resolution dvs_offset = {0, 0};
+				if (binary->dvs_envelope.width || binary->dvs_envelope.height) {
+					dvs_offset.width  =
+#endif
+						(PIX_SHIFT_FILTER_RUN_IN_X + binary->dvs_envelope.width) / 2;
+#ifndef ISP2401
+				dvs_offset.height =
+#else
+					dvs_offset.height =
+#endif
+						(PIX_SHIFT_FILTER_RUN_IN_Y + binary->dvs_envelope.height) / 2;
+#ifdef ISP2401
+				}
+#endif
+
+				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] == NULL) {
+					IA_CSS_LEAVE_ERR_PRIVATE(IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY);
+					return IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+				}
+				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;
+		hrt_vaddress *virt_addr_tetra_x[
+			IA_CSS_MORPH_TABLE_NUM_PLANES];
+		size_t *virt_size_tetra_x[
+			IA_CSS_MORPH_TABLE_NUM_PLANES];
+		hrt_vaddress *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_SUCCESS) {
+				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_SUCCESS) {
+				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 != NULL) &&
+			    (table->width < binary->morph_tbl_width ||
+			     table->height < binary->morph_tbl_height)) {
+				table = NULL;
+			}
+			if (table == NULL) {
+				err = sh_css_params_default_morph_table(&id_table,
+								  binary);
+				if (err != IA_CSS_SUCCESS) {
+					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 != NULL)
+				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_SUCCESS) {
+			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(IA_CSS_SUCCESS);
+	return IA_CSS_SUCCESS;
+}
+
+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 != NULL);
+
+	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 != NULL);
+
+	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;
+				}
+#ifndef ISP2401
+				sh_css_params_shading_id_table_generate(&params->sc_config, binary);
+
+#else
+				sh_css_params_shading_id_table_generate(&params->sc_config,
+					binary->sctbl_width_per_color, binary->sctbl_height);
+#endif
+				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] != NULL);
+			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;
+}
+
+
+hrt_vaddress sh_css_store_sp_group_to_ddr(void)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	mmgr_store(xmem_sp_group_ptrs,
+			     &sh_css_sp_group,
+			     sizeof(struct sh_css_sp_group));
+	return xmem_sp_group_ptrs;
+}
+
+hrt_vaddress sh_css_store_sp_stage_to_ddr(
+	unsigned pipe,
+	unsigned stage)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	mmgr_store(xmem_sp_stage_ptrs[pipe][stage],
+			     &sh_css_sp_stage,
+			     sizeof(struct sh_css_sp_stage));
+	return xmem_sp_stage_ptrs[pipe][stage];
+}
+
+hrt_vaddress sh_css_store_isp_stage_to_ddr(
+	unsigned pipe,
+	unsigned stage)
+{
+	IA_CSS_ENTER_LEAVE_PRIVATE("void");
+	mmgr_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 enum ia_css_err ref_sh_css_ddr_address_map(
+	struct sh_css_ddr_address_map *map,
+	struct sh_css_ddr_address_map *out)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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;
+		hrt_vaddress *addrs;
+	} in_addrs, to_addrs;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(map != NULL);
+	assert(out != NULL);
+
+	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(hrt_vaddress));
+
+	/* 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 enum ia_css_err write_ia_css_isp_parameter_set_info_to_ddr(
+	struct ia_css_isp_parameter_set_info *me,
+	hrt_vaddress *out)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	bool succ;
+
+	IA_CSS_ENTER_PRIVATE("void");
+
+	assert(me != NULL);
+	assert(out != NULL);
+
+	*out = ia_css_refcount_increment(IA_CSS_REFCOUNT_PARAM_SET_POOL, mmgr_malloc(
+				sizeof(struct ia_css_isp_parameter_set_info)));
+	succ = (*out != mmgr_NULL);
+	if (succ)
+		mmgr_store(*out,
+			me, sizeof(struct ia_css_isp_parameter_set_info));
+	else
+		err = IA_CSS_ERR_CANNOT_ALLOCATE_MEMORY;
+
+	IA_CSS_LEAVE_ERR_PRIVATE(err);
+	return err;
+}
+
+static enum ia_css_err
+free_ia_css_isp_parameter_set_info(
+	hrt_vaddress ptr)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	struct ia_css_isp_parameter_set_info isp_params_info;
+	unsigned int i;
+	hrt_vaddress *addrs = (hrt_vaddress *)&isp_params_info.mem_map;
+
+	IA_CSS_ENTER_PRIVATE("ptr = %p", 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 = IA_CSS_ERR_INVALID_ARGUMENTS;
+		IA_CSS_LEAVE_ERR_PRIVATE(err);
+		return err;
+	}
+
+	mmgr_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 */
+#ifndef ISP2401
+		if (!ia_css_refcount_is_valid(addrs[i])) {
+#else
+		if (ia_css_refcount_is_valid(addrs[i])) {
+			ia_css_refcount_decrement(IA_CSS_REFCOUNT_PARAM_BUFFER, addrs[i]);
+		} else {
+#endif
+			IA_CSS_ERROR("%s: IA_CSS_REFCOUNT_PARAM_BUFFER(0x%x) invalid arg", __func__, ptr);
+			err = IA_CSS_ERR_INVALID_ARGUMENTS;
+			continue;
+		}
+#ifndef ISP2401
+
+		ia_css_refcount_decrement(IA_CSS_REFCOUNT_PARAM_BUFFER, addrs[i]);
+#endif
+	}
+	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 i, j, mem;
+
+	IA_CSS_ENTER_PRIVATE("void");
+	assert(stream != NULL);
+
+	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 != NULL);
+	assert(in_frame_info != NULL);
+	assert(out_frame_info != NULL);
+	assert(dvs_env != NULL);
+	assert(zoom != NULL);
+	assert(motion_vector != NULL);
+	assert(uds != NULL);
+	assert(sp_out_crop_pos != NULL);
+
+	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 enum ia_css_err
+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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	/* 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 != NULL);
+	assert(in_frame_info != NULL);
+	assert(out_frame_info != NULL);
+	assert(dvs_env != NULL);
+	assert(zoom != NULL);
+	assert(motion_vector != NULL);
+	assert(uds != NULL);
+	assert(sp_out_crop_pos != NULL);
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	} 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 != NULL);
+
+	me = sh_css_calloc(1, sizeof(*me));
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+	grid_size = grid->width * grid->height;
+	me->data = sh_css_malloc(grid_size * sizeof(*me->data));
+	if (!me->data)
+		goto err;
+#if !defined(HAS_NO_HMEM)
+	/* No weighted histogram, no structure, treat the histogram data as a byte dump in a byte array */
+	me->rgby_data = (struct ia_css_3a_rgby_output *)sh_css_malloc(sizeof_hmem(HMEM0_ID));
+#else
+	me->rgby_data = NULL;
+#endif
+
+	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) {
+		sh_css_free(me->rgby_data);
+		sh_css_free(me->data);
+		memset(me, 0, sizeof(struct ia_css_3a_statistics));
+		sh_css_free(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 != NULL);
+
+	me = sh_css_calloc(1, sizeof(*me));
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+	me->hor_proj = sh_css_malloc(grid->height * IA_CSS_DVS_NUM_COEF_TYPES *
+					sizeof(*me->hor_proj));
+	if (!me->hor_proj)
+		goto err;
+
+	me->ver_proj = sh_css_malloc(grid->width * IA_CSS_DVS_NUM_COEF_TYPES *
+					sizeof(*me->ver_proj));
+	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) {
+		sh_css_free(me->hor_proj);
+		sh_css_free(me->ver_proj);
+		memset(me, 0, sizeof(struct ia_css_dvs_statistics));
+		sh_css_free(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 != NULL);
+
+	me = sh_css_calloc(1, sizeof(*me));
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_coefs = sh_css_malloc(grid->num_hor_coefs *
+				IA_CSS_DVS_NUM_COEF_TYPES *
+				sizeof(*me->hor_coefs));
+	if (!me->hor_coefs)
+		goto err;
+
+	me->ver_coefs = sh_css_malloc(grid->num_ver_coefs *
+				IA_CSS_DVS_NUM_COEF_TYPES *
+				sizeof(*me->ver_coefs));
+	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) {
+		sh_css_free(me->hor_coefs);
+		sh_css_free(me->ver_coefs);
+		memset(me, 0, sizeof(struct ia_css_dvs_coefficients));
+		sh_css_free(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 != NULL);
+
+	me = sh_css_calloc(1, sizeof(*me));
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_prod.odd_real = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->hor_prod.odd_real));
+	if (!me->hor_prod.odd_real)
+		goto err;
+
+	me->hor_prod.odd_imag = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->hor_prod.odd_imag));
+	if (!me->hor_prod.odd_imag)
+		goto err;
+
+	me->hor_prod.even_real = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->hor_prod.even_real));
+	if (!me->hor_prod.even_real)
+		goto err;
+
+	me->hor_prod.even_imag = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->hor_prod.even_imag));
+	if (!me->hor_prod.even_imag)
+		goto err;
+
+	me->ver_prod.odd_real = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->ver_prod.odd_real));
+	if (!me->ver_prod.odd_real)
+		goto err;
+
+	me->ver_prod.odd_imag = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->ver_prod.odd_imag));
+	if (!me->ver_prod.odd_imag)
+		goto err;
+
+	me->ver_prod.even_real = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->ver_prod.even_real));
+	if (!me->ver_prod.even_real)
+		goto err;
+
+	me->ver_prod.even_imag = sh_css_malloc(grid->aligned_width *
+		grid->aligned_height * sizeof(*me->ver_prod.even_imag));
+	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) {
+		sh_css_free(me->hor_prod.odd_real);
+		sh_css_free(me->hor_prod.odd_imag);
+		sh_css_free(me->hor_prod.even_real);
+		sh_css_free(me->hor_prod.even_imag);
+		sh_css_free(me->ver_prod.odd_real);
+		sh_css_free(me->ver_prod.odd_imag);
+		sh_css_free(me->ver_prod.even_real);
+		sh_css_free(me->ver_prod.even_imag);
+		memset(me, 0, sizeof(struct ia_css_dvs2_statistics));
+		sh_css_free(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 != NULL);
+
+	me = sh_css_calloc(1, sizeof(*me));
+	if (!me)
+		goto err;
+
+	me->grid = *grid;
+
+	me->hor_coefs.odd_real = sh_css_malloc(grid->num_hor_coefs *
+		sizeof(*me->hor_coefs.odd_real));
+	if (!me->hor_coefs.odd_real)
+		goto err;
+
+	me->hor_coefs.odd_imag = sh_css_malloc(grid->num_hor_coefs *
+		sizeof(*me->hor_coefs.odd_imag));
+	if (!me->hor_coefs.odd_imag)
+		goto err;
+
+	me->hor_coefs.even_real = sh_css_malloc(grid->num_hor_coefs *
+		sizeof(*me->hor_coefs.even_real));
+	if (!me->hor_coefs.even_real)
+		goto err;
+
+	me->hor_coefs.even_imag = sh_css_malloc(grid->num_hor_coefs *
+		sizeof(*me->hor_coefs.even_imag));
+	if (!me->hor_coefs.even_imag)
+		goto err;
+
+	me->ver_coefs.odd_real = sh_css_malloc(grid->num_ver_coefs *
+		sizeof(*me->ver_coefs.odd_real));
+	if (!me->ver_coefs.odd_real)
+		goto err;
+
+	me->ver_coefs.odd_imag = sh_css_malloc(grid->num_ver_coefs *
+		sizeof(*me->ver_coefs.odd_imag));
+	if (!me->ver_coefs.odd_imag)
+		goto err;
+
+	me->ver_coefs.even_real = sh_css_malloc(grid->num_ver_coefs *
+		sizeof(*me->ver_coefs.even_real));
+	if (!me->ver_coefs.even_real)
+		goto err;
+
+	me->ver_coefs.even_imag = sh_css_malloc(grid->num_ver_coefs *
+		sizeof(*me->ver_coefs.even_imag));
+	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) {
+		sh_css_free(me->hor_coefs.odd_real);
+		sh_css_free(me->hor_coefs.odd_imag);
+		sh_css_free(me->hor_coefs.even_real);
+		sh_css_free(me->hor_coefs.even_imag);
+		sh_css_free(me->ver_coefs.odd_real);
+		sh_css_free(me->ver_coefs.odd_imag);
+		sh_css_free(me->ver_coefs.even_real);
+		sh_css_free(me->ver_coefs.even_imag);
+		memset(me, 0, sizeof(struct ia_css_dvs2_coefficients));
+		sh_css_free(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 != NULL);
+	params = stream->isp_params_configs;
+
+	/* Backward compatibility by default consider pipe as Video*/
+	if (!params || (params && !params->pipe_dvs_6axis_config[IA_CSS_PIPE_ID_VIDEO])) {
+		goto err;
+	}
+
+	dvs_config = (struct ia_css_dvs_6axis_config *)sh_css_calloc(1, sizeof(struct ia_css_dvs_6axis_config));
+	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 = (uint32_t *)sh_css_malloc(width_y * height_y * sizeof(uint32_t));
+	if (!dvs_config->xcoords_y)
+		goto err;
+
+	dvs_config->ycoords_y = (uint32_t *)sh_css_malloc(width_y * height_y * sizeof(uint32_t));
+	if (!dvs_config->ycoords_y)
+		goto err;
+
+	dvs_config->xcoords_uv = (uint32_t *)sh_css_malloc(width_uv * height_uv * sizeof(uint32_t));
+	if (!dvs_config->xcoords_uv)
+		goto err;
+
+	dvs_config->ycoords_uv = (uint32_t *)sh_css_malloc(width_uv * height_uv * sizeof(uint32_t));
+	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) {
+		sh_css_free(dvs_6axis_config->xcoords_y);
+		sh_css_free(dvs_6axis_config->ycoords_y);
+		sh_css_free(dvs_6axis_config->xcoords_uv);
+		sh_css_free(dvs_6axis_config->ycoords_uv);
+		memset(dvs_6axis_config, 0, sizeof(struct ia_css_dvs_6axis_config));
+		sh_css_free(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;
+	enum ia_css_err err;
+	int i;
+
+	if (stream == NULL)
+		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 == IA_CSS_SUCCESS)
+				stage->enable_zoom = enable;
+			break;
+		}
+	}
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.h
new file mode 100644
index 000000000000..a7ffe6d8331b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params.h
@@ -0,0 +1,188 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 sc_table_dirty;
+	unsigned int sc_table_last_pipe_num;
+	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) */
+	uint32_t 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(
+	hrt_vaddress ddr_addr,
+	struct ia_css_host_data *data);
+
+enum ia_css_err
+ia_css_params_store_sctbl(
+	    const struct ia_css_pipeline_stage *stage,
+	    hrt_vaddress 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);
+
+/* ipu address allocation/free for gdc lut */
+hrt_vaddress
+sh_css_params_alloc_gdc_lut(void);
+void
+sh_css_params_free_gdc_lut(hrt_vaddress addr);
+
+enum ia_css_err
+sh_css_params_map_and_store_default_gdc_lut(void);
+
+void
+sh_css_params_free_default_gdc_lut(void);
+
+hrt_vaddress
+sh_css_params_get_default_gdc_lut(void);
+
+hrt_vaddress
+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/atomisp2/css2400/sh_css_params_internal.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params_internal.h
new file mode 100644
index 000000000000..baca24532f9f
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_params_internal.h
@@ -0,0 +1,21 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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/atomisp2/css2400/sh_css_pipe.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_pipe.c
new file mode 100644
index 000000000000..1f57ffad8921
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_pipe.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_pipe.h and ia_css_pipe_public.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_properties.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_properties.c
new file mode 100644
index 000000000000..ad46996cfbd3
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_properties.c
@@ -0,0 +1,43 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 != NULL);
+#if defined(HAS_GDC_VERSION_2) || defined(HAS_GDC_VERSION_3)
+/*
+ * 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;
+#else
+#error "Unknown GDC version"
+#endif
+
+	properties->l1_base_is_index = true;
+
+#if defined(HAS_VAMEM_VERSION_1)
+	properties->vamem_type = IA_CSS_VAMEM_TYPE_1;
+#elif defined(HAS_VAMEM_VERSION_2)
+	properties->vamem_type = IA_CSS_VAMEM_TYPE_2;
+#else
+#error "Unknown VAMEM version"
+#endif
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_shading.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_shading.c
new file mode 100644
index 000000000000..2a2d0f4db44b
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_shading.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_shading.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_sp.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_sp.c
new file mode 100644
index 000000000000..e6a345979ff1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_sp.c
@@ -0,0 +1,1814 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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"
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+#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"
+#if !defined(HAS_NO_INPUT_SYSTEM)
+#include "ia_css_isys.h"
+#endif
+
+#include "gdc_device.h"				/* HRT_GDC_N */
+
+/*#include "sp.h"*/	/* host2sp_enqueue_frame_data() */
+
+#include "memory_access.h"
+
+#include "assert_support.h"
+#include "platform_support.h"	/* hrt_sleep() */
+
+#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)&(((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"
+
+#ifndef ISP2401
+#include "isp/kernels/io_ls/bayer_io_ls/ia_css_bayer_io.host.h"
+#else
+#include "isp/kernels/ipu2_io_ls/bayer_io_ls/ia_css_bayer_io.host.h"
+#endif
+
+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;
+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 enum ia_css_err
+set_output_frame_buffer(const struct ia_css_frame *frame,
+			unsigned 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 hrt_vaddress 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;
+#ifdef ISP2401	
+	sh_css_sp_stage.enable.lace_stats = sh_css_isp_stage.binary_info.enable.lace_stats;
+#endif	
+}
+
+void
+store_sp_stage_data(enum ia_css_pipe_id id, unsigned int pipe_num, unsigned 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 != NULL);
+
+	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 i;
+	unsigned offset = (unsigned int)offsetof(struct sh_css_sp_output, debug)/sizeof(int);
+
+	assert(state != NULL);
+
+	(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 two_ppc)
+{
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	struct sh_css_sp_pipeline *pipe;
+	uint8_t stage_num = 0;
+
+	assert(out_frame != NULL);
+	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(HAS_NO_INPUT_FORMATTER)
+	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 pipe_num,
+			 unsigned two_ppc,
+			 unsigned 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;
+	uint8_t stage_num = 0;
+	struct sh_css_sp_pipeline *pipe;
+
+	assert(out_frame != NULL);
+
+	{
+		/**
+		 * Clear sh_css_sp_stage for easy debugging.
+		 * program_input_circuit must be saved as it is set outside
+		 * this function.
+		 */
+		uint8_t 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(HAS_NO_INPUT_FORMATTER)
+	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 pipe_num, unsigned max_input_width, unsigned int if_config_index)
+{
+	enum ia_css_pipe_id pipe_id;
+	unsigned int thread_id;
+	uint8_t stage_num = 0;
+	struct sh_css_sp_pipeline *pipe;
+#if defined SH_CSS_ENABLE_METADATA
+	enum sh_css_queue_id queue_id;
+#endif
+
+	assert(out_frame != NULL);
+
+	{
+		/**
+		 * Clear sh_css_sp_stage for easy debugging.
+		 * program_input_circuit must be saved as it is set outside
+		 * this function.
+		 */
+		uint8_t 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 defined SH_CSS_ENABLE_METADATA
+	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);
+	}
+#endif
+
+	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 hrt_vaddress 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 != NULL);
+
+	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 enum ia_css_err
+set_input_frame_buffer(const struct ia_css_frame *frame)
+{
+	if (frame == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.in, frame);
+
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+set_output_frame_buffer(const struct ia_css_frame *frame,
+			unsigned idx)
+{
+	if (frame == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.out[idx], frame);
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+set_view_finder_buffer(const struct ia_css_frame *frame)
+{
+	if (frame == NULL)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+
+	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 IA_CSS_ERR_INVALID_ARGUMENTS;
+	}
+
+	sh_css_copy_frame_to_spframe(&sh_css_sp_stage.frames.out_vf, frame);
+	return IA_CSS_SUCCESS;
+}
+
+#if !defined(HAS_NO_INPUT_FORMATTER)
+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 != NULL);
+
+	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 != NULL) {
+		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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+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(HAS_NO_INPUT_SYSTEM) && defined(USE_INPUT_SYSTEM_VERSION_2)
+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)
+{
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	sh_css_sp_group.config.enable_isys_event_queue = enable;
+#else
+	(void)enable;
+#endif
+}
+
+void
+sh_css_sp_set_disable_continuous_viewfinder(bool flag)
+{
+	sh_css_sp_group.config.disable_cont_vf = flag;
+}
+
+static enum ia_css_err
+sh_css_sp_write_frame_pointers(const struct sh_css_binary_args *args)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	int i;
+
+	assert(args != NULL);
+
+	if (args->in_frame)
+		err = set_input_frame_buffer(args->in_frame);
+	if (err == IA_CSS_SUCCESS && 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 == IA_CSS_SUCCESS && 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 != IA_CSS_SUCCESS) assert(false);
+	return err;
+}
+
+static void
+sh_css_sp_init_group(bool two_ppc,
+		     enum ia_css_stream_format input_format,
+		     bool no_isp_sync,
+		     uint8_t if_config_index)
+{
+#if !defined(HAS_NO_INPUT_FORMATTER)
+	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(HAS_NO_INPUT_FORMATTER)
+	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 != NULL);
+	sh_css_isp_stage.binary_info = *info;
+}
+
+static enum ia_css_err
+copy_isp_mem_if_to_ddr(struct ia_css_binary *binary)
+{
+	enum ia_css_err 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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+		return err;
+	return IA_CSS_SUCCESS;
+}
+
+static bool
+is_sp_stage(struct ia_css_pipeline_stage *stage)
+{
+	assert(stage != NULL);
+	return stage->sp_func != IA_CSS_PIPELINE_NO_FUNC;
+}
+
+static enum ia_css_err
+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)
+{
+	enum ia_css_err err = IA_CSS_SUCCESS;
+#ifdef ISP2401
+	struct ia_css_pipe *pipe = find_pipe_by_num(pipeline->pipe_num);
+	const struct ia_css_resolution *res;
+
+#endif
+	ia_css_fpn_configure(binary,  &binary->in_frame_info);
+	ia_css_crop_configure(binary, &args->delay_frames[0]->info);
+	ia_css_qplane_configure(pipeline, binary, &binary->in_frame_info);
+	ia_css_output0_configure(binary, &args->out_frame[0]->info);
+	ia_css_output1_configure(binary, &args->out_vf_frame->info);
+	ia_css_copy_output_configure(binary, args->copy_output);
+	ia_css_output0_configure(binary, &args->out_frame[0]->info);
+#ifdef ISP2401
+	ia_css_sc_configure(binary, pipeline->shading.internal_frame_origin_x_bqs_on_sctbl,
+				    pipeline->shading.internal_frame_origin_y_bqs_on_sctbl);
+#endif
+	ia_css_iterator_configure(binary, &args->in_frame->info);
+	ia_css_dvs_configure(binary, &args->out_frame[0]->info);
+	ia_css_output_configure(binary, &args->out_frame[0]->info);
+	ia_css_raw_configure(pipeline, binary, &args->in_frame->info, &binary->in_frame_info, two_ppc, deinterleaved);
+	ia_css_ref_configure(binary, (const struct ia_css_frame **)args->delay_frames, pipeline->dvs_frame_delay);
+	ia_css_tnr_configure(binary, (const struct ia_css_frame **)args->tnr_frames);
+	ia_css_bayer_io_config(binary, args);
+	return err;
+}
+
+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);
+#if defined SH_CSS_ENABLE_METADATA
+	initialize_frame_buffer_attribute(&frames->metadata_buf);
+#endif
+}
+
+static enum ia_css_err
+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 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;
+	enum ia_css_err err = IA_CSS_SUCCESS;
+	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 != NULL);
+	assert(blob_info != NULL);
+	assert(args != NULL);
+	assert(isp_mem_if != NULL);
+
+	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.
+		 */
+		uint8_t 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 == NULL) {
+		sh_css_sp_group.pipe[thread_id].sp_stage_addr[stage] = mmgr_NULL;
+		return IA_CSS_SUCCESS;
+	}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401)
+	(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);
+	strncpy(sh_css_isp_stage.binary_name, binary_name, SH_CSS_MAX_BINARY_NAME-1);
+	sh_css_isp_stage.binary_name[SH_CSS_MAX_BINARY_NAME - 1] = 0;
+	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);
+	}
+#if defined SH_CSS_ENABLE_METADATA
+	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);
+#endif
+	if (err != IA_CSS_SUCCESS)
+		return err;
+
+#ifdef USE_INPUT_SYSTEM_VERSION_2401
+#ifndef ISP2401
+	if (args->in_frame) {
+		pipe = find_pipe_by_num(sh_css_sp_group.pipe[thread_id].pipe_num);
+		if (pipe == NULL)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		ia_css_get_crop_offsets(pipe, &args->in_frame->info);
+	} else if (&binary->in_frame_info) {
+		pipe = find_pipe_by_num(sh_css_sp_group.pipe[thread_id].pipe_num);
+		if (pipe == NULL)
+			return IA_CSS_ERR_INTERNAL_ERROR;
+		ia_css_get_crop_offsets(pipe, &binary->in_frame_info);
+#else
+	if (stage == 0) {
+		if (args->in_frame) {
+			pipe = find_pipe_by_num(sh_css_sp_group.pipe[thread_id].pipe_num);
+			if (pipe == NULL)
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			ia_css_get_crop_offsets(pipe, &args->in_frame->info);
+		} else if (&binary->in_frame_info) {
+			pipe = find_pipe_by_num(sh_css_sp_group.pipe[thread_id].pipe_num);
+			if (pipe == NULL)
+				return IA_CSS_ERR_INTERNAL_ERROR;
+			ia_css_get_crop_offsets(pipe, &binary->in_frame_info);
+		}
+#endif
+	}
+#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 != IA_CSS_SUCCESS)
+		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 != IA_CSS_SUCCESS)
+		return err;
+
+	return IA_CSS_SUCCESS;
+}
+
+static enum ia_css_err
+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 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;
+
+	enum ia_css_err err = IA_CSS_SUCCESS;
+
+	assert(stage != NULL);
+
+	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,
+			    IA_CSS_STREAM_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 IA_CSS_ERR_INTERNAL_ERROR;
+	}
+
+	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 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 != NULL);
+	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 */
+	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);
+	}
+}
+
+void
+sh_css_sp_init_pipeline(struct ia_css_pipeline *me,
+			enum ia_css_pipe_id id,
+			uint8_t 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,
+#if !defined(HAS_NO_INPUT_SYSTEM)
+			const mipi_port_ID_t port_id
+#endif
+#ifdef ISP2401
+			,
+			const struct ia_css_coordinate *internal_frame_origin_bqs_on_sctbl, /* Origin of internal frame
+							positioned on shading table at shading correction in ISP. */
+			const struct ia_css_isp_parameters *params
+#endif
+	)
+{
+	/* Get first stage */
+	struct ia_css_pipeline_stage *stage        = NULL;
+	struct ia_css_binary	     *first_binary = NULL;
+	struct ia_css_pipe *pipe = NULL;
+	unsigned num;
+
+	enum ia_css_pipe_id pipe_id = id;
+	unsigned int thread_id;
+	uint8_t if_config_index, tmp_if_config_index;
+
+	assert(me != NULL);
+
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	assert(me->stages != NULL);
+
+	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;
+	}
+#else
+	(void)input_mode;
+	if_config_index = SH_CSS_IF_CONFIG_NOT_NEEDED;
+#endif
+
+	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 != NULL) {
+		/* Init pipeline data */
+		sh_css_sp_init_group(two_ppc, first_binary->input_format,
+				     offline, if_config_index);
+	} /* if (first_binary != NULL) */
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2401) || defined(USE_INPUT_SYSTEM_VERSION_2)
+	/* 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;
+#endif
+
+	/* 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;
+#if !defined(HAS_NO_INPUT_SYSTEM)
+	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;
+#endif
+	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 != NULL);
+	if (pipe == NULL) {
+		return;
+	}
+	sh_css_sp_group.pipe[thread_id].scaler_pp_lut = sh_css_pipe_get_pp_gdc_lut(pipe);
+
+#if defined(SH_CSS_ENABLE_METADATA)
+	if (md_info != NULL && 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);
+	}
+#else
+	(void)md_config;
+	(void)md_info;
+#endif
+
+#if defined(SH_CSS_ENABLE_PER_FRAME_PARAMS)
+	sh_css_sp_group.pipe[thread_id].output_frame_queue_id = (uint32_t)SH_CSS_INVALID_QUEUE_ID;
+	if (IA_CSS_PIPE_ID_COPY != pipe_id) {
+		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));
+	}
+#endif
+
+#ifdef ISP2401
+	/* For the shading correction type 1 (the legacy shading table conversion in css is not used),
+	 * the parameters are passed to the isp for the shading table centering.
+	 */
+	if (internal_frame_origin_bqs_on_sctbl != NULL &&
+			params != NULL && params->shading_settings.enable_shading_table_conversion == 0) {
+		sh_css_sp_group.pipe[thread_id].shading.internal_frame_origin_x_bqs_on_sctbl
+								= (uint32_t)internal_frame_origin_bqs_on_sctbl->x;
+		sh_css_sp_group.pipe[thread_id].shading.internal_frame_origin_y_bqs_on_sctbl
+								= (uint32_t)internal_frame_origin_bqs_on_sctbl->y;
+	} else {
+		sh_css_sp_group.pipe[thread_id].shading.internal_frame_origin_x_bqs_on_sctbl = 0;
+		sh_css_sp_group.pipe[thread_id].shading.internal_frame_origin_y_bqs_on_sctbl = 0;
+	}
+
+#endif
+	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
+ * seperate 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
+	 * seperate 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 frame_num,
+				struct ia_css_frame *frame,
+				struct ia_css_metadata *metadata)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	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);
+}
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+/**
+ * @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 frame_num,
+				struct ia_css_frame *frame)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	/* 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 frame_num,
+				struct ia_css_metadata *metadata)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int o;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	/* 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 num_frames)
+{
+	unsigned int HIVE_ADDR_host_sp_com;
+	unsigned int offset;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	/* 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);
+}
+#endif
+
+void
+sh_css_update_host2sp_cont_num_raw_frames(unsigned 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;
+
+	(void)HIVE_ADDR_host_sp_com; /* Suppres warnings in CRUN */
+
+	/* 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));
+	}
+
+}
+
+enum ia_css_err
+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 != NULL);
+
+	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
+	 */
+#ifndef __KERNEL__
+	assert(or_mask <= UINT16_MAX);
+	assert(and_mask <= UINT16_MAX);
+#endif
+
+	(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 IA_CSS_ERR_INTERNAL_ERROR;
+	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 IA_CSS_SUCCESS;
+}
+
+enum ia_css_err
+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 != NULL);
+	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 IA_CSS_ERR_INTERNAL_ERROR;
+	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 IA_CSS_SUCCESS;
+}
+
+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)
+{
+	uint32_t sw_reg;
+	uint32_t bit_val;
+	uint32_t bit_offset;
+	uint32_t 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/atomisp2/css2400/sh_css_sp.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_sp.h
new file mode 100644
index 000000000000..98444a3cc3e4
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_sp.h
@@ -0,0 +1,248 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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(HAS_NO_INPUT_FORMATTER)
+#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 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 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,
+			uint8_t 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,
+#if !defined(HAS_NO_INPUT_SYSTEM)
+			const mipi_port_ID_t port_id
+#endif
+#ifdef ISP2401
+			,
+			const struct ia_css_coordinate *internal_frame_origin_bqs_on_sctbl, /* Origin of internal frame
+							positioned on shading table at shading correction in ISP. */
+			const struct ia_css_isp_parameters *params
+#endif
+			);
+
+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 frame_num,
+				struct ia_css_frame *frame,
+				struct ia_css_metadata *metadata);
+
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+/**
+ * @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 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 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 num_frames);
+#endif
+
+/**
+ * @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 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(HAS_NO_INPUT_FORMATTER)
+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/atomisp2/css2400/sh_css_stream.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream.c
new file mode 100644
index 000000000000..60bddbb3d4c6
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream.c
@@ -0,0 +1,16 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 will contain the code to implement the functions declared in ia_css_stream.h
+   and associated helper functions */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.c
new file mode 100644
index 000000000000..52d0a6471597
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.c
@@ -0,0 +1,76 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 ia_css_stream_format format)
+{
+	unsigned int rval;
+
+	switch (format) {
+	case IA_CSS_STREAM_FORMAT_RGB_444:
+		rval = 4;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_555:
+		rval = 5;
+		break;
+	case IA_CSS_STREAM_FORMAT_RGB_565:
+	case IA_CSS_STREAM_FORMAT_RGB_666:
+	case IA_CSS_STREAM_FORMAT_RAW_6:
+		rval = 6;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_7:
+		rval = 7;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_8_LEGACY:
+	case IA_CSS_STREAM_FORMAT_YUV420_8:
+	case IA_CSS_STREAM_FORMAT_YUV422_8:
+	case IA_CSS_STREAM_FORMAT_RGB_888:
+	case IA_CSS_STREAM_FORMAT_RAW_8:
+	case IA_CSS_STREAM_FORMAT_BINARY_8:
+	case IA_CSS_STREAM_FORMAT_USER_DEF1:
+	case IA_CSS_STREAM_FORMAT_USER_DEF2:
+	case IA_CSS_STREAM_FORMAT_USER_DEF3:
+	case IA_CSS_STREAM_FORMAT_USER_DEF4:
+	case IA_CSS_STREAM_FORMAT_USER_DEF5:
+	case IA_CSS_STREAM_FORMAT_USER_DEF6:
+	case IA_CSS_STREAM_FORMAT_USER_DEF7:
+	case IA_CSS_STREAM_FORMAT_USER_DEF8:
+		rval = 8;
+		break;
+	case IA_CSS_STREAM_FORMAT_YUV420_10:
+	case IA_CSS_STREAM_FORMAT_YUV422_10:
+	case IA_CSS_STREAM_FORMAT_RAW_10:
+		rval = 10;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_12:
+		rval = 12;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_14:
+		rval = 14;
+		break;
+	case IA_CSS_STREAM_FORMAT_RAW_16:
+	case IA_CSS_STREAM_FORMAT_YUV420_16:
+	case IA_CSS_STREAM_FORMAT_YUV422_16:
+		rval = 16;
+		break;
+	default:
+		rval = 0;
+		break;
+	}
+
+	return rval;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.h
new file mode 100644
index 000000000000..aab2b6207051
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_stream_format.h
@@ -0,0 +1,23 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 ia_css_stream_format format);
+
+#endif /* __SH_CSS_STREAM_FORMAT_H */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_struct.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_struct.h
new file mode 100644
index 000000000000..e49e478ab354
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_struct.h
@@ -0,0 +1,80 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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_types.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);
+#ifdef 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);
+#endif
+	void (*flush)(struct ia_css_acc_fw *fw);
+	bool                           check_system_idle;
+#ifndef ISP2401
+	bool stop_copy_preview;
+#endif
+	unsigned int                   num_cont_raw_frames;
+#if defined(USE_INPUT_SYSTEM_VERSION_2) || defined(USE_INPUT_SYSTEM_VERSION_2401)
+	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];
+#endif
+	hrt_vaddress                   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/atomisp2/css2400/sh_css_uds.h b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_uds.h
new file mode 100644
index 000000000000..5ded3a1437bf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_uds.h
@@ -0,0 +1,37 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY 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 {
+	uint16_t curr_dx;
+	uint16_t curr_dy;
+	uint16_t xc;
+	uint16_t yc;
+};
+
+struct sh_css_crop_pos {
+	uint16_t x;
+	uint16_t y;
+};
+
+#endif /* _SH_CSS_UDS_H_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_version.c b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_version.c
new file mode 100644
index 000000000000..6e0c5e7f8620
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/css2400/sh_css_version.c
@@ -0,0 +1,30 @@
+/*
+ * Support for Intel Camera Imaging ISP subsystem.
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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_version.h"
+#include "ia_css_version_data.h"
+#include "ia_css_err.h"
+#include "sh_css_firmware.h"
+
+enum ia_css_err
+ia_css_get_version(char *version, int max_size)
+{
+	if (max_size <= (int)strlen(CSS_VERSION_STRING) + (int)strlen(sh_css_get_fw_version()) + 5)
+		return IA_CSS_ERR_INVALID_ARGUMENTS;
+	strcpy(version, CSS_VERSION_STRING);
+	strcat(version, "FW:");
+	strcat(version, sh_css_get_fw_version());
+	strcat(version, "; ");
+	return IA_CSS_SUCCESS;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c
new file mode 100644
index 000000000000..57295397da3e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm.c
@@ -0,0 +1,728 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * 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_pool.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;
+struct hmm_pool	dynamic_pool;
+struct hmm_pool	reserved_pool;
+static ia_css_ptr dummy_ptr;
+struct _hmm_mem_stat hmm_mem_stat;
+
+/* 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 ssize_t reserved_pool_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	ssize_t ret = 0;
+
+	struct hmm_reserved_pool_info *pinfo = reserved_pool.pool_info;
+	unsigned long flags;
+
+	if (!pinfo || !pinfo->initialized)
+		return 0;
+
+	spin_lock_irqsave(&pinfo->list_lock, flags);
+	ret = scnprintf(buf, PAGE_SIZE, "%d out of %d pages available\n",
+					pinfo->index, pinfo->pgnr);
+	spin_unlock_irqrestore(&pinfo->list_lock, flags);
+
+	if (ret > 0)
+		ret++; /* Add trailing zero, not included by scnprintf */
+
+	return ret;
+};
+
+static ssize_t dynamic_pool_show(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	ssize_t ret = 0;
+
+	struct hmm_dynamic_pool_info *pinfo = dynamic_pool.pool_info;
+	unsigned long flags;
+
+	if (!pinfo || !pinfo->initialized)
+		return 0;
+
+	spin_lock_irqsave(&pinfo->list_lock, flags);
+	ret = scnprintf(buf, PAGE_SIZE, "%d (max %d) pages available\n",
+					pinfo->pgnr, pinfo->pool_size);
+	spin_unlock_irqrestore(&pinfo->list_lock, flags);
+
+	if (ret > 0)
+		ret++; /* Add trailing zero, not included by scnprintf */
+
+	return ret;
+};
+
+static DEVICE_ATTR(active_bo, 0444, active_bo_show, NULL);
+static DEVICE_ATTR(free_bo, 0444, free_bo_show, NULL);
+static DEVICE_ATTR(reserved_pool, 0444, reserved_pool_show, NULL);
+static DEVICE_ATTR(dynamic_pool, 0444, dynamic_pool_show, NULL);
+
+static struct attribute *sysfs_attrs_ctrl[] = {
+	&dev_attr_active_bo.attr,
+	&dev_attr_free_bo.attr,
+	&dev_attr_reserved_pool.attr,
+	&dev_attr_dynamic_pool.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");
+
+	/*
+	 * 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, HMM_BO_PRIVATE, 0, 0, HMM_UNCACHED);
+
+	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)
+{
+	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);
+}
+
+ia_css_ptr hmm_alloc(size_t bytes, enum hmm_bo_type type,
+		int from_highmem, void *userptr, bool cached)
+{
+	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 (!dummy_ptr)
+		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, from_highmem, userptr, cached);
+	if (ret) {
+		dev_err(atomisp_dev,
+			    "hmm_bo_alloc_pages failed.\n");
+		goto alloc_page_err;
+	}
+
+	/*Combind the virtual address and pages togather*/
+	ret = hmm_bo_bind(bo);
+	if (ret) {
+		dev_err(atomisp_dev, "hmm_bo_bind failed.\n");
+		goto bind_err;
+	}
+
+	hmm_mem_stat.tol_cnt += pgnr;
+
+	return bo->start;
+
+bind_err:
+	hmm_bo_free_pages(bo);
+alloc_page_err:
+	hmm_bo_unref(bo);
+create_bo_err:
+	return 0;
+}
+
+void hmm_free(ia_css_ptr virt)
+{
+	struct hmm_buffer_object *bo;
+
+	WARN_ON(!virt);
+
+	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_mem_stat.tol_cnt -= bo->pgnr;
+
+	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(bo->page_obj[idx].page) + 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(bo->page_obj[idx].page);
+	}
+
+	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 (!data) {
+		dev_err(atomisp_dev,
+			 "hmm_load 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;
+
+	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);
+
+		if (in_atomic())
+			des = (char *)kmap_atomic(bo->page_obj[idx].page);
+		else
+			des = (char *)kmap(bo->page_obj[idx].page);
+
+		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);
+
+		if (in_atomic())
+			/*
+			 * Note: kunmap_atomic requires return addr from
+			 * kmap_atomic, not the page. See linux/highmem.h
+			 */
+			kunmap_atomic(des - offset);
+		else
+			kunmap(bo->page_obj[idx].page);
+	}
+
+	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(bo->page_obj[idx].page) + 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(bo->page_obj[idx].page);
+	}
+
+	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->page_obj[idx].page) + 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;
+	}
+
+	return hmm_bo_vunmap(bo);
+}
+
+int hmm_pool_register(unsigned int pool_size,
+			enum hmm_pool_type pool_type)
+{
+	switch (pool_type) {
+	case HMM_POOL_TYPE_RESERVED:
+		reserved_pool.pops = &reserved_pops;
+		return reserved_pool.pops->pool_init(&reserved_pool.pool_info,
+							pool_size);
+	case HMM_POOL_TYPE_DYNAMIC:
+		dynamic_pool.pops = &dynamic_pops;
+		return dynamic_pool.pops->pool_init(&dynamic_pool.pool_info,
+							pool_size);
+	default:
+		dev_err(atomisp_dev, "invalid pool type.\n");
+		return -EINVAL;
+	}
+}
+
+void hmm_pool_unregister(enum hmm_pool_type pool_type)
+{
+	switch (pool_type) {
+	case HMM_POOL_TYPE_RESERVED:
+		if (reserved_pool.pops && reserved_pool.pops->pool_exit)
+			reserved_pool.pops->pool_exit(&reserved_pool.pool_info);
+		break;
+	case HMM_POOL_TYPE_DYNAMIC:
+		if (dynamic_pool.pops && dynamic_pool.pops->pool_exit)
+			dynamic_pool.pops->pool_exit(&dynamic_pool.pool_info);
+		break;
+	default:
+		dev_err(atomisp_dev, "invalid pool type.\n");
+		break;
+	}
+
+	return;
+}
+
+void *hmm_isp_vaddr_to_host_vaddr(ia_css_ptr ptr, bool cached)
+{
+	return hmm_vmap(ptr, cached);
+	/* vmunmap will be done in hmm_bo_release() */
+}
+
+ia_css_ptr hmm_host_vaddr_to_hrt_vaddr(const void *ptr)
+{
+	struct hmm_buffer_object *bo;
+
+	bo = hmm_bo_device_search_vmap_start(&bo_device, ptr);
+	if (bo)
+		return bo->start;
+
+	dev_err(atomisp_dev,
+		"can not find buffer object whose kernel virtual address is %p\n",
+		ptr);
+	return 0;
+}
+
+void hmm_show_mem_stat(const char *func, const int line)
+{
+	trace_printk("tol_cnt=%d usr_size=%d res_size=%d res_cnt=%d sys_size=%d  dyc_thr=%d dyc_size=%d.\n",
+			hmm_mem_stat.tol_cnt,
+			hmm_mem_stat.usr_size, hmm_mem_stat.res_size,
+			hmm_mem_stat.res_cnt, hmm_mem_stat.sys_size,
+			hmm_mem_stat.dyc_thr, hmm_mem_stat.dyc_size);
+}
+
+void hmm_init_mem_stat(int res_pgnr, int dyc_en, int dyc_pgnr)
+{
+	hmm_mem_stat.res_size = res_pgnr;
+	/* If reserved mem pool is not enabled, set its "mem stat" values as -1. */
+	if (0 == hmm_mem_stat.res_size) {
+		hmm_mem_stat.res_size = -1;
+		hmm_mem_stat.res_cnt = -1;
+	}
+
+	/* If dynamic memory pool is not enabled, set its "mem stat" values as -1. */
+	if (!dyc_en) {
+		hmm_mem_stat.dyc_size = -1;
+		hmm_mem_stat.dyc_thr = -1;
+	} else {
+		hmm_mem_stat.dyc_size = 0;
+		hmm_mem_stat.dyc_thr = dyc_pgnr;
+	}
+	hmm_mem_stat.usr_size = 0;
+	hmm_mem_stat.sys_size = 0;
+	hmm_mem_stat.tol_cnt = 0;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_bo.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_bo.c
new file mode 100644
index 000000000000..40ac3582fb7a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_bo.c
@@ -0,0 +1,1543 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * 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 <asm/cacheflush.h>
+#include <linux/io.h>
+#include <asm/current.h>
+#include <linux/sched/signal.h>
+#include <linux/file.h>
+
+#include "atomisp_internal.h"
+#include "hmm/hmm_common.h"
+#include "hmm/hmm_pool.h"
+#include "hmm/hmm_bo.h"
+
+static unsigned int order_to_nr(unsigned int order)
+{
+	return 1U << order;
+}
+
+static unsigned int nr_to_order_bottom(unsigned int nr)
+{
+	return fls(nr) - 1;
+}
+
+struct hmm_buffer_object *__bo_alloc(struct kmem_cache *bo_cache)
+{
+	struct hmm_buffer_object *bo;
+
+	bo = kmem_cache_alloc(bo_cache, GFP_KERNEL);
+	if (!bo)
+		dev_err(atomisp_dev, "%s: failed!\n", __func__);
+
+	return bo;
+}
+
+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;
+}
+
+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 == NULL)) {
+		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 == NULL) {
+		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;
+}
+
+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 == NULL)
+				return NULL;
+			n = n->rb_left;
+		} else if (bo->start < start) {
+			if (n->rb_right == NULL)
+				return NULL;
+			n = n->rb_right;
+		} else {
+			return bo;
+		}
+	} while (n);
+
+	return NULL;
+}
+
+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 == NULL)
+				return NULL;
+			n = n->rb_left;
+		} else {
+			if (bo->end > start)
+				return bo;
+			if (n->rb_right == NULL)
+				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);
+}
+
+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 = __bo_alloc(bdev->bo_cache);
+	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 == NULL && bo->next == NULL) {
+		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 == NULL && bo->next != NULL) {
+		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 != NULL && bo->next == NULL) {
+		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 {
+		bo->next->prev = bo->prev;
+		bo->prev->next = bo->next;
+		bo->next = NULL;
+		bo->prev = NULL;
+	}
+}
+
+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 = __bo_alloc(bdev->bo_cache);
+	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_private_bo_pages(struct hmm_buffer_object *bo,
+				struct hmm_pool *dypool,
+				struct hmm_pool *repool,
+				int free_pgnr)
+{
+	int i, ret;
+
+	for (i = 0; i < free_pgnr; i++) {
+		switch (bo->page_obj[i].type) {
+		case HMM_PAGE_TYPE_RESERVED:
+			if (repool->pops
+			    && repool->pops->pool_free_pages) {
+				repool->pops->pool_free_pages(repool->pool_info,
+							&bo->page_obj[i]);
+				hmm_mem_stat.res_cnt--;
+			}
+			break;
+		/*
+		 * HMM_PAGE_TYPE_GENERAL indicates that pages are from system
+		 * memory, so when free them, they should be put into dynamic
+		 * pool.
+		 */
+		case HMM_PAGE_TYPE_DYNAMIC:
+		case HMM_PAGE_TYPE_GENERAL:
+			if (dypool->pops
+			    && dypool->pops->pool_inited
+			    && dypool->pops->pool_inited(dypool->pool_info)) {
+				if (dypool->pops->pool_free_pages)
+					dypool->pops->pool_free_pages(
+							      dypool->pool_info,
+							      &bo->page_obj[i]);
+				break;
+			}
+
+			/*
+			 * if dynamic memory pool doesn't exist, need to free
+			 * pages to system directly.
+			 */
+		default:
+			ret = set_pages_wb(bo->page_obj[i].page, 1);
+			if (ret)
+				dev_err(atomisp_dev,
+						"set page to WB err ...ret = %d\n",
+							ret);
+			/*
+			W/A: set_pages_wb seldom return value = -EFAULT
+			indicate that address of page is not in valid
+			range(0xffff880000000000~0xffffc7ffffffffff)
+			then, _free_pages would panic; Do not know why page
+			address be valid,it maybe memory corruption by lowmemory
+			*/
+			if (!ret) {
+				__free_pages(bo->page_obj[i].page, 0);
+				hmm_mem_stat.sys_size--;
+			}
+			break;
+		}
+	}
+
+	return;
+}
+
+/*Allocate pages which will be used only by ISP*/
+static int alloc_private_pages(struct hmm_buffer_object *bo,
+				int from_highmem,
+				bool cached,
+				struct hmm_pool *dypool,
+				struct hmm_pool *repool)
+{
+	int ret;
+	unsigned int pgnr, order, blk_pgnr, alloc_pgnr;
+	struct page *pages;
+	gfp_t gfp = GFP_NOWAIT | __GFP_NOWARN; /* REVISIT: need __GFP_FS too? */
+	int i, j;
+	int failure_number = 0;
+	bool reduce_order = false;
+	bool lack_mem = true;
+
+	if (from_highmem)
+		gfp |= __GFP_HIGHMEM;
+
+	pgnr = bo->pgnr;
+
+	bo->page_obj = kmalloc(sizeof(struct hmm_page_object) * pgnr,
+				GFP_KERNEL);
+	if (unlikely(!bo->page_obj)) {
+		dev_err(atomisp_dev, "out of memory for bo->page_obj\n");
+		return -ENOMEM;
+	}
+
+	i = 0;
+	alloc_pgnr = 0;
+
+	/*
+	 * get physical pages from dynamic pages pool.
+	 */
+	if (dypool->pops && dypool->pops->pool_alloc_pages) {
+		alloc_pgnr = dypool->pops->pool_alloc_pages(dypool->pool_info,
+							bo->page_obj, pgnr,
+							cached);
+		hmm_mem_stat.dyc_size -= alloc_pgnr;
+
+		if (alloc_pgnr == pgnr)
+			return 0;
+	}
+
+	pgnr -= alloc_pgnr;
+	i += alloc_pgnr;
+
+	/*
+	 * get physical pages from reserved pages pool for atomisp.
+	 */
+	if (repool->pops && repool->pops->pool_alloc_pages) {
+		alloc_pgnr = repool->pops->pool_alloc_pages(repool->pool_info,
+							&bo->page_obj[i], pgnr,
+							cached);
+		hmm_mem_stat.res_cnt += alloc_pgnr;
+		if (alloc_pgnr == pgnr)
+			return 0;
+	}
+
+	pgnr -= alloc_pgnr;
+	i += alloc_pgnr;
+
+	while (pgnr) {
+		order = nr_to_order_bottom(pgnr);
+		/*
+		 * if be short of memory, we will set order to 0
+		 * everytime.
+		 */
+		if (lack_mem)
+			order = HMM_MIN_ORDER;
+		else if (order > HMM_MAX_ORDER)
+			order = HMM_MAX_ORDER;
+retry:
+		/*
+		 * When order > HMM_MIN_ORDER, for performance reasons we don't
+		 * want alloc_pages() to sleep. In case it fails and fallbacks
+		 * to HMM_MIN_ORDER or in case the requested order is originally
+		 * the minimum value, we can allow alloc_pages() to sleep for
+		 * robustness purpose.
+		 *
+		 * REVISIT: why __GFP_FS is necessary?
+		 */
+		if (order == HMM_MIN_ORDER) {
+			gfp &= ~GFP_NOWAIT;
+			gfp |= __GFP_RECLAIM | __GFP_FS;
+		}
+
+		pages = alloc_pages(gfp, order);
+		if (unlikely(!pages)) {
+			/*
+			 * in low memory case, if allocation page fails,
+			 * we turn to try if order=0 allocation could
+			 * succeed. if order=0 fails too, that means there is
+			 * no memory left.
+			 */
+			if (order == HMM_MIN_ORDER) {
+				dev_err(atomisp_dev,
+					"%s: cannot allocate pages\n",
+					 __func__);
+				goto cleanup;
+			}
+			order = HMM_MIN_ORDER;
+			failure_number++;
+			reduce_order = true;
+			/*
+			 * if fail two times continuously, we think be short
+			 * of memory now.
+			 */
+			if (failure_number == 2) {
+				lack_mem = true;
+				failure_number = 0;
+			}
+			goto retry;
+		} else {
+			blk_pgnr = order_to_nr(order);
+
+			if (!cached) {
+				/*
+				 * set memory to uncacheable -- UC_MINUS
+				 */
+				ret = set_pages_uc(pages, blk_pgnr);
+				if (ret) {
+					dev_err(atomisp_dev,
+						     "set page uncacheable"
+							"failed.\n");
+
+					__free_pages(pages, order);
+
+					goto cleanup;
+				}
+			}
+
+			for (j = 0; j < blk_pgnr; j++) {
+				bo->page_obj[i].page = pages + j;
+				bo->page_obj[i++].type = HMM_PAGE_TYPE_GENERAL;
+			}
+
+			pgnr -= blk_pgnr;
+			hmm_mem_stat.sys_size += blk_pgnr;
+
+			/*
+			 * if order is not reduced this time, clear
+			 * failure_number.
+			 */
+			if (reduce_order)
+				reduce_order = false;
+			else
+				failure_number = 0;
+		}
+	}
+
+	return 0;
+cleanup:
+	alloc_pgnr = i;
+	free_private_bo_pages(bo, dypool, repool, alloc_pgnr);
+
+	kfree(bo->page_obj);
+
+	return -ENOMEM;
+}
+
+static void free_private_pages(struct hmm_buffer_object *bo,
+				struct hmm_pool *dypool,
+				struct hmm_pool *repool)
+{
+	free_private_bo_pages(bo, dypool, repool, bo->pgnr);
+
+	kfree(bo->page_obj);
+}
+
+/*
+ * Hacked from kernel function __get_user_pages in mm/memory.c
+ *
+ * Handle buffers allocated by other kernel space driver and mmaped into user
+ * space, function Ignore the VM_PFNMAP and VM_IO flag in VMA structure
+ *
+ * Get physical pages from user space virtual address and update into page list
+ */
+static int __get_pfnmap_pages(struct task_struct *tsk, struct mm_struct *mm,
+			      unsigned long start, int nr_pages,
+			      unsigned int gup_flags, struct page **pages,
+			      struct vm_area_struct **vmas)
+{
+	int i, ret;
+	unsigned long vm_flags;
+
+	if (nr_pages <= 0)
+		return 0;
+
+	VM_BUG_ON(!!pages != !!(gup_flags & FOLL_GET));
+
+	/*
+	 * Require read or write permissions.
+	 * If FOLL_FORCE is set, we only require the "MAY" flags.
+	 */
+	vm_flags  = (gup_flags & FOLL_WRITE) ?
+			(VM_WRITE | VM_MAYWRITE) : (VM_READ | VM_MAYREAD);
+	vm_flags &= (gup_flags & FOLL_FORCE) ?
+			(VM_MAYREAD | VM_MAYWRITE) : (VM_READ | VM_WRITE);
+	i = 0;
+
+	do {
+		struct vm_area_struct *vma;
+
+		vma = find_vma(mm, start);
+		if (!vma) {
+			dev_err(atomisp_dev, "find_vma failed\n");
+			return i ? : -EFAULT;
+		}
+
+		if (is_vm_hugetlb_page(vma)) {
+			/*
+			i = follow_hugetlb_page(mm, vma, pages, vmas,
+					&start, &nr_pages, i, gup_flags);
+			*/
+			continue;
+		}
+
+		do {
+			struct page *page;
+			unsigned long pfn;
+
+			/*
+			 * If we have a pending SIGKILL, don't keep faulting
+			 * pages and potentially allocating memory.
+			 */
+			if (unlikely(fatal_signal_pending(current))) {
+				dev_err(atomisp_dev,
+					"fatal_signal_pending in %s\n",
+					__func__);
+				return i ? i : -ERESTARTSYS;
+			}
+
+			ret = follow_pfn(vma, start, &pfn);
+			if (ret) {
+				dev_err(atomisp_dev, "follow_pfn() failed\n");
+				return i ? : -EFAULT;
+			}
+
+			page = pfn_to_page(pfn);
+			if (IS_ERR(page))
+				return i ? i : PTR_ERR(page);
+			if (pages) {
+				pages[i] = page;
+				get_page(page);
+				flush_anon_page(vma, page, start);
+				flush_dcache_page(page);
+			}
+			if (vmas)
+				vmas[i] = vma;
+			i++;
+			start += PAGE_SIZE;
+			nr_pages--;
+		} while (nr_pages && start < vma->vm_end);
+	} while (nr_pages);
+
+	return i;
+}
+
+static int get_pfnmap_pages(struct task_struct *tsk, struct mm_struct *mm,
+		     unsigned long start, int nr_pages, int write, int force,
+		     struct page **pages, struct vm_area_struct **vmas)
+{
+	int flags = FOLL_TOUCH;
+
+	if (pages)
+		flags |= FOLL_GET;
+	if (write)
+		flags |= FOLL_WRITE;
+	if (force)
+		flags |= FOLL_FORCE;
+
+	return __get_pfnmap_pages(tsk, mm, start, nr_pages, flags, pages, vmas);
+}
+
+/*
+ * Convert user space virtual address into pages list
+ */
+static int alloc_user_pages(struct hmm_buffer_object *bo,
+			      void *userptr, bool cached)
+{
+	int page_nr;
+	int i;
+	struct vm_area_struct *vma;
+	struct page **pages;
+
+	pages = kmalloc(sizeof(struct page *) * bo->pgnr, GFP_KERNEL);
+	if (unlikely(!pages)) {
+		dev_err(atomisp_dev, "out of memory for pages...\n");
+		return -ENOMEM;
+	}
+
+	bo->page_obj = kmalloc(sizeof(struct hmm_page_object) * bo->pgnr,
+		GFP_KERNEL);
+	if (unlikely(!bo->page_obj)) {
+		dev_err(atomisp_dev, "out of memory for bo->page_obj...\n");
+		kfree(pages);
+		return -ENOMEM;
+	}
+
+	mutex_unlock(&bo->mutex);
+	down_read(&current->mm->mmap_sem);
+	vma = find_vma(current->mm, (unsigned long)userptr);
+	up_read(&current->mm->mmap_sem);
+	if (vma == NULL) {
+		dev_err(atomisp_dev, "find_vma failed\n");
+		kfree(bo->page_obj);
+		kfree(pages);
+		mutex_lock(&bo->mutex);
+		return -EFAULT;
+	}
+	mutex_lock(&bo->mutex);
+	/*
+	 * Handle frame buffer allocated in other kerenl space driver
+	 * and map to user space
+	 */
+	if (vma->vm_flags & (VM_IO | VM_PFNMAP)) {
+		page_nr = get_pfnmap_pages(current, current->mm,
+					   (unsigned long)userptr,
+					   (int)(bo->pgnr), 1, 0,
+					   pages, NULL);
+		bo->mem_type = HMM_BO_MEM_TYPE_PFN;
+	} else {
+		/*Handle frame buffer allocated in user space*/
+		mutex_unlock(&bo->mutex);
+		down_read(&current->mm->mmap_sem);
+		page_nr = get_user_pages((unsigned long)userptr,
+					 (int)(bo->pgnr), 1, pages, NULL);
+		up_read(&current->mm->mmap_sem);
+		mutex_lock(&bo->mutex);
+		bo->mem_type = HMM_BO_MEM_TYPE_USER;
+	}
+
+	/* 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);
+		goto out_of_mem;
+	}
+
+	for (i = 0; i < bo->pgnr; i++) {
+		bo->page_obj[i].page = pages[i];
+		bo->page_obj[i].type = HMM_PAGE_TYPE_GENERAL;
+	}
+	hmm_mem_stat.usr_size += bo->pgnr;
+	kfree(pages);
+
+	return 0;
+
+out_of_mem:
+	for (i = 0; i < page_nr; i++)
+		put_page(pages[i]);
+	kfree(pages);
+	kfree(bo->page_obj);
+
+	return -ENOMEM;
+}
+
+static void free_user_pages(struct hmm_buffer_object *bo)
+{
+	int i;
+
+	for (i = 0; i < bo->pgnr; i++)
+		put_page(bo->page_obj[i].page);
+	hmm_mem_stat.usr_size -= bo->pgnr;
+
+	kfree(bo->page_obj);
+}
+
+/*
+ * 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, HMM_BO_SHARE.
+ *
+ * from_highmem is only valid when type is HMM_BO_PRIVATE, it will
+ * try to alloc memory from highmem if from_highmem is set.
+ *
+ * userptr is only valid when type is HMM_BO_USER, it indicates
+ * the start address from user space task.
+ *
+ * from_highmem and userptr will both be ignored when type is
+ * HMM_BO_SHARE.
+ */
+int hmm_bo_alloc_pages(struct hmm_buffer_object *bo,
+		       enum hmm_bo_type type, int from_highmem,
+		       void *userptr, bool cached)
+{
+	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);
+
+	/*
+	 * TO DO:
+	 * add HMM_BO_USER type
+	 */
+	if (type == HMM_BO_PRIVATE) {
+		ret = alloc_private_pages(bo, from_highmem,
+				cached, &dynamic_pool, &reserved_pool);
+	} else if (type == HMM_BO_USER) {
+		ret = alloc_user_pages(bo, userptr, cached);
+	} 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:
+	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_pages(bo, &dynamic_pool, &reserved_pool);
+	else if (bo->type == HMM_BO_USER)
+		free_user_pages(bo);
+	else
+		dev_err(atomisp_dev, "invalid buffer type.\n");
+	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)
+{
+	int ret;
+
+	check_bo_null_return(bo, 0);
+
+	ret = bo->status & HMM_BO_PAGE_ALLOCED;
+
+	return ret;
+}
+
+/*
+ * get physical page info of the bo.
+ */
+int hmm_bo_get_page_info(struct hmm_buffer_object *bo,
+			 struct hmm_page_object **page_obj, int *pgnr)
+{
+	check_bo_null_return(bo, -EINVAL);
+
+	mutex_lock(&bo->mutex);
+
+	check_bo_status_yes_goto(bo, HMM_BO_PAGE_ALLOCED, status_err);
+
+	*page_obj = bo->page_obj;
+	*pgnr = bo->pgnr;
+
+	mutex_unlock(&bo->mutex);
+
+	return 0;
+
+status_err:
+	dev_err(atomisp_dev,
+			"buffer object not page allocated yet.\n");
+	mutex_unlock(&bo->mutex);
+	return -EINVAL;
+}
+
+/*
+ * 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->page_obj[i].page), 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)
+{
+	struct page **pages;
+	int i;
+
+	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);
+	}
+
+	pages = kmalloc(sizeof(*pages) * bo->pgnr, GFP_KERNEL);
+	if (unlikely(!pages)) {
+		mutex_unlock(&bo->mutex);
+		dev_err(atomisp_dev, "out of memory for pages...\n");
+		return NULL;
+	}
+
+	for (i = 0; i < bo->pgnr; i++)
+		pages[i] = bo->page_obj[i].page;
+
+	bo->vmap_addr = vmap(pages, bo->pgnr, VM_MAP,
+		cached ? PAGE_KERNEL : PAGE_KERNEL_NOCACHE);
+	if (unlikely(!bo->vmap_addr)) {
+		kfree(pages);
+		mutex_unlock(&bo->mutex);
+		dev_err(atomisp_dev, "vmap failed...\n");
+		return NULL;
+	}
+	bo->status |= (cached ? HMM_BO_VMAPED_CACHED : HMM_BO_VMAPED);
+
+	kfree(pages);
+
+	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->page_obj[i].page);
+		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 explictly.
+	 */
+	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/atomisp2/hmm/hmm_dynamic_pool.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_dynamic_pool.c
new file mode 100644
index 000000000000..639b8cdf7a5e
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_dynamic_pool.c
@@ -0,0 +1,241 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * This file contains functions for dynamic memory pool management
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+
+#include "asm/cacheflush.h"
+
+#include "atomisp_internal.h"
+
+#include "hmm/hmm_pool.h"
+
+/*
+ * dynamic memory pool ops.
+ */
+static unsigned int get_pages_from_dynamic_pool(void *pool,
+					struct hmm_page_object *page_obj,
+					unsigned int size, bool cached)
+{
+	struct hmm_page *hmm_page;
+	unsigned long flags;
+	unsigned int i = 0;
+	struct hmm_dynamic_pool_info *dypool_info = pool;
+
+	if (!dypool_info)
+		return 0;
+
+	spin_lock_irqsave(&dypool_info->list_lock, flags);
+	if (dypool_info->initialized) {
+		while (!list_empty(&dypool_info->pages_list)) {
+			hmm_page = list_entry(dypool_info->pages_list.next,
+						struct hmm_page, list);
+
+			list_del(&hmm_page->list);
+			dypool_info->pgnr--;
+			spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+
+			page_obj[i].page = hmm_page->page;
+			page_obj[i++].type = HMM_PAGE_TYPE_DYNAMIC;
+			kmem_cache_free(dypool_info->pgptr_cache, hmm_page);
+
+			if (i == size)
+				return i;
+
+			spin_lock_irqsave(&dypool_info->list_lock, flags);
+		}
+	}
+	spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+
+	return i;
+}
+
+static void free_pages_to_dynamic_pool(void *pool,
+					struct hmm_page_object *page_obj)
+{
+	struct hmm_page *hmm_page;
+	unsigned long flags;
+	int ret;
+	struct hmm_dynamic_pool_info *dypool_info = pool;
+
+	if (!dypool_info)
+		return;
+
+	spin_lock_irqsave(&dypool_info->list_lock, flags);
+	if (!dypool_info->initialized) {
+		spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+		return;
+	}
+	spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+
+	if (page_obj->type == HMM_PAGE_TYPE_RESERVED)
+		return;
+
+	if (dypool_info->pgnr >= dypool_info->pool_size) {
+		/* free page directly back to system */
+		ret = set_pages_wb(page_obj->page, 1);
+		if (ret)
+			dev_err(atomisp_dev,
+				"set page to WB err ...ret=%d\n", ret);
+		/*
+		W/A: set_pages_wb seldom return value = -EFAULT
+		indicate that address of page is not in valid
+		range(0xffff880000000000~0xffffc7ffffffffff)
+		then, _free_pages would panic; Do not know why page
+		address be valid, it maybe memory corruption by lowmemory
+		*/
+		if (!ret) {
+			__free_pages(page_obj->page, 0);
+			hmm_mem_stat.sys_size--;
+		}
+		return;
+	}
+	hmm_page = kmem_cache_zalloc(dypool_info->pgptr_cache,
+						GFP_KERNEL);
+	if (!hmm_page) {
+		dev_err(atomisp_dev, "out of memory for hmm_page.\n");
+
+		/* free page directly */
+		ret = set_pages_wb(page_obj->page, 1);
+		if (ret)
+			dev_err(atomisp_dev,
+				"set page to WB err ...ret=%d\n", ret);
+		if (!ret) {
+			__free_pages(page_obj->page, 0);
+			hmm_mem_stat.sys_size--;
+		}
+		return;
+	}
+
+	hmm_page->page = page_obj->page;
+
+	/*
+	 * add to pages_list of pages_pool
+	 */
+	spin_lock_irqsave(&dypool_info->list_lock, flags);
+	list_add_tail(&hmm_page->list, &dypool_info->pages_list);
+	dypool_info->pgnr++;
+	spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+	hmm_mem_stat.dyc_size++;
+}
+
+static int hmm_dynamic_pool_init(void **pool, unsigned int pool_size)
+{
+	struct hmm_dynamic_pool_info *dypool_info;
+
+	if (pool_size == 0)
+		return 0;
+
+	dypool_info = kmalloc(sizeof(struct hmm_dynamic_pool_info),
+		GFP_KERNEL);
+	if (unlikely(!dypool_info)) {
+		dev_err(atomisp_dev, "out of memory for repool_info.\n");
+		return -ENOMEM;
+	}
+
+	dypool_info->pgptr_cache = kmem_cache_create("pgptr_cache",
+						sizeof(struct hmm_page), 0,
+						SLAB_HWCACHE_ALIGN, NULL);
+	if (!dypool_info->pgptr_cache) {
+		kfree(dypool_info);
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&dypool_info->pages_list);
+	spin_lock_init(&dypool_info->list_lock);
+	dypool_info->initialized = true;
+	dypool_info->pool_size = pool_size;
+	dypool_info->pgnr = 0;
+
+	*pool = dypool_info;
+
+	return 0;
+}
+
+static void hmm_dynamic_pool_exit(void **pool)
+{
+	struct hmm_dynamic_pool_info *dypool_info = *pool;
+	struct hmm_page *hmm_page;
+	unsigned long flags;
+	int ret;
+
+	if (!dypool_info)
+		return;
+
+	spin_lock_irqsave(&dypool_info->list_lock, flags);
+	if (!dypool_info->initialized) {
+		spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+		return;
+	}
+	dypool_info->initialized = false;
+
+	while (!list_empty(&dypool_info->pages_list)) {
+		hmm_page = list_entry(dypool_info->pages_list.next,
+					struct hmm_page, list);
+
+		list_del(&hmm_page->list);
+		spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+
+		/* can cause thread sleep, so cannot be put into spin_lock */
+		ret = set_pages_wb(hmm_page->page, 1);
+		if (ret)
+			dev_err(atomisp_dev,
+				"set page to WB err...ret=%d\n", ret);
+		if (!ret) {
+			__free_pages(hmm_page->page, 0);
+			hmm_mem_stat.dyc_size--;
+			hmm_mem_stat.sys_size--;
+		}
+		kmem_cache_free(dypool_info->pgptr_cache, hmm_page);
+		spin_lock_irqsave(&dypool_info->list_lock, flags);
+	}
+
+	spin_unlock_irqrestore(&dypool_info->list_lock, flags);
+
+	kmem_cache_destroy(dypool_info->pgptr_cache);
+
+	kfree(dypool_info);
+
+	*pool = NULL;
+}
+
+static int hmm_dynamic_pool_inited(void *pool)
+{
+	struct hmm_dynamic_pool_info *dypool_info = pool;
+
+	if (!dypool_info)
+		return 0;
+
+	return dypool_info->initialized;
+}
+
+struct hmm_pool_ops dynamic_pops = {
+	.pool_init		= hmm_dynamic_pool_init,
+	.pool_exit		= hmm_dynamic_pool_exit,
+	.pool_alloc_pages	= get_pages_from_dynamic_pool,
+	.pool_free_pages	= free_pages_to_dynamic_pool,
+	.pool_inited		= hmm_dynamic_pool_inited,
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_reserved_pool.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_reserved_pool.c
new file mode 100644
index 000000000000..4000c05652e1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_reserved_pool.c
@@ -0,0 +1,258 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * This file contains functions for reserved memory pool management
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+
+#include "asm/cacheflush.h"
+#include "atomisp_internal.h"
+#include "hmm/hmm_pool.h"
+
+/*
+ * reserved memory pool ops.
+ */
+static unsigned int get_pages_from_reserved_pool(void *pool,
+					struct hmm_page_object *page_obj,
+					unsigned int size, bool cached)
+{
+	unsigned long flags;
+	unsigned int i = 0;
+	unsigned int repool_pgnr;
+	int j;
+	struct hmm_reserved_pool_info *repool_info = pool;
+
+	if (!repool_info)
+		return 0;
+
+	spin_lock_irqsave(&repool_info->list_lock, flags);
+	if (repool_info->initialized) {
+		repool_pgnr = repool_info->index;
+
+		for (j = repool_pgnr-1; j >= 0; j--) {
+			page_obj[i].page = repool_info->pages[j];
+			page_obj[i].type = HMM_PAGE_TYPE_RESERVED;
+			i++;
+			repool_info->index--;
+			if (i == size)
+				break;
+		}
+	}
+	spin_unlock_irqrestore(&repool_info->list_lock, flags);
+	return i;
+}
+
+static void free_pages_to_reserved_pool(void *pool,
+					struct hmm_page_object *page_obj)
+{
+	unsigned long flags;
+	struct hmm_reserved_pool_info *repool_info = pool;
+
+	if (!repool_info)
+		return;
+
+	spin_lock_irqsave(&repool_info->list_lock, flags);
+
+	if (repool_info->initialized &&
+	    repool_info->index < repool_info->pgnr &&
+	    page_obj->type == HMM_PAGE_TYPE_RESERVED) {
+		repool_info->pages[repool_info->index++] = page_obj->page;
+	}
+
+	spin_unlock_irqrestore(&repool_info->list_lock, flags);
+}
+
+static int hmm_reserved_pool_setup(struct hmm_reserved_pool_info **repool_info,
+					unsigned int pool_size)
+{
+	struct hmm_reserved_pool_info *pool_info;
+
+	pool_info = kmalloc(sizeof(struct hmm_reserved_pool_info),
+				GFP_KERNEL);
+	if (unlikely(!pool_info)) {
+		dev_err(atomisp_dev, "out of memory for repool_info.\n");
+		return -ENOMEM;
+	}
+
+	pool_info->pages = kmalloc(sizeof(struct page *) * pool_size,
+			GFP_KERNEL);
+	if (unlikely(!pool_info->pages)) {
+		dev_err(atomisp_dev, "out of memory for repool_info->pages.\n");
+		kfree(pool_info);
+		return -ENOMEM;
+	}
+
+	pool_info->index = 0;
+	pool_info->pgnr = 0;
+	spin_lock_init(&pool_info->list_lock);
+	pool_info->initialized = true;
+
+	*repool_info = pool_info;
+
+	return 0;
+}
+
+static int hmm_reserved_pool_init(void **pool, unsigned int pool_size)
+{
+	int ret;
+	unsigned int blk_pgnr;
+	unsigned int pgnr = pool_size;
+	unsigned int order = 0;
+	unsigned int i = 0;
+	int fail_number = 0;
+	struct page *pages;
+	int j;
+	struct hmm_reserved_pool_info *repool_info;
+	if (pool_size == 0)
+		return 0;
+
+	ret = hmm_reserved_pool_setup(&repool_info, pool_size);
+	if (ret) {
+		dev_err(atomisp_dev, "hmm_reserved_pool_setup failed.\n");
+		return ret;
+	}
+
+	pgnr = pool_size;
+
+	i = 0;
+	order = MAX_ORDER;
+
+	while (pgnr) {
+		blk_pgnr = 1U << order;
+		while (blk_pgnr > pgnr) {
+			order--;
+			blk_pgnr >>= 1U;
+		}
+		BUG_ON(order > MAX_ORDER);
+
+		pages = alloc_pages(GFP_KERNEL | __GFP_NOWARN, order);
+		if (unlikely(!pages)) {
+			if (order == 0) {
+				fail_number++;
+				dev_err(atomisp_dev, "%s: alloc_pages failed: %d\n",
+						__func__, fail_number);
+				/* if fail five times, will goto end */
+
+				/* FIXME: whether is the mechanism is ok? */
+				if (fail_number == ALLOC_PAGE_FAIL_NUM)
+					goto end;
+			} else {
+				order--;
+			}
+		} else {
+			blk_pgnr = 1U << order;
+
+			ret = set_pages_uc(pages, blk_pgnr);
+			if (ret) {
+				dev_err(atomisp_dev,
+						"set pages uncached failed\n");
+				__free_pages(pages, order);
+				goto end;
+			}
+
+			for (j = 0; j < blk_pgnr; j++)
+				repool_info->pages[i++] = pages + j;
+
+			repool_info->index += blk_pgnr;
+			repool_info->pgnr += blk_pgnr;
+
+			pgnr -= blk_pgnr;
+
+			fail_number = 0;
+		}
+	}
+
+end:
+	repool_info->initialized = true;
+
+	*pool = repool_info;
+
+	dev_info(atomisp_dev,
+			"hmm_reserved_pool init successfully,"
+			"hmm_reserved_pool is with %d pages.\n",
+			repool_info->pgnr);
+	return 0;
+}
+
+static void hmm_reserved_pool_exit(void **pool)
+{
+	unsigned long flags;
+	int i, ret;
+	unsigned int pgnr;
+	struct hmm_reserved_pool_info *repool_info = *pool;
+
+	if (!repool_info)
+		return;
+
+	spin_lock_irqsave(&repool_info->list_lock, flags);
+	if (!repool_info->initialized) {
+		spin_unlock_irqrestore(&repool_info->list_lock, flags);
+		return;
+	}
+	pgnr = repool_info->pgnr;
+	repool_info->index = 0;
+	repool_info->pgnr = 0;
+	repool_info->initialized = false;
+	spin_unlock_irqrestore(&repool_info->list_lock, flags);
+
+	for (i = 0; i < pgnr; i++) {
+		ret = set_pages_wb(repool_info->pages[i], 1);
+		if (ret)
+			dev_err(atomisp_dev,
+				"set page to WB err...ret=%d\n", ret);
+		/*
+		W/A: set_pages_wb seldom return value = -EFAULT
+		indicate that address of page is not in valid
+		range(0xffff880000000000~0xffffc7ffffffffff)
+		then, _free_pages would panic; Do not know why
+		page address be valid, it maybe memory corruption by lowmemory
+		*/
+		if (!ret)
+			__free_pages(repool_info->pages[i], 0);
+	}
+
+	kfree(repool_info->pages);
+	kfree(repool_info);
+
+	*pool = NULL;
+}
+
+static int hmm_reserved_pool_inited(void *pool)
+{
+	struct hmm_reserved_pool_info *repool_info = pool;
+
+	if (!repool_info)
+		return 0;
+
+	return repool_info->initialized;
+}
+
+struct hmm_pool_ops reserved_pops = {
+	.pool_init		= hmm_reserved_pool_init,
+	.pool_exit		= hmm_reserved_pool_exit,
+	.pool_alloc_pages	= get_pages_from_reserved_pool,
+	.pool_free_pages	= free_pages_to_reserved_pool,
+	.pool_inited		= hmm_reserved_pool_inited,
+};
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_vm.c b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_vm.c
new file mode 100644
index 000000000000..0722a68a49e7
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hmm/hmm_vm.c
@@ -0,0 +1,218 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * This file contains function for ISP virtual address management in ISP driver
+ */
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/page.h>
+
+#include "atomisp_internal.h"
+#include "mmu/isp_mmu.h"
+#include "hmm/hmm_vm.h"
+#include "hmm/hmm_common.h"
+
+static unsigned int vm_node_end(unsigned int start, unsigned int pgnr)
+{
+	return start + pgnr_to_size(pgnr);
+}
+
+static int addr_in_vm_node(unsigned int addr,
+		struct hmm_vm_node *node)
+{
+	return (addr >= node->start) && (addr < (node->start + node->size));
+}
+
+int hmm_vm_init(struct hmm_vm *vm, unsigned int start,
+		unsigned int size)
+{
+	if (!vm)
+		return -1;
+
+	vm->start = start;
+	vm->pgnr = size_to_pgnr_ceil(size);
+	vm->size = pgnr_to_size(vm->pgnr);
+
+	INIT_LIST_HEAD(&vm->vm_node_list);
+	spin_lock_init(&vm->lock);
+	vm->cache = kmem_cache_create("atomisp_vm", sizeof(struct hmm_vm_node),
+				      0, 0, NULL);
+
+	return vm->cache != NULL ? 0 : -ENOMEM;
+}
+
+void hmm_vm_clean(struct hmm_vm *vm)
+{
+	struct hmm_vm_node *node, *tmp;
+	struct list_head new_head;
+
+	if (!vm)
+		return;
+
+	spin_lock(&vm->lock);
+	list_replace_init(&vm->vm_node_list, &new_head);
+	spin_unlock(&vm->lock);
+
+	list_for_each_entry_safe(node, tmp, &new_head, list) {
+		list_del(&node->list);
+		kmem_cache_free(vm->cache, node);
+	}
+
+	kmem_cache_destroy(vm->cache);
+}
+
+static struct hmm_vm_node *alloc_hmm_vm_node(unsigned int pgnr,
+					     struct hmm_vm *vm)
+{
+	struct hmm_vm_node *node;
+
+	node = kmem_cache_alloc(vm->cache, GFP_KERNEL);
+	if (!node) {
+		dev_err(atomisp_dev, "out of memory.\n");
+		return NULL;
+	}
+
+	INIT_LIST_HEAD(&node->list);
+	node->pgnr = pgnr;
+	node->size = pgnr_to_size(pgnr);
+	node->vm = vm;
+
+	return node;
+}
+
+struct hmm_vm_node *hmm_vm_alloc_node(struct hmm_vm *vm, unsigned int pgnr)
+{
+	struct list_head *head;
+	struct hmm_vm_node *node, *cur, *next;
+	unsigned int vm_start, vm_end;
+	unsigned int addr;
+	unsigned int size;
+
+	if (!vm)
+		return NULL;
+
+	vm_start = vm->start;
+	vm_end = vm_node_end(vm->start, vm->pgnr);
+	size = pgnr_to_size(pgnr);
+
+	addr = vm_start;
+	head = &vm->vm_node_list;
+
+	node = alloc_hmm_vm_node(pgnr, vm);
+	if (!node) {
+		dev_err(atomisp_dev, "no memory to allocate hmm vm node.\n");
+		return NULL;
+	}
+
+	spin_lock(&vm->lock);
+	/*
+	 * if list is empty, the loop code will not be executed.
+	 */
+	list_for_each_entry(cur, head, list) {
+		/* Add gap between vm areas as helper to not hide overflow */
+		addr = PAGE_ALIGN(vm_node_end(cur->start, cur->pgnr) + 1);
+
+		if (list_is_last(&cur->list, head)) {
+			if (addr + size > vm_end) {
+				/* vm area does not have space anymore */
+				spin_unlock(&vm->lock);
+				kmem_cache_free(vm->cache, node);
+				dev_err(atomisp_dev,
+					  "no enough virtual address space.\n");
+				return NULL;
+			}
+
+			/* We still have vm space to add new node to tail */
+			break;
+		}
+
+		next = list_entry(cur->list.next, struct hmm_vm_node, list);
+		if ((next->start - addr) > size)
+			break;
+	}
+	node->start = addr;
+	node->vm = vm;
+	list_add(&node->list, &cur->list);
+	spin_unlock(&vm->lock);
+
+	return node;
+}
+
+void hmm_vm_free_node(struct hmm_vm_node *node)
+{
+	struct hmm_vm *vm;
+
+	if (!node)
+		return;
+
+	vm = node->vm;
+
+	spin_lock(&vm->lock);
+	list_del(&node->list);
+	spin_unlock(&vm->lock);
+
+	kmem_cache_free(vm->cache, node);
+}
+
+struct hmm_vm_node *hmm_vm_find_node_start(struct hmm_vm *vm, unsigned int addr)
+{
+	struct hmm_vm_node *node;
+
+	if (!vm)
+		return NULL;
+
+	spin_lock(&vm->lock);
+
+	list_for_each_entry(node, &vm->vm_node_list, list) {
+		if (node->start == addr) {
+			spin_unlock(&vm->lock);
+			return node;
+		}
+	}
+
+	spin_unlock(&vm->lock);
+	return NULL;
+}
+
+struct hmm_vm_node *hmm_vm_find_node_in_range(struct hmm_vm *vm,
+					      unsigned int addr)
+{
+	struct hmm_vm_node *node;
+
+	if (!vm)
+		return NULL;
+
+	spin_lock(&vm->lock);
+
+	list_for_each_entry(node, &vm->vm_node_list, list) {
+		if (addr_in_vm_node(addr, node)) {
+			spin_unlock(&vm->lock);
+			return node;
+		}
+	}
+
+	spin_unlock(&vm->lock);
+	return NULL;
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_custom_host_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_custom_host_hrt.h
new file mode 100644
index 000000000000..46a5d29e2d3a
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_custom_host_hrt.h
@@ -0,0 +1,107 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef _hive_isp_css_custom_host_hrt_h_
+#define _hive_isp_css_custom_host_hrt_h_
+
+#include <linux/delay.h>
+#include "atomisp_helper.h"
+
+/*
+ * _hrt_master_port_store/load/uload -macros using __force attributed
+ * cast to intentional dereferencing __iomem attributed (noderef)
+ * pointer from atomisp_get_io_virt_addr
+ */
+#define _hrt_master_port_store_8(a, d) \
+	(*((s8 __force *)atomisp_get_io_virt_addr(a)) = (d))
+
+#define _hrt_master_port_store_16(a, d) \
+	(*((s16 __force *)atomisp_get_io_virt_addr(a)) = (d))
+
+#define _hrt_master_port_store_32(a, d) \
+	(*((s32 __force *)atomisp_get_io_virt_addr(a)) = (d))
+
+#define _hrt_master_port_load_8(a) \
+	(*(s8 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_load_16(a) \
+	(*(s16 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_load_32(a) \
+	(*(s32 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_uload_8(a) \
+	(*(u8 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_uload_16(a) \
+	(*(u16 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_uload_32(a) \
+	(*(u32 __force *)atomisp_get_io_virt_addr(a))
+
+#define _hrt_master_port_store_8_volatile(a, d)  _hrt_master_port_store_8(a, d)
+#define _hrt_master_port_store_16_volatile(a, d) _hrt_master_port_store_16(a, d)
+#define _hrt_master_port_store_32_volatile(a, d) _hrt_master_port_store_32(a, d)
+
+#define _hrt_master_port_load_8_volatile(a)      _hrt_master_port_load_8(a)
+#define _hrt_master_port_load_16_volatile(a)     _hrt_master_port_load_16(a)
+#define _hrt_master_port_load_32_volatile(a)     _hrt_master_port_load_32(a)
+
+#define _hrt_master_port_uload_8_volatile(a)     _hrt_master_port_uload_8(a)
+#define _hrt_master_port_uload_16_volatile(a)    _hrt_master_port_uload_16(a)
+#define _hrt_master_port_uload_32_volatile(a)    _hrt_master_port_uload_32(a)
+
+static inline void hrt_sleep(void)
+{
+	udelay(1);
+}
+
+static inline uint32_t _hrt_mem_store(uint32_t to, const void *from, size_t n)
+{
+	unsigned i;
+	uint32_t _to = to;
+	const char *_from = (const char *)from;
+	for (i = 0; i < n; i++, _to++, _from++)
+		_hrt_master_port_store_8(_to, *_from);
+	return _to;
+}
+
+static inline void *_hrt_mem_load(uint32_t from, void *to, size_t n)
+{
+	unsigned i;
+	char *_to = (char *)to;
+	uint32_t _from = from;
+	for (i = 0; i < n; i++, _to++, _from++)
+		*_to = _hrt_master_port_load_8(_from);
+	return _to;
+}
+
+static inline uint32_t _hrt_mem_set(uint32_t to, int c, size_t n)
+{
+	unsigned i;
+	uint32_t _to = to;
+	for (i = 0; i < n; i++, _to++)
+		_hrt_master_port_store_8(_to, c);
+	return _to;
+}
+
+#endif /* _hive_isp_css_custom_host_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c
new file mode 100644
index 000000000000..7dff22f59e29
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.c
@@ -0,0 +1,129 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#include "atomisp_internal.h"
+
+#include "hive_isp_css_mm_hrt.h"
+#include "hmm/hmm.h"
+
+#define __page_align(size)	(((size) + (PAGE_SIZE-1)) & (~(PAGE_SIZE-1)))
+
+static void *my_userptr;
+static unsigned my_num_pages;
+static enum hrt_userptr_type my_usr_type;
+
+void hrt_isp_css_mm_set_user_ptr(void *userptr,
+				 unsigned int num_pages,
+				 enum hrt_userptr_type type)
+{
+	my_userptr = userptr;
+	my_num_pages = num_pages;
+	my_usr_type = type;
+}
+
+static ia_css_ptr __hrt_isp_css_mm_alloc(size_t bytes, void *userptr,
+				    unsigned int num_pages,
+				    enum hrt_userptr_type type,
+				    bool cached)
+{
+#ifdef CONFIG_ION
+	if (type == HRT_USR_ION)
+		return hmm_alloc(bytes, HMM_BO_ION, 0,
+					 userptr, cached);
+
+#endif
+	if (type == HRT_USR_PTR) {
+		if (userptr == NULL)
+			return hmm_alloc(bytes, HMM_BO_PRIVATE, 0,
+						 0, cached);
+		else {
+			if (num_pages < ((__page_align(bytes)) >> PAGE_SHIFT))
+				dev_err(atomisp_dev,
+					 "user space memory size is less"
+					 " than the expected size..\n");
+			else if (num_pages > ((__page_align(bytes))
+					      >> PAGE_SHIFT))
+				dev_err(atomisp_dev,
+					 "user space memory size is"
+					 " large than the expected size..\n");
+
+			return hmm_alloc(bytes, HMM_BO_USER, 0,
+						 userptr, cached);
+		}
+	} else {
+		dev_err(atomisp_dev, "user ptr type is incorrect.\n");
+		return 0;
+	}
+}
+
+ia_css_ptr hrt_isp_css_mm_alloc(size_t bytes)
+{
+	return __hrt_isp_css_mm_alloc(bytes, my_userptr,
+				      my_num_pages, my_usr_type, false);
+}
+
+ia_css_ptr hrt_isp_css_mm_alloc_user_ptr(size_t bytes, void *userptr,
+				    unsigned int num_pages,
+				    enum hrt_userptr_type type,
+				    bool cached)
+{
+	return __hrt_isp_css_mm_alloc(bytes, userptr, num_pages,
+				      type, cached);
+}
+
+ia_css_ptr hrt_isp_css_mm_alloc_cached(size_t bytes)
+{
+	if (my_userptr == NULL)
+		return hmm_alloc(bytes, HMM_BO_PRIVATE, 0, 0,
+						HMM_CACHED);
+	else {
+		if (my_num_pages < ((__page_align(bytes)) >> PAGE_SHIFT))
+			dev_err(atomisp_dev,
+					"user space memory size is less"
+					" than the expected size..\n");
+		else if (my_num_pages > ((__page_align(bytes)) >> PAGE_SHIFT))
+			dev_err(atomisp_dev,
+					"user space memory size is"
+					" large than the expected size..\n");
+
+		return hmm_alloc(bytes, HMM_BO_USER, 0,
+						my_userptr, HMM_CACHED);
+	}
+}
+
+ia_css_ptr hrt_isp_css_mm_calloc(size_t bytes)
+{
+	ia_css_ptr ptr = hrt_isp_css_mm_alloc(bytes);
+	if (ptr)
+		hmm_set(ptr, 0, bytes);
+	return ptr;
+}
+
+ia_css_ptr hrt_isp_css_mm_calloc_cached(size_t bytes)
+{
+	ia_css_ptr ptr = hrt_isp_css_mm_alloc_cached(bytes);
+	if (ptr)
+		hmm_set(ptr, 0, bytes);
+	return ptr;
+}
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.h b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.h
new file mode 100644
index 000000000000..1328944a7afd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/hrt/hive_isp_css_mm_hrt.h
@@ -0,0 +1,60 @@
+/*
+ * Support for Medfield 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef _hive_isp_css_mm_hrt_h_
+#define _hive_isp_css_mm_hrt_h_
+
+#include <hmm/hmm.h>
+#include <hrt/hive_isp_css_custom_host_hrt.h>
+
+#define HRT_BUF_FLAG_CACHED (1 << 0)
+
+enum hrt_userptr_type {
+	HRT_USR_PTR = 0,
+#ifdef CONFIG_ION
+	HRT_USR_ION,
+#endif
+};
+
+struct hrt_userbuffer_attr {
+	enum hrt_userptr_type	type;
+	unsigned int		pgnr;
+};
+
+void hrt_isp_css_mm_set_user_ptr(void *userptr,
+				unsigned int num_pages, enum hrt_userptr_type);
+
+/* Allocate memory, returns a virtual address */
+ia_css_ptr hrt_isp_css_mm_alloc(size_t bytes);
+ia_css_ptr hrt_isp_css_mm_alloc_user_ptr(size_t bytes, void *userptr,
+				    unsigned int num_pages,
+				    enum hrt_userptr_type,
+				    bool cached);
+ia_css_ptr hrt_isp_css_mm_alloc_cached(size_t bytes);
+
+/* allocate memory and initialize with zeros,
+   returns a virtual address */
+ia_css_ptr hrt_isp_css_mm_calloc(size_t bytes);
+ia_css_ptr hrt_isp_css_mm_calloc_cached(size_t bytes);
+
+#endif /* _hive_isp_css_mm_hrt_h_ */
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm.h
new file mode 100644
index 000000000000..6b9fb1b2caaf
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm.h
@@ -0,0 +1,106 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef	__HMM_H__
+#define	__HMM_H__
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+
+#include "hmm/hmm_pool.h"
+#include "ia_css_types.h"
+
+#define HMM_CACHED true
+#define HMM_UNCACHED false
+
+int hmm_pool_register(unsigned int pool_size, enum hmm_pool_type pool_type);
+void hmm_pool_unregister(enum hmm_pool_type pool_type);
+
+int hmm_init(void);
+void hmm_cleanup(void);
+
+ia_css_ptr hmm_alloc(size_t bytes, enum hmm_bo_type type,
+		int from_highmem, void *userptr, bool cached);
+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);
+
+/*
+ * Address translation from ISP shared memory address to kernel virtual address
+ * if the memory is not vmmaped,  then do it.
+ */
+void *hmm_isp_vaddr_to_host_vaddr(ia_css_ptr ptr, bool cached);
+
+/*
+ * Address translation from kernel virtual address to ISP shared memory address
+ */
+ia_css_ptr hmm_host_vaddr_to_hrt_vaddr(const void *ptr);
+
+/*
+ * 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);
+
+/* show memory statistic
+ */
+void hmm_show_mem_stat(const char *func, const int line);
+
+/* init memory statistic
+ */
+void hmm_init_mem_stat(int res_pgnr, int dyc_en, int dyc_pgnr);
+
+extern bool dypool_enable;
+extern unsigned int dypool_pgnr;
+extern struct hmm_bo_device bo_device;
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_bo.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_bo.h
new file mode 100644
index 000000000000..dffd6e9cf693
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_bo.h
@@ -0,0 +1,323 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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	HMM_MAX_ORDER		3
+#define	HMM_MIN_ORDER		0
+
+#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_SHARE,
+	HMM_BO_USER,
+#ifdef CONFIG_ION
+	HMM_BO_ION,
+#endif
+	HMM_BO_LAST,
+};
+
+enum hmm_page_type {
+	HMM_PAGE_TYPE_RESERVED,
+	HMM_PAGE_TYPE_DYNAMIC,
+	HMM_PAGE_TYPE_GENERAL,
+};
+
+#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
+#define	HMM_BO_MEM_TYPE_USER     0x1
+#define	HMM_BO_MEM_TYPE_PFN      0x2
+
+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;
+#ifdef CONFIG_ION
+	struct ion_client	*iclient;
+#endif
+	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_page_object {
+	struct page		*page;
+	enum hmm_page_type	type;
+};
+
+struct hmm_buffer_object {
+	struct hmm_bo_device	*bdev;
+	struct list_head	list;
+	struct kref	kref;
+
+	/* mutex protecting this BO */
+	struct mutex		mutex;
+	enum hmm_bo_type	type;
+	struct hmm_page_object	*page_obj;	/* physical pages */
+	int		from_highmem;
+	int		mmap_count;
+#ifdef CONFIG_ION
+	struct ion_handle	*ihandle;
+#endif
+	int		status;
+	int		mem_type;
+	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);
+
+
+/*
+ * allocate/free physical pages for the bo. will try to alloc mem
+ * from highmem if from_highmem is set, and type indicate that the
+ * pages will be allocated by using video driver (for share buffer)
+ * or by ISP driver itself.
+ */
+
+
+int hmm_bo_allocated(struct hmm_buffer_object *bo);
+
+
+/*
+ * allocate/free physical pages for the bo. will try to alloc mem
+ * from highmem if from_highmem is set, and type indicate that 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, int from_highmem,
+		void *userptr, bool cached);
+void hmm_bo_free_pages(struct hmm_buffer_object *bo);
+int hmm_bo_page_allocated(struct hmm_buffer_object *bo);
+
+/*
+ * get physical page info of the bo.
+ */
+int hmm_bo_get_page_info(struct hmm_buffer_object *bo,
+		struct hmm_page_object **page_obj, int *pgnr);
+
+/*
+ * 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);
+
+extern struct hmm_pool	dynamic_pool;
+extern struct hmm_pool	reserved_pool;
+
+/*
+ * 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/pci/atomisp2/include/hmm/hmm_bo_dev.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_bo_dev.h
new file mode 100644
index 000000000000..a9446adb4c70
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_bo_dev.h
@@ -0,0 +1,130 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef	__HMM_BO_DEV_H__
+#define	__HMM_BO_DEV_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 "hmm/hmm_vm.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	HMM_BO_DEVICE_INITED	0x1
+
+#define	HMM_BO_CACHE_SIZE	2
+
+
+struct hmm_buffer_object;
+
+struct hmm_bo_device {
+	/* isp_mmu provides lock itself */
+	struct isp_mmu		mmu;
+
+	/* hmm_vm provides lock itself */
+	struct hmm_vm		vaddr_space;
+
+	struct list_head	free_bo_list;
+	struct list_head	active_bo_list;
+
+	/* list lock is used to protect both of the buffer object lists */
+	spinlock_t		list_lock;
+#ifdef CONFIG_ION
+	struct ion_client	*iclient;
+#endif
+	int			flag;
+};
+
+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);
+
+/*
+ * find the buffer object with 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 with virtual address vaddr.
+ * 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);
+
+/*
+ * find a buffer object with pgnr pages from free_bo_list and
+ * activate it (remove from free_bo_list and add to
+ * active_bo_list)
+ *
+ * return NULL if no such buffer object found.
+ */
+struct hmm_buffer_object *hmm_bo_device_get_bo(
+		struct hmm_bo_device *bdev, unsigned int pgnr);
+
+/*
+ * destroy all buffer objects in the free_bo_list.
+ */
+void hmm_bo_device_destroy_free_bo_list(struct hmm_bo_device *bdev);
+/*
+ * destroy buffer object with start virtual address vaddr.
+ */
+void hmm_bo_device_destroy_free_bo_addr(struct hmm_bo_device *bdev,
+		ia_css_ptr vaddr);
+/*
+ * destroy all buffer objects with pgnr pages.
+ */
+void hmm_bo_device_destroy_free_bo_size(struct hmm_bo_device *bdev,
+		unsigned int pgnr);
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_common.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_common.h
new file mode 100644
index 000000000000..f1593aa38ce1
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_common.h
@@ -0,0 +1,100 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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)
+
+/* hmm_mem_stat is used to trace the hmm mem used by ISP pipe. The unit is page
+ * number.
+ *
+ * res_size:  reserved mem pool size, being allocated from system at system boot time.
+ *		res_size >= res_cnt.
+ * sys_size:  system mem pool size, being allocated from system at camera running time.
+ *		dyc_size:  dynamic mem pool size.
+ *		dyc_thr:   dynamic mem pool high watermark.
+ *		dyc_size <= dyc_thr.
+ * usr_size:  user ptr mem size.
+ *
+ * res_cnt:   track the mem allocated from reserved pool at camera running time.
+ * tol_cnt:   track the total mem used by ISP pipe at camera running time.
+ */
+struct _hmm_mem_stat {
+	int res_size;
+	int sys_size;
+	int dyc_size;
+	int dyc_thr;
+	int usr_size;
+	int res_cnt;
+	int tol_cnt;
+};
+
+extern struct _hmm_mem_stat hmm_mem_stat;
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_pool.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_pool.h
new file mode 100644
index 000000000000..1ba360433d88
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_pool.h
@@ -0,0 +1,119 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef __HMM_POOL_H__
+#define __HMM_POOL_H__
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/kref.h>
+#include "hmm_common.h"
+#include "hmm/hmm_bo.h"
+
+#define ALLOC_PAGE_FAIL_NUM		5
+
+enum hmm_pool_type {
+	HMM_POOL_TYPE_RESERVED,
+	HMM_POOL_TYPE_DYNAMIC,
+};
+
+/**
+ * struct hmm_pool_ops  -  memory pool callbacks.
+ *
+ * @pool_init:		   initialize the memory pool.
+ * @pool_exit:		   uninitialize the memory pool.
+ * @pool_alloc_pages:	   allocate pages from memory pool.
+ * @pool_free_pages:	   free pages to memory pool.
+ * @pool_inited:	   check whether memory pool is initialized.
+ */
+struct hmm_pool_ops {
+	int (*pool_init)(void **pool, unsigned int pool_size);
+	void (*pool_exit)(void **pool);
+	unsigned int (*pool_alloc_pages)(void *pool,
+					struct hmm_page_object *page_obj,
+					unsigned int size, bool cached);
+	void (*pool_free_pages)(void *pool,
+				struct hmm_page_object *page_obj);
+	int (*pool_inited)(void *pool);
+};
+
+struct hmm_pool {
+	struct hmm_pool_ops	*pops;
+
+	void			*pool_info;
+};
+
+/**
+ * struct hmm_reserved_pool_info  - represents reserved pool private data.
+ * @pages:			    a array that store physical pages.
+ *				    The array is as reserved memory pool.
+ * @index:			    to indicate the first blank page number
+ *				    in reserved memory pool(pages array).
+ * @pgnr:			    the valid page amount in reserved memory
+ *				    pool.
+ * @list_lock:			    list lock is used to protect the operation
+ *				    to reserved memory pool.
+ * @flag:			    reserved memory pool state flag.
+ */
+struct hmm_reserved_pool_info {
+	struct page		**pages;
+
+	unsigned int		index;
+	unsigned int		pgnr;
+	spinlock_t		list_lock;
+	bool			initialized;
+};
+
+/**
+ * struct hmm_dynamic_pool_info  -  represents dynamic pool private data.
+ * @pages_list:			    a list that store physical pages.
+ *				    The pages list is as dynamic memory pool.
+ * @list_lock:			    list lock is used to protect the operation
+ *				    to dynamic memory pool.
+ * @flag:			    dynamic memory pool state flag.
+ * @pgptr_cache:		    struct kmem_cache, manages a cache.
+ */
+struct hmm_dynamic_pool_info {
+	struct list_head	pages_list;
+
+	/* list lock is used to protect the free pages block lists */
+	spinlock_t		list_lock;
+
+	struct kmem_cache	*pgptr_cache;
+	bool			initialized;
+
+	unsigned int		pool_size;
+	unsigned int		pgnr;
+};
+
+struct hmm_page {
+	struct page		*page;
+	struct list_head	list;
+};
+
+extern struct hmm_pool_ops	reserved_pops;
+extern struct hmm_pool_ops	dynamic_pops;
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_vm.h b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_vm.h
new file mode 100644
index 000000000000..07d40662de32
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/hmm/hmm_vm.h
@@ -0,0 +1,68 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#ifndef	__HMM_VM_H__
+#define	__HMM_VM_H__
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/list.h>
+
+struct hmm_vm {
+	unsigned int start;
+	unsigned int pgnr;
+	unsigned int size;
+	struct list_head vm_node_list;
+	spinlock_t lock;
+	struct kmem_cache *cache;
+};
+
+struct hmm_vm_node {
+	struct list_head list;
+	unsigned int start;
+	unsigned int pgnr;
+	unsigned int size;
+	struct hmm_vm *vm;
+};
+#define	ISP_VM_START	0x0
+#define	ISP_VM_SIZE	(0x7FFFFFFF)	/* 2G address space */
+#define	ISP_PTR_NULL	NULL
+
+int hmm_vm_init(struct hmm_vm *vm, unsigned int start,
+		unsigned int size);
+
+void hmm_vm_clean(struct hmm_vm *vm);
+
+struct hmm_vm_node *hmm_vm_alloc_node(struct hmm_vm *vm,
+		unsigned int pgnr);
+
+void hmm_vm_free_node(struct hmm_vm_node *node);
+
+struct hmm_vm_node *hmm_vm_find_node_start(struct hmm_vm *vm,
+		unsigned int addr);
+
+struct hmm_vm_node *hmm_vm_find_node_in_range(struct hmm_vm *vm,
+		unsigned int addr);
+
+#endif
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/isp_mmu.h b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/isp_mmu.h
new file mode 100644
index 000000000000..6b4eefc929e2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/isp_mmu.h
@@ -0,0 +1,175 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * 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		(1U << 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;
+
+	/*
+	 * set/get page directory base address (physical address).
+	 *
+	 * must be provided.
+	 */
+	int (*set_pd_base) (struct isp_mmu *mmu,
+			phys_addr_t pd_base);
+	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;
+	struct kmem_cache *tbl_cache;
+};
+
+/* 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 continous 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/pci/atomisp2/include/mmu/sh_mmu.h b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/sh_mmu.h
new file mode 100644
index 000000000000..06041e94cbb2
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/sh_mmu.h
@@ -0,0 +1,76 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#ifndef	SH_MMU_H_
+#define	SH_MMU_H_
+
+
+#include <sh_css.h>
+
+#include "mmu/isp_mmu.h"
+
+
+/*
+ * include SH header file here
+ */
+
+/*
+ * set page directory base address (physical address).
+ *
+ * must be provided.
+ */
+static int sh_set_pd_base(struct isp_mmu *mmu,
+		unsigned int phys)
+{
+	sh_css_mmu_set_page_table_base_address((void *)phys);
+	return 0;
+}
+
+/*
+ * 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)
+{
+	sh_css_mmu_invalidate_cache();
+}
+
+static struct isp_mmu_driver sh_mmu_driver = {
+	.name = "Silicon Hive ISP3000 MMU",
+	.pte_valid_mask = 0x1,
+	.set_pd_base = sh_set_pd_base,
+	.tlb_flush_all = sh_tlb_flush,
+};
+
+#define	ISP_VM_START	0x0
+#define	ISP_VM_SIZE	(1 << 30)	/* 1G address space */
+#define	ISP_PTR_NULL	NULL
+
+#endif /* SH_MMU_H_ */
+
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/sh_mmu_mrfld.h b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/sh_mmu_mrfld.h
new file mode 100644
index 000000000000..b9bad9f06235
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/include/mmu/sh_mmu_mrfld.h
@@ -0,0 +1,28 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+
+#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/pci/atomisp2/mmu/isp_mmu.c b/drivers/staging/media/atomisp/pci/atomisp2/mmu/isp_mmu.c
new file mode 100644
index 000000000000..2009e3a11b86
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/mmu/isp_mmu.c
@@ -0,0 +1,594 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+/*
+ * 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 <asm/cacheflush.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/string.h>
+#include <linux/errno.h>
+#include <linux/sizes.h>
+
+#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;
+
+	/*page table lock may needed here*/
+	/*
+	 * The slab allocator(kmem_cache and kmalloc family) doesn't handle
+	 * GFP_DMA32 flag, so we have to use buddy allocator.
+	 */
+	if (totalram_pages > (unsigned long)NR_PAGES_2GB)
+		virt = (void *)__get_free_page(GFP_KERNEL | GFP_DMA32);
+	else
+		virt = kmem_cache_zalloc(mmu->tbl_cache, GFP_KERNEL);
+	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
+
+	kmem_cache_free(mmu->tbl_cache, 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 unvalid 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 unvalid 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 unvalid 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
+		 */
+		ret = mmu->driver->set_pd_base(mmu, l1_pt);
+		if (ret) {
+			dev_err(atomisp_dev,
+				 "set page directory base address fail.\n");
+			mutex_unlock(&mmu->pt_mutex);
+			return ret;
+		}
+		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 dont
+		 * 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->set_pd_base || !driver->tlb_flush_all) {
+		dev_err(atomisp_dev,
+			    "set_pd_base or 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);
+
+	mmu->tbl_cache = kmem_cache_create("iopte_cache", ISP_PAGE_SIZE,
+					   ISP_PAGE_SIZE, SLAB_HWCACHE_ALIGN,
+					   NULL);
+	if (!mmu->tbl_cache)
+		return -ENOMEM;
+
+	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);
+
+	kmem_cache_destroy(mmu->tbl_cache);
+}
diff --git a/drivers/staging/media/atomisp/pci/atomisp2/mmu/sh_mmu_mrfld.c b/drivers/staging/media/atomisp/pci/atomisp2/mmu/sh_mmu_mrfld.c
new file mode 100644
index 000000000000..97546bd124cd
--- /dev/null
+++ b/drivers/staging/media/atomisp/pci/atomisp2/mmu/sh_mmu_mrfld.c
@@ -0,0 +1,93 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ */
+#include "type_support.h"
+#include "mmu/isp_mmu.h"
+#include "memory_access/memory_access.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);
+}
+
+/*
+ * set page directory base address (physical address).
+ *
+ * must be provided.
+ */
+static int sh_set_pd_base(struct isp_mmu *mmu,
+			  phys_addr_t phys)
+{
+	unsigned int pte = sh_phys_to_pte(mmu, phys);
+	/*mmgr_set_base_address(HOST_ADDRESS(pte));*/
+	atomisp_css_mmu_set_page_table_base_index(HOST_ADDRESS(pte));
+	return 0;
+}
+
+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)
+{
+	atomisp_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,
+	.set_pd_base = sh_set_pd_base,
+	.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/platform/Makefile b/drivers/staging/media/atomisp/platform/Makefile
new file mode 100644
index 000000000000..df157630bda9
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for camera drivers.
+#
+
+obj-$(CONFIG_INTEL_ATOMISP) += clock/
+obj-$(CONFIG_INTEL_ATOMISP) += intel-mid/
diff --git a/drivers/staging/media/atomisp/platform/clock/Makefile b/drivers/staging/media/atomisp/platform/clock/Makefile
new file mode 100644
index 000000000000..82fbe8b6968a
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/clock/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for clock devices.
+#
+
+obj-$(CONFIG_INTEL_ATOMISP)	+= vlv2_plat_clock.o
+obj-$(CONFIG_INTEL_ATOMISP)     += platform_vlv2_plat_clk.o
diff --git a/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.c b/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.c
new file mode 100644
index 000000000000..0aae9b0283bb
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.c
@@ -0,0 +1,40 @@
+/*
+ * platform_vlv2_plat_clk.c - VLV2 platform clock driver
+ * Copyright (C) 2013 Intel Corporation
+ *
+ * Author: Asutosh Pathak <asutosh.pathak@intel.com>
+ * Author: Chandra Sekhar Anagani <chandra.sekhar.anagani@intel.com>
+ * Author: Sergio Aguirre <sergio.a.aguirre.rodriguez@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * 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/module.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+
+static int __init vlv2_plat_clk_init(void)
+{
+	struct platform_device *pdev;
+
+	pdev = platform_device_register_simple("vlv2_plat_clk", -1, NULL, 0);
+	if (IS_ERR(pdev)) {
+		pr_err("platform_vlv2_plat_clk:register failed: %ld\n",
+			PTR_ERR(pdev));
+		return PTR_ERR(pdev);
+	}
+
+	return 0;
+}
+
+device_initcall(vlv2_plat_clk_init);
diff --git a/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.h b/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.h
new file mode 100644
index 000000000000..b730ab0e8223
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/clock/platform_vlv2_plat_clk.h
@@ -0,0 +1,27 @@
+/*
+ * platform_vlv2_plat_clk.h: platform clock driver library header file
+ * Copyright (C) 2013 Intel Corporation
+ *
+ * Author: Asutosh Pathak <asutosh.pathak@intel.com>
+ * Author: Chandra Sekhar Anagani <chandra.sekhar.anagani@intel.com>
+ * Author: Sergio Aguirre <sergio.a.aguirre.rodriguez@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * 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 _PLATFORM_VLV2_PLAT_CLK_H_
+#define _PLATFORM_VLV2_PLAT_CLK_H_
+
+#include <linux/sfi.h>
+#include <asm/intel-mid.h>
+
+extern void __init *vlv2_plat_clk_device_platform_data(
+				void *info) __attribute__((weak));
+#endif
diff --git a/drivers/staging/media/atomisp/platform/clock/vlv2_plat_clock.c b/drivers/staging/media/atomisp/platform/clock/vlv2_plat_clock.c
new file mode 100644
index 000000000000..f96789a31819
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/clock/vlv2_plat_clock.c
@@ -0,0 +1,247 @@
+/*
+ * vlv2_plat_clock.c - VLV2 platform clock driver
+ * Copyright (C) 2013 Intel Corporation
+ *
+ * Author: Asutosh Pathak <asutosh.pathak@intel.com>
+ * Author: Chandra Sekhar Anagani <chandra.sekhar.anagani@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * 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; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include "../../include/linux/vlv2_plat_clock.h"
+
+/* NOTE: Most of below constants could come from platform data.
+ * To be fixed when appropriate ACPI support comes.
+ */
+#define VLV2_PMC_CLK_BASE_ADDRESS	0xfed03060
+#define PLT_CLK_CTL_OFFSET(x)		(0x04 * (x))
+
+#define CLK_CONFG_BIT_POS		0
+#define CLK_CONFG_BIT_LEN		2
+#define CLK_CONFG_D3_GATED		0
+#define CLK_CONFG_FORCE_ON		1
+#define CLK_CONFG_FORCE_OFF		2
+
+#define CLK_FREQ_TYPE_BIT_POS		2
+#define CLK_FREQ_TYPE_BIT_LEN		1
+#define CLK_FREQ_TYPE_XTAL		0	/* 25 MHz */
+#define CLK_FREQ_TYPE_PLL		1	/* 19.2 MHz */
+
+#define MAX_CLK_COUNT			5
+
+/* Helper macros to manipulate bitfields */
+#define REG_MASK(n)		(((1 << (n##_BIT_LEN)) - 1) << (n##_BIT_POS))
+#define REG_SET_FIELD(r, n, v)	(((r) & ~REG_MASK(n)) | \
+				 (((v) << (n##_BIT_POS)) & REG_MASK(n)))
+#define REG_GET_FIELD(r, n)	(((r) & REG_MASK(n)) >> n##_BIT_POS)
+/*
+ * vlv2 platform has 6 platform clocks, controlled by 4 byte registers
+ * Total size required for mapping is 6*4 = 24 bytes
+ */
+#define PMC_MAP_SIZE			24
+
+static DEFINE_MUTEX(clk_mutex);
+static void __iomem *pmc_base;
+
+/*
+ * vlv2_plat_set_clock_freq - Set clock frequency to a specified platform clock
+ * @clk_num: Platform clock number (i.e. 0, 1, 2, ...,5)
+ * @freq_type: Clock frequency (0-25 MHz(XTAL), 1-19.2 MHz(PLL) )
+ */
+int vlv2_plat_set_clock_freq(int clk_num, int freq_type)
+{
+	void __iomem *addr;
+
+	if (clk_num < 0 || clk_num >= MAX_CLK_COUNT) {
+		pr_err("Clock number out of range (%d)\n", clk_num);
+		return -EINVAL;
+	}
+
+	if (freq_type != CLK_FREQ_TYPE_XTAL &&
+	    freq_type != CLK_FREQ_TYPE_PLL) {
+		pr_err("wrong clock type\n");
+		return -EINVAL;
+	}
+
+	if (!pmc_base) {
+		pr_err("memio map is not set\n");
+		return -EINVAL;
+	}
+
+	addr = pmc_base + PLT_CLK_CTL_OFFSET(clk_num);
+
+	mutex_lock(&clk_mutex);
+	writel(REG_SET_FIELD(readl(addr), CLK_FREQ_TYPE, freq_type), addr);
+	mutex_unlock(&clk_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vlv2_plat_set_clock_freq);
+
+/*
+ * vlv2_plat_get_clock_freq - Get the status of specified platform clock
+ * @clk_num: Platform clock number (i.e. 0, 1, 2, ...,5)
+ *
+ * Returns 0 for 25 MHz(XTAL) and 1 for 19.2 MHz(PLL)
+ */
+int vlv2_plat_get_clock_freq(int clk_num)
+{
+	u32 ret;
+
+	if (clk_num < 0 || clk_num >= MAX_CLK_COUNT) {
+		pr_err("Clock number out of range (%d)\n", clk_num);
+		return -EINVAL;
+	}
+
+	if (!pmc_base) {
+		pr_err("memio map is not set\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&clk_mutex);
+	ret = REG_GET_FIELD(readl(pmc_base + PLT_CLK_CTL_OFFSET(clk_num)),
+			    CLK_FREQ_TYPE);
+	mutex_unlock(&clk_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vlv2_plat_get_clock_freq);
+
+/*
+ * vlv2_plat_configure_clock - Configure the specified platform clock
+ * @clk_num: Platform clock number (i.e. 0, 1, 2, ...,5)
+ * @conf:      Clock gating:
+ *		0   - Clock gated on D3 state
+ *		1   - Force on
+ *		2,3 - Force off
+ */
+int vlv2_plat_configure_clock(int clk_num, u32 conf)
+{
+	void __iomem *addr;
+
+	if (clk_num < 0 || clk_num >= MAX_CLK_COUNT) {
+		pr_err("Clock number out of range (%d)\n", clk_num);
+		return -EINVAL;
+	}
+
+	if (conf != CLK_CONFG_D3_GATED &&
+	    conf != CLK_CONFG_FORCE_ON &&
+	    conf != CLK_CONFG_FORCE_OFF) {
+		pr_err("Invalid clock configuration requested\n");
+		return -EINVAL;
+	}
+
+	if (!pmc_base) {
+		pr_err("memio map is not set\n");
+		return -EINVAL;
+	}
+
+	addr = pmc_base + PLT_CLK_CTL_OFFSET(clk_num);
+
+	mutex_lock(&clk_mutex);
+	writel(REG_SET_FIELD(readl(addr), CLK_CONFG, conf), addr);
+	mutex_unlock(&clk_mutex);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vlv2_plat_configure_clock);
+
+/*
+ * vlv2_plat_get_clock_status - Get the status of specified platform clock
+ * @clk_num: Platform clock number (i.e. 0, 1, 2, ...,5)
+ *
+ * Returns 1 - On, 0 - Off
+ */
+int vlv2_plat_get_clock_status(int clk_num)
+{
+	int ret;
+
+	if (clk_num < 0 || clk_num >= MAX_CLK_COUNT) {
+		pr_err("Clock number out of range (%d)\n", clk_num);
+		return -EINVAL;
+	}
+
+	if (!pmc_base) {
+		pr_err("memio map is not set\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&clk_mutex);
+	ret = (int)REG_GET_FIELD(readl(pmc_base + PLT_CLK_CTL_OFFSET(clk_num)),
+				 CLK_CONFG);
+	mutex_unlock(&clk_mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vlv2_plat_get_clock_status);
+
+static int vlv2_plat_clk_probe(struct platform_device *pdev)
+{
+	int i = 0;
+
+	pmc_base = ioremap_nocache(VLV2_PMC_CLK_BASE_ADDRESS, PMC_MAP_SIZE);
+	if (!pmc_base) {
+		dev_err(&pdev->dev, "I/O memory remapping failed\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize all clocks as disabled */
+	for (i = 0; i < MAX_CLK_COUNT; i++)
+		vlv2_plat_configure_clock(i, CLK_CONFG_FORCE_OFF);
+
+	dev_info(&pdev->dev, "vlv2_plat_clk initialized\n");
+	return 0;
+}
+
+static const struct platform_device_id vlv2_plat_clk_id[] = {
+	{"vlv2_plat_clk", 0},
+	{}
+};
+
+static int vlv2_resume(struct device *device)
+{
+	int i;
+
+	/* Initialize all clocks as disabled */
+	for (i = 0; i < MAX_CLK_COUNT; i++)
+		vlv2_plat_configure_clock(i, CLK_CONFG_FORCE_OFF);
+
+	return 0;
+}
+
+static int vlv2_suspend(struct device *device)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops vlv2_pm_ops = {
+	.suspend = vlv2_suspend,
+	.resume = vlv2_resume,
+};
+
+static struct platform_driver vlv2_plat_clk_driver = {
+	.probe = vlv2_plat_clk_probe,
+	.id_table = vlv2_plat_clk_id,
+	.driver = {
+		.name = "vlv2_plat_clk",
+		.pm = &vlv2_pm_ops,
+	},
+};
+
+static int __init vlv2_plat_clk_init(void)
+{
+	return platform_driver_register(&vlv2_plat_clk_driver);
+}
+arch_initcall(vlv2_plat_clk_init);
diff --git a/drivers/staging/media/atomisp/platform/intel-mid/Makefile b/drivers/staging/media/atomisp/platform/intel-mid/Makefile
new file mode 100644
index 000000000000..4621261c35db
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/intel-mid/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for intel-mid devices.
+#
+obj-$(CONFIG_INTEL_ATOMISP) += intel_mid_pcihelpers.o
+obj-$(CONFIG_INTEL_ATOMISP) += atomisp_gmin_platform.o
diff --git a/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c b/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
new file mode 100644
index 000000000000..5b4506a71126
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/intel-mid/atomisp_gmin_platform.c
@@ -0,0 +1,758 @@
+#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/delay.h>
+#include <media/v4l2-subdev.h>
+#include <linux/mfd/intel_soc_pmic.h>
+#include "../../include/linux/vlv2_plat_clock.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
+
+/* Should be defined in vlv2_plat_clock API, isn't: */
+#define VLV2_CLK_PLL_19P2MHZ 1
+#define VLV2_CLK_XTAL_19P2MHZ 0
+#define VLV2_CLK_ON      1
+#define VLV2_CLK_OFF     2
+#define ELDO1_SEL_REG	0x19
+#define ELDO1_1P8V	0x16
+#define ELDO1_CTRL_SHIFT 0x00
+#define ELDO2_SEL_REG	0x1a
+#define ELDO2_1P8V	0x16
+#define ELDO2_CTRL_SHIFT 0x01
+
+struct gmin_subdev {
+	struct v4l2_subdev *subdev;
+	int clock_num;
+	int clock_src;
+	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 v1p8_on;
+	bool v2p8_on;
+	bool v1p2_on;
+	bool v2p8_vcm_on;
+};
+
+static struct gmin_subdev gmin_subdevs[MAX_SUBDEVS];
+
+static enum { PMIC_UNSET = 0, PMIC_REGULATOR, PMIC_AXP, PMIC_TI ,
+	PMIC_CRYSTALCOVE } pmic_id;
+
+/* 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,
+};
+
+/*
+ * Something of a hack.  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.
+ */
+enum { V2P8_GPIO_UNSET = -2, V2P8_GPIO_NONE = -1 };
+static int v2p8_gpio = V2P8_GPIO_UNSET;
+
+/*
+ * Something of a hack. The CHT RVP board drives camera 1.8v from an
+ * external regulator instead of the PMIC just like ECS E7 board, see the
+ * comments above.
+ */
+enum { V1P8_GPIO_UNSET = -2, V1P8_GPIO_NONE = -1 };
+static int v1p8_gpio = V1P8_GPIO_UNSET;
+
+static LIST_HEAD(vcm_devices);
+static DEFINE_MUTEX(vcm_lock);
+
+static struct gmin_subdev *find_gmin_subdev(struct v4l2_subdev *subdev);
+
+/*
+ * Legacy/stub behavior copied from upstream platform_camera.c.  The
+ * atomisp driver relies on these values being non-NULL in a few
+ * places, even though they are hard-coded in all current
+ * implementations.
+ */
+const struct atomisp_camera_caps *atomisp_get_default_camera_caps(void)
+{
+	static const struct atomisp_camera_caps caps = {
+		.sensor_num = 1,
+		.sensor = {
+			{ .stream_num = 1, },
+		},
+	};
+	return &caps;
+}
+EXPORT_SYMBOL_GPL(atomisp_get_default_camera_caps);
+
+const struct atomisp_platform_data *atomisp_get_platform_data(void)
+{
+	return &pdata;
+}
+EXPORT_SYMBOL_GPL(atomisp_get_platform_data);
+
+static int af_power_ctrl(struct v4l2_subdev *subdev, int flag)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+
+	if (gs && gs->v2p8_vcm_on == flag)
+		return 0;
+	gs->v2p8_vcm_on = flag;
+
+	/*
+	 * The power here is used for dw9817,
+	 * regulator is from rear sensor
+	*/
+	if (gs->v2p8_vcm_reg) {
+		if (flag)
+			return regulator_enable(gs->v2p8_vcm_reg);
+		else
+			return regulator_disable(gs->v2p8_vcm_reg);
+	}
+	return 0;
+}
+
+/*
+ * Used in a handful of modules.  Focus motor control, I think.  Note
+ * that there is no configurability in the API, so this needs to be
+ * fixed where it is used.
+ *
+ * struct camera_af_platform_data {
+ *     int (*power_ctrl)(struct v4l2_subdev *subdev, int flag);
+ * };
+ *
+ * Note that the implementation in MCG platform_camera.c is stubbed
+ * out anyway (i.e. returns zero from the callback) on BYT.  So
+ * neither needed on gmin platforms or supported upstream.
+ */
+const struct camera_af_platform_data *camera_get_af_platform_data(void)
+{
+	static struct camera_af_platform_data afpd = {
+		.power_ctrl = af_power_ctrl,
+	};
+	return &afpd;
+}
+EXPORT_SYMBOL_GPL(camera_get_af_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;
+
+	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 = ACPI_COMPANION(&client->dev);
+	if (adev)
+		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);
+
+	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 const 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 const struct gmin_cfg_var t100_vars[] = {
+	{ "INT33F0:00_CsiPort",  "0" },
+	{ "INT33F0:00_CsiLanes", "1" },
+	{ "INT33F0:00_CamClk",   "1" },
+	{},
+};
+
+static const 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 const 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 const 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"},
+        {},
+};
+
+static const struct {
+	const char *dmi_board_name;
+	const struct gmin_cfg_var *vars;
+} hard_vars[] = {
+	{ "BYT-T FFD8", ffrd8_vars },
+	{ "T100TA", t100_vars },
+        { "MRD7", mrd7_vars },
+        { "ST70408", ecs7_vars },
+        { "VTA0803", i8880_vars },
+};
+
+
+#define GMIN_CFG_VAR_EFI_GUID EFI_GUID(0xecb54cd9, 0xe5ae, 0x4fdc, \
+				       0xa9, 0x71, 0xe8, 0x77,	   \
+				       0x75, 0x60, 0x68, 0xf7)
+
+#define CFG_VAR_NAME_MAX 64
+
+static int gmin_platform_init(struct i2c_client *client)
+{
+	return 0;
+}
+
+static int gmin_platform_deinit(void)
+{
+	return 0;
+}
+
+static struct gmin_subdev *gmin_subdev_add(struct v4l2_subdev *subdev)
+{
+	int i, ret;
+	struct device *dev;
+        struct i2c_client *client = v4l2_get_subdevdata(subdev);
+
+	if (!pmic_id) {
+
+			pmic_id = PMIC_REGULATOR;
+	}
+
+	if (!client)
+		return NULL;
+
+	dev = &client->dev;
+
+	for (i=0; i < MAX_SUBDEVS && gmin_subdevs[i].subdev; i++)
+		;
+	if (i >= MAX_SUBDEVS)
+		return NULL;
+
+	dev_info(dev,
+		"gmin: initializing atomisp module subdev data.PMIC ID %d\n",
+		pmic_id);
+
+	gmin_subdevs[i].subdev = subdev;
+	gmin_subdevs[i].clock_num = gmin_get_var_int(dev, "CamClk", 0);
+	/*WA:CHT requires XTAL clock as PLL is not stable.*/
+	gmin_subdevs[i].clock_src = gmin_get_var_int(dev, "ClkSrc",
+							VLV2_CLK_PLL_19P2MHZ);
+	gmin_subdevs[i].csi_port = gmin_get_var_int(dev, "CsiPort", 0);
+	gmin_subdevs[i].csi_lanes = gmin_get_var_int(dev, "CsiLanes", 1);
+	gmin_subdevs[i].gpio0 = gpiod_get_index(dev, NULL, 0, GPIOD_OUT_LOW);
+	gmin_subdevs[i].gpio1 = gpiod_get_index(dev, NULL, 1, GPIOD_OUT_LOW);
+
+	if (!IS_ERR(gmin_subdevs[i].gpio0)) {
+		ret = gpiod_direction_output(gmin_subdevs[i].gpio0, 0);
+		if (ret)
+			dev_err(dev, "gpio0 set output failed: %d\n", ret);
+	} else {
+		gmin_subdevs[i].gpio0 = NULL;
+	}
+
+	if (!IS_ERR(gmin_subdevs[i].gpio1)) {
+		ret = gpiod_direction_output(gmin_subdevs[i].gpio1, 0);
+		if (ret)
+			dev_err(dev, "gpio1 set output failed: %d\n", ret);
+	} else {
+		gmin_subdevs[i].gpio1 = NULL;
+	}
+
+	if (pmic_id == PMIC_REGULATOR) {
+		gmin_subdevs[i].v1p8_reg = regulator_get(dev, "V1P8SX");
+		gmin_subdevs[i].v2p8_reg = regulator_get(dev, "V2P8SX");
+		gmin_subdevs[i].v1p2_reg = regulator_get(dev, "V1P2A");
+		gmin_subdevs[i].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. */
+	}
+
+	return &gmin_subdevs[i];
+}
+
+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 gmin_subdev_add(subdev);
+}
+
+static int gmin_gpio0_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	if (gs && gs->gpio0) {
+		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 && gs->gpio1) {
+		gpiod_set_value(gs->gpio1, on);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+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;
+
+	if (gs->v1p2_reg) {
+		if (on)
+			return regulator_enable(gs->v1p2_reg);
+		else
+			return regulator_disable(gs->v1p2_reg);
+	}
+
+	/*TODO:v1p2 needs to extend to other PMICs*/
+
+	return -EINVAL;
+}
+int gmin_v1p8_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	int ret;
+
+	if (v1p8_gpio == V1P8_GPIO_UNSET) {
+		v1p8_gpio = gmin_get_var_int(NULL, "V1P8GPIO", V1P8_GPIO_NONE);
+		if (v1p8_gpio != V1P8_GPIO_NONE) {
+			pr_info("atomisp_gmin_platform: 1.8v power on GPIO %d\n",
+				v1p8_gpio);
+			ret = gpio_request(v1p8_gpio, "camera_v1p8_en");
+			if (!ret)
+				ret = gpio_direction_output(v1p8_gpio, 0);
+			if (ret)
+				pr_err("V1P8 GPIO initialization failed\n");
+		}
+	}
+
+	if (gs && gs->v1p8_on == on)
+		return 0;
+	gs->v1p8_on = on;
+
+	if (v1p8_gpio >= 0)
+		gpio_set_value(v1p8_gpio, on);
+
+	if (gs->v1p8_reg) {
+           regulator_set_voltage(gs->v1p8_reg, 1800000, 1800000);
+		if (on)
+			return regulator_enable(gs->v1p8_reg);
+		else
+			return regulator_disable(gs->v1p8_reg);
+	}
+
+	return -EINVAL;
+}
+
+int gmin_v2p8_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	int ret;
+
+	if (v2p8_gpio == V2P8_GPIO_UNSET) {
+		v2p8_gpio = gmin_get_var_int(NULL, "V2P8GPIO", V2P8_GPIO_NONE);
+		if (v2p8_gpio != V2P8_GPIO_NONE) {
+			pr_info("atomisp_gmin_platform: 2.8v power on GPIO %d\n",
+				v2p8_gpio);
+			ret = gpio_request(v2p8_gpio, "camera_v2p8");
+			if (!ret)
+				ret = gpio_direction_output(v2p8_gpio, 0);
+			if (ret)
+				pr_err("V2P8 GPIO initialization failed\n");
+		}
+	}
+
+	if (gs && gs->v2p8_on == on)
+		return 0;
+	gs->v2p8_on = on;
+
+	if (v2p8_gpio >= 0)
+		gpio_set_value(v2p8_gpio, on);
+
+	if (gs->v2p8_reg) {
+           regulator_set_voltage(gs->v2p8_reg, 2900000, 2900000);
+		if (on)
+			return regulator_enable(gs->v2p8_reg);
+		else
+			return regulator_disable(gs->v2p8_reg);
+	}
+
+	return -EINVAL;
+}
+
+int gmin_flisclk_ctrl(struct v4l2_subdev *subdev, int on)
+{
+	int ret = 0;
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	if (on)
+		ret = vlv2_plat_set_clock_freq(gs->clock_num, gs->clock_src);
+	if (ret)
+		return ret;
+	return vlv2_plat_configure_clock(gs->clock_num,
+					 on ? VLV2_CLK_ON : VLV2_CLK_OFF);
+}
+
+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;
+
+	return camera_sensor_csi(sd, gs->csi_port, gs->csi_lanes,
+				 gs->csi_fmt, gs->csi_bayer, flag);
+}
+
+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 == NULL || gs == NULL)
+		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 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,
+	.platform_init = gmin_platform_init,
+	.platform_deinit = gmin_platform_deinit,
+	.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)
+{
+	struct gmin_subdev *gs = find_gmin_subdev(subdev);
+	gs->csi_fmt = csi_format;
+	gs->csi_bayer = csi_bayer;
+
+	return &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);
+
+/* 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. */
+int gmin_get_config_var(struct device *dev, const char *var, char *out, size_t *out_len)
+{
+	char var8[CFG_VAR_NAME_MAX];
+	efi_char16_t var16[CFG_VAR_NAME_MAX];
+	struct efivar_entry *ev;
+	u32 efiattr_dummy;
+	int i, j, ret;
+	unsigned long efilen;
+
+        if (dev && ACPI_COMPANION(dev))
+                dev = &ACPI_COMPANION(dev)->dev;
+
+        if (dev)
+                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. */
+	for (i = 0; i < ARRAY_SIZE(hard_vars); i++) {
+		if (dmi_match(DMI_BOARD_NAME, hard_vars[i].dmi_board_name)) {
+			for (j = 0; hard_vars[i].vars[j].name; j++) {
+				size_t vl;
+				const struct gmin_cfg_var *gv;
+
+				gv = &hard_vars[i].vars[j];
+				vl = strlen(gv->val);
+
+				if (strcmp(var8, gv->name))
+					continue;
+				if (vl > *out_len - 1)
+					return -ENOSPC;
+
+				memcpy(out, gv->val, min(*out_len, vl+1));
+				out[*out_len-1] = 0;
+				*out_len = vl;
+
+				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];
+
+	/* To avoid owerflows when calling the efivar API */
+	if (*out_len > ULONG_MAX)
+		return -EINVAL;
+
+	/* Not sure this API usage is kosher; efivar_entry_get()'s
+	 * implementation simply uses VariableName and VendorGuid from
+	 * the struct and ignores the rest, but it seems like there
+	 * ought to be an "official" efivar_entry registered
+	 * somewhere? */
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+	memcpy(&ev->var.VariableName, var16, sizeof(var16));
+	ev->var.VendorGuid = GMIN_CFG_VAR_EFI_GUID;
+
+	efilen = *out_len;
+	ret = efivar_entry_get(ev, &efiattr_dummy, &efilen, out);
+
+	kfree(ev);
+	*out_len = efilen;
+
+	if (ret)
+ 		dev_warn(dev, "Failed to find gmin variable %s\n", var8);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(gmin_get_config_var);
+
+int gmin_get_var_int(struct device *dev, 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, var, val, &len);
+	if (!ret) {
+		val[len] = 0;
+		ret = kstrtol(val, 0, &result);
+	}
+
+	return ret ? def : result;
+}
+EXPORT_SYMBOL_GPL(gmin_get_var_int);
+
+int camera_sensor_csi(struct v4l2_subdev *sd, u32 port,
+		      u32 lanes, u32 format, u32 bayer_order, int flag)
+{
+        struct i2c_client *client = v4l2_get_subdevdata(sd);
+        struct camera_mipi_info *csi = NULL;
+
+        if (flag) {
+                csi = kzalloc(sizeof(*csi), GFP_KERNEL);
+                if (!csi) {
+                        dev_err(&client->dev, "out of memory\n");
+                        return -ENOMEM;
+                }
+                csi->port = port;
+                csi->num_lanes = lanes;
+                csi->input_format = format;
+                csi->raw_bayer_order = bayer_order;
+                v4l2_set_subdev_hostdata(sd, (void *)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);
+        } else {
+                csi = v4l2_get_subdev_hostdata(sd);
+                kfree(csi);
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(camera_sensor_csi);
+
+/* 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 *dev)
+{
+	dev_info(&dev->dev, "Disabling PCI power management on camera ISP\n");
+	dev->pm_cap = 0;
+}
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, 0x0f38, isp_pm_cap_fixup);
diff --git a/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c b/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c
new file mode 100644
index 000000000000..a6c0f5f8c3f8
--- /dev/null
+++ b/drivers/staging/media/atomisp/platform/intel-mid/intel_mid_pcihelpers.c
@@ -0,0 +1,297 @@
+#include <linux/export.h>
+#include <linux/pci.h>
+#include <linux/pm_qos.h>
+#include <linux/delay.h>
+
+/* G-Min addition: "platform_is()" lives in intel_mid_pm.h in the MCG
+ * tree, but it's just platform ID info and we don't want to pull in
+ * the whole SFI-based PM architecture. */
+#define INTEL_ATOM_MRST 0x26
+#define INTEL_ATOM_MFLD 0x27
+#define INTEL_ATOM_CLV 0x35
+#define INTEL_ATOM_MRFLD 0x4a
+#define INTEL_ATOM_BYT 0x37
+#define INTEL_ATOM_MOORFLD 0x5a
+#define INTEL_ATOM_CHT 0x4c
+/* synchronization for sharing the I2C controller */
+#define PUNIT_PORT	0x04
+#define PUNIT_DOORBELL_OPCODE	(0xE0)
+#define PUNIT_DOORBELL_REG	(0x0)
+#ifndef CSTATE_EXIT_LATENCY
+#define CSTATE_EXIT_LATENCY_C1 1
+#endif
+static inline int platform_is(u8 model)
+{
+        return (boot_cpu_data.x86_model == model);
+}
+
+#include "../../include/asm/intel_mid_pcihelpers.h"
+
+/* Unified message bus read/write operation */
+static DEFINE_SPINLOCK(msgbus_lock);
+
+static struct pci_dev *pci_root;
+static struct pm_qos_request pm_qos;
+int qos;
+
+#define DW_I2C_NEED_QOS	(platform_is(INTEL_ATOM_BYT))
+
+static int intel_mid_msgbus_init(void)
+{
+	pci_root = pci_get_bus_and_slot(0, PCI_DEVFN(0, 0));
+	if (!pci_root) {
+		pr_err("%s: Error: msgbus PCI handle NULL\n", __func__);
+		return -ENODEV;
+	}
+
+	if (DW_I2C_NEED_QOS) {
+		pm_qos_add_request(&pm_qos,
+			PM_QOS_CPU_DMA_LATENCY,
+			PM_QOS_DEFAULT_VALUE);
+	}
+	return 0;
+}
+fs_initcall(intel_mid_msgbus_init);
+
+u32 intel_mid_msgbus_read32_raw(u32 cmd)
+{
+	unsigned long irq_flags;
+	u32 data;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	pci_read_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, &data);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+
+	return data;
+}
+EXPORT_SYMBOL(intel_mid_msgbus_read32_raw);
+
+/*
+ * GU: this function is only used by the VISA and 'VXD' drivers.
+ */
+u32 intel_mid_msgbus_read32_raw_ext(u32 cmd, u32 cmd_ext)
+{
+	unsigned long irq_flags;
+	u32 data;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_EXT_REG, cmd_ext);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	pci_read_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, &data);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+
+	return data;
+}
+EXPORT_SYMBOL(intel_mid_msgbus_read32_raw_ext);
+
+void intel_mid_msgbus_write32_raw(u32 cmd, u32 data)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, data);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+}
+EXPORT_SYMBOL(intel_mid_msgbus_write32_raw);
+
+/*
+ * GU: this function is only used by the VISA and 'VXD' drivers.
+ */
+void intel_mid_msgbus_write32_raw_ext(u32 cmd, u32 cmd_ext, u32 data)
+{
+	unsigned long irq_flags;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, data);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_EXT_REG, cmd_ext);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+}
+EXPORT_SYMBOL(intel_mid_msgbus_write32_raw_ext);
+
+u32 intel_mid_msgbus_read32(u8 port, u32 addr)
+{
+	unsigned long irq_flags;
+	u32 data;
+	u32 cmd;
+	u32 cmdext;
+
+	cmd = (PCI_ROOT_MSGBUS_READ << 24) | (port << 16) |
+		((addr & 0xff) << 8) | PCI_ROOT_MSGBUS_DWORD_ENABLE;
+	cmdext = addr & 0xffffff00;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+
+	if (cmdext) {
+		/* This resets to 0 automatically, no need to write 0 */
+		pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_EXT_REG,
+					cmdext);
+	}
+
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	pci_read_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, &data);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+
+	return data;
+}
+
+EXPORT_SYMBOL(intel_mid_msgbus_read32);
+void intel_mid_msgbus_write32(u8 port, u32 addr, u32 data)
+{
+	unsigned long irq_flags;
+	u32 cmd;
+	u32 cmdext;
+
+	cmd = (PCI_ROOT_MSGBUS_WRITE << 24) | (port << 16) |
+		((addr & 0xFF) << 8) | PCI_ROOT_MSGBUS_DWORD_ENABLE;
+	cmdext = addr & 0xffffff00;
+
+	spin_lock_irqsave(&msgbus_lock, irq_flags);
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_DATA_REG, data);
+
+	if (cmdext) {
+		/* This resets to 0 automatically, no need to write 0 */
+		pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_EXT_REG,
+					cmdext);
+	}
+
+	pci_write_config_dword(pci_root, PCI_ROOT_MSGBUS_CTRL_REG, cmd);
+	spin_unlock_irqrestore(&msgbus_lock, irq_flags);
+}
+EXPORT_SYMBOL(intel_mid_msgbus_write32);
+
+/* called only from where is later then fs_initcall */
+u32 intel_mid_soc_stepping(void)
+{
+	return pci_root->revision;
+}
+EXPORT_SYMBOL(intel_mid_soc_stepping);
+
+static bool is_south_complex_device(struct pci_dev *dev)
+{
+	unsigned base_class = dev->class >> 16;
+	unsigned sub_class  = (dev->class & SUB_CLASS_MASK) >> 8;
+
+	/* other than camera, pci bridges and display,
+	 * everything else are south complex devices.
+	 */
+	if (((base_class == PCI_BASE_CLASS_MULTIMEDIA) &&
+	     (sub_class == ISP_SUB_CLASS)) ||
+	    (base_class == PCI_BASE_CLASS_BRIDGE) ||
+	    ((base_class == PCI_BASE_CLASS_DISPLAY) && !sub_class))
+		return false;
+	else
+		return true;
+}
+
+/* In BYT platform, d3_delay for internal south complex devices,
+ * they are not subject to 10 ms d3 to d0 delay required by pci spec.
+ */
+static void pci_d3_delay_fixup(struct pci_dev *dev)
+{
+	if (platform_is(INTEL_ATOM_BYT) ||
+		platform_is(INTEL_ATOM_CHT)) {
+		/* All internal devices are in bus 0. */
+		if (dev->bus->number == 0 && is_south_complex_device(dev)) {
+			dev->d3_delay = INTERNAL_PCI_PM_D3_WAIT;
+			dev->d3cold_delay = INTERNAL_PCI_PM_D3_WAIT;
+		}
+	}
+}
+DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_INTEL, PCI_ANY_ID, pci_d3_delay_fixup);
+
+#define PUNIT_SEMAPHORE	(platform_is(INTEL_ATOM_BYT) ? 0x7 : 0x10E)
+#define GET_SEM() (intel_mid_msgbus_read32(PUNIT_PORT, PUNIT_SEMAPHORE) & 0x1)
+
+static void reset_semaphore(void)
+{
+	u32 data;
+
+	data = intel_mid_msgbus_read32(PUNIT_PORT, PUNIT_SEMAPHORE);
+	smp_mb();
+	data = data & 0xfffffffc;
+	intel_mid_msgbus_write32(PUNIT_PORT, PUNIT_SEMAPHORE, data);
+	smp_mb();
+
+}
+
+int intel_mid_dw_i2c_acquire_ownership(void)
+{
+	u32 ret = 0;
+	u32 data = 0; /* data sent to PUNIT */
+	u32 cmd;
+	u32 cmdext;
+	int timeout = 1000;
+
+	if (DW_I2C_NEED_QOS)
+		pm_qos_update_request(&pm_qos, CSTATE_EXIT_LATENCY_C1 - 1);
+
+	/*
+	 * We need disable irq. Otherwise, the main thread
+	 * might be preempted and the other thread jumps to
+	 * disable irq for a long time. Another case is
+	 * some irq handlers might trigger power voltage change
+	 */
+	BUG_ON(irqs_disabled());
+	local_irq_disable();
+
+	/* host driver writes 0x2 to side band register 0x7 */
+	intel_mid_msgbus_write32(PUNIT_PORT, PUNIT_SEMAPHORE, 0x2);
+	smp_mb();
+
+	/* host driver sends 0xE0 opcode to PUNIT and writes 0 register */
+	cmd = (PUNIT_DOORBELL_OPCODE << 24) | (PUNIT_PORT << 16) |
+	((PUNIT_DOORBELL_REG & 0xFF) << 8) | PCI_ROOT_MSGBUS_DWORD_ENABLE;
+	cmdext = PUNIT_DOORBELL_REG & 0xffffff00;
+
+	if (cmdext)
+		intel_mid_msgbus_write32_raw_ext(cmd, cmdext, data);
+	else
+		intel_mid_msgbus_write32_raw(cmd, data);
+
+	/* host driver waits for bit 0 to be set in side band 0x7 */
+	while (GET_SEM() != 0x1) {
+		udelay(100);
+		timeout--;
+		if (timeout <= 0) {
+			pr_err("Timeout: semaphore timed out, reset sem\n");
+			ret = -ETIMEDOUT;
+			reset_semaphore();
+			/*Delay 1ms in case race with punit*/
+			udelay(1000);
+			if (GET_SEM() != 0) {
+				/*Reset again as kernel might race with punit*/
+				reset_semaphore();
+			}
+			pr_err("PUNIT SEM: %d\n",
+					intel_mid_msgbus_read32(PUNIT_PORT,
+						PUNIT_SEMAPHORE));
+			local_irq_enable();
+
+			if (DW_I2C_NEED_QOS) {
+				pm_qos_update_request(&pm_qos,
+					 PM_QOS_DEFAULT_VALUE);
+			}
+
+			return ret;
+		}
+	}
+	smp_mb();
+
+	return ret;
+}
+EXPORT_SYMBOL(intel_mid_dw_i2c_acquire_ownership);
+
+int intel_mid_dw_i2c_release_ownership(void)
+{
+	reset_semaphore();
+	local_irq_enable();
+
+	if (DW_I2C_NEED_QOS)
+		pm_qos_update_request(&pm_qos, PM_QOS_DEFAULT_VALUE);
+
+	return 0;
+}
+EXPORT_SYMBOL(intel_mid_dw_i2c_release_ownership);
diff --git a/drivers/staging/media/cxd2099/cxd2099.c b/drivers/staging/media/cxd2099/cxd2099.c
index c72c3f09f175..18186d0fa1a6 100644
--- a/drivers/staging/media/cxd2099/cxd2099.c
+++ b/drivers/staging/media/cxd2099/cxd2099.c
@@ -131,7 +131,6 @@ static int read_reg(struct cxd *ci, u8 reg, u8 *val)
 	return read_block(ci, reg, val, 1);
 }
 
-
 static int read_pccard(struct cxd *ci, u16 address, u8 *data, u8 n)
 {
 	int status;
@@ -152,8 +151,8 @@ static int write_pccard(struct cxd *ci, u16 address, u8 *data, u8 n)
 	if (!status) {
 		u8 buf[256] = {3};
 
-		memcpy(buf+1, data, n);
-		status = i2c_write(ci->i2c, ci->cfg.adr, buf, n+1);
+		memcpy(buf + 1, data, n);
+		status = i2c_write(ci->i2c, ci->cfg.adr, buf, n + 1);
 	}
 	return status;
 }
@@ -181,34 +180,6 @@ static int write_io(struct cxd *ci, u16 address, u8 val)
 	return status;
 }
 
-#if 0
-static int read_io_data(struct cxd *ci, u8 *data, u8 n)
-{
-	int status;
-	u8 addr[3] = { 2, 0, 0 };
-
-	status = i2c_write(ci->i2c, ci->cfg.adr, addr, 3);
-	if (!status)
-		status = i2c_read(ci->i2c, ci->cfg.adr, 3, data, n);
-	return 0;
-}
-
-static int write_io_data(struct cxd *ci, u8 *data, u8 n)
-{
-	int status;
-	u8 addr[3] = {2, 0, 0};
-
-	status = i2c_write(ci->i2c, ci->cfg.adr, addr, 3);
-	if (!status) {
-		u8 buf[256] = {3};
-
-		memcpy(buf+1, data, n);
-		status = i2c_write(ci->i2c, ci->cfg.adr, buf, n + 1);
-	}
-	return 0;
-}
-#endif
-
 static int write_regm(struct cxd *ci, u8 reg, u8 val, u8 mask)
 {
 	int status;
@@ -292,8 +263,6 @@ static void cam_mode(struct cxd *ci, int mode)
 	ci->cammode = mode;
 }
 
-
-
 static int init(struct cxd *ci)
 {
 	int status;
@@ -329,12 +298,6 @@ static int init(struct cxd *ci)
 		if (status < 0)
 			break;
 
-#if 0
-		/* Input Mode C, BYPass Serial, TIVAL = low, MSB */
-		status = write_reg(ci, 0x09, 0x4D);
-		if (status < 0)
-			break;
-#endif
 		/* TOSTRT = 8, Mode B (gated clock), falling Edge,
 		 * Serial, POL=HIGH, MSB
 		 */
@@ -432,23 +395,6 @@ static int read_attribute_mem(struct dvb_ca_en50221 *ca,
 			      int slot, int address)
 {
 	struct cxd *ci = ca->data;
-#if 0
-	if (ci->amem_read) {
-		if (address <= 0 || address > 1024)
-			return -EIO;
-		return ci->amem[address];
-	}
-
-	mutex_lock(&ci->lock);
-	write_regm(ci, 0x06, 0x00, 0x05);
-	read_pccard(ci, 0, &ci->amem[0], 128);
-	read_pccard(ci, 128, &ci->amem[0], 128);
-	read_pccard(ci, 256, &ci->amem[0], 128);
-	read_pccard(ci, 384, &ci->amem[0], 128);
-	write_regm(ci, 0x06, 0x05, 0x05);
-	mutex_unlock(&ci->lock);
-	return ci->amem[address];
-#else
 	u8 val;
 
 	mutex_lock(&ci->lock);
@@ -457,7 +403,6 @@ static int read_attribute_mem(struct dvb_ca_en50221 *ca,
 	mutex_unlock(&ci->lock);
 	/* printk(KERN_INFO "%02x:%02x\n", address,val); */
 	return val;
-#endif
 }
 
 static int write_attribute_mem(struct dvb_ca_en50221 *ca, int slot,
@@ -502,15 +447,6 @@ static int slot_reset(struct dvb_ca_en50221 *ca, int slot)
 	struct cxd *ci = ca->data;
 
 	mutex_lock(&ci->lock);
-#if 0
-	write_reg(ci, 0x00, 0x21);
-	write_reg(ci, 0x06, 0x1F);
-	write_reg(ci, 0x00, 0x31);
-#else
-#if 0
-	write_reg(ci, 0x06, 0x1F);
-	write_reg(ci, 0x06, 0x2F);
-#else
 	cam_mode(ci, 0);
 	write_reg(ci, 0x00, 0x21);
 	write_reg(ci, 0x06, 0x1F);
@@ -518,25 +454,14 @@ static int slot_reset(struct dvb_ca_en50221 *ca, int slot)
 	write_regm(ci, 0x20, 0x80, 0x80);
 	write_reg(ci, 0x03, 0x02);
 	ci->ready = 0;
-#endif
-#endif
 	ci->mode = -1;
 	{
 		int i;
-#if 0
-		u8 val;
-#endif
+
 		for (i = 0; i < 100; i++) {
 			usleep_range(10000, 11000);
-#if 0
-			read_reg(ci, 0x06, &val);
-			dev_info(&ci->i2c->dev, "%d:%02x\n", i, val);
-			if (!(val&0x10))
-				break;
-#else
 			if (ci->ready)
 				break;
-#endif
 		}
 	}
 	mutex_unlock(&ci->lock);
@@ -572,7 +497,6 @@ static int slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
 	return 0;
 }
 
-
 static int campoll(struct cxd *ci)
 {
 	u8 istat;
@@ -582,18 +506,18 @@ static int campoll(struct cxd *ci)
 		return 0;
 	write_reg(ci, 0x05, istat);
 
-	if (istat&0x40) {
+	if (istat & 0x40) {
 		ci->dr = 1;
 		dev_info(&ci->i2c->dev, "DR\n");
 	}
-	if (istat&0x20)
+	if (istat & 0x20)
 		dev_info(&ci->i2c->dev, "WC\n");
 
-	if (istat&2) {
+	if (istat & 2) {
 		u8 slotstat;
 
 		read_reg(ci, 0x01, &slotstat);
-		if (!(2&slotstat)) {
+		if (!(2 & slotstat)) {
 			if (!ci->slot_stat) {
 				ci->slot_stat = DVB_CA_EN50221_POLL_CAM_PRESENT;
 				write_regm(ci, 0x03, 0x08, 0x08);
@@ -607,7 +531,7 @@ static int campoll(struct cxd *ci)
 				ci->ready = 0;
 			}
 		}
-		if (istat&8 &&
+		if (istat & 8 &&
 		    ci->slot_stat == DVB_CA_EN50221_POLL_CAM_PRESENT) {
 			ci->ready = 1;
 			ci->slot_stat |= DVB_CA_EN50221_POLL_CAM_READY;
@@ -616,7 +540,6 @@ static int campoll(struct cxd *ci)
 	return 0;
 }
 
-
 static int poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open)
 {
 	struct cxd *ci = ca->data;
@@ -648,7 +571,7 @@ static int read_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
 	mutex_lock(&ci->lock);
 	read_reg(ci, 0x0f, &msb);
 	read_reg(ci, 0x10, &lsb);
-	len = (msb<<8)|lsb;
+	len = (msb << 8) | lsb;
 	read_block(ci, 0x12, ebuf, len);
 	ci->dr = 0;
 	mutex_unlock(&ci->lock);
@@ -662,8 +585,8 @@ static int write_data(struct dvb_ca_en50221 *ca, int slot, u8 *ebuf, int ecount)
 
 	mutex_lock(&ci->lock);
 	dev_info(&ci->i2c->dev, "write_data %d\n", ecount);
-	write_reg(ci, 0x0d, ecount>>8);
-	write_reg(ci, 0x0e, ecount&0xff);
+	write_reg(ci, 0x0d, ecount >> 8);
+	write_reg(ci, 0x0e, ecount & 0xff);
 	write_block(ci, 0x11, ebuf, ecount);
 	mutex_unlock(&ci->lock);
 	return ecount;
@@ -698,7 +621,7 @@ struct dvb_ca_en50221 *cxd2099_attach(struct cxd2099_cfg *cfg,
 		return NULL;
 	}
 
-	ci = kzalloc(sizeof(struct cxd), GFP_KERNEL);
+	ci = kzalloc(sizeof(*ci), GFP_KERNEL);
 	if (!ci)
 		return NULL;
 
diff --git a/drivers/staging/media/davinci_vpfe/davinci_vpfe_user.h b/drivers/staging/media/davinci_vpfe/davinci_vpfe_user.h
index d3f34f9bf712..7cc115c9ebe6 100644
--- a/drivers/staging/media/davinci_vpfe/davinci_vpfe_user.h
+++ b/drivers/staging/media/davinci_vpfe/davinci_vpfe_user.h
@@ -155,8 +155,8 @@ struct vpfe_isif_dfc {
 };
 
 /************************************************************************
-*   Digital/Black clamp or DC Subtract parameters
-************************************************************************/
+ *   Digital/Black clamp or DC Subtract parameters
+ ************************************************************************/
 /**
  * Horizontal Black Clamp modes
  */
@@ -309,8 +309,8 @@ struct vpfe_isif_black_clamp {
 };
 
 /*************************************************************************
-** Color Space Conversion (CSC)
-*************************************************************************/
+ ** Color Space Conversion (CSC)
+ *************************************************************************/
 /**
  * Number of Coefficient values used for CSC
  */
@@ -331,8 +331,8 @@ struct float_16_bit {
 };
 
 /*************************************************************************
-**  Color Space Conversion parameters
-*************************************************************************/
+ **  Color Space Conversion parameters
+ *************************************************************************/
 /**
  * Structure used for CSC config params
  */
@@ -365,8 +365,8 @@ enum vpfe_isif_datasft {
 
 #define VPFE_ISIF_LINEAR_TAB_SIZE		192
 /*************************************************************************
-**  Linearization parameters
-*************************************************************************/
+ **  Linearization parameters
+ *************************************************************************/
 /**
  * Structure for Sensor data linearization
  */
@@ -382,8 +382,8 @@ struct vpfe_isif_linearize {
 };
 
 /*************************************************************************
-**  ISIF Raw configuration parameters
-*************************************************************************/
+ **  ISIF Raw configuration parameters
+ *************************************************************************/
 enum vpfe_isif_fmt_mode {
 	VPFE_ISIF_SPLIT,
 	VPFE_ISIF_COMBINE
@@ -1189,8 +1189,8 @@ struct vpfe_ipipe_config {
 };
 
 /*******************************************************************
-**  Resizer API structures
-*******************************************************************/
+ **  Resizer API structures
+ *******************************************************************/
 /* Interpolation types used for horizontal rescale */
 enum vpfe_rsz_intp_t {
 	VPFE_RSZ_INTP_CUBIC,
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
index ff47a8f369fc..6a3434cebd79 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
@@ -1803,14 +1803,14 @@ vpfe_ipipe_init(struct vpfe_ipipe_device *ipipe, struct platform_device *pdev)
 		return -EBUSY;
 	ipipe->base_addr = ioremap_nocache(res->start, res_len);
 	if (!ipipe->base_addr)
-		return -EBUSY;
+		goto error_release;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 6);
 	if (!res)
-		return -ENOENT;
+		goto error_unmap;
 	ipipe->isp5_base_addr = ioremap_nocache(res->start, res_len);
 	if (!ipipe->isp5_base_addr)
-		return -EBUSY;
+		goto error_unmap;
 
 	v4l2_subdev_init(sd, &ipipe_v4l2_ops);
 	sd->internal_ops = &ipipe_v4l2_internal_ops;
@@ -1839,6 +1839,12 @@ vpfe_ipipe_init(struct vpfe_ipipe_device *ipipe, struct platform_device *pdev)
 	sd->ctrl_handler = &ipipe->ctrls;
 
 	return media_entity_pads_init(me, IPIPE_PADS_NUM, pads);
+
+error_unmap:
+	iounmap(ipipe->base_addr);
+error_release:
+	release_mem_region(res->start, res_len);
+	return -ENOMEM;
 }
 
 /*
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
index 958ef71ee4d5..a893072d0f04 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
@@ -1003,8 +1003,8 @@ void ipipe_set_car_regs(void __iomem *base_addr, struct vpfe_ipipe_car *car)
 		ipipe_set_mf(base_addr);
 		ipipe_set_gain_ctrl(base_addr, car);
 		/* Set the threshold for switching between
-		  * the two Here we overwrite the MF SW0 value
-		  */
+		 * the two Here we overwrite the MF SW0 value
+		 */
 		regw_ip(base_addr, VPFE_IPIPE_CAR_DYN_SWITCH, CAR_TYP);
 		val = car->sw1;
 		val <<= CAR_SW1_SHIFT;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif_regs.h b/drivers/staging/media/davinci_vpfe/dm365_isif_regs.h
index 8aceabb43f8e..64fbb459baa2 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif_regs.h
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif_regs.h
@@ -59,8 +59,8 @@
 #define REC656IF				0x84
 #define CCDCFG					0x88
 /*****************************************************
-* Defect Correction registers
-*****************************************************/
+ * Defect Correction registers
+ *****************************************************/
 #define DFCCTL					0x8c
 #define VDFSATLV				0x90
 #define DFCMEMCTL				0x94
@@ -70,8 +70,8 @@
 #define DFCMEM3					0xa4
 #define DFCMEM4					0xa8
 /****************************************************
-* Black Clamp registers
-****************************************************/
+ * Black Clamp registers
+ ****************************************************/
 #define CLAMPCFG				0xac
 #define CLDCOFST				0xb0
 #define CLSV					0xb4
@@ -84,8 +84,8 @@
 #define CLVWIN2					0xd0
 #define CLVWIN3					0xd4
 /****************************************************
-* Lense Shading Correction
-****************************************************/
+ * Lense Shading Correction
+ ****************************************************/
 #define DATAHOFST				0xd8
 #define DATAVOFST				0xdc
 #define LSCHVAL					0xe0
@@ -102,8 +102,8 @@
 #define TWODLSCIRQEN				0x10c
 #define TWODLSCIRQST				0x110
 /****************************************************
-* Data formatter
-****************************************************/
+ * Data formatter
+ ****************************************************/
 #define FMTCFG					0x114
 #define FMTPLEN					0x118
 #define FMTSPH					0x11c
@@ -128,8 +128,8 @@
 #define FMTPGMAPS6				0x19c
 #define FMTPGMAPS7				0x1a0
 /************************************************
-* Color Space Converter
-************************************************/
+ * Color Space Converter
+ ************************************************/
 #define CSCCTL					0x1a4
 #define CSCM0					0x1a8
 #define CSCM1					0x1ac
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index 5fbc2d447ff2..857b0e847c5e 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -1133,9 +1133,9 @@ void vpfe_resizer_buffer_isr(struct vpfe_resizer_device *resizer)
 		}
 	} else if (fid == 0) {
 		/*
-		* out of sync. Recover from any hardware out-of-sync.
-		* May loose one frame
-		*/
+		 * out of sync. Recover from any hardware out-of-sync.
+		 * May loose one frame
+		 */
 		video_out->field_id = fid;
 	}
 }
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index 32109cdd73a6..bffe2153b910 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -228,7 +228,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 
 	vpfe_dev->clks = kcalloc(vpfe_cfg->num_clocks,
 				 sizeof(*vpfe_dev->clks), GFP_KERNEL);
-	if (vpfe_dev->clks == NULL)
+	if (!vpfe_dev->clks)
 		return -ENOMEM;
 
 	for (i = 0; i < vpfe_cfg->num_clocks; i++) {
@@ -348,7 +348,7 @@ static int register_i2c_devices(struct vpfe_device *vpfe_dev)
 	vpfe_dev->sd =
 		  kcalloc(num_subdevs, sizeof(struct v4l2_subdev *),
 			  GFP_KERNEL);
-	if (vpfe_dev->sd == NULL)
+	if (!vpfe_dev->sd)
 		return -ENOMEM;
 
 	for (i = 0, k = 0; i < num_subdevs; i++) {
diff --git a/drivers/staging/most/aim-cdev/cdev.c b/drivers/staging/most/aim-cdev/cdev.c
index 7f51024dc5eb..1e5cbc893496 100644
--- a/drivers/staging/most/aim-cdev/cdev.c
+++ b/drivers/staging/most/aim-cdev/cdev.c
@@ -99,11 +99,16 @@ static void destroy_cdev(struct aim_channel *c)
 
 	device_destroy(aim_class, c->devno);
 	cdev_del(&c->cdev);
-	kfifo_free(&c->fifo);
 	spin_lock_irqsave(&ch_list_lock, flags);
 	list_del(&c->list);
 	spin_unlock_irqrestore(&ch_list_lock, flags);
+}
+
+static void destroy_channel(struct aim_channel *c)
+{
 	ida_simple_remove(&minor_id, MINOR(c->devno));
+	kfifo_free(&c->fifo);
+	kfree(c);
 }
 
 /**
@@ -170,9 +175,8 @@ static int aim_close(struct inode *inode, struct file *filp)
 		stop_channel(c);
 		mutex_unlock(&c->io_mutex);
 	} else {
-		destroy_cdev(c);
 		mutex_unlock(&c->io_mutex);
-		kfree(c);
+		destroy_channel(c);
 	}
 	return 0;
 }
@@ -337,14 +341,14 @@ static int aim_disconnect_channel(struct most_interface *iface, int channel_id)
 	spin_lock(&c->unlink);
 	c->dev = NULL;
 	spin_unlock(&c->unlink);
+	destroy_cdev(c);
 	if (c->access_ref) {
 		stop_channel(c);
 		wake_up_interruptible(&c->wq);
 		mutex_unlock(&c->io_mutex);
 	} else {
-		destroy_cdev(c);
 		mutex_unlock(&c->io_mutex);
-		kfree(c);
+		destroy_channel(c);
 	}
 	return 0;
 }
@@ -546,7 +550,7 @@ static void __exit mod_exit(void)
 
 	list_for_each_entry_safe(c, tmp, &channel_list, list) {
 		destroy_cdev(c);
-		kfree(c);
+		destroy_channel(c);
 	}
 	class_destroy(aim_class);
 	unregister_chrdev_region(aim_devno, 1);
diff --git a/drivers/staging/most/aim-sound/sound.c b/drivers/staging/most/aim-sound/sound.c
index e4198e5e064b..ea1366a44008 100644
--- a/drivers/staging/most/aim-sound/sound.c
+++ b/drivers/staging/most/aim-sound/sound.c
@@ -429,7 +429,7 @@ static int pcm_trigger(struct snd_pcm_substream *substream, int cmd)
 		return 0;
 
 	default:
-		pr_info("pcm_trigger(), invalid\n");
+		pr_info("%s(), invalid\n", __func__);
 		return -EINVAL;
 	}
 	return 0;
diff --git a/drivers/staging/most/hdm-dim2/dim2_hal.c b/drivers/staging/most/hdm-dim2/dim2_hal.c
index 0b9816ce1761..d604ec09df28 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hal.c
+++ b/drivers/staging/most/hdm-dim2/dim2_hal.c
@@ -18,6 +18,7 @@
 #include "dim2_errors.h"
 #include "dim2_reg.h"
 #include <linux/stddef.h>
+#include <linux/kernel.h>
 
 /*
  * Size factor for isochronous DBR buffer.
@@ -49,7 +50,7 @@
 #define DBR_SIZE  (16 * 1024) /* specified by IP */
 #define DBR_BLOCK_SIZE  (DBR_SIZE / 32 / DBR_MAP_SIZE)
 
-#define ROUND_UP_TO(x, d)  (((x) + (d) - 1) / (d) * (d))
+#define ROUND_UP_TO(x, d)  (DIV_ROUND_UP(x, (d)) * (d))
 
 /* -------------------------------------------------------------------------- */
 /* generic helper functions and macros */
@@ -117,7 +118,7 @@ static int alloc_dbr(u16 size)
 		return DBR_SIZE; /* out of memory */
 
 	for (i = 0; i < DBR_MAP_SIZE; i++) {
-		u32 const blocks = (size + DBR_BLOCK_SIZE - 1) / DBR_BLOCK_SIZE;
+		u32 const blocks = DIV_ROUND_UP(size, DBR_BLOCK_SIZE);
 		u32 mask = ~((~(u32)0) << blocks);
 
 		do {
@@ -137,7 +138,7 @@ static int alloc_dbr(u16 size)
 static void free_dbr(int offs, int size)
 {
 	int block_idx = offs / DBR_BLOCK_SIZE;
-	u32 const blocks = (size + DBR_BLOCK_SIZE - 1) / DBR_BLOCK_SIZE;
+	u32 const blocks = DIV_ROUND_UP(size, DBR_BLOCK_SIZE);
 	u32 mask = ~((~(u32)0) << blocks);
 
 	mask <<= block_idx % 32;
diff --git a/drivers/staging/most/hdm-usb/hdm_usb.c b/drivers/staging/most/hdm-usb/hdm_usb.c
index 2bfea9b48366..a95b5910d9fc 100644
--- a/drivers/staging/most/hdm-usb/hdm_usb.c
+++ b/drivers/staging/most/hdm-usb/hdm_usb.c
@@ -281,7 +281,6 @@ static int hdm_add_padding(struct most_dev *mdev, int channel, struct mbo *mbo)
 	struct most_channel_config *conf = &mdev->conf[channel];
 	unsigned int frame_size = get_stream_frame_size(conf);
 	unsigned int j, num_frames;
-	u16 rd_addr, wr_addr;
 
 	if (!frame_size)
 		return -EIO;
@@ -293,13 +292,10 @@ static int hdm_add_padding(struct most_dev *mdev, int channel, struct mbo *mbo)
 		return -EIO;
 	}
 
-	for (j = 1; j < num_frames; j++) {
-		wr_addr = (num_frames - j) * USB_MTU;
-		rd_addr = (num_frames - j) * frame_size;
-		memmove(mbo->virt_address + wr_addr,
-			mbo->virt_address + rd_addr,
+	for (j = num_frames - 1; j > 0; j--)
+		memmove(mbo->virt_address + j * USB_MTU,
+			mbo->virt_address + j * frame_size,
 			frame_size);
-	}
 	mbo->buffer_length = num_frames * USB_MTU;
 	return 0;
 }
@@ -649,8 +645,6 @@ static int hdm_configure_channel(struct most_interface *iface, int channel,
 {
 	unsigned int num_frames;
 	unsigned int frame_size;
-	unsigned int temp_size;
-	unsigned int tail_space;
 	struct most_dev *mdev = to_mdev(iface);
 	struct device *dev = &mdev->usb_device->dev;
 
@@ -685,7 +679,6 @@ static int hdm_configure_channel(struct most_interface *iface, int channel,
 	}
 
 	mdev->padding_active[channel] = true;
-	temp_size = conf->buffer_size;
 
 	frame_size = get_stream_frame_size(conf);
 	if (frame_size == 0 || frame_size > USB_MTU) {
@@ -693,25 +686,19 @@ static int hdm_configure_channel(struct most_interface *iface, int channel,
 		return -EINVAL;
 	}
 
+	num_frames = conf->buffer_size / frame_size;
+
 	if (conf->buffer_size % frame_size) {
-		u16 tmp_val;
-
-		tmp_val = conf->buffer_size / frame_size;
-		conf->buffer_size = tmp_val * frame_size;
-		dev_notice(dev,
-			   "Channel %d - rounding buffer size to %d bytes, channel config says %d bytes\n",
-			   channel,
-			   conf->buffer_size,
-			   temp_size);
-	}
+		u16 old_size = conf->buffer_size;
 
-	num_frames = conf->buffer_size / frame_size;
-	tail_space = num_frames * (USB_MTU - frame_size);
-	temp_size += tail_space;
+		conf->buffer_size = num_frames * frame_size;
+		dev_warn(dev, "%s: fixed buffer size (%d -> %d)\n",
+			 mdev->suffix[channel], old_size, conf->buffer_size);
+	}
 
 	/* calculate extra length to comply w/ HW padding */
-	conf->extra_len = (DIV_ROUND_UP(temp_size, USB_MTU) * USB_MTU)
-			  - conf->buffer_size;
+	conf->extra_len = num_frames * (USB_MTU - frame_size);
+
 exit:
 	mdev->conf[channel] = *conf;
 	if (conf->data_type == MOST_CH_ASYNC) {
@@ -1018,7 +1005,7 @@ static ssize_t store_value(struct most_dci_obj *dci_obj,
 		err = drci_wr_reg(usb_dev, dci_obj->reg_addr, val);
 	else if (!strcmp(name, "sync_ep"))
 		err = start_sync_ep(usb_dev, val);
-	else if (!get_static_reg_addr(ro_regs, name, &reg_addr))
+	else if (!get_static_reg_addr(rw_regs, name, &reg_addr))
 		err = drci_wr_reg(usb_dev, reg_addr, val);
 	else
 		return -EFAULT;
diff --git a/drivers/staging/most/mostcore/core.c b/drivers/staging/most/mostcore/core.c
index 191404bc5906..675b2a9e66c1 100644
--- a/drivers/staging/most/mostcore/core.c
+++ b/drivers/staging/most/mostcore/core.c
@@ -127,10 +127,6 @@ struct most_c_attr {
 
 #define to_channel_attr(a) container_of(a, struct most_c_attr, attr)
 
-#define MOST_CHNL_ATTR(_name, _mode, _show, _store) \
-		struct most_c_attr most_chnl_attr_##_name = \
-		__ATTR(_name, _mode, _show, _store)
-
 /**
  * channel_attr_show - show function of channel object
  * @kobj: pointer to its kobject
@@ -256,7 +252,7 @@ static void most_channel_release(struct kobject *kobj)
 	kfree(c);
 }
 
-static ssize_t show_available_directions(struct most_c_obj *c,
+static ssize_t available_directions_show(struct most_c_obj *c,
 					 struct most_c_attr *attr,
 					 char *buf)
 {
@@ -271,7 +267,7 @@ static ssize_t show_available_directions(struct most_c_obj *c,
 	return strlen(buf);
 }
 
-static ssize_t show_available_datatypes(struct most_c_obj *c,
+static ssize_t available_datatypes_show(struct most_c_obj *c,
 					struct most_c_attr *attr,
 					char *buf)
 {
@@ -290,10 +286,9 @@ static ssize_t show_available_datatypes(struct most_c_obj *c,
 	return strlen(buf);
 }
 
-static
-ssize_t show_number_of_packet_buffers(struct most_c_obj *c,
-				      struct most_c_attr *attr,
-				      char *buf)
+static ssize_t number_of_packet_buffers_show(struct most_c_obj *c,
+					     struct most_c_attr *attr,
+					     char *buf)
 {
 	unsigned int i = c->channel_id;
 
@@ -301,10 +296,9 @@ ssize_t show_number_of_packet_buffers(struct most_c_obj *c,
 			c->iface->channel_vector[i].num_buffers_packet);
 }
 
-static
-ssize_t show_number_of_stream_buffers(struct most_c_obj *c,
-				      struct most_c_attr *attr,
-				      char *buf)
+static ssize_t number_of_stream_buffers_show(struct most_c_obj *c,
+					     struct most_c_attr *attr,
+					     char *buf)
 {
 	unsigned int i = c->channel_id;
 
@@ -312,10 +306,9 @@ ssize_t show_number_of_stream_buffers(struct most_c_obj *c,
 			c->iface->channel_vector[i].num_buffers_streaming);
 }
 
-static
-ssize_t show_size_of_packet_buffer(struct most_c_obj *c,
-				   struct most_c_attr *attr,
-				   char *buf)
+static ssize_t size_of_packet_buffer_show(struct most_c_obj *c,
+					  struct most_c_attr *attr,
+					  char *buf)
 {
 	unsigned int i = c->channel_id;
 
@@ -323,10 +316,9 @@ ssize_t show_size_of_packet_buffer(struct most_c_obj *c,
 			c->iface->channel_vector[i].buffer_size_packet);
 }
 
-static
-ssize_t show_size_of_stream_buffer(struct most_c_obj *c,
-				   struct most_c_attr *attr,
-				   char *buf)
+static ssize_t size_of_stream_buffer_show(struct most_c_obj *c,
+					  struct most_c_attr *attr,
+					  char *buf)
 {
 	unsigned int i = c->channel_id;
 
@@ -334,32 +326,21 @@ ssize_t show_size_of_stream_buffer(struct most_c_obj *c,
 			c->iface->channel_vector[i].buffer_size_streaming);
 }
 
-static ssize_t show_channel_starving(struct most_c_obj *c,
+static ssize_t channel_starving_show(struct most_c_obj *c,
 				     struct most_c_attr *attr,
 				     char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", c->is_starving);
 }
 
-#define create_show_channel_attribute(val) \
-	static MOST_CHNL_ATTR(val, 0444, show_##val, NULL)
-
-create_show_channel_attribute(available_directions);
-create_show_channel_attribute(available_datatypes);
-create_show_channel_attribute(number_of_packet_buffers);
-create_show_channel_attribute(number_of_stream_buffers);
-create_show_channel_attribute(size_of_stream_buffer);
-create_show_channel_attribute(size_of_packet_buffer);
-create_show_channel_attribute(channel_starving);
-
-static ssize_t show_set_number_of_buffers(struct most_c_obj *c,
+static ssize_t set_number_of_buffers_show(struct most_c_obj *c,
 					  struct most_c_attr *attr,
 					  char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", c->cfg.num_buffers);
 }
 
-static ssize_t store_set_number_of_buffers(struct most_c_obj *c,
+static ssize_t set_number_of_buffers_store(struct most_c_obj *c,
 					   struct most_c_attr *attr,
 					   const char *buf,
 					   size_t count)
@@ -371,14 +352,14 @@ static ssize_t store_set_number_of_buffers(struct most_c_obj *c,
 	return count;
 }
 
-static ssize_t show_set_buffer_size(struct most_c_obj *c,
+static ssize_t set_buffer_size_show(struct most_c_obj *c,
 				    struct most_c_attr *attr,
 				    char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", c->cfg.buffer_size);
 }
 
-static ssize_t store_set_buffer_size(struct most_c_obj *c,
+static ssize_t set_buffer_size_store(struct most_c_obj *c,
 				     struct most_c_attr *attr,
 				     const char *buf,
 				     size_t count)
@@ -390,7 +371,7 @@ static ssize_t store_set_buffer_size(struct most_c_obj *c,
 	return count;
 }
 
-static ssize_t show_set_direction(struct most_c_obj *c,
+static ssize_t set_direction_show(struct most_c_obj *c,
 				  struct most_c_attr *attr,
 				  char *buf)
 {
@@ -401,7 +382,7 @@ static ssize_t show_set_direction(struct most_c_obj *c,
 	return snprintf(buf, PAGE_SIZE, "unconfigured\n");
 }
 
-static ssize_t store_set_direction(struct most_c_obj *c,
+static ssize_t set_direction_store(struct most_c_obj *c,
 				   struct most_c_attr *attr,
 				   const char *buf,
 				   size_t count)
@@ -421,7 +402,7 @@ static ssize_t store_set_direction(struct most_c_obj *c,
 	return count;
 }
 
-static ssize_t show_set_datatype(struct most_c_obj *c,
+static ssize_t set_datatype_show(struct most_c_obj *c,
 				 struct most_c_attr *attr,
 				 char *buf)
 {
@@ -434,7 +415,7 @@ static ssize_t show_set_datatype(struct most_c_obj *c,
 	return snprintf(buf, PAGE_SIZE, "unconfigured\n");
 }
 
-static ssize_t store_set_datatype(struct most_c_obj *c,
+static ssize_t set_datatype_store(struct most_c_obj *c,
 				  struct most_c_attr *attr,
 				  const char *buf,
 				  size_t count)
@@ -455,14 +436,14 @@ static ssize_t store_set_datatype(struct most_c_obj *c,
 	return count;
 }
 
-static ssize_t show_set_subbuffer_size(struct most_c_obj *c,
+static ssize_t set_subbuffer_size_show(struct most_c_obj *c,
 				       struct most_c_attr *attr,
 				       char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", c->cfg.subbuffer_size);
 }
 
-static ssize_t store_set_subbuffer_size(struct most_c_obj *c,
+static ssize_t set_subbuffer_size_store(struct most_c_obj *c,
 					struct most_c_attr *attr,
 					const char *buf,
 					size_t count)
@@ -474,14 +455,14 @@ static ssize_t store_set_subbuffer_size(struct most_c_obj *c,
 	return count;
 }
 
-static ssize_t show_set_packets_per_xact(struct most_c_obj *c,
+static ssize_t set_packets_per_xact_show(struct most_c_obj *c,
 					 struct most_c_attr *attr,
 					 char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "%d\n", c->cfg.packets_per_xact);
 }
 
-static ssize_t store_set_packets_per_xact(struct most_c_obj *c,
+static ssize_t set_packets_per_xact_store(struct most_c_obj *c,
 					  struct most_c_attr *attr,
 					  const char *buf,
 					  size_t count)
@@ -493,33 +474,39 @@ static ssize_t store_set_packets_per_xact(struct most_c_obj *c,
 	return count;
 }
 
-#define create_channel_attribute(value) \
-	static MOST_CHNL_ATTR(value, 0644, show_##value, store_##value)
-
-create_channel_attribute(set_buffer_size);
-create_channel_attribute(set_number_of_buffers);
-create_channel_attribute(set_direction);
-create_channel_attribute(set_datatype);
-create_channel_attribute(set_subbuffer_size);
-create_channel_attribute(set_packets_per_xact);
+static struct most_c_attr most_c_attrs[] = {
+	__ATTR_RO(available_directions),
+	__ATTR_RO(available_datatypes),
+	__ATTR_RO(number_of_packet_buffers),
+	__ATTR_RO(number_of_stream_buffers),
+	__ATTR_RO(size_of_stream_buffer),
+	__ATTR_RO(size_of_packet_buffer),
+	__ATTR_RO(channel_starving),
+	__ATTR_RW(set_buffer_size),
+	__ATTR_RW(set_number_of_buffers),
+	__ATTR_RW(set_direction),
+	__ATTR_RW(set_datatype),
+	__ATTR_RW(set_subbuffer_size),
+	__ATTR_RW(set_packets_per_xact),
+};
 
 /**
  * most_channel_def_attrs - array of default attributes of channel object
  */
 static struct attribute *most_channel_def_attrs[] = {
-	&most_chnl_attr_available_directions.attr,
-	&most_chnl_attr_available_datatypes.attr,
-	&most_chnl_attr_number_of_packet_buffers.attr,
-	&most_chnl_attr_number_of_stream_buffers.attr,
-	&most_chnl_attr_size_of_packet_buffer.attr,
-	&most_chnl_attr_size_of_stream_buffer.attr,
-	&most_chnl_attr_set_number_of_buffers.attr,
-	&most_chnl_attr_set_buffer_size.attr,
-	&most_chnl_attr_set_direction.attr,
-	&most_chnl_attr_set_datatype.attr,
-	&most_chnl_attr_set_subbuffer_size.attr,
-	&most_chnl_attr_set_packets_per_xact.attr,
-	&most_chnl_attr_channel_starving.attr,
+	&most_c_attrs[0].attr,
+	&most_c_attrs[1].attr,
+	&most_c_attrs[2].attr,
+	&most_c_attrs[3].attr,
+	&most_c_attrs[4].attr,
+	&most_c_attrs[5].attr,
+	&most_c_attrs[6].attr,
+	&most_c_attrs[7].attr,
+	&most_c_attrs[8].attr,
+	&most_c_attrs[9].attr,
+	&most_c_attrs[10].attr,
+	&most_c_attrs[11].attr,
+	&most_c_attrs[12].attr,
 	NULL,
 };
 
@@ -562,9 +549,6 @@ create_most_c_obj(const char *name, struct kobject *parent)
 /*		     ___	       ___
  *		     ___I N S T A N C E___
  */
-#define MOST_INST_ATTR(_name, _mode, _show, _store) \
-		struct most_inst_attribute most_inst_attr_##_name = \
-		__ATTR(_name, _mode, _show, _store)
 
 static struct list_head instance_list;
 
@@ -652,7 +636,7 @@ static void most_inst_release(struct kobject *kobj)
 	kfree(inst);
 }
 
-static ssize_t show_description(struct most_inst_obj *instance_obj,
+static ssize_t description_show(struct most_inst_obj *instance_obj,
 				struct most_inst_attribute *attr,
 				char *buf)
 {
@@ -660,7 +644,7 @@ static ssize_t show_description(struct most_inst_obj *instance_obj,
 			instance_obj->iface->description);
 }
 
-static ssize_t show_interface(struct most_inst_obj *instance_obj,
+static ssize_t interface_show(struct most_inst_obj *instance_obj,
 			      struct most_inst_attribute *attr,
 			      char *buf)
 {
@@ -687,11 +671,11 @@ static ssize_t show_interface(struct most_inst_obj *instance_obj,
 	return snprintf(buf, PAGE_SIZE, "unknown\n");
 }
 
-#define create_inst_attribute(value) \
-	static MOST_INST_ATTR(value, 0444, show_##value, NULL)
+static struct most_inst_attribute most_inst_attr_description =
+	__ATTR_RO(description);
 
-create_inst_attribute(description);
-create_inst_attribute(interface);
+static struct most_inst_attribute most_inst_attr_interface =
+	__ATTR_RO(interface);
 
 static struct attribute *most_inst_def_attrs[] = {
 	&most_inst_attr_description.attr,
@@ -847,9 +831,9 @@ static void most_aim_release(struct kobject *kobj)
 	kfree(aim_obj);
 }
 
-static ssize_t add_link_show(struct most_aim_obj *aim_obj,
-			     struct most_aim_attribute *attr,
-			     char *buf)
+static ssize_t links_show(struct most_aim_obj *aim_obj,
+			  struct most_aim_attribute *attr,
+			  char *buf)
 {
 	struct most_c_obj *c;
 	struct most_inst_obj *i;
@@ -943,7 +927,7 @@ most_c_obj *get_channel_by_name(char *mdev, char *mdev_ch)
 }
 
 /**
- * store_add_link - store() function for add_link attribute
+ * add_link_store - store() function for add_link attribute
  * @aim_obj: pointer to AIM object
  * @attr: its attributes
  * @buf: buffer
@@ -1013,11 +997,8 @@ static ssize_t add_link_store(struct most_aim_obj *aim_obj,
 	return len;
 }
 
-static struct most_aim_attribute most_aim_attr_add_link =
-	__ATTR_RW(add_link);
-
 /**
- * store_remove_link - store function for remove_link attribute
+ * remove_link_store - store function for remove_link attribute
  * @aim_obj: pointer to AIM object
  * @attr: its attributes
  * @buf: buffer
@@ -1056,12 +1037,16 @@ static ssize_t remove_link_store(struct most_aim_obj *aim_obj,
 	return len;
 }
 
-static struct most_aim_attribute most_aim_attr_remove_link =
-	__ATTR_WO(remove_link);
+static struct most_aim_attribute most_aim_attrs[] = {
+	__ATTR_RO(links),
+	__ATTR_WO(add_link),
+	__ATTR_WO(remove_link),
+};
 
 static struct attribute *most_aim_def_attrs[] = {
-	&most_aim_attr_add_link.attr,
-	&most_aim_attr_remove_link.attr,
+	&most_aim_attrs[0].attr,
+	&most_aim_attrs[1].attr,
+	&most_aim_attrs[2].attr,
 	NULL,
 };
 
diff --git a/drivers/staging/nvec/nvec-keytable.h b/drivers/staging/nvec/nvec-keytable.h
index 1dc22cb8812a..7008c96bdbbe 100644
--- a/drivers/staging/nvec/nvec-keytable.h
+++ b/drivers/staging/nvec/nvec-keytable.h
@@ -17,8 +17,7 @@
  * more details.
  *
  * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * with this program; if not, see http://www.gnu.org/licenses
  */
 
 static unsigned short code_tab_102us[] = {
diff --git a/drivers/staging/nvec/nvec_kbd.c b/drivers/staging/nvec/nvec_kbd.c
index e881e6b26a4c..a01f486621eb 100644
--- a/drivers/staging/nvec/nvec_kbd.c
+++ b/drivers/staging/nvec/nvec_kbd.c
@@ -58,7 +58,7 @@ static int nvec_keys_notifier(struct notifier_block *nb,
 			      unsigned long event_type, void *data)
 {
 	int code, state;
-	unsigned char *msg = (unsigned char *)data;
+	unsigned char *msg = data;
 
 	if (event_type == NVEC_KB_EVT) {
 		int _size = (msg[0] & (3 << 5)) >> 5;
diff --git a/drivers/staging/olpc_dcon/olpc_dcon.c b/drivers/staging/olpc_dcon/olpc_dcon.c
index 684815c98789..f7f3a780ec10 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon.c
@@ -290,7 +290,7 @@ static void dcon_source_switch(struct work_struct *work)
 
 	switch (source) {
 	case DCON_SOURCE_CPU:
-		pr_info("dcon_source_switch to CPU\n");
+		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))
@@ -330,7 +330,7 @@ static void dcon_source_switch(struct work_struct *work)
 	{
 		ktime_t delta_t;
 
-		pr_info("dcon_source_switch to DCON\n");
+		pr_info("%s to DCON\n", __func__);
 
 		/* Clear DCONLOAD - this implies that the DCON is in control */
 		pdata->set_dconload(0);
diff --git a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
index 75c3c2fe9560..64584425b01c 100644
--- a/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
+++ b/drivers/staging/olpc_dcon/olpc_dcon_xo_1_5.c
@@ -64,7 +64,7 @@ static int dcon_init_xo_1_5(struct dcon_priv *dcon)
 	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);
+	outb(inb(VX855_GPI_SCI_SMI) | BIT_GPIO12, VX855_GPI_SCI_SMI);
 
 	/* Determine the current state of DCONLOAD, likely set by firmware */
 	/* GPIO1 */
@@ -129,7 +129,7 @@ static void dcon_wiggle_xo_1_5(void)
 	udelay(5);
 
 	/* set   PMIO_Rx52[6] to enable SCI/SMI on gpio12 */
-	outb(inb(VX855_GPI_SCI_SMI)|BIT_GPIO12, VX855_GPI_SCI_SMI);
+	outb(inb(VX855_GPI_SCI_SMI) | BIT_GPIO12, VX855_GPI_SCI_SMI);
 }
 
 static void dcon_set_dconload_xo_1_5(int val)
diff --git a/drivers/staging/rtl8188eu/Kconfig b/drivers/staging/rtl8188eu/Kconfig
index 94f38793ab47..cb836c59d564 100644
--- a/drivers/staging/rtl8188eu/Kconfig
+++ b/drivers/staging/rtl8188eu/Kconfig
@@ -1,6 +1,7 @@
 config R8188EU
 	tristate "Realtek RTL8188EU Wireless LAN NIC driver"
 	depends on WLAN && USB && CFG80211
+	depends on m
 	select WIRELESS_EXT
 	select WEXT_PRIV
 	---help---
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index 1c8fa3a1f5bb..519b4d3584a2 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -30,7 +30,6 @@ void init_mlme_ap_info(struct adapter *padapter)
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct wlan_acl_pool *pacl_list = &pstapriv->acl_list;
 
-
 	spin_lock_init(&pmlmepriv->bcn_update_lock);
 
 	/* for ACL */
@@ -448,10 +447,8 @@ void	expire_timeout_chk(struct adapter *padapter)
 void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
 {
 	int i;
-	u8 rf_type;
 	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;
@@ -472,16 +469,9 @@ void add_RATid(struct adapter *padapter, struct sta_info *psta, u8 rssi_level)
 	}
 	/* n mode ra_bitmap */
 	if (psta_ht->ht_option) {
-		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
-		if (rf_type == RF_2T2R)
-			limit = 16;/*  2R */
-		else
-			limit = 8;/*   1R */
-
-		for (i = 0; i < limit; i++) {
-			if (psta_ht->ht_cap.mcs.rx_mask[i / 8] & BIT(i % 8))
+		for (i = 0; i < 8; i++)
+			if (psta_ht->ht_cap.mcs.rx_mask[0] & BIT(i))
 				tx_ra_bitmap |= BIT(i + 12);
-		}
 
 		/* max short GI rate */
 		shortGIrate = psta_ht->sgi;
@@ -729,7 +719,7 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 	u8 val8, cur_channel, cur_bwmode, cur_ch_offset;
 	u16 bcn_interval;
 	u32	acparm;
-	int	ie_len;
+	uint	ie_len;
 	struct registry_priv	 *pregpriv = &padapter->registrypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
@@ -888,7 +878,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 		return _FAIL;
 
 
-	if (len > MAX_IE_SZ)
+	if (len < 0 || len > MAX_IE_SZ)
 		return _FAIL;
 
 	pbss_network->IELength = len;
@@ -1033,15 +1023,12 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len,
 		       (pbss_network->IELength - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
-		u8 rf_type;
 		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_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
-
 		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));
@@ -1051,10 +1038,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 		/* set  Max Rx AMPDU size  to 64K */
 		pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR & 0x03);
 
-		if (rf_type == RF_1T1R) {
-			pht_cap->mcs.rx_mask[0] = 0xff;
-			pht_cap->mcs.rx_mask[1] = 0x0;
-		}
+		pht_cap->mcs.rx_mask[0] = 0xff;
+		pht_cap->mcs.rx_mask[1] = 0x0;
 		memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len);
 	}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index 14979666dadd..9754322b506e 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -1295,7 +1295,7 @@ void rtw_setstaKey_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *pc
 
 
 	if (psta == NULL) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: %s => can't get sta_info\n\n", __func__));
 		goto exit;
 	}
 exit:
@@ -1312,7 +1312,7 @@ void rtw_setassocsta_cmdrsp_callback(struct adapter *padapter,  struct cmd_obj *
 
 
 	if (psta == NULL) {
-		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: %s => can't get sta_info\n\n", __func__));
 		goto exit;
 	}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
index d1cd34011602..d1dafe0f20c6 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -158,7 +158,7 @@ u8 *rtw_set_ie
 /*----------------------------------------------------------------------------
 index: the information element id index, limit is the limit for search
 -----------------------------------------------------------------------------*/
-u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit)
+u8 *rtw_get_ie(u8 *pbuf, int index, uint *len, int limit)
 {
 	int tmp, i;
 	u8 *p;
@@ -226,7 +226,8 @@ uint	rtw_get_rateset_len(u8	*rateset)
 int rtw_generate_ie(struct registry_priv *pregistrypriv)
 {
 	u8	wireless_mode;
-	int	sz = 0, rateLen;
+	int	rateLen;
+	uint    sz = 0;
 	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
 	u8 *ie = pdev_network->IEs;
 
@@ -291,9 +292,9 @@ int rtw_generate_ie(struct registry_priv *pregistrypriv)
 	return sz;
 }
 
-unsigned char *rtw_get_wpa_ie(unsigned char *pie, int *wpa_ie_len, int limit)
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, uint *wpa_ie_len, int limit)
 {
-	int len;
+	uint len;
 	u16 val16;
 	__le16 le_tmp;
 	unsigned char wpa_oui_type[] = {0x00, 0x50, 0xf2, 0x01};
@@ -331,7 +332,7 @@ check_next_ie:
 	return NULL;
 }
 
-unsigned char *rtw_get_wpa2_ie(unsigned char *pie, int *rsn_ie_len, int limit)
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, uint *rsn_ie_len, int limit)
 {
 
 	return rtw_get_ie(pie, _WPA2_IE_ID_, rsn_ie_len, limit);
@@ -1000,7 +1001,7 @@ int ieee80211_get_hdrlen(u16 fc)
 
 static int rtw_get_cipher_info(struct wlan_network *pnetwork)
 {
-	int wpa_ielen;
+	uint wpa_ielen;
 	unsigned char *pbuf;
 	int group_cipher = 0, pairwise_cipher = 0, is8021x = 0;
 	int ret = _FAIL;
@@ -1045,7 +1046,7 @@ void rtw_get_bcn_info(struct wlan_network *pnetwork)
 	__le16 le_tmp;
 	u16 wpa_len = 0, rsn_len = 0;
 	struct HT_info_element *pht_info = NULL;
-	int len;
+	uint len;
 	unsigned char		*p;
 
 	memcpy(&le_tmp, rtw_get_capability_from_ie(pnetwork->network.IEs), 2);
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
index 67508a6cf0e5..d8d88b5f68e5 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -569,7 +569,6 @@ u16 rtw_get_cur_max_rate(struct adapter *adapter)
 	struct registry_priv *pregistrypriv = &adapter->registrypriv;
 	struct mlme_priv	*pmlmepriv = &adapter->mlmepriv;
 	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
-	u8	rf_type = 0;
 	u8	bw_40MHz = 0, short_GI_20 = 0, short_GI_40 = 0;
 	u32	ht_ielen = 0;
 
@@ -586,9 +585,8 @@ u16 rtw_get_cur_max_rate(struct adapter *adapter)
 			short_GI_20 = (le16_to_cpu(pmlmeinfo->HT_caps.cap_info) & IEEE80211_HT_CAP_SGI_20) ? 1 : 0;
 			short_GI_40 = (le16_to_cpu(pmlmeinfo->HT_caps.cap_info) & IEEE80211_HT_CAP_SGI_40) ? 1 : 0;
 
-			rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
 			max_rate = rtw_mcs_rate(
-				rf_type,
+				RF_1T1R,
 				bw_40MHz & (pregistrypriv->cbw40_enable),
 				short_GI_20,
 				short_GI_40,
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index a71928952eca..301085a459c9 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -28,7 +28,6 @@
 #include <rtw_ioctl_set.h>
 #include <linux/vmalloc.h>
 
-extern unsigned char	MCS_rate_2R[16];
 extern unsigned char	MCS_rate_1R[16];
 
 int rtw_init_mlme_priv(struct adapter *padapter)
@@ -56,7 +55,7 @@ int rtw_init_mlme_priv(struct adapter *padapter)
 
 	pbuf = vzalloc(MAX_BSS_CNT * (sizeof(struct wlan_network)));
 
-	if (pbuf == NULL) {
+	if (!pbuf) {
 		res = _FAIL;
 		goto exit;
 	}
@@ -148,7 +147,7 @@ static void _rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *
 	u32 lifetime = SCANQUEUE_LIFETIME;
 	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
 
-	if (pnetwork == NULL)
+	if (!pnetwork)
 		return;
 
 	if (pnetwork->fixed)
@@ -172,7 +171,7 @@ void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *
 {
 	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
 
-	if (pnetwork == NULL)
+	if (!pnetwork)
 		return;
 	if (pnetwork->fixed)
 		return;
@@ -181,10 +180,10 @@ void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *
 }
 
 /*
-	return the wlan_network with the matching addr
-
-	Shall be calle under atomic context... to avoid possible racing condition...
-*/
+ * 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;
@@ -322,7 +321,6 @@ int is_same_network(struct wlan_bssid_ex *src, struct wlan_bssid_ex *dst)
 	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);
 
@@ -347,7 +345,7 @@ struct	wlan_network	*rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
 		pwlan = container_of(plist, struct wlan_network, list);
 
 		if (!pwlan->fixed) {
-			if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
+			if (!oldest || time_after(oldest->last_scanned, pwlan->last_scanned))
 				oldest = pwlan;
 		}
 	}
@@ -392,7 +390,6 @@ void update_network(struct wlan_bssid_ex *dst, struct wlan_bssid_ex *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)
@@ -408,8 +405,8 @@ static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex
 }
 
 /*
-Caller must hold pmlmepriv->lock first.
-*/
+ * Caller must hold pmlmepriv->lock first.
+ */
 void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
 {
 	struct list_head *plist, *phead;
@@ -434,7 +431,8 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 		plist = plist->next;
 	}
 	/* If we didn't find a match, then get a new network slot to initialize
-	 * with this beacon's information */
+	 * 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 */
@@ -458,7 +456,7 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 
 			pnetwork = rtw_alloc_network(pmlmepriv); /*  will update scan_time */
 
-			if (pnetwork == NULL) {
+			if (!pnetwork) {
 				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
 				goto exit;
 			}
@@ -493,7 +491,6 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 
 exit:
 	spin_unlock_bh(&queue->lock);
-
 }
 
 static void rtw_add_network(struct adapter *adapter,
@@ -527,7 +524,7 @@ static int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *
 	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) != NULL)
+		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;
@@ -548,7 +545,6 @@ static int rtw_is_desired_network(struct adapter *adapter, struct wlan_network *
 			bselected = false;
 	}
 
-
 	return bselected;
 }
 
@@ -558,7 +554,6 @@ void rtw_atimdone_event_callback(struct adapter *adapter, u8 *pbuf)
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("receive atimdone_evet\n"));
 }
 
-
 void rtw_survey_event_callback(struct adapter	*adapter, u8 *pbuf)
 {
 	u32 len;
@@ -663,7 +658,7 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 			set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 			pmlmepriv->to_join = false;
 			s_ret = rtw_select_and_join_from_scanned_queue(pmlmepriv);
-			if (_SUCCESS == s_ret) {
+			if (s_ret == _SUCCESS) {
 				mod_timer(&pmlmepriv->assoc_timer,
 					jiffies + msecs_to_jiffies(MAX_JOIN_TIMEOUT));
 			} else if (s_ret == 2) { /* there is no need to wait for join */
@@ -673,7 +668,7 @@ void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
 				DBG_88E("try_to_join, but select scanning queue fail, to_roaming:%d\n", pmlmepriv->to_roaming);
 				if (pmlmepriv->to_roaming != 0) {
 					if (--pmlmepriv->to_roaming == 0 ||
-					    _SUCCESS != rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0)) {
+					    rtw_sitesurvey_cmd(adapter, &pmlmepriv->assoc_ssid, 1, NULL, 0) != _SUCCESS) {
 						pmlmepriv->to_roaming = 0;
 						rtw_free_assoc_resources(adapter);
 						rtw_indicate_disconnect(adapter);
@@ -726,8 +721,8 @@ static void free_scanqueue(struct	mlme_priv *pmlmepriv)
 }
 
 /*
-*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
-*/
+ * rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
+ */
 void rtw_free_assoc_resources(struct adapter *adapter)
 {
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -738,8 +733,8 @@ void rtw_free_assoc_resources(struct adapter *adapter)
 }
 
 /*
-*rtw_free_assoc_resources_locked: the caller has to lock pmlmepriv->lock
-*/
+ * rtw_free_assoc_resources_locked: the caller has to lock pmlmepriv->lock
+ */
 void rtw_free_assoc_resources_locked(struct adapter *adapter)
 {
 	struct wlan_network *pwlan = NULL;
@@ -789,8 +784,8 @@ void rtw_free_assoc_resources_locked(struct adapter *adapter)
 }
 
 /*
-*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
-*/
+ * rtw_indicate_connect: the caller has to lock pmlmepriv->lock
+ */
 void rtw_indicate_connect(struct adapter *padapter)
 {
 	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
@@ -815,8 +810,8 @@ void rtw_indicate_connect(struct adapter *padapter)
 }
 
 /*
-*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
-*/
+ * rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+ */
 void rtw_indicate_disconnect(struct adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -825,7 +820,6 @@ void rtw_indicate_disconnect(struct adapter *padapter)
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING | WIFI_UNDER_WPS);
 
-
 	if (pmlmepriv->to_roaming > 0)
 		_clr_fwstate_(pmlmepriv, _FW_LINKED);
 
@@ -877,7 +871,7 @@ static struct sta_info *rtw_joinbss_update_stainfo(struct adapter *padapter, str
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	psta = rtw_get_stainfo(pstapriv, pnetwork->network.MacAddress);
-	if (psta == NULL)
+	if (!psta)
 		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
 
 	if (psta) { /* update ptarget_sta */
@@ -959,7 +953,6 @@ static void rtw_joinbss_update_network(struct adapter *padapter, struct wlan_net
 
 	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;
@@ -1011,7 +1004,6 @@ void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
 
 	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
 
-
 	if (pmlmepriv->assoc_ssid.SsidLength == 0)
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@   joinbss event call back  for Any SSid\n"));
 	else
@@ -1071,11 +1063,10 @@ void rtw_joinbss_event_prehandle(struct adapter *adapter, u8 *pbuf)
 				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 == NULL) {
+				if (!ptarget_sta) {
 					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't update stainfo when joinbss_event callback\n"));
 					spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
 					goto ignore_joinbss_callback;
@@ -1145,7 +1136,7 @@ static u8 search_max_mac_id(struct adapter *padapter)
 #if defined(CONFIG_88EU_AP_MODE)
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		for (aid = (pstapriv->max_num_sta); aid > 0; aid--) {
-			if (pstapriv->sta_aid[aid-1] != NULL)
+			if (pstapriv->sta_aid[aid-1])
 				break;
 		}
 		mac_id = aid + 1;
@@ -1166,7 +1157,7 @@ void rtw_stassoc_hw_rpt(struct adapter *adapter, struct sta_info *psta)
 	u16 media_status;
 	u8 macid;
 
-	if (psta == NULL)
+	if (!psta)
 		return;
 
 	macid = search_max_mac_id(adapter);
@@ -1198,13 +1189,13 @@ void rtw_stassoc_event_callback(struct adapter *adapter, u8 *pbuf)
 #endif
 	/* for AD-HOC mode */
 	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
-	if (psta != NULL) {
+	if (psta) {
 		/* the sta have been in sta_info_queue => do nothing */
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue\n"));
 		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 == NULL) {
+	if (!psta) {
 		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't alloc sta_info when rtw_stassoc_event_callback\n"));
 		return;
 	}
@@ -1338,9 +1329,9 @@ void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
 }
 
 /*
-* _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
-* @adapter: pointer to struct adapter structure
-*/
+ * _rtw_join_timeout_handler - Timeout/faliure handler for CMD JoinBss
+ * @adapter: pointer to struct adapter structure
+ */
 void _rtw_join_timeout_handler (unsigned long data)
 {
 	struct adapter *adapter = (struct adapter *)data;
@@ -1352,7 +1343,6 @@ void _rtw_join_timeout_handler (unsigned long data)
 	if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
 		return;
 
-
 	spin_lock_bh(&pmlmepriv->lock);
 
 	if (pmlmepriv->to_roaming > 0) { /*  join timeout caused by roaming */
@@ -1361,7 +1351,7 @@ void _rtw_join_timeout_handler (unsigned long data)
 			if (pmlmepriv->to_roaming != 0) { /* try another , */
 				DBG_88E("%s try another roaming\n", __func__);
 				do_join_r = rtw_do_join(adapter);
-				if (_SUCCESS != do_join_r) {
+				if (do_join_r != _SUCCESS) {
 					DBG_88E("%s roaming do_join return %d\n", __func__, do_join_r);
 					continue;
 				}
@@ -1380,9 +1370,9 @@ void _rtw_join_timeout_handler (unsigned long data)
 }
 
 /*
-* rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
-* @adapter: pointer to struct adapter structure
-*/
+ * rtw_scan_timeout_handler - Timeout/Faliure handler for CMD SiteSurvey
+ * @adapter: pointer to struct adapter structure
+ */
 void rtw_scan_timeout_handler (unsigned long data)
 {
 	struct adapter *adapter = (struct adapter *)data;
@@ -1437,10 +1427,10 @@ exit:
 #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
-*/
+ * 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)
 {
@@ -1448,7 +1438,6 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 	unsigned long since_scan;
 	struct adapter *adapter = container_of(pmlmepriv, struct adapter, mlmepriv);
 
-
 	/* check bssid, if needed */
 	if (pmlmepriv->assoc_by_bssid) {
 		if (memcmp(competitor->network.MacAddress, pmlmepriv->assoc_bssid, ETH_ALEN))
@@ -1491,11 +1480,11 @@ exit:
 }
 
 /*
-Calling context:
-The caller of the sub-routine will be in critical section...
-The caller must hold the following spinlock
-pmlmepriv->lock
-*/
+ * 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)
 {
@@ -1521,7 +1510,7 @@ int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
 		pmlmepriv->pscanned = pmlmepriv->pscanned->next;
 		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
 	}
-	if (candidate == NULL) {
+	if (!candidate) {
 		DBG_88E("%s: return _FAIL(candidate==NULL)\n", __func__);
 		ret = _FAIL;
 		goto exit;
@@ -1531,7 +1520,6 @@ int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
 			candidate->network.Configuration.DSConfig);
 	}
 
-
 	/*  check for situation of  _FW_LINKED */
 	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
 		DBG_88E("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
@@ -1547,8 +1535,8 @@ int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
 
 		rtw_hal_get_def_var(adapter, HAL_DEF_CURRENT_ANTENNA, &(cur_ant));
 		DBG_88E("#### Opt_Ant_(%s), cur_Ant(%s)\n",
-			(2 == candidate->network.PhyInfo.Optimum_antenna) ? "A" : "B",
-			(2 == cur_ant) ? "A" : "B"
+			(candidate->network.PhyInfo.Optimum_antenna == 2) ? "A" : "B",
+			(cur_ant == 2) ? "A" : "B"
 		);
 	}
 
@@ -1929,7 +1917,6 @@ unsigned int rtw_restructure_ht_ie(struct adapter *padapter, u8 *in_ie, u8 *out_
 	struct ht_priv		*phtpriv = &pmlmepriv->htpriv;
 	u32 rx_packet_offset, max_recvbuf_sz;
 
-
 	phtpriv->ht_option = false;
 
 	p = rtw_get_ie(in_ie+12, _HT_CAPABILITY_IE_, &ielen, in_len-12);
@@ -2018,17 +2005,10 @@ void rtw_update_ht_cap(struct adapter *padapter, u8 *pie, uint ie_len)
 	    (le16_to_cpu(pmlmeinfo->HT_caps.cap_info) & BIT(1)) &&
 	    (pmlmeinfo->HT_info.infos[0] & BIT(2))) {
 		int i;
-		u8	rf_type;
-
-		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
 
 		/* update the MCS rates */
-		for (i = 0; i < 16; i++) {
-			if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
-				((u8 *)&pmlmeinfo->HT_caps.mcs)[i] &= MCS_rate_1R[i];
-			else
-				((u8 *)&pmlmeinfo->HT_caps.mcs)[i] &= MCS_rate_2R[i];
-		}
+		for (i = 0; i < 16; i++)
+			((u8 *)&pmlmeinfo->HT_caps.mcs)[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)) {
@@ -2072,7 +2052,7 @@ void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitfr
 	else
 		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
 
-	if (psta == NULL)
+	if (!psta)
 		return;
 
 	phtpriv = &psta->htpriv;
@@ -2081,7 +2061,7 @@ void rtw_issue_addbareq_cmd(struct adapter *padapter, struct xmit_frame *pxmitfr
 		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
 		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
 
-		if (0 == issued) {
+		if (issued == 0) {
 			DBG_88E("rtw_issue_addbareq_cmd, p=%d\n", priority);
 			psta->htpriv.candidate_tid_bitmap |= BIT((u8)priority);
 			rtw_addbareq_cmd(padapter, (u8)priority, pattrib->ra);
@@ -2097,6 +2077,7 @@ void rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
 	_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;
@@ -2104,12 +2085,12 @@ void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
 
 	struct wlan_network *pnetwork;
 
-	if (tgt_network != NULL)
+	if (tgt_network)
 		pnetwork = tgt_network;
 	else
 		pnetwork = &pmlmepriv->cur_network;
 
-	if (0 < pmlmepriv->to_roaming) {
+	if (pmlmepriv->to_roaming > 0) {
 		DBG_88E("roaming from %s(%pM length:%d\n",
 			pnetwork->network.Ssid.Ssid, pnetwork->network.MacAddress,
 			pnetwork->network.Ssid.SsidLength);
@@ -2119,13 +2100,13 @@ void _rtw_roaming(struct adapter *padapter, struct wlan_network *tgt_network)
 
 		while (1) {
 			do_join_r = rtw_do_join(padapter);
-			if (_SUCCESS == do_join_r) {
+			if (do_join_r == _SUCCESS) {
 				break;
 			} else {
 				DBG_88E("roaming do_join return %d\n", do_join_r);
 				pmlmepriv->to_roaming--;
 
-				if (0 < pmlmepriv->to_roaming) {
+				if (pmlmepriv->to_roaming > 0) {
 					continue;
 				} else {
 					DBG_88E("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index f45af407f76d..88a3a2b9c144 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -47,7 +47,6 @@ extern unsigned char REALTEK_96B_IE[];
 /********************************************************
 MCS rate definitions
 *********************************************************/
-unsigned char	MCS_rate_2R[16] = {0xff, 0xff, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 unsigned char	MCS_rate_1R[16] = {0xff, 0x00, 0x0, 0x0, 0x01, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 
 /********************************************************
@@ -287,7 +286,7 @@ static s32 dump_mgntframe_and_wait_ack(struct adapter *padapter,
 static int update_hidden_ssid(u8 *ies, u32 ies_len, u8 hidden_ssid_mode)
 {
 	u8 *ssid_ie;
-	int ssid_len_ori;
+	uint ssid_len_ori;
 	int len_diff = 0;
 
 	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
@@ -1027,7 +1026,7 @@ static void issue_assocreq(struct adapter *padapter)
 	struct ieee80211_hdr *pwlanhdr;
 	__le16 *fctrl;
 	unsigned int	i, j, ie_len, index = 0;
-	unsigned char	rf_type, bssrate[NumRates], sta_bssrate[NumRates];
+	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);
@@ -1150,25 +1149,9 @@ static void issue_assocreq(struct adapter *padapter)
 			/* todo: disable SM power save mode */
 			pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x000c);
 
-			rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
-			switch (rf_type) {
-			case RF_1T1R:
-				if (pregpriv->rx_stbc)
-					pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
-				memcpy((u8 *)&pmlmeinfo->HT_caps.mcs, MCS_rate_1R, 16);
-				break;
-			case RF_2T2R:
-			case RF_1T2R:
-			default:
-				if ((pregpriv->rx_stbc == 0x3) ||/* enable for 2.4/5 GHz */
-				    ((pmlmeext->cur_wireless_mode & WIRELESS_11_24N) && (pregpriv->rx_stbc == 0x1)) || /* enable for 2.4GHz */
-				    (pregpriv->wifi_spec == 1)) {
-					DBG_88E("declare supporting RX STBC\n");
-					pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x0200);/* RX STBC two spatial stream */
-				}
-				memcpy(&pmlmeinfo->HT_caps.mcs, MCS_rate_2R, 16);
-				break;
-			}
+			if (pregpriv->rx_stbc)
+				pmlmeinfo->HT_caps.cap_info |= cpu_to_le16(0x0100);/* RX STBC One spatial stream */
+			memcpy((u8 *)&pmlmeinfo->HT_caps.mcs, MCS_rate_1R, 16);
 			pframe = rtw_set_ie(pframe, _HT_CAPABILITY_IE_, ie_len, (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
 		}
 	}
@@ -1803,7 +1786,7 @@ static void issue_action_BSSCoexistPacket(struct adapter *padapter)
 		plist = phead->next;
 
 		while (phead != plist) {
-			int len;
+			uint len;
 			u8 *p;
 			struct wlan_bssid_ex *pbss_network;
 
@@ -2573,7 +2556,7 @@ static unsigned int OnProbeReq(struct adapter *padapter,
 	    !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,
+	p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _SSID_IE_, &ielen,
 			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
 
 	/* check (wildcard) SSID */
@@ -2810,7 +2793,7 @@ static unsigned int OnAuth(struct adapter *padapter,
 			/* checking for challenging txt... */
 			DBG_88E("checking for challenging txt...\n");
 
-			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, (int *)&ie_len,
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + 4 + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, &ie_len,
 					len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_ - 4);
 
 			if ((p == NULL) || (ie_len <= 0)) {
@@ -2904,7 +2887,7 @@ static unsigned int OnAuthClient(struct adapter *padapter,
 	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,
+			p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _AUTH_IE_OFFSET_, _CHLGETXT_IE_, &len,
 				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
 
 			if (p == NULL)
@@ -2948,7 +2931,7 @@ static unsigned int OnAssocReq(struct adapter *padapter,
 	struct sta_info	*pstat;
 	unsigned char		reassoc, *p, *pos, *wpa_ie;
 	unsigned char WMM_IE[] = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
-	int		i, ie_len, wpa_ie_len, left;
+	int		i, wpa_ie_len, left;
 	unsigned char		supportRate[16];
 	int					supportRateNum;
 	unsigned short		status = _STATS_SUCCESSFUL_;
@@ -2960,7 +2943,7 @@ static unsigned int OnAssocReq(struct adapter *padapter,
 	struct wlan_bssid_ex *cur = &(pmlmeinfo->network);
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	u8 *pframe = precv_frame->pkt->data;
-	uint pkt_len = precv_frame->pkt->len;
+	uint ie_len, pkt_len = precv_frame->pkt->len;
 
 	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
 		return _FAIL;
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index 53dc33c3f913..c6c4404e717b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -392,7 +392,7 @@ static struct recv_frame *decryptor(struct adapter *padapter,
 		}
 	}
 
-	if ((prxattrib->encrypt > 0) && ((prxattrib->bdecrypted == 0) || (psecuritypriv->sw_decrypt))) {
+	if ((prxattrib->encrypt > 0) && (prxattrib->bdecrypted == 0)) {
 		psecuritypriv->hw_decrypted = false;
 
 		switch (prxattrib->encrypt) {
@@ -452,7 +452,7 @@ static struct recv_frame *portctrl(struct adapter *adapter,
 
 	if (auth_alg == 2) {
 		/* get ether_type */
-		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE;
+		ptr = ptr + pfhdr->attrib.hdrlen + LLC_HEADER_SIZE + pfhdr->attrib.iv_len;
 		memcpy(&be_tmp, ptr, 2);
 		ether_type = ntohs(be_tmp);
 
@@ -1979,7 +1979,7 @@ static int recv_func(struct adapter *padapter, struct recv_frame *rframe)
 		/* 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) &&
+		    prxattrib->bdecrypted == 0 &&
 		    !is_wep_enc(psecuritypriv->dot11PrivacyAlgrthm) &&
 		    !psecuritypriv->busetkipkey) {
 			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
@@ -2050,19 +2050,13 @@ static void rtw_signal_stat_timer_hdl(unsigned long data)
 	if (check_fwstate(&adapter->mlmepriv, _FW_UNDER_SURVEY) == false) {
 		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;
+		tmp_s = DIV_ROUND_UP(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;
+		tmp_q = DIV_ROUND_UP(tmp_q, _alpha);
 		if (tmp_q > 100)
 			tmp_q = 100;
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
index f6f1b09466a8..2db8a5d11c0d 100644
--- a/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8188eu/core/rtw_wlan_util.c
@@ -708,7 +708,6 @@ static void bwmode_update_check(struct adapter *padapter, struct ndis_802_11_var
 void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
 {
 	unsigned int	i;
-	u8	rf_type;
 	u8	max_AMPDU_len, min_MPDU_spacing;
 	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
@@ -744,15 +743,9 @@ void HT_caps_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
 		}
 	}
 
-	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
-
 	/* update the MCS rates */
-	for (i = 0; i < 16; i++) {
-		if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
-			((u8 *)&pmlmeinfo->HT_caps.mcs)[i] &= MCS_rate_1R[i];
-		else
-			((u8 *)&pmlmeinfo->HT_caps.mcs)[i] &= MCS_rate_2R[i];
-	}
+	for (i = 0; i < 16; i++)
+		((u8 *)&pmlmeinfo->HT_caps.mcs)[i] &= MCS_rate_1R[i];
 }
 
 void HT_info_handler(struct adapter *padapter, struct ndis_802_11_var_ie *pIE)
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index 630fdc33d58a..be2f46eb9f78 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -587,15 +587,13 @@ static s32 update_attrib(struct adapter *padapter, struct sk_buff *pkt, struct p
 	}
 
 	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
-		 ("update_attrib: encrypt=%d  securitypriv.sw_encrypt=%d\n",
-		  pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+		 ("update_attrib: encrypt=%d\n", pattrib->encrypt));
 
-	if (pattrib->encrypt &&
-	    (padapter->securitypriv.sw_encrypt || !psecuritypriv->hw_decrypted)) {
+	if (pattrib->encrypt && !psecuritypriv->hw_decrypted) {
 		pattrib->bswenc = true;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
-			 ("update_attrib: encrypt=%d securitypriv.hw_decrypted=%d bswenc = true\n",
-			  pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+			 ("update_attrib: encrypt=%d bswenc = true\n",
+			  pattrib->encrypt));
 	} else {
 		pattrib->bswenc = false;
 		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc = false\n"));
@@ -1141,9 +1139,8 @@ s32 rtw_put_snap(u8 *data, u16 h_proto)
 
 void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len)
 {
-	uint	protection;
+	uint	protection, erp_len;
 	u8	*perp;
-	int	 erp_len;
 	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	struct	registry_priv *pregistrypriv = &padapter->registrypriv;
 
@@ -1763,20 +1760,20 @@ int xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_fra
 			switch (pattrib->priority) {
 			case 1:
 			case 2:
-				wmmps_ac = psta->uapsd_bk&BIT(0);
+				wmmps_ac = psta->uapsd_bk & BIT(0);
 				break;
 			case 4:
 			case 5:
-				wmmps_ac = psta->uapsd_vi&BIT(0);
+				wmmps_ac = psta->uapsd_vi & BIT(0);
 				break;
 			case 6:
 			case 7:
-				wmmps_ac = psta->uapsd_vo&BIT(0);
+				wmmps_ac = psta->uapsd_vo & BIT(0);
 				break;
 			case 0:
 			case 3:
 			default:
-				wmmps_ac = psta->uapsd_be&BIT(0);
+				wmmps_ac = psta->uapsd_be & BIT(0);
 				break;
 			}
 
@@ -1890,20 +1887,20 @@ void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
 		switch (pxmitframe->attrib.priority) {
 		case 1:
 		case 2:
-			wmmps_ac = psta->uapsd_bk&BIT(1);
+			wmmps_ac = psta->uapsd_bk & BIT(1);
 			break;
 		case 4:
 		case 5:
-			wmmps_ac = psta->uapsd_vi&BIT(1);
+			wmmps_ac = psta->uapsd_vi & BIT(1);
 			break;
 		case 6:
 		case 7:
-			wmmps_ac = psta->uapsd_vo&BIT(1);
+			wmmps_ac = psta->uapsd_vo & BIT(1);
 			break;
 		case 0:
 		case 3:
 		default:
-			wmmps_ac = psta->uapsd_be&BIT(1);
+			wmmps_ac = psta->uapsd_be & BIT(1);
 			break;
 		}
 
@@ -2016,20 +2013,20 @@ void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *pst
 		switch (pxmitframe->attrib.priority) {
 		case 1:
 		case 2:
-			wmmps_ac = psta->uapsd_bk&BIT(1);
+			wmmps_ac = psta->uapsd_bk & BIT(1);
 			break;
 		case 4:
 		case 5:
-			wmmps_ac = psta->uapsd_vi&BIT(1);
+			wmmps_ac = psta->uapsd_vi & BIT(1);
 			break;
 		case 6:
 		case 7:
-			wmmps_ac = psta->uapsd_vo&BIT(1);
+			wmmps_ac = psta->uapsd_vo & BIT(1);
 			break;
 		case 0:
 		case 3:
 		default:
-			wmmps_ac = psta->uapsd_be&BIT(1);
+			wmmps_ac = psta->uapsd_be & BIT(1);
 			break;
 		}
 
diff --git a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
index 201c15b07f9e..81bf4944ef44 100644
--- a/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
+++ b/drivers/staging/rtl8188eu/hal/Hal8188ERateAdaptive.c
@@ -733,7 +733,7 @@ void ODM_RA_TxRPT2Handle_8188E(struct odm_dm_struct *dm_odm, u8 *TxRPT_Buf, u16
 					     pRAInfo->RTY[0], pRAInfo->RTY[1],
 					     pRAInfo->RTY[2], pRAInfo->RTY[3],
 					     pRAInfo->RTY[4], pRAInfo->DROP,
-					     macid_entry0 , macid_entry1));
+					     macid_entry0, macid_entry1));
 				if (pRAInfo->PTActive) {
 					if (pRAInfo->RAstage < 5)
 						odm_RateDecision_8188E(dm_odm, pRAInfo);
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index 16476e735011..ec8aae76bf40 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -832,6 +832,7 @@ void odm_RefreshRateAdaptiveMaskCE(struct odm_dm_struct *pDM_Odm)
 
 	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)) {
 				ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD,
@@ -896,6 +897,7 @@ void odm_DynamicTxPowerInit(struct odm_dm_struct *pDM_Odm)
 {
 	struct adapter *Adapter = pDM_Odm->Adapter;
 	struct dm_priv	*pdmpriv = &Adapter->HalData->dmpriv;
+
 	pdmpriv->bDynamicTxPowerEnable = false;
 	pdmpriv->LastDTPLvl = TxHighPwrLevel_Normal;
 	pdmpriv->DynamicTxHighPowerLvl = TxHighPwrLevel_Normal;
@@ -1052,6 +1054,7 @@ void odm_HwAntDiv(struct odm_dm_struct *pDM_Odm)
 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;
diff --git a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
index dd9b902c8ae3..91e0f6cee8f4 100644
--- a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
+++ b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
@@ -81,9 +81,9 @@ static void dm_trx_hw_antenna_div_init(struct odm_dm_struct *dm_odm)
 
 	/* antenna mapping table */
 	if (!dm_odm->bIsMPChip) { /* testchip */
-		phy_set_bb_reg(adapter, ODM_REG_RX_DEFUALT_A_11N,
+		phy_set_bb_reg(adapter, ODM_REG_RX_DEFAULT_A_11N,
 			       BIT(10) | BIT(9) | BIT(8), 1);
-		phy_set_bb_reg(adapter, ODM_REG_RX_DEFUALT_A_11N,
+		phy_set_bb_reg(adapter, ODM_REG_RX_DEFAULT_A_11N,
 			       BIT(13) | BIT(12) | BIT(11), 2);
 	} else { /* MPchip */
 		phy_set_bb_reg(adapter, ODM_REG_ANT_MAPPING1_11N, bMaskDWord,
@@ -248,6 +248,7 @@ void rtl88eu_dm_ant_sel_statistics(struct odm_dm_struct *dm_odm,
 				   u8 antsel_tr_mux, u32 mac_id, u8 rx_pwdb_all)
 {
 	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[mac_id] += rx_pwdb_all;
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index 35c91e06cc47..054f5996f60d 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -1005,6 +1005,7 @@ static void phy_iq_calibrate(struct adapter *adapt, s32 result[][8],
 					      rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD};
 
 	u32 retry_count = 9;
+
 	if (*(dm_odm->mp_mode) == 1)
 		retry_count = 9;
 	else
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
index d0f59b7836f1..9b7ba9bffb0d 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
@@ -210,6 +210,7 @@ void rtl8188e_set_FwMediaStatus_cmd(struct adapter *adapt, __le16 mstatus_rpt)
 {
 	u8 opmode, macid;
 	u16 mst_rpt = le16_to_cpu(mstatus_rpt);
+
 	opmode = (u8)mst_rpt;
 	macid = (u8)(mst_rpt >> 8);
 
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index 0ce7db723a5d..3673f573ac3d 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -70,6 +70,7 @@ s32 iol_execute(struct adapter *padapter, u8 control)
 static s32 iol_InitLLTTable(struct adapter *padapter, u8 txpktbuf_bndy)
 {
 	s32 rst = _SUCCESS;
+
 	iol_mode_enable(padapter, 1);
 	usb_write8(padapter, REG_TDECTRL+1, txpktbuf_bndy);
 	rst = iol_execute(padapter, CMD_INIT_LLT);
@@ -459,7 +460,7 @@ void Hal_ReadPowerSavingMode88E(struct adapter *padapter, u8 *hwinfo, bool AutoL
 		padapter->pwrctrlpriv.bSupportRemoteWakeup = (hwinfo[EEPROM_USB_OPTIONAL_FUNCTION0] & BIT(1)) ? true : false;
 
 		DBG_88E("%s...bHWPwrPindetect(%x)-bHWPowerdown(%x) , bSupportRemoteWakeup(%x)\n", __func__,
-		padapter->pwrctrlpriv.bHWPwrPindetect, padapter->pwrctrlpriv.bHWPowerdown , padapter->pwrctrlpriv.bSupportRemoteWakeup);
+		padapter->pwrctrlpriv.bHWPwrPindetect, padapter->pwrctrlpriv.bHWPowerdown, padapter->pwrctrlpriv.bSupportRemoteWakeup);
 
 		DBG_88E("### PS params =>  power_mgnt(%x), usbss_enable(%x) ###\n", padapter->registrypriv.power_mgnt, padapter->registrypriv.usbss_enable);
 	}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
index 53e312aaefb5..a9912b60eb59 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
@@ -346,7 +346,7 @@ static s32 rtw_dump_xframe(struct adapter *adapt, struct xmit_frame *pxmitframe)
 	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) &&
@@ -365,7 +365,7 @@ static s32 rtw_dump_xframe(struct adapter *adapt, struct xmit_frame *pxmitframe)
 			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("pattrib->nr_frags=%d\n", pattrib->nr_frags));
 
 			sz = pxmitpriv->frag_len;
-			sz = sz - 4 - (psecuritypriv->sw_encrypt ? 0 : pattrib->icv_len);
+			sz = sz - 4 - pattrib->icv_len;
 		} else {
 			/* no frag */
 			sz = pattrib->last_txcmdsz;
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index 3675edb61942..674ac5396d00 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -337,6 +337,7 @@ static void _InitTransferPageSize(struct adapter *Adapter)
 	/*  Tx page size is always 128. */
 
 	u8 value8;
+
 	value8 = _PSRX(PBP_128) | _PSTX(PBP_128);
 	usb_write8(Adapter, REG_PBP, value8);
 }
@@ -1361,6 +1362,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 		if (*((u8 *)val)) { /* under sitesurvey */
 			/* config RCR to receive different BSSID & not to receive data frame */
 			u32 v = usb_read32(Adapter, REG_RCR);
+
 			v &= ~(RCR_CBSSID_BCN);
 			usb_write32(Adapter, REG_RCR, v);
 			/* reject all data frame */
@@ -1514,6 +1516,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 	case HW_VAR_CAM_WRITE:
 		{
 			u32 cmd;
+
 			u32 *cam_val = (u32 *)val;
 			usb_write32(Adapter, WCAMI, cam_val[0]);
 
@@ -1618,6 +1621,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 	case HW_VAR_RXDMA_AGG_PG_TH:
 		{
 			u8 threshold = *((u8 *)val);
+
 			if (threshold == 0)
 				threshold = haldata->UsbRxAggPageCount;
 			usb_write8(Adapter, REG_RXDMA_AGG_PG_TH, threshold);
@@ -1639,6 +1643,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 	case HW_VAR_H2C_FW_JOINBSSRPT:
 		{
 			u8 mstatus = (*(u8 *)val);
+
 			rtl8188e_set_FwJoinBssReport_cmd(Adapter, mstatus);
 		}
 		break;
@@ -1661,6 +1666,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 	case HW_VAR_RPT_TIMER_SETTING:
 		{
 			u16 min_rpt_time = (*(u16 *)val);
+
 			ODM_RA_Set_TxRPT_Time(podmpriv, min_rpt_time);
 		}
 		break;
@@ -1717,6 +1723,7 @@ void rtw_hal_set_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 	case HW_VAR_TX_RPT_MAX_MACID:
 		{
 			u8 maxMacid = *val;
+
 			DBG_88E("### MacID(%d),Set Max Tx RPT MID(%d)\n", maxMacid, maxMacid+1);
 			usb_write8(Adapter, REG_TX_RPT_CTRL+1, maxMacid+1);
 		}
@@ -1745,9 +1752,6 @@ void rtw_hal_get_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 		/* BCN_VALID, BIT16 of REG_TDECTRL = BIT0 of REG_TDECTRL+2 */
 		val[0] = (BIT(0) & usb_read8(Adapter, REG_TDECTRL+2)) ? true : false;
 		break;
-	case HW_VAR_RF_TYPE:
-		val[0] = RF_1T1R;
-		break;
 	case HW_VAR_FWLPS_RF_ON:
 		{
 			/* When we halt NIC, we should check if FW LPS is leave. */
@@ -1757,6 +1761,7 @@ void rtw_hal_get_hwreg(struct adapter *Adapter, u8 variable, u8 *val)
 				val[0] = true;
 			} else {
 				u32 valRCR;
+
 				valRCR = usb_read32(Adapter, REG_RCR);
 				valRCR &= 0x00070000;
 				if (valRCR)
@@ -1802,6 +1807,7 @@ u8 rtw_hal_get_def_var(
 			struct mlme_priv *pmlmepriv = &Adapter->mlmepriv;
 			struct sta_priv *pstapriv = &Adapter->stapriv;
 			struct sta_info *psta;
+
 			psta = rtw_get_stainfo(pstapriv, pmlmepriv->cur_network.network.MacAddress);
 			if (psta)
 				*((int *)pValue) = psta->rssi_stat.UndecoratedSmoothedPWDB;
@@ -1828,18 +1834,21 @@ u8 rtw_hal_get_def_var(
 	case HAL_DEF_RA_DECISION_RATE:
 		{
 			u8 MacID = *((u8 *)pValue);
+
 			*((u8 *)pValue) = ODM_RA_GetDecisionRate_8188E(&haldata->odmpriv, MacID);
 		}
 		break;
 	case HAL_DEF_RA_SGI:
 		{
 			u8 MacID = *((u8 *)pValue);
+
 			*((u8 *)pValue) = ODM_RA_GetShortGI_8188E(&haldata->odmpriv, MacID);
 		}
 		break;
 	case HAL_DEF_PT_PWR_STATUS:
 		{
 			u8 MacID = *((u8 *)pValue);
+
 			*((u8 *)pValue) = ODM_RA_GetHwPwrStatus_8188E(&haldata->odmpriv, MacID);
 		}
 		break;
@@ -1849,6 +1858,7 @@ u8 rtw_hal_get_def_var(
 	case HW_DEF_RA_INFO_DUMP:
 		{
 			u8 entry_id = *((u8 *)pValue);
+
 			if (check_fwstate(&Adapter->mlmepriv, _FW_LINKED)) {
 				DBG_88E("============ RA status check ===================\n");
 				DBG_88E("Mac_id:%d , RateID = %d, RAUseRate = 0x%08x, RateSGI = %d, DecisionRate = 0x%02x ,PTStage = %d\n",
@@ -1864,6 +1874,7 @@ u8 rtw_hal_get_def_var(
 	case HW_DEF_ODM_DBG_FLAG:
 		{
 			struct odm_dm_struct *dm_ocm = &haldata->odmpriv;
+
 			pr_info("dm_ocm->DebugComponents = 0x%llx\n", dm_ocm->DebugComponents);
 		}
 		break;
diff --git a/drivers/staging/rtl8188eu/include/Hal8188EPhyReg.h b/drivers/staging/rtl8188eu/include/Hal8188EPhyReg.h
index 344c73d1081b..04159a9f90d3 100644
--- a/drivers/staging/rtl8188eu/include/Hal8188EPhyReg.h
+++ b/drivers/staging/rtl8188eu/include/Hal8188EPhyReg.h
@@ -179,7 +179,7 @@
 
 /* RxIQ DC offset, Rx digital filter, DC notch filter */
 #define	rOFDM0_XARxAFE			0xc10
-#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imblance matrix */
+#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
 #define	rOFDM0_XBRxAFE			0xc18
 #define	rOFDM0_XBRxIQImbalance		0xc1c
 #define	rOFDM0_XCRxAFE			0xc20
diff --git a/drivers/staging/rtl8188eu/include/drv_types.h b/drivers/staging/rtl8188eu/include/drv_types.h
index 0fd2a2d9be20..c3517c0903ca 100644
--- a/drivers/staging/rtl8188eu/include/drv_types.h
+++ b/drivers/staging/rtl8188eu/include/drv_types.h
@@ -65,8 +65,6 @@ struct registry_priv {
 	u8	ips_mode;
 	u8	smart_ps;
 	u8	mp_mode;
-	u8	software_encrypt;
-	u8	software_decrypt;
 	u8	acm_method;
 	  /* UAPSD */
 	u8	wmm_enable;
diff --git a/drivers/staging/rtl8188eu/include/hal_intf.h b/drivers/staging/rtl8188eu/include/hal_intf.h
index e1114a95d442..dfdbd0254886 100644
--- a/drivers/staging/rtl8188eu/include/hal_intf.h
+++ b/drivers/staging/rtl8188eu/include/hal_intf.h
@@ -57,7 +57,6 @@ enum hw_variables {
 	HW_VAR_ACK_PREAMBLE,
 	HW_VAR_SEC_CFG,
 	HW_VAR_BCN_VALID,
-	HW_VAR_RF_TYPE,
 	HW_VAR_DM_FUNC_OP,
 	HW_VAR_DM_FUNC_SET,
 	HW_VAR_DM_FUNC_CLR,
diff --git a/drivers/staging/rtl8188eu/include/ieee80211.h b/drivers/staging/rtl8188eu/include/ieee80211.h
index fc58621368c1..22ab0c43e376 100644
--- a/drivers/staging/rtl8188eu/include/ieee80211.h
+++ b/drivers/staging/rtl8188eu/include/ieee80211.h
@@ -857,12 +857,12 @@ enum secondary_ch_offset {
 	SCB = 3,  /* secondary channel below */
 };
 
-u8 *rtw_get_ie(u8 *pbuf, int index, int *len, int limit);
+u8 *rtw_get_ie(u8 *pbuf, int index, uint *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);
+unsigned char *rtw_get_wpa_ie(unsigned char *pie, uint *wpa_ie_len, int limit);
+unsigned char *rtw_get_wpa2_ie(unsigned char *pie, uint *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);
diff --git a/drivers/staging/rtl8188eu/include/odm_RegDefine11N.h b/drivers/staging/rtl8188eu/include/odm_RegDefine11N.h
index c82c09013487..f46f7d43ce00 100644
--- a/drivers/staging/rtl8188eu/include/odm_RegDefine11N.h
+++ b/drivers/staging/rtl8188eu/include/odm_RegDefine11N.h
@@ -41,8 +41,8 @@
 #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_DEFUALT_A_11N		0x858
-#define	ODM_REG_RX_DEFUALT_B_11N		0x85A
+#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
diff --git a/drivers/staging/rtl8188eu/include/odm_debug.h b/drivers/staging/rtl8188eu/include/odm_debug.h
index 52e51f19f752..687ff3e9c09a 100644
--- a/drivers/staging/rtl8188eu/include/odm_debug.h
+++ b/drivers/staging/rtl8188eu/include/odm_debug.h
@@ -37,7 +37,7 @@
 /*	resource allocation failed, unexpected HW behavior, HW BUG and so on. */
 #define ODM_DBG_SERIOUS				2
 
-/*	Abnormal, rare, or unexpeted cases. */
+/*	Abnormal, rare, or unexpected cases. */
 /*	For example, IRP/Packet/OID canceled, device suprisely unremoved and so on. */
 #define ODM_DBG_WARNING				3
 
diff --git a/drivers/staging/rtl8188eu/include/pwrseq.h b/drivers/staging/rtl8188eu/include/pwrseq.h
index afd61cf4cb15..addf90b60ce9 100644
--- a/drivers/staging/rtl8188eu/include/pwrseq.h
+++ b/drivers/staging/rtl8188eu/include/pwrseq.h
@@ -29,7 +29,7 @@
 	4: LPS--Low Power State
 	5: SUS--Suspend
 
-	The transision from different states are defined below
+	The transition from different states are defined below
 	TRANS_CARDEMU_TO_ACT
 	TRANS_ACT_TO_CARDEMU
 	TRANS_CARDEMU_TO_SUS
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_spec.h b/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
index fb82f663b1f5..c93e19d1c50f 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
@@ -1350,7 +1350,7 @@ Current IOREG MAP
 #define EEPROM_Default_CrystalCap_88E		0x20
 #define	EEPROM_Default_ThermalMeter_88E		0x18
 
-/* New EFUSE deafult value */
+/* New EFUSE default value */
 #define		EEPROM_DEFAULT_24G_INDEX	0x2D
 #define		EEPROM_DEFAULT_24G_HT20_DIFF	0X02
 #define		EEPROM_DEFAULT_24G_OFDM_DIFF	0X04
diff --git a/drivers/staging/rtl8188eu/include/rtw_cmd.h b/drivers/staging/rtl8188eu/include/rtw_cmd.h
index 18a6530c9dde..f79feeb4e38f 100644
--- a/drivers/staging/rtl8188eu/include/rtw_cmd.h
+++ b/drivers/staging/rtl8188eu/include/rtw_cmd.h
@@ -214,7 +214,7 @@ struct set_assocsta_rsp {
 
 	mac[0] == 0
 	==> CMD mode, return H2C_SUCCESS.
-	The following condition must be ture under CMD mode
+	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);
diff --git a/drivers/staging/rtl8188eu/include/rtw_ioctl.h b/drivers/staging/rtl8188eu/include/rtw_ioctl.h
index a6b1c854a061..0fa78ed2c1ab 100644
--- a/drivers/staging/rtl8188eu/include/rtw_ioctl.h
+++ b/drivers/staging/rtl8188eu/include/rtw_ioctl.h
@@ -58,12 +58,6 @@
 #define OID_MP_SEG3		0xFF818700
 #define OID_MP_SEG4		0xFF011100
 
-#define DEBUG_OID(dbg, str)						\
-	if ((!dbg)) {							\
-		RT_TRACE(_module_rtl871x_ioctl_c_, _drv_info_,		\
-			 ("%s(%d): %s", __func__, __line__, str));	\
-	}
-
 enum oid_type {
 	QUERY_OID,
 	SET_OID
diff --git a/drivers/staging/rtl8188eu/include/rtw_mlme.h b/drivers/staging/rtl8188eu/include/rtw_mlme.h
index 7324a95bb162..5c5d0ae8bdd1 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mlme.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mlme.h
@@ -216,7 +216,6 @@ 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);
diff --git a/drivers/staging/rtl8188eu/include/rtw_mp_phy_regdef.h b/drivers/staging/rtl8188eu/include/rtw_mp_phy_regdef.h
index 02b300217185..4872a21b3103 100644
--- a/drivers/staging/rtl8188eu/include/rtw_mp_phy_regdef.h
+++ b/drivers/staging/rtl8188eu/include/rtw_mp_phy_regdef.h
@@ -198,7 +198,7 @@
 #define	rOFDM0_TRSWIsolation		0xc0c
 
 #define	rOFDM0_XARxAFE			0xc10  /* RxIQ DC offset, Rx digital filter, DC notch filter */
-#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imblance matrix */
+#define	rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imbalance matrix */
 #define	rOFDM0_XBRxAFE			0xc18
 #define	rOFDM0_XBRxIQImbalance		0xc1c
 #define	rOFDM0_XCRxAFE			0xc20
diff --git a/drivers/staging/rtl8188eu/include/rtw_recv.h b/drivers/staging/rtl8188eu/include/rtw_recv.h
index b369f08d4a15..121150860450 100644
--- a/drivers/staging/rtl8188eu/include/rtw_recv.h
+++ b/drivers/staging/rtl8188eu/include/rtw_recv.h
@@ -106,7 +106,7 @@ struct rx_pkt_attrib {
 	u8	privacy; /* in frame_ctrl field */
 	u8	bdecrypted;
 	u8	encrypt; /* when 0 indicate no encrypt. when non-zero,
-			  * indicate the encrypt algorith */
+			  * indicate the encrypt algorithm */
 	u8	iv_len;
 	u8	icv_len;
 	u8	crc_err;
@@ -176,7 +176,7 @@ struct recv_priv {
 	struct sk_buff_head rx_skb_queue;
 	struct recv_buf *precv_buf;    /*  4 alignment */
 	struct __queue free_recv_buf_queue;
-	/* For display the phy informatiom */
+	/* For display the phy information */
 	s8 rssi;
 	s8 rxpwdb;
 	u8 signal_strength;
diff --git a/drivers/staging/rtl8188eu/include/rtw_security.h b/drivers/staging/rtl8188eu/include/rtw_security.h
index 7100d6b01b32..74fe664787e5 100644
--- a/drivers/staging/rtl8188eu/include/rtw_security.h
+++ b/drivers/staging/rtl8188eu/include/rtw_security.h
@@ -133,8 +133,6 @@ struct security_priv {
 	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. */
 
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index 5f6a24546a91..add1ba00f3e9 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -49,9 +49,6 @@ MODULE_PARM_DESC(rtw_ips_mode, "The default IPS mode");
 
 static int rtw_debug = 1;
 
-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. */
@@ -166,8 +163,6 @@ static void loadparam(struct adapter *padapter, struct net_device *pnetdev)
 	registry_par->power_mgnt = (u8)rtw_power_mgnt;
 	registry_par->ips_mode = (u8)rtw_ips_mode;
 	registry_par->mp_mode = 0;
-	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 */
@@ -393,8 +388,6 @@ static u8 rtw_init_default_value(struct adapter *padapter)
 
 	/* security_priv */
 	psecuritypriv->binstallGrpkey = _FAIL;
-	psecuritypriv->sw_encrypt = pregistrypriv->software_encrypt;
-	psecuritypriv->sw_decrypt = pregistrypriv->software_decrypt;
 	psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
 	psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
 	psecuritypriv->dot11PrivacyKeyIndex = 0;
diff --git a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
index e097c619ed1b..8bf8248e4ac7 100644
--- a/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/xmit_linux.c
@@ -72,7 +72,7 @@ int rtw_os_xmit_resource_alloc(struct adapter *padapter, struct xmit_buf *pxmitb
 
 	for (i = 0; i < 8; i++) {
 		pxmitbuf->pxmit_urb[i] = usb_alloc_urb(0, GFP_KERNEL);
-		if (pxmitbuf->pxmit_urb[i] == NULL) {
+		if (!pxmitbuf->pxmit_urb[i]) {
 			DBG_88E("pxmitbuf->pxmit_urb[i]==NULL");
 			return _FAIL;
 		}
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
index 34453e38ba93..03421033d14a 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_def.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 
 #ifndef R8190P_DEF_H
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
index 81b3cf6588d4..85f93056d28b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include "rtl_core.h"
 #include "r8192E_phyreg.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
index 7873a73a2f76..bbea13b452b2 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #ifndef RTL8225H
 #define RTL8225H
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 757ffd4f2f89..467287ae6c1c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include "rtl_core.h"
 #include "r8192E_hw.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
index 7dd15d993481..a8c63ad2ac2e 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef R819XUSB_CMDPKT_H
 #define R819XUSB_CMDPKT_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 8d6bca61e7aa..4723a0bd5067 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #include "rtl_core.h"
 #include "r8192E_phy.h"
 #include "r8192E_phyreg.h"
@@ -135,7 +135,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 	{
 		u32	RegRCR, Type;
 
-		Type = ((u8 *)(val))[0];
+		Type = val[0];
 		RegRCR = rtl92e_readl(dev, RCR);
 		priv->ReceiveConfig = RegRCR;
 
@@ -161,7 +161,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 	{
 		u32 regTmp;
 
-		priv->short_preamble = (bool)(*(u8 *)val);
+		priv->short_preamble = (bool)*val;
 		regTmp = priv->basic_rate;
 		if (priv->short_preamble)
 			regTmp |= BRSR_AckShortPmb;
@@ -175,7 +175,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 
 	case HW_VAR_AC_PARAM:
 	{
-		u8	pAcParam = *((u8 *)val);
+		u8	pAcParam = *val;
 		u32	eACI = pAcParam;
 		u8		u1bAIFS;
 		u32		u4bAcParam;
@@ -221,7 +221,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 			break;
 		}
 		priv->rtllib->SetHwRegHandler(dev, HW_VAR_ACM_CTRL,
-					      (u8 *)(&pAcParam));
+					      &pAcParam);
 		break;
 	}
 
@@ -229,7 +229,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 	{
 		struct rtllib_qos_parameters *qos_parameters =
 			 &priv->rtllib->current_network.qos_data.parameters;
-		u8 pAcParam = *((u8 *)val);
+		u8 pAcParam = *val;
 		u32 eACI = pAcParam;
 		union aci_aifsn *pAciAifsn = (union aci_aifsn *) &
 					      (qos_parameters->aifs[0]);
@@ -293,7 +293,7 @@ void rtl92e_set_reg(struct net_device *dev, u8 variable, u8 *val)
 
 	case HW_VAR_RF_TIMING:
 	{
-		u8 Rf_Timing = *((u8 *)val);
+		u8 Rf_Timing = *val;
 
 		rtl92e_writeb(dev, rFPGA0_RFTiming1, Rf_Timing);
 		break;
@@ -372,7 +372,7 @@ static void _rtl92e_read_eeprom_info(struct net_device *dev)
 	if (!priv->AutoloadFailFlag) {
 		for (i = 0; i < 6; i += 2) {
 			usValue = rtl92e_eeprom_read(dev,
-				 (u16)((EEPROM_NODE_ADDRESS_BYTE_0 + i) >> 1));
+				 (EEPROM_NODE_ADDRESS_BYTE_0 + i) >> 1);
 			*(u16 *)(&dev->dev_addr[i]) = usValue;
 		}
 	} else {
@@ -436,8 +436,7 @@ static void _rtl92e_read_eeprom_info(struct net_device *dev)
 			for (i = 0; i < 14; i += 2) {
 				if (!priv->AutoloadFailFlag)
 					usValue = rtl92e_eeprom_read(dev,
-						  (u16)((EEPROM_TxPwIndex_CCK +
-						  i) >> 1));
+						  (EEPROM_TxPwIndex_CCK + i) >> 1);
 				else
 					usValue = EEPROM_Default_TxPower;
 				*((u16 *)(&priv->EEPROMTxPowerLevelCCK[i])) =
@@ -452,8 +451,7 @@ static void _rtl92e_read_eeprom_info(struct net_device *dev)
 			for (i = 0; i < 14; i += 2) {
 				if (!priv->AutoloadFailFlag)
 					usValue = rtl92e_eeprom_read(dev,
-						(u16)((EEPROM_TxPwIndex_OFDM_24G
-						+ i) >> 1));
+						(EEPROM_TxPwIndex_OFDM_24G + i) >> 1);
 				else
 					usValue = EEPROM_Default_TxPower;
 				*((u16 *)(&priv->EEPROMTxPowerLevelOFDM24G[i]))
@@ -1650,15 +1648,11 @@ static void _rtl92e_query_rxphystatus(
 			evm = rtl92e_evm_db_to_percent(rx_evmX);
 			if (bpacket_match_bssid) {
 				if (i == 0) {
-					pstats->SignalQuality = (u8)(evm &
-								 0xff);
-					precord_stats->SignalQuality = (u8)(evm
-									& 0xff);
+					pstats->SignalQuality = evm & 0xff;
+					precord_stats->SignalQuality = evm & 0xff;
 				}
-				pstats->RxMIMOSignalQuality[i] = (u8)(evm &
-								 0xff);
-				precord_stats->RxMIMOSignalQuality[i] = (u8)(evm
-									& 0xff);
+				pstats->RxMIMOSignalQuality[i] = evm & 0xff;
+				precord_stats->RxMIMOSignalQuality[i] = evm & 0xff;
 			}
 		}
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
index 6bb58193fc5c..f4233bb12f81 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.h
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _RTL8192E_H
 #define _RTL8192E_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index bbe399010be1..3c7831250987 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include "rtl_core.h"
 #include "r8192E_hw.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
index b48ec9410c09..61c8dac826a8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef __INC_FIRMWARE_H
 #define __INC_FIRMWARE_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
index d298023ef079..5c20cb476281 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hw.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 
 #ifndef R8180_HW
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
index d96b87d77e7a..4e2bbab6a413 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_hwimg.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef __INC_HAL8192PciE_FW_IMG_H
 #define __INC_HAL8192PciE_FW_IMG_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index dde492261451..73497d559b77 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include <linux/bitops.h>
 #include "rtl_core.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
index 9ddfd4e3adb3..b534d72bf708 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _R819XU_PHY_H
 #define _R819XU_PHY_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
index 50c79d3dc782..03d6d70b2d28 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phyreg.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _R819XU_PHYREG_H
 #define _R819XU_PHYREG_H
 
@@ -692,7 +692,7 @@
  * #define bRxPath4		0x08
  * #define bTxPath1		0x10
  * #define bTxPath2		0x20
-*/
+ */
 #define bHTDetect		0x100
 #define bCFOEn			0x10000
 #define bCFOValue		0xfff00000
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
index 30f65af4d614..c62481fcf0b1 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #include "rtl_core.h"
 #include "r8192E_phy.h"
 #include "r8192E_phyreg.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
index aa12941dc685..12f01f196752 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.h
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _RTL_CAM_H
 #define _RTL_CAM_H
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 4c0caa6701a9..a4d1bac4a844 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #include <linux/uaccess.h>
 #include <linux/pci.h>
 #include <linux/vmalloc.h>
@@ -101,8 +101,8 @@ static int _rtl92e_down(struct net_device *dev, bool shutdownrf);
 static void _rtl92e_restart(void *data);
 
 /****************************************************************************
-   -----------------------------IO STUFF-------------------------
-*****************************************************************************/
+ *  -----------------------------IO STUFF-------------------------
+ ****************************************************************************/
 
 u8 rtl92e_readb(struct net_device *dev, int x)
 {
@@ -141,8 +141,8 @@ void rtl92e_writew(struct net_device *dev, int x, u16 y)
 }
 
 /****************************************************************************
-   -----------------------------GENERAL FUNCTION-------------------------
-*****************************************************************************/
+ *  -----------------------------GENERAL FUNCTION-------------------------
+ ****************************************************************************/
 bool rtl92e_set_rf_state(struct net_device *dev,
 			 enum rt_rf_power_state StateToSet,
 			 RT_RF_CHANGE_SOURCE ChangeSource)
@@ -346,7 +346,7 @@ static void _rtl92e_update_cap(struct net_device *dev, u16 cap)
 		}
 	}
 
-	if (net->mode & (IEEE_G|IEEE_N_24G)) {
+	if (net->mode & (IEEE_G | IEEE_N_24G)) {
 		u8	slot_time_val;
 		u8	CurSlotTime = priv->slot_time;
 
@@ -477,7 +477,7 @@ static int _rtl92e_qos_assoc_resp(struct r8192_priv *priv,
 	u32 size = sizeof(struct rtllib_qos_parameters);
 	int set_qos_param = 0;
 
-	if ((priv == NULL) || (network == NULL))
+	if (!priv || !network)
 		return 0;
 
 	if (priv->rtllib->state != RTLLIB_LINKED)
@@ -680,7 +680,7 @@ static u8 _rtl92e_get_supported_wireless_mode(struct net_device *dev)
 	case RF_8256:
 	case RF_6052:
 	case RF_PSEUDO_11N:
-		ret = (WIRELESS_MODE_N_24G|WIRELESS_MODE_G | WIRELESS_MODE_B);
+		ret = (WIRELESS_MODE_N_24G | WIRELESS_MODE_G | WIRELESS_MODE_B);
 		break;
 	case RF_8258:
 		ret = (WIRELESS_MODE_A | WIRELESS_MODE_N_5G);
@@ -742,7 +742,7 @@ 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));
+					(&priv->rtllib->PowerSaveControl);
 	bool init_status = true;
 
 	priv->bDriverIsGoingToUnload = false;
@@ -790,7 +790,7 @@ static int _rtl92e_sta_down(struct net_device *dev, bool shutdownrf)
 	if (priv->up == 0)
 		return -1;
 
-	if (priv->rtllib->rtllib_ips_leave != NULL)
+	if (priv->rtllib->rtllib_ips_leave)
 		priv->rtllib->rtllib_ips_leave(dev);
 
 	if (priv->rtllib->state == RTLLIB_LINKED)
@@ -888,7 +888,7 @@ 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);
+					&priv->rtllib->PowerSaveControl;
 
 	pPSC->RegMaxLPSAwakeIntvl = 5;
 }
@@ -1015,9 +1015,9 @@ static void _rtl92e_init_priv_task(struct net_device *dev)
 			      (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);
+			      (void *)rtl92e_hw_wakeup_wq, dev);
 	INIT_DELAYED_WORK_RSL(&priv->rtllib->hw_sleep_wq,
-			      (void *) rtl92e_hw_sleep_wq, dev);
+			      (void *)rtl92e_hw_sleep_wq, dev);
 	tasklet_init(&priv->irq_rx_tasklet,
 		     (void(*)(unsigned long))_rtl92e_irq_rx_tasklet,
 		     (unsigned long)priv);
@@ -1035,8 +1035,8 @@ static short _rtl92e_get_channel_map(struct net_device *dev)
 
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	if ((priv->rf_chip != RF_8225) && (priv->rf_chip != RF_8256)
-			&& (priv->rf_chip != RF_6052)) {
+	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;
@@ -1062,7 +1062,7 @@ static short _rtl92e_init(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 
-	memset(&(priv->stats), 0, sizeof(struct rt_stats));
+	memset(&priv->stats, 0, sizeof(struct rt_stats));
 
 	_rtl92e_init_priv_handler(dev);
 	_rtl92e_init_priv_constant(dev);
@@ -1077,7 +1077,7 @@ static short _rtl92e_init(struct net_device *dev)
 
 	setup_timer(&priv->watch_dog_timer,
 		    _rtl92e_watchdog_timer_cb,
-		    (unsigned long) dev);
+		    (unsigned long)dev);
 
 	setup_timer(&priv->gpio_polling_timer,
 		    rtl92e_check_rfctrl_gpio_timer,
@@ -1102,8 +1102,8 @@ static short _rtl92e_init(struct net_device *dev)
 }
 
 /***************************************************************************
-	-------------------------------WATCHDOG STUFF---------------------------
-***************************************************************************/
+ * -------------------------------WATCHDOG STUFF---------------------------
+ **************************************************************************/
 static short _rtl92e_is_tx_queue_empty(struct net_device *dev)
 {
 	int i = 0;
@@ -1134,7 +1134,7 @@ static enum reset_type _rtl92e_tx_check_stuck(struct net_device *dev)
 	switch (priv->rtllib->ps) {
 	case RTLLIB_PS_DISABLED:
 		break;
-	case (RTLLIB_PS_MBCAST|RTLLIB_PS_UNICAST):
+	case (RTLLIB_PS_MBCAST | RTLLIB_PS_UNICAST):
 		break;
 	default:
 		break;
@@ -1387,7 +1387,7 @@ static void _rtl92e_watchdog_wq_cb(void *data)
 	static u8 check_reset_cnt;
 	unsigned long flags;
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
-					(&(priv->rtllib->PowerSaveControl));
+					(&priv->rtllib->PowerSaveControl);
 	bool bBusyTraffic = false;
 	bool	bHigherBusyTraffic = false;
 	bool	bHigherBusyRxTraffic = false;
@@ -1469,7 +1469,7 @@ static void _rtl92e_watchdog_wq_cb(void *data)
 
 		_rtl92e_update_rxcounts(priv, &TotalRxBcnNum, &TotalRxDataNum);
 
-		if ((TotalRxBcnNum+TotalRxDataNum) == 0)
+		if ((TotalRxBcnNum + TotalRxDataNum) == 0)
 			priv->check_roaming_cnt++;
 		else
 			priv->check_roaming_cnt = 0;
@@ -1497,7 +1497,7 @@ static void _rtl92e_watchdog_wq_cb(void *data)
 			notify_wx_assoc_event(ieee);
 
 			if (!(ieee->rtllib_ap_sec_type(ieee) &
-			     (SEC_ALG_CCMP|SEC_ALG_TKIP)))
+			     (SEC_ALG_CCMP | SEC_ALG_TKIP)))
 				schedule_delayed_work(
 					&ieee->associate_procedure_wq, 0);
 
@@ -1541,8 +1541,8 @@ static void _rtl92e_watchdog_timer_cb(unsigned long data)
 }
 
 /****************************************************************************
- ---------------------------- NIC TX/RX STUFF---------------------------
-*****************************************************************************/
+ * ---------------------------- NIC TX/RX STUFF---------------------------
+ ****************************************************************************/
 void rtl92e_rx_enable(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
@@ -1603,7 +1603,7 @@ static void _rtl92e_free_tx_ring(struct net_device *dev, unsigned int prio)
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
 
-	pci_free_consistent(priv->pdev, sizeof(*ring->desc)*ring->entries,
+	pci_free_consistent(priv->pdev, sizeof(*ring->desc) * ring->entries,
 	ring->desc, ring->dma);
 	ring->desc = NULL;
 }
@@ -1712,7 +1712,7 @@ static void _rtl92e_tx_cmd(struct net_device *dev, struct sk_buff *skb)
 	ring = &priv->tx_ring[TXCMD_QUEUE];
 
 	idx = (ring->idx + skb_queue_len(&ring->queue)) % ring->entries;
-	entry = (struct tx_desc_cmd *) &ring->desc[idx];
+	entry = (struct tx_desc_cmd *)&ring->desc[idx];
 
 	tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
 
@@ -2031,7 +2031,7 @@ static void _rtl92e_rx_normal(struct net_device *dev)
 
 	struct rtllib_rx_stats stats = {
 		.signal = 0,
-		.noise = (u8) -98,
+		.noise = (u8)-98,
 		.rate = 0,
 		.freq = RTLLIB_24GHZ_BAND,
 	};
@@ -2104,7 +2104,7 @@ static void _rtl92e_rx_normal(struct net_device *dev)
 
 		priv->rx_buf[rx_queue_idx][priv->rx_idx[rx_queue_idx]] =
 								 skb;
-		*((dma_addr_t *) skb->cb) = pci_map_single(priv->pdev,
+		*((dma_addr_t *)skb->cb) = pci_map_single(priv->pdev,
 					    skb_tail_pointer_rsl(skb),
 					    priv->rxbuffersize,
 					    PCI_DMA_FROMDEVICE);
@@ -2117,7 +2117,7 @@ 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)
+		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;
@@ -2156,8 +2156,8 @@ static void _rtl92e_irq_rx_tasklet(struct r8192_priv *priv)
 }
 
 /****************************************************************************
- ---------------------------- NIC START/CLOSE STUFF---------------------------
-*****************************************************************************/
+ * ---------------------------- NIC START/CLOSE STUFF---------------------------
+ ****************************************************************************/
 static void _rtl92e_cancel_deferred_work(struct r8192_priv *priv)
 {
 	cancel_delayed_work_sync(&priv->watch_dog_wq);
@@ -2348,8 +2348,9 @@ static int _rtl92e_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 							       0, key);
 					}
 				}
-				if ((ieee->pairwise_key_type == KEY_TYPE_CCMP)
-				     && ieee->pHTInfo->bCurrentHTSupport) {
+				if ((ieee->pairwise_key_type ==
+				     KEY_TYPE_CCMP) &&
+				     ieee->pHTInfo->bCurrentHTSupport) {
 					rtl92e_writeb(dev, 0x173, 1);
 				}
 
@@ -2535,8 +2536,8 @@ done:
 
 
 /****************************************************************************
-	---------------------------- PCI_STUFF---------------------------
-*****************************************************************************/
+ * ---------------------------- PCI_STUFF---------------------------
+ ****************************************************************************/
 static const struct net_device_ops rtl8192_netdev_ops = {
 	.ndo_open = _rtl92e_open,
 	.ndo_stop = _rtl92e_close,
@@ -2726,7 +2727,7 @@ 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));
+					(&priv->rtllib->PowerSaveControl);
 
 	if (!priv->up) {
 		netdev_warn(dev, "%s(): Driver is already down!\n", __func__);
@@ -2751,6 +2752,7 @@ bool rtl92e_enable_nic(struct net_device *dev)
 	RT_TRACE(COMP_PS, "<===========%s()\n", __func__);
 	return init_status;
 }
+
 bool rtl92e_disable_nic(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
@@ -2785,8 +2787,8 @@ void rtl92e_check_rfctrl_gpio_timer(unsigned long data)
 }
 
 /***************************************************************************
-	------------------- module init / exit stubs ----------------
-****************************************************************************/
+ * ------------------- module init / exit stubs ----------------
+ ***************************************************************************/
 MODULE_DESCRIPTION("Linux driver for Realtek RTL819x WiFi cards");
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_VERSION(DRV_VERSION);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
index babc0b3bce95..0335823e2766 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.h
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #ifndef _RTL_CORE_H
 #define _RTL_CORE_H
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index dbb58fb16482..1a43c684f9f3 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #include "rtl_core.h"
 #include "rtl_dm.h"
 #include "r8192E_hw.h"
@@ -886,11 +886,14 @@ static void _rtl92e_dm_tx_power_tracking_cb_thermal(struct net_device *dev)
 		if (tmpCCK40Mindex >= CCK_Table_length)
 			tmpCCK40Mindex = CCK_Table_length-1;
 	} else {
-		tmpval = ((u8)tmpRegA - priv->ThermalMeter[0]);
-		if (tmpval >= 6)
-			tmpOFDMindex = tmpCCK20Mindex = 0;
-		else
-			tmpOFDMindex = tmpCCK20Mindex = 6 - tmpval;
+		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)
@@ -1330,7 +1333,7 @@ static void _rtl92e_dm_ctrl_initgain_byrssi(struct net_device *dev)
  *	When		Who		Remark
  *	03/04/2009	hpfan	Create Version 0.
  *
- *---------------------------------------------------------------------------*/
+ ******************************************************************************/
 
 static void _rtl92e_dm_ctrl_initgain_byrssi_driver(struct net_device *dev)
 {
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
index 756a0dd00d56..52a4a1522bae 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef	__R8192UDM_H__
 #define __R8192UDM_H__
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
index 162e06c0883c..e1d305d4fa20 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.c
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #include "rtl_core.h"
 #include "rtl_eeprom.h"
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
index d63e8b0c185b..6212e5eadede 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_eeprom.h
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 
 #define EPROM_DELAY 10
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
index 9e04dc29fbbb..3e3273d3e043 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include "rtl_core.h"
 #include "r8192E_hw.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
index 7625e3f31ea8..03fe79ff5a1b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_pm.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #ifndef R8192E_PM_H
 #define R8192E_PM_H
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
index aa4b015c3cc7..9281116366d2 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
@@ -35,7 +35,7 @@ static void _rtl92e_hw_sleep(struct net_device *dev)
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
 		RT_TRACE(COMP_DBG,
-			 "_rtl92e_hw_sleep(): RF Change in progress!\n");
+			 "%s(): RF Change in progress!\n", __func__);
 		return;
 	}
 	spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
@@ -62,7 +62,7 @@ void rtl92e_hw_wakeup(struct net_device *dev)
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
 		RT_TRACE(COMP_DBG,
-			 "rtl92e_hw_wakeup(): RF Change in progress!\n");
+			 "%s(): RF Change in progress!\n", __func__);
 		schedule_delayed_work(&priv->rtllib->hw_wakeup_wq,
 				      msecs_to_jiffies(10));
 		return;
@@ -121,15 +121,15 @@ static void _rtl92e_ps_update_rf_state(struct net_device *dev)
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
 					&(priv->rtllib->PowerSaveControl);
 
-	RT_TRACE(COMP_PS, "_rtl92e_ps_update_rf_state() --------->\n");
+	RT_TRACE(COMP_PS, "%s() --------->\n", __func__);
 	pPSC->bSwRfProcessing = true;
 
-	RT_TRACE(COMP_PS, "_rtl92e_ps_update_rf_state(): Set RF to %s.\n",
+	RT_TRACE(COMP_PS, "%s(): Set RF to %s.\n", __func__,
 		 pPSC->eInactivePowerState == eRfOff ? "OFF" : "ON");
 	rtl92e_set_rf_state(dev, pPSC->eInactivePowerState, RF_CHANGE_BY_IPS);
 
 	pPSC->bSwRfProcessing = false;
-	RT_TRACE(COMP_PS, "_rtl92e_ps_update_rf_state() <---------\n");
+	RT_TRACE(COMP_PS, "%s() <---------\n", __func__);
 }
 
 void rtl92e_ips_enter(struct net_device *dev)
@@ -144,7 +144,7 @@ void rtl92e_ips_enter(struct net_device *dev)
 		if (rtState == eRfOn && !pPSC->bSwRfProcessing &&
 			(priv->rtllib->state != RTLLIB_LINKED) &&
 			(priv->rtllib->iw_mode != IW_MODE_MASTER)) {
-			RT_TRACE(COMP_PS, "rtl92e_ips_enter(): Turn off RF.\n");
+			RT_TRACE(COMP_PS, "%s(): Turn off RF.\n", __func__);
 			pPSC->eInactivePowerState = eRfOff;
 			priv->isRFOff = true;
 			priv->bInPowerSaveMode = true;
@@ -164,7 +164,7 @@ void rtl92e_ips_leave(struct net_device *dev)
 		rtState = priv->rtllib->eRFPowerState;
 		if (rtState != eRfOn  && !pPSC->bSwRfProcessing &&
 		    priv->rtllib->RfOffReason <= RF_CHANGE_BY_IPS) {
-			RT_TRACE(COMP_PS, "rtl92e_ips_leave(): Turn on RF.\n");
+			RT_TRACE(COMP_PS, "%s(): Turn on RF.\n", __func__);
 			pPSC->eInactivePowerState = eRfOn;
 			priv->bInPowerSaveMode = false;
 			_rtl92e_ps_update_rf_state(dev);
@@ -247,7 +247,7 @@ void rtl92e_leisure_ps_enter(struct net_device *dev)
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
 					&(priv->rtllib->PowerSaveControl);
 
-	RT_TRACE(COMP_PS, "rtl92e_leisure_ps_enter()...\n");
+	RT_TRACE(COMP_PS, "%s()...\n", __func__);
 	RT_TRACE(COMP_PS,
 		 "pPSC->bLeisurePs = %d, ieee->ps = %d,pPSC->LpsIdleCount is %d,RT_CHECK_FOR_HANG_PERIOD is %d\n",
 		 pPSC->bLeisurePs, priv->rtllib->ps, pPSC->LpsIdleCount,
@@ -265,7 +265,7 @@ void rtl92e_leisure_ps_enter(struct net_device *dev)
 			if (priv->rtllib->ps == RTLLIB_PS_DISABLED) {
 
 				RT_TRACE(COMP_LPS,
-					 "rtl92e_leisure_ps_enter(): Enter 802.11 power save mode...\n");
+					 "%s(): Enter 802.11 power save mode...\n", __func__);
 
 				if (!pPSC->bFwCtrlLPS) {
 					if (priv->rtllib->SetFwCmdHandler)
@@ -287,14 +287,14 @@ void rtl92e_leisure_ps_leave(struct net_device *dev)
 					&(priv->rtllib->PowerSaveControl);
 
 
-	RT_TRACE(COMP_PS, "rtl92e_leisure_ps_leave()...\n");
+	RT_TRACE(COMP_PS, "%s()...\n", __func__);
 	RT_TRACE(COMP_PS, "pPSC->bLeisurePs = %d, ieee->ps = %d\n",
 		pPSC->bLeisurePs, priv->rtllib->ps);
 
 	if (pPSC->bLeisurePs) {
 		if (priv->rtllib->ps != RTLLIB_PS_DISABLED) {
 			RT_TRACE(COMP_LPS,
-				 "rtl92e_leisure_ps_leave(): Busy Traffic , Leave 802.11 power save..\n");
+				 "%s(): Busy Traffic , Leave 802.11 power save..\n", __func__);
 			_rtl92e_ps_set_mode(dev, RTLLIB_PS_DISABLED);
 
 			if (!pPSC->bFwCtrlLPS) {
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 7413a100ca19..f802f60281f8 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #include <linux/string.h>
 #include "rtl_core.h"
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
index 7ecf6c5cf58b..c313fb79de4d 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 
 #ifndef R819x_WX_H
 #define R819x_WX_H
diff --git a/drivers/staging/rtl8192e/rtl819x_BA.h b/drivers/staging/rtl8192e/rtl819x_BA.h
index 5002b4d1fe51..978c9a54043e 100644
--- a/drivers/staging/rtl8192e/rtl819x_BA.h
+++ b/drivers/staging/rtl8192e/rtl819x_BA.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _BATYPE_H_
 #define _BATYPE_H_
 
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 20260af49ee7..1d3963136295 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -88,7 +88,7 @@ static struct sk_buff *rtllib_ADDBA(struct rtllib_device *ieee, u8 *Dst,
 		return NULL;
 	}
 	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
-	if (skb == NULL)
+	if (!skb)
 		return NULL;
 
 	memset(skb->data, 0, sizeof(struct rtllib_hdr_3addr));
@@ -154,7 +154,7 @@ static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
 	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;
 
 	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
-	if (skb == NULL)
+	if (!skb)
 		return NULL;
 
 	skb_reserve(skb, ieee->tx_headroom);
diff --git a/drivers/staging/rtl8192e/rtl819x_HT.h b/drivers/staging/rtl8192e/rtl819x_HT.h
index 6eb018f5feaa..24e86620c94c 100644
--- a/drivers/staging/rtl8192e/rtl819x_HT.h
+++ b/drivers/staging/rtl8192e/rtl819x_HT.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _RTL819XU_HTTYPE_H_
 #define _RTL819XU_HTTYPE_H_
 
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index cded0f43cd33..4ae1d382ac5c 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -489,7 +489,7 @@ u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 				if ((bitMap%2) != 0) {
 					if (HTMcsToDataRate(ieee, (8*i+j)) >
 					    HTMcsToDataRate(ieee, mcsRate))
-						mcsRate = (8*i+j);
+						mcsRate = 8 * i + j;
 				}
 				bitMap >>= 1;
 			}
diff --git a/drivers/staging/rtl8192e/rtl819x_Qos.h b/drivers/staging/rtl8192e/rtl819x_Qos.h
index 61da8f7475bb..576241233a35 100644
--- a/drivers/staging/rtl8192e/rtl819x_Qos.h
+++ b/drivers/staging/rtl8192e/rtl819x_Qos.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef __INC_QOS_TYPE_H
 #define __INC_QOS_TYPE_H
 
diff --git a/drivers/staging/rtl8192e/rtl819x_TS.h b/drivers/staging/rtl8192e/rtl819x_TS.h
index 2cabf4026e77..654c223030e3 100644
--- a/drivers/staging/rtl8192e/rtl819x_TS.h
+++ b/drivers/staging/rtl8192e/rtl819x_TS.h
@@ -11,7 +11,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _TSTYPE_H_
 #define _TSTYPE_H_
 #include "rtl819x_Qos.h"
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index b895a537d3e4..827651b62791 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -619,7 +619,8 @@ struct ieee_ibss_seq {
 
 /* 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. */
+ *       any adverse affects.
+ */
 struct rtllib_rx_stats {
 	u64 mac_time;
 	s8  rssi;
@@ -1138,7 +1139,8 @@ enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
 
 #define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
 #define ETHERNET_HEADER_SIZE    14      /* length of two Ethernet address
-					 * plus ether type*/
+					 * plus ether type
+					 */
 
 enum erp_t {
 	ERP_NonERPpresent	= 0x01,
diff --git a/drivers/staging/rtl8192e/rtllib_debug.h b/drivers/staging/rtl8192e/rtllib_debug.h
index f1c39c3f9d26..7b0e6e9c4456 100644
--- a/drivers/staging/rtl8192e/rtllib_debug.h
+++ b/drivers/staging/rtl8192e/rtllib_debug.h
@@ -17,7 +17,7 @@
  *
  * Contact Information:
  * wlanfae <wlanfae@realtek.com>
-******************************************************************************/
+ *****************************************************************************/
 #ifndef _RTL_DEBUG_H
 #define _RTL_DEBUG_H
 
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index 9d5788e04dd5..cdf4c9060c51 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -1,30 +1,30 @@
 /*******************************************************************************
-
-  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>
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms of version 2 of the GNU General Public License 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.
-
-  The full GNU General Public License is included in this distribution in the
-  file called LICENSE.
-
-  Contact Information:
-  James P. Ketrenos <ipw2100-admin@linux.intel.com>
-  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
-
-*******************************************************************************/
+ *
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * 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>
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index b1500ee9a5cf..c5c0d9676dc8 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -1,30 +1,30 @@
 /******************************************************************************
-
-  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>
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms of version 2 of the GNU General Public License 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.
-
-  The full GNU General Public License is included in this distribution in the
-  file called LICENSE.
-
-  Contact Information:
-  James P. Ketrenos <ipw2100-admin@linux.intel.com>
-  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
-
-******************************************************************************/
+ *
+ * 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>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * 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>
@@ -668,7 +668,7 @@ done:
 	if (ieee->set_security)
 		ieee->set_security(ieee->dev, &sec);
 
-	 if (ieee->reset_on_keychange &&
+	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");
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index 097147071df0..899c77ed2a43 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -1456,10 +1456,10 @@ enum ieee80211_state {
 
 
 
-typedef struct tx_pending_t{
+struct tx_pending {
 	int frag;
 	struct ieee80211_txb *txb;
-}tx_pending_t;
+};
 
 typedef struct _bandwidth_autoswitch {
 	long threshold_20Mhzto40Mhz;
@@ -1883,7 +1883,7 @@ struct ieee80211_device {
 	RT_POWER_SAVE_CONTROL	PowerSaveControl;
 //}
 	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
-	struct  tx_pending_t tx_pending;
+	struct  tx_pending tx_pending;
 
 	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
 	struct timer_list associate_timer;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
index 2453413757b6..5039172409e3 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_tkip.c
@@ -67,7 +67,7 @@ static void *ieee80211_tkip_init(int key_idx)
 	struct ieee80211_tkip_data *priv;
 
 	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
-	if (priv == NULL)
+	if (!priv)
 		goto fail;
 	priv->key_idx = key_idx;
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
index 0e8c876c1404..7ba4b07aa842 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_wep.c
@@ -42,7 +42,7 @@ static void *prism2_wep_init(int keyidx)
 	struct prism2_wep_data *priv;
 
 	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
-	if (priv == NULL)
+	if (!priv)
 		return NULL;
 	priv->key_idx = keyidx;
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index 5fdfff0816c5..a791175b86f5 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -283,8 +283,7 @@ int __init ieee80211_debug_init(void)
 				" proc directory\n");
 		return -EIO;
 	}
-	e = proc_create("debug_level", S_IRUGO | S_IWUSR,
-			      ieee80211_proc, &fops);
+	e = proc_create("debug_level", 0644, ieee80211_proc, &fops);
 	if (!e) {
 		remove_proc_entry(DRV_NAME, init_net.proc_net);
 		ieee80211_proc = NULL;
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index 5241c5003ebf..7a31510f0524 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -559,10 +559,8 @@ void ieee80211_indicate_packets(struct ieee80211_device *ieee, struct ieee80211_
 				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 = htons(sub_skb->len);
-				memcpy(skb_push(sub_skb, 2), &len, 2);
+				put_unaligned_be16(sub_skb->len, skb_push(sub_skb, 2));
 				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
 				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
 			}
@@ -920,7 +918,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 	int i;
 	struct ieee80211_rxb *rxb = NULL;
-	// cheat the the hdr type
+	// cheat the hdr type
 	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
 	stats = &ieee->stats;
 
@@ -941,7 +939,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	if (HTCCheck(ieee, skb->data))
 	{
 		if(net_ratelimit())
-		printk("find HTCControl\n");
+			printk("find HTCControl\n");
 		hdrlen += 4;
 		rx_stats->bContainHTC = true;
 	}
@@ -1317,7 +1315,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 				} else {
 					u16 len;
 					/* Leave Ethernet header part of hdr and full payload */
-					len = htons(sub_skb->len);
+					len = be16_to_cpu(htons(sub_skb->len));
 					memcpy(skb_push(sub_skb, 2), &len, 2);
 					memcpy(skb_push(sub_skb, ETH_ALEN), src, ETH_ALEN);
 					memcpy(skb_push(sub_skb, ETH_ALEN), dst, ETH_ALEN);
@@ -1480,13 +1478,15 @@ static int ieee80211_qos_convert_ac_to_parameters(struct
 		/* 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] = ac_params->ecw_min_max & 0x0F;
+		qos_param->cw_min[aci] =
+		    cpu_to_le16(ac_params->ecw_min_max & 0x0F);
 
-		qos_param->cw_max[aci] = (ac_params->ecw_min_max & 0xF0) >> 4;
+		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] = le16_to_cpu(ac_params->tx_op_limit);
+		qos_param->tx_op_limit[aci] = ac_params->tx_op_limit;
 	}
 	return 0;
 }
@@ -2394,39 +2394,41 @@ static inline void ieee80211_process_probe_response(
 #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;
 
 	memset(&network, 0, sizeof(struct ieee80211_network));
+	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,
-		(beacon->capability & (1<<0xf)) ? '1' : '0',
-		(beacon->capability & (1<<0xe)) ? '1' : '0',
-		(beacon->capability & (1<<0xd)) ? '1' : '0',
-		(beacon->capability & (1<<0xc)) ? '1' : '0',
-		(beacon->capability & (1<<0xb)) ? '1' : '0',
-		(beacon->capability & (1<<0xa)) ? '1' : '0',
-		(beacon->capability & (1<<0x9)) ? '1' : '0',
-		(beacon->capability & (1<<0x8)) ? '1' : '0',
-		(beacon->capability & (1<<0x7)) ? '1' : '0',
-		(beacon->capability & (1<<0x6)) ? '1' : '0',
-		(beacon->capability & (1<<0x5)) ? '1' : '0',
-		(beacon->capability & (1<<0x4)) ? '1' : '0',
-		(beacon->capability & (1<<0x3)) ? '1' : '0',
-		(beacon->capability & (1<<0x2)) ? '1' : '0',
-		(beacon->capability & (1<<0x1)) ? '1' : '0',
-		(beacon->capability & (1<<0x0)) ? '1' : '0');
+		(capability & (1 << 0xf)) ? '1' : '0',
+		(capability & (1 << 0xe)) ? '1' : '0',
+		(capability & (1 << 0xd)) ? '1' : '0',
+		(capability & (1 << 0xc)) ? '1' : '0',
+		(capability & (1 << 0xb)) ? '1' : '0',
+		(capability & (1 << 0xa)) ? '1' : '0',
+		(capability & (1 << 0x9)) ? '1' : '0',
+		(capability & (1 << 0x8)) ? '1' : '0',
+		(capability & (1 << 0x7)) ? '1' : '0',
+		(capability & (1 << 0x6)) ? '1' : '0',
+		(capability & (1 << 0x5)) ? '1' : '0',
+		(capability & (1 << 0x4)) ? '1' : '0',
+		(capability & (1 << 0x3)) ? '1' : '0',
+		(capability & (1 << 0x2)) ? '1' : '0',
+		(capability & (1 << 0x1)) ? '1' : '0',
+		(capability & (1 << 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,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
 				     "PROBE RESPONSE" : "BEACON");
 		return;
 	}
@@ -2442,7 +2444,7 @@ static inline void ieee80211_process_probe_response(
 		return;
 	if (ieee->bGlobalDomain)
 	{
-		if (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_PROBE_RESP)
+		if (fc == IEEE80211_STYPE_PROBE_RESP)
 		{
 			// Case 1: Country code
 			if(IS_COUNTRY_IE_VALID(ieee) )
@@ -2549,8 +2551,7 @@ static inline void ieee80211_process_probe_response(
 				     escape_essid(network.ssid,
 						  network.ssid_len),
 				     network.bssid,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
 				     "PROBE RESPONSE" : "BEACON");
 #endif
 		memcpy(target, &network, sizeof(*target));
@@ -2562,8 +2563,7 @@ static inline void ieee80211_process_probe_response(
 				     escape_essid(target->ssid,
 						  target->ssid_len),
 				     target->bssid,
-				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
-				     IEEE80211_STYPE_PROBE_RESP ?
+				     fc == IEEE80211_STYPE_PROBE_RESP ?
 				     "PROBE RESPONSE" : "BEACON");
 
 		/* we have an entry and we are going to update it. But this entry may
@@ -2600,11 +2600,11 @@ void ieee80211_rx_mgt(struct ieee80211_device *ieee,
 		      struct rtl_80211_hdr_4addr *header,
 		      struct ieee80211_rx_stats *stats)
 {
-	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+	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(header->frame_ctl));
+			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);
@@ -2612,7 +2612,7 @@ void ieee80211_rx_mgt(struct ieee80211_device *ieee,
 
 	case IEEE80211_STYPE_PROBE_RESP:
 		IEEE80211_DEBUG_MGMT("received PROBE RESPONSE (%d)\n",
-				     WLAN_FC_GET_STYPE(header->frame_ctl));
+			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);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index 0ea90aae4283..14aea26804f4 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -466,7 +466,7 @@ void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
 			goto out;
 		ieee->set_chan(ieee->dev, ch);
 		if(channel_map[ch] == 1)
-		ieee80211_send_probe_requests(ieee);
+			ieee80211_send_probe_requests(ieee);
 
 		/* this prevent excessive time wait when we
 		 * need to wait for a syncro scan to end..
@@ -3025,7 +3025,7 @@ static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
 		ieee80211_crypt_delayed_deinit(ieee, crypt);
 
 		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
-		if (new_crypt == NULL) {
+		if (!new_crypt) {
 			ret = -ENOMEM;
 			goto done;
 		}
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 5704e4d7aa68..bdb96a45a9eb 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -171,7 +171,7 @@ static inline int ieee80211_put_snap(u8 *data, u16 h_proto)
 	snap->oui[1] = oui[1];
 	snap->oui[2] = oui[2];
 
-	*(u16 *)(data + SNAP_SIZE) = htons(h_proto);
+	*(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
 
 	return SNAP_SIZE + sizeof(u16);
 }
@@ -281,7 +281,6 @@ ieee80211_classify(struct sk_buff *skb, struct ieee80211_network *network)
 	if (eth->h_proto != htons(ETH_P_IP))
 		return 0;
 
-//	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, skb->data, skb->len);
 	ip = ip_hdr(skb);
 	switch (ip->tos & 0xfc) {
 	case 0x20:
@@ -887,7 +886,6 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 		if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
 			tcb_desc->data_rate = ieee->basic_rate;
 		else
-			//tcb_desc->data_rate = CURRENT_RATE(ieee->current_network.mode, ieee->rate, ieee->HTCurrentOperaRate);
 			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);
@@ -895,8 +893,6 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 		ieee80211_query_BandwidthMode(ieee, tcb_desc);
 		ieee80211_query_protectionmode(ieee, tcb_desc, txb->fragments[0]);
 		ieee80211_query_seqnum(ieee, txb->fragments[0], header.addr1);
-//		IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, txb->fragments[0]->data, txb->fragments[0]->len);
-		//IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA, tcb_desc, sizeof(cb_desc));
 	}
 	spin_unlock_irqrestore(&ieee->lock, flags);
 	dev_kfree_skb_any(skb);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
index e383ec2fb335..c925e53bf013 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -362,7 +362,7 @@ int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
 		/* take WEP into use */
 		new_crypt = kzalloc(sizeof(struct ieee80211_crypt_data),
 				    GFP_KERNEL);
-		if (new_crypt == NULL)
+		if (!new_crypt)
 			return -ENOMEM;
 		new_crypt->ops = ieee80211_get_crypto_ops("WEP");
 		if (!new_crypt->ops) {
@@ -610,7 +610,7 @@ int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
 		ieee80211_crypt_delayed_deinit(ieee, crypt);
 
 		new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
-		if (new_crypt == NULL) {
+		if (!new_crypt) {
 			ret = -ENOMEM;
 			goto done;
 		}
@@ -665,7 +665,7 @@ done:
 	if (ieee->set_security)
 		ieee->set_security(ieee->dev, &sec);
 
-	 if (ieee->reset_on_keychange &&
+	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);
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h
index 2c398ca9a8ac..7abedc27d7c1 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BA.h
@@ -31,8 +31,8 @@ typedef union _SEQUENCE_CONTROL{
 	struct {
 		u16	FragNum:4;
 		u16	SeqNum:12;
-	}field;
-}SEQUENCE_CONTROL, *PSEQUENCE_CONTROL;
+	} field;
+} SEQUENCE_CONTROL, *PSEQUENCE_CONTROL;
 
 typedef union _BA_PARAM_SET {
 	u8 charData[2];
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index 6619b8fb9700..e82b5073c3f1 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -117,7 +117,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 		return NULL;
 	}
 	skb = dev_alloc_skb(len + sizeof( struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
-	if (skb == NULL) {
+	if (!skb) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
 		return NULL;
 	}
@@ -202,7 +202,7 @@ static struct sk_buff *ieee80211_DELBA(
 	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;
 
 	skb = dev_alloc_skb(len + sizeof( struct rtl_80211_hdr_3addr)); //need to add something others? FIXME
-	if (skb == NULL) {
+	if (!skb) {
 		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
 		return NULL;
 	}
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index c27397b14adb..60720997784b 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -976,17 +976,16 @@ void HTOnAssocRsp(struct ieee80211_device *ieee)
 	//
 	HTSetConnectBwMode(ieee, (HT_CHANNEL_WIDTH)(pPeerHTCap->ChlWidth), (HT_EXTCHNL_OFFSET)(pPeerHTInfo->ExtChlOffset));
 
-//	if (pHTInfo->bCurBW40MHz)
-		pHTInfo->bCurTxBW40MHz = ((pPeerHTInfo->RecommemdedTxWidth == 1)?true:false);
+	pHTInfo->bCurTxBW40MHz = (pPeerHTInfo->RecommemdedTxWidth == 1);
 
 	//
 	// Update short GI/ long GI setting
 	//
 	// TODO:
-	pHTInfo->bCurShortGI20MHz=
-		((pHTInfo->bRegShortGI20MHz)?((pPeerHTCap->ShortGI20Mhz==1)?true:false):false);
-	pHTInfo->bCurShortGI40MHz=
-		((pHTInfo->bRegShortGI40MHz)?((pPeerHTCap->ShortGI40Mhz==1)?true:false):false);
+	pHTInfo->bCurShortGI20MHz = pHTInfo->bRegShortGI20MHz &&
+				    (pPeerHTCap->ShortGI20Mhz == 1);
+	pHTInfo->bCurShortGI40MHz = pHTInfo->bRegShortGI40MHz &&
+				   (pPeerHTCap->ShortGI40Mhz == 1);
 
 	//
 	// Config TX STBC setting
@@ -997,8 +996,8 @@ void HTOnAssocRsp(struct ieee80211_device *ieee)
 	// Config DSSS/CCK  mode in 40MHz mode
 	//
 	// TODO:
-	pHTInfo->bCurSuppCCK =
-		((pHTInfo->bRegSuppCCK)?((pPeerHTCap->DssCCk==1)?true:false):false);
+	pHTInfo->bCurSuppCCK = pHTInfo->bRegSuppCCK &&
+			       (pPeerHTCap->DssCCk == 1);
 
 
 	//
diff --git a/drivers/staging/rtl8192u/r8192U.h b/drivers/staging/rtl8192u/r8192U.h
index a7ba8f37384e..e702afb5a70e 100644
--- a/drivers/staging/rtl8192u/r8192U.h
+++ b/drivers/staging/rtl8192u/r8192U.h
@@ -793,12 +793,12 @@ typedef struct _phy_cck_rx_status_report_819xusb {
 } phy_sts_cck_819xusb_t;
 
 
-typedef struct _phy_ofdm_rx_status_rxsc_sgien_exintfflag {
+struct phy_ofdm_rx_status_rxsc_sgien_exintfflag {
 	u8			reserved:4;
 	u8			rxsc:2;
 	u8			sgi_en:1;
 	u8			ex_intf_flag:1;
-} phy_ofdm_rx_status_rxsc_sgien_exintfflag;
+};
 
 typedef enum _RT_CUSTOMER_ID {
 	RT_CID_DEFAULT = 0,
@@ -1041,10 +1041,10 @@ typedef struct r8192_priv {
 	u8 rfc_txpowertrackingindex;
 	u8 rfc_txpowertrackingindex_real;
 
-	s8 cck_present_attentuation;
-	u8 cck_present_attentuation_20Mdefault;
-	u8 cck_present_attentuation_40Mdefault;
-	s8 cck_present_attentuation_difference;
+	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;
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index b631990b4969..9f370e8d84d3 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -269,7 +269,7 @@ int write_nic_byte_E(struct net_device *dev, int indx, u8 data)
 				 indx | 0xfe00, 0, usbdata, 1, HZ / 2);
 	kfree(usbdata);
 
-	if (status < 0){
+	if (status < 0) {
 		netdev_err(dev, "write_nic_byte_E TimeOut! status: %d\n",
 			   status);
 		return status;
@@ -1814,7 +1814,7 @@ static void rtl8192_link_change(struct net_device *dev)
 	}
 }
 
-static struct ieee80211_qos_parameters def_qos_parameters = {
+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 */
@@ -2519,7 +2519,7 @@ static int rtl8192_read_eeprom_info(struct net_device *dev)
 			for (i = 0; i < 3; i++) {
 				if (bLoad_From_EEPOM) {
 					ret = eprom_read(dev, (EEPROM_TxPwIndex_OFDM_24G + i) >> 1);
-					if ( ret < 0)
+					if (ret < 0)
 						return ret;
 					if (((EEPROM_TxPwIndex_OFDM_24G + i) % 2) == 0)
 						tmpValue = (u16)ret & 0x00ff;
@@ -3023,14 +3023,14 @@ static bool rtl8192_adapter_start(struct net_device *dev)
 
 			for (i = 0; i < CCKTxBBGainTableLength; i++) {
 				if (TempCCk == priv->cck_txbbgain_table[i].ccktxbb_valuearray[0]) {
-					priv->cck_present_attentuation_20Mdefault = (u8)i;
+					priv->cck_present_attenuation_20Mdefault = (u8)i;
 					break;
 				}
 			}
-			priv->cck_present_attentuation_40Mdefault = 0;
-			priv->cck_present_attentuation_difference = 0;
-			priv->cck_present_attentuation =
-				priv->cck_present_attentuation_20Mdefault;
+			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);
@@ -4242,7 +4242,7 @@ static void rtl8192_query_rxphystatus(struct r8192_priv *priv,
 {
 	phy_sts_ofdm_819xusb_t *pofdm_buf;
 	phy_sts_cck_819xusb_t	*pcck_buf;
-	phy_ofdm_rx_status_rxsc_sgien_exintfflag *prxsc;
+	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;
@@ -4432,7 +4432,7 @@ static void rtl8192_query_rxphystatus(struct r8192_priv *priv,
 
 		/* record rx statistics for debug */
 		rxsc_sgien_exflg = pofdm_buf->rxsc_sgien_exflg;
-		prxsc =	(phy_ofdm_rx_status_rxsc_sgien_exintfflag *)
+		prxsc =	(struct phy_ofdm_rx_status_rxsc_sgien_exintfflag *)
 			&rxsc_sgien_exflg;
 		if (pdrvinfo->BW)	/* 40M channel */
 			priv->stats.received_bwtype[1 + prxsc->rxsc]++;
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index 9209aad0515e..975f707827e1 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -598,8 +598,8 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 				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_attentuation_difference = %d\n", priv->cck_present_attentuation_difference);
-				RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attentuation = %d\n", priv->cck_present_attentuation);
+				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) {
@@ -618,17 +618,17 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 
 				}
 			}
-			priv->cck_present_attentuation_difference
+			priv->cck_present_attenuation_difference
 				= priv->rfa_txpowertrackingindex - priv->rfa_txpowertracking_default;
 
 			if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
-				priv->cck_present_attentuation
-					= priv->cck_present_attentuation_20Mdefault + priv->cck_present_attentuation_difference;
+				priv->cck_present_attenuation
+					= priv->cck_present_attenuation_20Mdefault + priv->cck_present_attenuation_difference;
 			else
-				priv->cck_present_attentuation
-					= priv->cck_present_attentuation_40Mdefault + priv->cck_present_attentuation_difference;
+				priv->cck_present_attenuation
+					= priv->cck_present_attenuation_40Mdefault + priv->cck_present_attenuation_difference;
 
-			if (priv->cck_present_attentuation > -1 && priv->cck_present_attentuation < 23) {
+			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);
@@ -640,10 +640,10 @@ static void dm_TXPowerTrackingCallback_TSSI(struct net_device *dev)
 			}
 			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_attentuation_difference = %d\n", priv->cck_present_attentuation_difference);
-			RT_TRACE(COMP_POWER_TRACKING, "priv->cck_present_attentuation = %d\n", priv->cck_present_attentuation);
+			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_attentuation_difference <= -12 || priv->cck_present_attentuation_difference >= 24) {
+			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");
@@ -725,10 +725,15 @@ static void dm_TXPowerTrackingCallback_ThermalMeter(struct net_device *dev)
 	} else {
 		tmpval = (u8)tmpRegA - priv->ThermalMeter[0];
 
-		if (tmpval >= 6) /* higher temperature */
-			tmpOFDMindex = tmpCCK20Mindex = 0; /* max to +6dB */
-		else
-			tmpOFDMindex = tmpCCK20Mindex = 6 - tmpval;
+		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",
@@ -1379,35 +1384,35 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 	TempVal = 0;
 	if (!bInCH14) {
 		/* Write 0xa22 0xa23 */
-		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[0] +
-					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[1]<<8);
+		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_attentuation].ccktxbb_valuearray[2] +
-					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
-					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
-					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
+		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_attentuation].ccktxbb_valuearray[6] +
-					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8);
+		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_attentuation].ccktxbb_valuearray[0] +
-					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[1]<<8);
+		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_attentuation].ccktxbb_valuearray[2] +
-					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
-					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
-					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
+		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_attentuation].ccktxbb_valuearray[6] +
-					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8);
+		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);
 	}
@@ -1490,7 +1495,7 @@ static void dm_txpower_reset_recovery(
 	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_attentuation);
+	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);
@@ -2304,10 +2309,10 @@ static void dm_check_edca_turbo(
 				/*  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;
-				u4bAcParam = (((u32)(qos_parameters->tx_op_limit[0])) << AC_PARAM_TXOP_LIMIT_OFFSET)|
-					(((u32)(qos_parameters->cw_max[0])) << AC_PARAM_ECW_MAX_OFFSET)|
-					(((u32)(qos_parameters->cw_min[0])) << AC_PARAM_ECW_MIN_OFFSET)|
-					((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET);
+				u4bAcParam = (((le16_to_cpu(qos_parameters->tx_op_limit[0])) << AC_PARAM_TXOP_LIMIT_OFFSET)|
+					((le16_to_cpu(qos_parameters->cw_max[0])) << AC_PARAM_ECW_MAX_OFFSET)|
+					((le16_to_cpu(qos_parameters->cw_min[0])) << AC_PARAM_ECW_MIN_OFFSET)|
+					((u32)u1bAIFS << AC_PARAM_AIFS_OFFSET));
 				/*write_nic_dword(dev, WDCAPARA_ADD[i], u4bAcParam);*/
 				write_nic_dword(dev, EDCAPARA_BE,  u4bAcParam);
 
diff --git a/drivers/staging/rtl8192u/r819xU_cmdpkt.c b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
index 3e0731b04619..bb6d8bd6c7ac 100644
--- a/drivers/staging/rtl8192u/r819xU_cmdpkt.c
+++ b/drivers/staging/rtl8192u/r819xU_cmdpkt.c
@@ -495,7 +495,7 @@ u32 cmpk_message_handle_rx(struct net_device *dev,
 	u8			element_id;
 	u8			*pcmd_buff;
 
-	/* 0. Check inpt arguments. If is is a command queue message or
+	/* 0. Check inpt arguments. It is a command queue message or
 	 * pointer is null.
 	 */
 	if (pstats == NULL)
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index c99130fdb8ee..3874f8307117 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -1559,17 +1559,17 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 				 0x00100000, 1);
 
 		/* Correct the tx power for CCK rate in 20M. */
-		priv->cck_present_attentuation =
-			priv->cck_present_attentuation_20Mdefault +
-			priv->cck_present_attentuation_difference;
-
-		if (priv->cck_present_attentuation > 22)
-			priv->cck_present_attentuation = 22;
-		if (priv->cck_present_attentuation < 0)
-			priv->cck_present_attentuation = 0;
+		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_attentuation);
+			 priv->cck_present_attenuation);
 
 		if (priv->chan == 14 && !priv->bcck_in_ch14) {
 			priv->bcck_in_ch14 = true;
@@ -1590,18 +1590,18 @@ void rtl8192_SetBWModeWorkItem(struct net_device *dev)
 		rtl8192_setBBreg(dev, rFPGA0_AnalogParameter1, 0x00100000, 0);
 		rtl8192_setBBreg(dev, rOFDM1_LSTF, 0xC00,
 				 priv->nCur40MhzPrimeSC);
-		priv->cck_present_attentuation =
-			priv->cck_present_attentuation_40Mdefault +
-			priv->cck_present_attentuation_difference;
+		priv->cck_present_attenuation =
+			priv->cck_present_attenuation_40Mdefault +
+			priv->cck_present_attenuation_difference;
 
-		if (priv->cck_present_attentuation > 22)
-			priv->cck_present_attentuation = 22;
-		if (priv->cck_present_attentuation < 0)
-			priv->cck_present_attentuation = 0;
+		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_attentuation);
+			 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);
diff --git a/drivers/staging/rtl8712/ieee80211.c b/drivers/staging/rtl8712/ieee80211.c
index d84da2b6d6b3..f35121eedac6 100644
--- a/drivers/staging/rtl8712/ieee80211.c
+++ b/drivers/staging/rtl8712/ieee80211.c
@@ -288,8 +288,9 @@ int r8712_parse_wpa_ie(u8 *wpa_ie, int wpa_ie_len, int *group_cipher,
 		/* 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, (void *)WPA_OUI_TYPE, WPA_SELECTOR_LEN)))
+	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 _FAIL;
 	pos = wpa_ie;
 	pos += 8;
diff --git a/drivers/staging/rtl8712/mlme_linux.c b/drivers/staging/rtl8712/mlme_linux.c
index 999c16d9c6c1..20372659d15d 100644
--- a/drivers/staging/rtl8712/mlme_linux.c
+++ b/drivers/staging/rtl8712/mlme_linux.c
@@ -110,12 +110,12 @@ void r8712_os_indicate_disconnect(struct _adapter *adapter)
 		 * disconnect with AP for 60 seconds.
 		 */
 
-		memcpy(&backupPMKIDList[0], &adapter->securitypriv.
-			PMKIDList[0], sizeof(struct RT_PMKID_LIST) *
-			NUM_PMKID_CACHE);
+		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 =
+			adapter->securitypriv.btkip_countermeasure;
 		memset((unsigned char *)&adapter->securitypriv, 0,
 		       sizeof(struct security_priv));
 		setup_timer(&adapter->securitypriv.tkip_timer,
diff --git a/drivers/staging/rtl8712/os_intfs.c b/drivers/staging/rtl8712/os_intfs.c
index cbe4de05d26b..8836b31b4ef8 100644
--- a/drivers/staging/rtl8712/os_intfs.c
+++ b/drivers/staging/rtl8712/os_intfs.c
@@ -13,10 +13,6 @@
  * 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; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * Modifications for inclusion into the Linux staging tree are
  * Copyright(c) 2010 Larry Finger. All rights reserved.
  *
@@ -168,7 +164,7 @@ static void loadparam(struct _adapter *padapter, struct  net_device *pnetdev)
 	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;
+	registry_par->wifi_test = (u8)wifi_test;
 	r8712_initmac = initmac;
 }
 
@@ -185,8 +181,8 @@ static int r871x_net_set_mac_address(struct net_device *pnetdev, void *p)
 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);
+	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;
@@ -392,7 +388,7 @@ static int netdev_open(struct net_device *pnetdev)
 		if (!r8712_initmac)
 			/* Use the mac address stored in the Efuse */
 			memcpy(pnetdev->dev_addr,
-				padapter->eeprompriv.mac_addr, ETH_ALEN);
+			       padapter->eeprompriv.mac_addr, ETH_ALEN);
 		else {
 			/* We have to inform f/w to use user-supplied MAC
 			 * address.
@@ -409,7 +405,7 @@ static int netdev_open(struct net_device *pnetdev)
 			 * users specify.
 			 */
 			memcpy(padapter->eeprompriv.mac_addr,
-				pnetdev->dev_addr, ETH_ALEN);
+			       pnetdev->dev_addr, ETH_ALEN);
 		}
 		if (start_drv_threads(padapter) != _SUCCESS)
 			goto netdev_open_error;
diff --git a/drivers/staging/rtl8712/rtl8712_led.c b/drivers/staging/rtl8712/rtl8712_led.c
index 317aeeed38e8..da1d4a641dcd 100644
--- a/drivers/staging/rtl8712/rtl8712_led.c
+++ b/drivers/staging/rtl8712/rtl8712_led.c
@@ -1734,7 +1734,7 @@ static void SwLedControlMode6(struct _adapter *padapter,
 	case LED_CTL_LINK:	/*solid blue*/
 	case LED_CTL_SITE_SURVEY:
 		if (IS_LED_WPS_BLINKING(pLed))
-				return;
+			return;
 		pLed->CurrLedState = LED_STATE_ON;
 		pLed->BlinkingLedState = LED_STATE_ON;
 		pLed->bLedBlinkInProgress = false;
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index 20fe45a43e53..266ffefd55ed 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -444,9 +444,9 @@ void r8712_rxcmd_event_hdl(struct _adapter *padapter, void *prxcmdbuf)
 	u16 cmd_len, drvinfo_sz;
 	struct recv_stat *prxstat;
 
-	poffset = (u8 *)prxcmdbuf;
+	poffset = prxcmdbuf;
 	voffset = *(__le32 *)poffset;
-	prxstat = (struct recv_stat *)prxcmdbuf;
+	prxstat = prxcmdbuf;
 	drvinfo_sz = (le32_to_cpu(prxstat->rxdw0) & 0x000f0000) >> 16;
 	drvinfo_sz <<= 3;
 	poffset += RXDESC_SIZE + drvinfo_sz;
@@ -634,8 +634,7 @@ _err_exit:
 void r8712_reordering_ctrl_timeout_handler(void *pcontext)
 {
 	unsigned long irql;
-	struct recv_reorder_ctrl *preorder_ctrl =
-				 (struct recv_reorder_ctrl *)pcontext;
+	struct recv_reorder_ctrl *preorder_ctrl = pcontext;
 	struct _adapter *padapter = preorder_ctrl->padapter;
 	struct  __queue *ppending_recvframe_queue =
 				 &preorder_ctrl->pending_recvframe_queue;
@@ -976,7 +975,7 @@ int recv_func(struct _adapter *padapter, void *pcontext)
 	struct  __queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
 	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 
-	prframe = (union recv_frame *)pcontext;
+	prframe = pcontext;
 	orig_prframe = prframe;
 	pattrib = &prframe->u.hdr.attrib;
 	if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
@@ -1124,7 +1123,7 @@ _exit_recvbuf2recvframe:
 static void recv_tasklet(void *priv)
 {
 	struct sk_buff *pskb;
-	struct _adapter *padapter = (struct _adapter *)priv;
+	struct _adapter *padapter = priv;
 	struct recv_priv *precvpriv = &padapter->recvpriv;
 
 	while (NULL != (pskb = skb_dequeue(&precvpriv->rx_skb_queue))) {
diff --git a/drivers/staging/rtl8712/rtl871x_cmd.h b/drivers/staging/rtl8712/rtl871x_cmd.h
index 4734ca856aa2..24da2ccea04f 100644
--- a/drivers/staging/rtl8712/rtl871x_cmd.h
+++ b/drivers/staging/rtl8712/rtl871x_cmd.h
@@ -144,7 +144,7 @@ struct disconnect_parm {
  * #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];
diff --git a/drivers/staging/rtl8712/rtl871x_event.h b/drivers/staging/rtl8712/rtl871x_event.h
index 5db8620980e5..517137906e6c 100644
--- a/drivers/staging/rtl8712/rtl871x_event.h
+++ b/drivers/staging/rtl8712/rtl871x_event.h
@@ -34,7 +34,7 @@
 
 /*
  * Used to report a bss has been scanned
-*/
+ */
 struct survey_event	{
 	struct wlan_bssid_ex bss;
 };
@@ -42,7 +42,7 @@ struct survey_event	{
 /*
  * 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;
 
@@ -54,7 +54,7 @@ struct surveydone_event {
  *  -1: authentication fail
  *  -2: association fail
  *  > 0: TID
-*/
+ */
 struct joinbss_event {
 	struct	wlan_network	network;
 };
@@ -62,7 +62,7 @@ struct joinbss_event {
 /*
  * 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];
diff --git a/drivers/staging/rtl8712/rtl871x_io.h b/drivers/staging/rtl8712/rtl871x_io.h
index 26dd24cddd38..dd054d7367b3 100644
--- a/drivers/staging/rtl8712/rtl871x_io.h
+++ b/drivers/staging/rtl8712/rtl871x_io.h
@@ -49,9 +49,9 @@
 #define _IO_CMDMASK_	(0x1F80)
 
 /*
-	For prompt mode accessing, caller shall free io_req
-	Otherwise, io_handler will free io_req
-*/
+ *	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)
@@ -69,8 +69,8 @@
 #define IO_WR16_ASYNC	(_IO_WRITE_ | _IO_HW_)
 #define IO_WR8_ASYNC	(_IO_WRITE_ | _IO_BYTE_)
 /*
-	Only Sync. burst accessing is provided.
-*/
+ *	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_))
@@ -218,8 +218,8 @@ struct reg_protocol_wt {
 };
 
 /*
-Below is the data structure used by _io_handler
-*/
+ * Below is the data structure used by _io_handler
+ */
 
 struct io_queue {
 	spinlock_t lock;
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index f4167f14af70..e30a5be5f318 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -100,10 +100,10 @@ static inline void handle_pairwise_key(struct sta_info *psta,
 	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);
+		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));
@@ -378,13 +378,12 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 	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)) {
-		if (param->u.crypt.idx >= WEP_KEYS) {
-			/* for large key indices, set the default (0) */
-			param->u.crypt.idx = 0;
-		}
-	} else {
+	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__);
@@ -396,23 +395,19 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 		wep_key_len = param->u.crypt.key_len;
 		if (wep_key_idx >= WEP_KEYS)
 			wep_key_idx = 0;
-		if (wep_key_len > 0) {
-			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 +
-				 FIELD_OFFSET(struct NDIS_802_11_WEP,
-				 KeyMaterial);
-			if (wep_key_len == 13) {
-				padapter->securitypriv.PrivacyAlgrthm =
-					 _WEP104_;
-				padapter->securitypriv.XGrpPrivacy =
-					 _WEP104_;
-			}
-		} else {
+		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 +
+			FIELD_OFFSET(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;
@@ -700,14 +695,14 @@ static int r8711_wx_get_freq(struct net_device *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.e = 1;
-		wrqu->freq.i = pcur_bss->Configuration.DSConfig;
-	} else {
+	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;
 }
 
@@ -1411,44 +1406,41 @@ static int r8711_wx_get_rate(struct net_device *dev,
 	u16 mcs_rate = 0;
 
 	i = 0;
-	if (check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE)) {
-		p = r8712_get_ie(&pcur_bss->IEs[12],
-				 _HT_CAPABILITY_IE_, &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->supp_mcs_set, 2);
-			bw_40MHz = (le16_to_cpu(pht_capie->cap_info) &
-				    IEEE80211_HT_CAP_SUP_WIDTH) ? 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;
-	} else {
+	if (!check_fwstate(pmlmepriv, _FW_LINKED | WIFI_ADHOC_MASTER_STATE))
 		return -ENOLINK;
-	}
+	p = r8712_get_ie(&pcur_bss->IEs[12], _HT_CAPABILITY_IE_, &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->supp_mcs_set, 2);
+		bw_40MHz = (le16_to_cpu(pht_capie->cap_info) &
+			    IEEE80211_HT_CAP_SUP_WIDTH) ? 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;
 }
 
@@ -1973,13 +1965,12 @@ static int r871x_get_ap_info(struct net_device *dev,
 			break;
 	}
 	pdata->flags = 0;
-	if (pdata->length >= 32) {
-		if (copy_from_user(data, pdata->pointer, 32))
-			return -EINVAL;
-		data[32] = 0;
-	} else {
+	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;
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c
index c7f2e5167cb7..ca769f781e96 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_rtl.c
@@ -282,8 +282,7 @@ uint oid_rt_get_total_rx_bytes_hdl(struct oid_par_priv
 	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;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
 	} else {
 		return RNDIS_STATUS_INVALID_LENGTH;
 	}
@@ -325,8 +324,7 @@ uint oid_rt_get_channel_hdl(struct oid_par_priv *poid_par_priv)
 	    check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE))
 		pnic_Config = &pmlmepriv->cur_network.network.Configuration;
 	else
-		pnic_Config = &padapter->registrypriv.dev_network.
-			      Configuration;
+		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;
@@ -483,8 +481,8 @@ uint oid_rt_pro_rf_read_registry_hdl(struct oid_par_priv *poid_par_priv)
 		 */
 			if (!r8712_getrfreg_cmd(Adapter,
 			    *(unsigned char *)poid_par_priv->information_buf,
-			    (unsigned char *)&Adapter->mppriv.workparam.
-			    io_value))
+			    (unsigned char *)&Adapter->mppriv.workparam.io_value
+			    ))
 				status = RNDIS_STATUS_NOT_ACCEPTED;
 		}
 	} else {
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.h b/drivers/staging/rtl8712/rtl871x_mlme.h
index 53a23234c598..b21f28140f53 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.h
+++ b/drivers/staging/rtl8712/rtl871x_mlme.h
@@ -68,14 +68,14 @@
 #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!
-*/
+ * 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
diff --git a/drivers/staging/rtl8712/rtl871x_mp_ioctl.h b/drivers/staging/rtl8712/rtl871x_mp_ioctl.h
index 1102451a733d..741006f1e45a 100644
--- a/drivers/staging/rtl8712/rtl871x_mp_ioctl.h
+++ b/drivers/staging/rtl8712/rtl871x_mp_ioctl.h
@@ -150,103 +150,126 @@ uint oid_rt_get_power_mode_hdl(
 #ifdef _RTL871X_MP_IOCTL_C_ /* CAUTION!!! */
 /* This ifdef _MUST_ be left in!! */
 static const struct oid_obj_priv oid_rtl_seg_81_80_00[] = {
-	{1, oid_null_function},		/*0x00	OID_RT_PRO_RESET_DUT */
-	{1, oid_rt_pro_set_data_rate_hdl},	/*0x01*/
-	{1, oid_rt_pro_start_test_hdl}, /*0x02*/
-	{1, oid_rt_pro_stop_test_hdl},	/*0x03*/
-	{1, oid_null_function},		/*0x04	OID_RT_PRO_SET_PREAMBLE*/
-	{1, oid_null_function},		/*0x05	OID_RT_PRO_SET_SCRAMBLER*/
-	{1, oid_null_function},		/*0x06	OID_RT_PRO_SET_FILTER_BB*/
-	{1, oid_null_function},		/*0x07
-					 * OID_RT_PRO_SET_MANUAL_DIVERS_BB
-					 */
-	{1, oid_rt_pro_set_channel_direct_call_hdl},	/*0x08*/
-	{1, oid_null_function},		/*0x09
-				* OID_RT_PRO_SET_SLEEP_MODE_DIRECT_CALL
-				*/
-	{1, oid_null_function},		/*0x0A
-				* OID_RT_PRO_SET_WAKE_MODE_DIRECT_CALL
-				*/
-	{1, oid_rt_pro_set_continuous_tx_hdl},	/*0x0B
-				* OID_RT_PRO_SET_TX_CONTINUOUS_DIRECT_CALL
-				*/
-	{1, oid_rt_pro_set_single_carrier_tx_hdl}, /*0x0C
-				* OID_RT_PRO_SET_SINGLE_CARRIER_TX_CONTINUOUS
-				*/
-	{1, oid_null_function},		/*0x0D
-				* OID_RT_PRO_SET_TX_ANTENNA_BB
-				*/
-	{1, oid_rt_pro_set_antenna_bb_hdl},		/*0x0E*/
-	{1, oid_null_function},		/*0x0F	OID_RT_PRO_SET_CR_SCRAMBLER*/
-	{1, oid_null_function},		/*0x10	OID_RT_PRO_SET_CR_NEW_FILTER*/
-	{1, oid_rt_pro_set_tx_power_control_hdl}, /*0x11
-				* OID_RT_PRO_SET_TX_POWER_CONTROL
-				*/
-	{1, oid_null_function},		/*0x12	OID_RT_PRO_SET_CR_TX_CONFIG*/
-	{1, oid_null_function},		/*0x13
-					 * OID_RT_PRO_GET_TX_POWER_CONTROL
-					 */
-	{1, oid_null_function},		/*0x14
-					 * OID_RT_PRO_GET_CR_SIGNAL_QUALITY
-					 */
-	{1, oid_null_function},		/*0x15	OID_RT_PRO_SET_CR_SETPOINT*/
-	{1, oid_null_function},		/*0x16	OID_RT_PRO_SET_INTEGRATOR*/
-	{1, oid_null_function},		/*0x17	OID_RT_PRO_SET_SIGNAL_QUALITY*/
-	{1, oid_null_function},		/*0x18	OID_RT_PRO_GET_INTEGRATOR*/
-	{1, oid_null_function},		/*0x19	OID_RT_PRO_GET_SIGNAL_QUALITY*/
-	{1, oid_null_function},		/*0x1A	OID_RT_PRO_QUERY_EEPROM_TYPE*/
-	{1, oid_null_function},		/*0x1B	OID_RT_PRO_WRITE_MAC_ADDRESS*/
-	{1, oid_null_function},		/*0x1C	OID_RT_PRO_READ_MAC_ADDRESS*/
-	{1, oid_null_function},		/*0x1D	OID_RT_PRO_WRITE_CIS_DATA*/
-	{1, oid_null_function},		/*0x1E	OID_RT_PRO_READ_CIS_DATA*/
-	{1, oid_null_function}		/*0x1F	OID_RT_PRO_WRITE_POWER_CONTROL*/
+	/* 0x00	OID_RT_PRO_RESET_DUT */
+	{1, oid_null_function},
+	/* 0x01 */
+	{1, oid_rt_pro_set_data_rate_hdl},
+	/* 0x02 */
+	{1, oid_rt_pro_start_test_hdl},
+	/* 0x03 */
+	{1, oid_rt_pro_stop_test_hdl},
+	/* 0x04	OID_RT_PRO_SET_PREAMBLE */
+	{1, oid_null_function},
+	/* 0x05	OID_RT_PRO_SET_SCRAMBLER */
+	{1, oid_null_function},
+	/* 0x06	OID_RT_PRO_SET_FILTER_BB */
+	{1, oid_null_function},
+	/* 0x07  OID_RT_PRO_SET_MANUAL_DIVERS_BB */
+	{1, oid_null_function},
+	/* 0x08 */
+	{1, oid_rt_pro_set_channel_direct_call_hdl},
+	/* 0x09  OID_RT_PRO_SET_SLEEP_MODE_DIRECT_CALL */
+	{1, oid_null_function},
+	/* 0x0A  OID_RT_PRO_SET_WAKE_MODE_DIRECT_CALL */
+	{1, oid_null_function},
+	/* 0x0B OID_RT_PRO_SET_TX_CONTINUOUS_DIRECT_CALL */
+	{1, oid_rt_pro_set_continuous_tx_hdl},
+	/* 0x0C OID_RT_PRO_SET_SINGLE_CARRIER_TX_CONTINUOUS */
+	{1, oid_rt_pro_set_single_carrier_tx_hdl},
+	/* 0x0D OID_RT_PRO_SET_TX_ANTENNA_BB */
+	{1, oid_null_function},
+	/* 0x0E */
+	{1, oid_rt_pro_set_antenna_bb_hdl},
+	/* 0x0F	OID_RT_PRO_SET_CR_SCRAMBLER */
+	{1, oid_null_function},
+	/* 0x10	OID_RT_PRO_SET_CR_NEW_FILTER */
+	{1, oid_null_function},
+	/* 0x11 OID_RT_PRO_SET_TX_POWER_CONTROL */
+	{1, oid_rt_pro_set_tx_power_control_hdl},
+	/* 0x12	OID_RT_PRO_SET_CR_TX_CONFIG */
+	{1, oid_null_function},
+	/* 0x13  OID_RT_PRO_GET_TX_POWER_CONTROL */
+	{1, oid_null_function},
+	/* 0x14  OID_RT_PRO_GET_CR_SIGNAL_QUALITY */
+	{1, oid_null_function},
+	/* 0x15	OID_RT_PRO_SET_CR_SETPOINT */
+	{1, oid_null_function},
+	/* 0x16	OID_RT_PRO_SET_INTEGRATOR */
+	{1, oid_null_function},
+	/* 0x17	OID_RT_PRO_SET_SIGNAL_QUALITY */
+	{1, oid_null_function},
+	/* 0x18	OID_RT_PRO_GET_INTEGRATOR */
+	{1, oid_null_function},
+	/* 0x19	OID_RT_PRO_GET_SIGNAL_QUALITY */
+	{1, oid_null_function},
+	/* 0x1A	OID_RT_PRO_QUERY_EEPROM_TYPE */
+	{1, oid_null_function},
+	/* 0x1B	OID_RT_PRO_WRITE_MAC_ADDRESS */
+	{1, oid_null_function},
+	/* 0x1C	OID_RT_PRO_READ_MAC_ADDRESS */
+	{1, oid_null_function},
+	/* 0x1D	OID_RT_PRO_WRITE_CIS_DATA */
+	{1, oid_null_function},
+	/* 0x1E	OID_RT_PRO_READ_CIS_DATA */
+	{1, oid_null_function},
+	/* 0x1F	OID_RT_PRO_WRITE_POWER_CONTROL */
+	{1, oid_null_function}
 };
 
 static const struct oid_obj_priv oid_rtl_seg_81_80_20[] = {
-	{1, oid_null_function},		/*0x20	OID_RT_PRO_READ_POWER_CONTROL*/
-	{1, oid_null_function},		/*0x21	OID_RT_PRO_WRITE_EEPROM*/
-	{1, oid_null_function},		/*0x22	OID_RT_PRO_READ_EEPROM*/
-	{1, oid_rt_pro_reset_tx_packet_sent_hdl},	/*0x23*/
-	{1, oid_rt_pro_query_tx_packet_sent_hdl},	/*0x24*/
-	{1, oid_rt_pro_reset_rx_packet_received_hdl},	/*0x25*/
-	{1, oid_rt_pro_query_rx_packet_received_hdl},	/*0x26*/
-	{1, oid_rt_pro_query_rx_packet_crc32_error_hdl},/*0x27*/
-	{1, oid_null_function},		/*0x28
-					 *OID_RT_PRO_QUERY_CURRENT_ADDRESS
-					 */
-	{1, oid_null_function},		/*0x29
-					 *OID_RT_PRO_QUERY_PERMANENT_ADDRESS
-					 */
-	{1, oid_null_function},		/*0x2A
-				 *OID_RT_PRO_SET_PHILIPS_RF_PARAMETERS
-				 */
-	{1, oid_rt_pro_set_carrier_suppression_tx_hdl},/*0x2B
-				 *OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX
-				 */
-	{1, oid_null_function},		/*0x2C	OID_RT_PRO_RECEIVE_PACKET*/
-	{1, oid_null_function},		/*0x2D	OID_RT_PRO_WRITE_EEPROM_BYTE*/
-	{1, oid_null_function},		/*0x2E	OID_RT_PRO_READ_EEPROM_BYTE*/
-	{1, oid_rt_pro_set_modulation_hdl}		/*0x2F*/
+	/* 0x20	OID_RT_PRO_READ_POWER_CONTROL */
+	{1, oid_null_function},
+	/* 0x21	OID_RT_PRO_WRITE_EEPROM */
+	{1, oid_null_function},
+	/* 0x22	OID_RT_PRO_READ_EEPROM */
+	{1, oid_null_function},
+	/* 0x23 */
+	{1, oid_rt_pro_reset_tx_packet_sent_hdl},
+	/* 0x24 */
+	{1, oid_rt_pro_query_tx_packet_sent_hdl},
+	/* 0x25 */
+	{1, oid_rt_pro_reset_rx_packet_received_hdl},
+	/* 0x26 */
+	{1, oid_rt_pro_query_rx_packet_received_hdl},
+	/* 0x27 */
+	{1, oid_rt_pro_query_rx_packet_crc32_error_hdl},
+	/* 0x28 OID_RT_PRO_QUERY_CURRENT_ADDRESS */
+	{1, oid_null_function},
+	/* 0x29 OID_RT_PRO_QUERY_PERMANENT_ADDRESS */
+	{1, oid_null_function},
+	/* 0x2A OID_RT_PRO_SET_PHILIPS_RF_PARAMETERS */
+	{1, oid_null_function},
+	/* 0x2B OID_RT_PRO_SET_CARRIER_SUPPRESSION_TX */
+	{1, oid_rt_pro_set_carrier_suppression_tx_hdl},
+	/* 0x2C	OID_RT_PRO_RECEIVE_PACKET */
+	{1, oid_null_function},
+	/* 0x2D	OID_RT_PRO_WRITE_EEPROM_BYTE */
+	{1, oid_null_function},
+	/* 0x2E	OID_RT_PRO_READ_EEPROM_BYTE */
+	{1, oid_null_function},
+	/* 0x2F */
+	{1, oid_rt_pro_set_modulation_hdl}
 };
 
 static const struct oid_obj_priv oid_rtl_seg_81_80_40[] = {
-	{1, oid_null_function},				/*0x40*/
-	{1, oid_null_function},				/*0x41*/
-	{1, oid_null_function},				/*0x42*/
-	{1, oid_rt_pro_set_single_tone_tx_hdl},		/*0x43*/
-	{1, oid_null_function},				/*0x44*/
-	{1, oid_null_function}				/*0x45*/
+	{1, oid_null_function},				/* 0x40 */
+	{1, oid_null_function},				/* 0x41 */
+	{1, oid_null_function},				/* 0x42 */
+	{1, oid_rt_pro_set_single_tone_tx_hdl},		/* 0x43 */
+	{1, oid_null_function},				/* 0x44 */
+	{1, oid_null_function}				/* 0x45 */
 };
 
 static const struct oid_obj_priv oid_rtl_seg_81_80_80[] = {
-	{1, oid_null_function},		/*0x80	OID_RT_DRIVER_OPTION*/
-	{1, oid_null_function},		/*0x81	OID_RT_RF_OFF*/
-	{1, oid_null_function}		/*0x82	OID_RT_AUTH_STATUS*/
+	{1, oid_null_function},		/* 0x80	OID_RT_DRIVER_OPTION */
+	{1, oid_null_function},		/* 0x81	OID_RT_RF_OFF */
+	{1, oid_null_function}		/* 0x82	OID_RT_AUTH_STATUS */
 
 };
 
 static const struct oid_obj_priv oid_rtl_seg_81_85[] = {
-	{1, oid_rt_wireless_mode_hdl}	/*0x00	OID_RT_WIRELESS_MODE*/
+	/* 0x00	OID_RT_WIRELESS_MODE */
+	{1, oid_rt_wireless_mode_hdl}
 };
 
 #else /* _RTL871X_MP_IOCTL_C_ */
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.h b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
index c82fdf85d474..bd2c3a2df48b 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.h
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.h
@@ -48,11 +48,11 @@ enum Power_Mgnt {
 };
 
 /*
-	BIT[2:0] = HW state
-	BIT[3] = Protocol PS state, 0: register active state,
-				    1: register sleep state
-	BIT[4] = sub-state
-*/
+ * 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)
diff --git a/drivers/staging/rtl8712/rtl871x_recv.h b/drivers/staging/rtl8712/rtl871x_recv.h
index f419943ad75d..9de06c5fe620 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.h
+++ b/drivers/staging/rtl8712/rtl871x_recv.h
@@ -74,12 +74,12 @@ struct rx_pkt_attrib {
 };
 
 /*
-accesser of recv_priv: recv_entry(dispatch / passive level);
-recv_thread(passive) ; returnpkt(dispatch)
-; halt(passive) ;
-
-using enter_critical section to protect
-*/
+ * 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;
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index de88819faf05..eda2aee02ff8 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -213,8 +213,9 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 		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);
+			r8712_set_ps_mode(padapter,
+					  padapter->registrypriv.power_mgnt,
+					  padapter->registrypriv.smart_ps);
 		}
 	}
 }
@@ -416,15 +417,13 @@ static sint xmitframe_addmic(struct _adapter *padapter,
 							   &pframe[10], 6);
 			}
 			if (pqospriv->qos_option == 1)
-					priority[0] = (u8)pxmitframe->
-						      attrib.priority;
+				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 = payload + pattrib->
-					  hdrlen + pattrib->iv_len;
+				payload += pattrib->hdrlen + pattrib->iv_len;
 				if ((curfragnum + 1) == pattrib->nr_frags) {
 					length = pattrib->last_txcmdsz -
 						  pattrib->hdrlen -
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
index 74dfc9b0e494..556367bfbe8a 100644
--- a/drivers/staging/rtl8712/wifi.h
+++ b/drivers/staging/rtl8712/wifi.h
@@ -370,7 +370,7 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 
 
 /*-----------------------------------------------------------------------------
-			Below is for the security related definition
+ *		Below is for the security related definition
  *-----------------------------------------------------------------------------
  */
 #define _RESERVED_FRAME_TYPE_	0
@@ -415,7 +415,7 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 
 
 /* ---------------------------------------------------------------------------
-					Below is the fixed elements...
+ *			Below is the fixed elements...
  * ---------------------------------------------------------------------------
  */
 #define _AUTH_ALGM_NUM_			2
@@ -444,14 +444,14 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 #define cap_ShortPremble BIT(5)
 
 /*-----------------------------------------------------------------------------
-				Below is the definition for 802.11i / 802.1x
+ *			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
+ *			Below is the definition for WMM
  *------------------------------------------------------------------------------
  */
 #define _WMM_IE_Length_				7  /* for WMM STA */
@@ -459,7 +459,7 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 
 
 /*-----------------------------------------------------------------------------
-				Below is the definition for 802.11n
+ *			Below is the definition for 802.11n
  *------------------------------------------------------------------------------
  */
 
@@ -498,7 +498,7 @@ struct ieee80211_bar {
 #define IEEE80211_BAR_CTRL_CBMTID_COMPRESSED_BA  0x0004
 
 
- /**
+/*
  * struct ieee80211_ht_cap - HT capabilities
  *
  * This structure refers to "HT capabilities element" as
diff --git a/drivers/staging/rtl8712/wlan_bssdef.h b/drivers/staging/rtl8712/wlan_bssdef.h
index c0654ae4d70d..9dc9ce5a2ccc 100644
--- a/drivers/staging/rtl8712/wlan_bssdef.h
+++ b/drivers/staging/rtl8712/wlan_bssdef.h
@@ -53,9 +53,9 @@ struct NDIS_802_11_CONFIGURATION_FH {
 };
 
 /*
-	FW will only save the channel number in DSConfig.
-	ODI Handler will convert the channel number to freq. number.
-*/
+ * 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 */
diff --git a/drivers/staging/rtl8723bs/Kconfig b/drivers/staging/rtl8723bs/Kconfig
new file mode 100644
index 000000000000..deae0427ba6c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/Kconfig
@@ -0,0 +1,11 @@
+config RTL8723BS
+	tristate "Realtek RTL8723BS SDIO Wireless LAN NIC driver"
+	depends on WLAN && MMC && CFG80211
+	depends on m
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	---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 000000000000..4e7b460a9c73
--- /dev/null
+++ b/drivers/staging/rtl8723bs/Makefile
@@ -0,0 +1,70 @@
+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_odm.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/Hal8723BPwrSeq.o \
+		hal/HalPhyRf.o \
+		hal/HalPwrSeqCmd.o \
+		hal/odm.o \
+		hal/odm_CfoTracking.o \
+		hal/odm_debug.o \
+		hal/odm_DIG.o \
+		hal/odm_DynamicBBPowerSaving.o \
+		hal/odm_DynamicTxPower.o \
+		hal/odm_EdcaTurboCheck.o \
+		hal/odm_HWConfig.o \
+		hal/odm_NoiseMonitor.o \
+		hal/odm_PathDiv.o \
+		hal/odm_RegConfig8723B.o \
+		hal/odm_RTL8723B.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/rtw_proc.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 000000000000..80dbdaca3a8f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/TODO
@@ -0,0 +1,16 @@
+TODO:
+- find and remove code blocks guarded by never set CONFIG_FOO defines
+- find and remove remaining code valid only for 5 HGz. Most of the obvious
+  ones have been removed, but things like channel > 14 still exist.
+- 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>,
+Bastien Nocera <hadess@hadess.net>, 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 000000000000..d3007c1c45e3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ap.c
@@ -0,0 +1,2678 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_AP_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WMM_OUI[];
+extern unsigned char WPS_OUI[];
+extern unsigned char P2P_OUI[];
+extern unsigned char WFD_OUI[];
+
+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);
+
+	/* 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;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	/* update TIM IE */
+	/* if (pstapriv->tim_bitmap) */
+	if (true) {
+
+		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_,
+			_TIM_IE_,
+			&tim_ielen,
+			pnetwork_mlmeext->IELength - _FIXED_IE_LENGTH_
+		);
+		if (p != NULL && tim_ielen > 0) {
+
+			tim_ielen += 2;
+
+			premainder_ie = p+tim_ielen;
+
+			tim_ie_offset = (sint)(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;
+
+			/* calucate head_len */
+			offset = _FIXED_IE_LENGTH_;
+
+			/* get ssid_ie len */
+			p = rtw_get_ie(
+				pie + _BEACON_IE_OFFSET_,
+				_SSID_IE_,
+				&tmp_len,
+				(pnetwork_mlmeext->IELength - _BEACON_IE_OFFSET_)
+			);
+			if (p != NULL)
+				offset += tmp_len+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 !=  NULL)
+				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 = rtw_malloc(remainder_ielen);
+			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&0x00fe))
+			tim_ielen = 5;
+		else
+			tim_ielen = 4;
+
+		*dst_ie++ = tim_ielen;
+
+		*dst_ie++ = 0;/* DTIM count */
+		*dst_ie++ = 1;/* DTIM peroid */
+
+		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->IELength = offset + remainder_ielen;
+
+	}
+}
+
+u8 chk_sta_is_alive(struct sta_info *psta);
+u8 chk_sta_is_alive(struct sta_info *psta)
+{
+	#ifdef DBG_EXPIRATION_CHK
+	DBG_871X(
+		"sta:"MAC_FMT", rssi:%d, rx:"STA_PKTS_FMT", expire_to:%u, %s%ssq_len:%u\n"
+		, MAC_ARG(psta->hwaddr)
+		, psta->rssi_stat.UndecoratedSmoothedPWDB
+		/*  STA_RX_PKTS_ARG(psta) */
+		, STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->expire_to
+		, psta->state&WIFI_SLEEP_STATE?"PS, ":""
+		, psta->state&WIFI_STA_ALIVE_CHK_STATE?"SAC, ":""
+		, psta->sleepq_len
+	);
+	#endif
+
+	sta_update_last_rx_pkts(psta);
+
+	return true;
+}
+
+void expire_timeout_chk(struct adapter *padapter)
+{
+	struct list_head	*phead, *plist;
+	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;
+	plist = get_next(phead);
+
+	/* check auth_queue */
+	#ifdef DBG_EXPIRATION_CHK
+	if (phead != plist) {
+		DBG_871X(FUNC_NDEV_FMT" auth_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->auth_list_cnt);
+	}
+	#endif
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, auth_list);
+
+		plist = get_next(plist);
+
+		if (psta->expire_to > 0) {
+
+			psta->expire_to--;
+			if (psta->expire_to == 0) {
+
+				list_del_init(&psta->auth_list);
+				pstapriv->auth_list_cnt--;
+
+				DBG_871X(
+					"auth expire %02X%02X%02X%02X%02X%02X\n",
+					psta->hwaddr[0],
+					psta->hwaddr[1],
+					psta->hwaddr[2],
+					psta->hwaddr[3],
+					psta->hwaddr[4],
+					psta->hwaddr[5]
+				);
+
+				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;
+	plist = get_next(phead);
+
+	/* check asoc_queue */
+	#ifdef DBG_EXPIRATION_CHK
+	if (phead != plist) {
+		DBG_871X(FUNC_NDEV_FMT" asoc_list, cnt:%u\n"
+			, FUNC_NDEV_ARG(padapter->pnetdev), pstapriv->asoc_list_cnt);
+	}
+	#endif
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+#ifdef CONFIG_AUTO_AP_MODE
+		if (psta->isrc)
+			continue;
+#endif
+		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 staion is at ps mode. */
+					psta->expire_to = pstapriv->expire_to;
+					psta->state |= WIFI_STA_ALIVE_CHK_STATE;
+
+					/* DBG_871X("alive chk, sta:" MAC_FMT " is at ps mode!\n", MAC_ARG(psta->hwaddr)); */
+
+					/* to update bcn with tim_bitmap for this station */
+					pstapriv->tim_bitmap |= BIT(psta->aid);
+					update_beacon(padapter, _TIM_IE_, 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--;
+			DBG_871X(
+				"asoc expire "MAC_FMT", state = 0x%x\n",
+				MAC_ARG(psta->hwaddr),
+				psta->state
+			);
+			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)
+			) {
+				DBG_871X(
+					"%s sta:"MAC_FMT", sleepq_len:%u, free_xmitframe_cnt:%u, asoc_list_cnt:%u, clear sleep_q\n",
+					__func__,
+					MAC_ARG(psta->hwaddr),
+					psta->sleepq_len,
+					padapter->xmitpriv.free_xmitframe_cnt,
+					pstapriv->asoc_list_cnt
+				);
+				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) {
+				DBG_871X(
+					"asoc check, sta(" MAC_FMT ") is alive\n",
+					MAC_ARG(psta->hwaddr)
+					);
+				psta->expire_to = pstapriv->expire_to;
+				psta->keep_alive_trycnt = 0;
+				continue;
+			} else if (psta->keep_alive_trycnt <= 3) {
+
+				DBG_871X(
+					"ack check for asoc expire, keep_alive_trycnt =%d\n",
+					psta->keep_alive_trycnt);
+				psta->expire_to = 1;
+				continue;
+			}
+
+			psta->keep_alive_trycnt = 0;
+			DBG_871X(
+				"asoc expire "MAC_FMT", state = 0x%x\n",
+				MAC_ARG(psta->hwaddr),
+				psta->state);
+			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.DSConfig > 14) {
+
+		if (tx_ra_bitmap & 0xffff000)
+			sta_band |= WIRELESS_11_5N;
+
+		if (tx_ra_bitmap & 0xff0)
+			sta_band |= WIRELESS_11A;
+	} 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 = rtw_hal_networktype_to_raid(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;
+
+		DBG_871X("%s => mac_id:%d , raid:%d , shortGIrate =%d, bitmap = 0x%x\n",
+			__func__, psta->mac_id, psta->raid, shortGIrate, tx_ra_bitmap);
+
+		rtw_hal_add_ra_tid(padapter, tx_ra_bitmap, arg, rssi_level);
+	} else{
+
+
+		DBG_871X("station aid %d exceed the max number\n", psta->aid);
+	}
+
+}
+
+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->SupportedRates);
+		network_type = rtw_check_network_type(
+			(u8 *)&pcur_network->SupportedRates,
+			supportRateNum,
+			pcur_network->Configuration.DSConfig
+		);
+		if (IsSupportedTxCCK(network_type)) {
+			network_type = WIRELESS_11B;
+		} else if (network_type == WIRELESS_INVALID) { /*  error handling */
+
+			if (pcur_network->Configuration.DSConfig > 14)
+				network_type = WIRELESS_11A;
+			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 = rtw_hal_networktype_to_raid(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;
+
+			DBG_871X("%s => mac_id:%d , raid:%d , bitmap = 0x%x\n",
+				__func__, psta->mac_id, psta->raid, tx_ra_bitmap);
+
+			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);
+
+	} else{
+
+
+		DBG_871X("add_RATid_bmc_sta error!\n");
+	}
+
+}
+
+/* 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; */
+
+	DBG_871X("%s\n", __func__);
+
+	/* 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));
+			DBG_871X("Enable HT Tx LDPC for STA(%d)\n", psta->aid);
+		}
+
+		/*  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));
+			DBG_871X("Enable HT Tx STBC for STA(%d)\n", psta->aid);
+		}
+	} 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->SupportedRates);
+	memcpy(psta->bssrateset, pnetwork->SupportedRates, 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);
+
+	DBG_871X("%s\n", __func__);
+
+
+	/* 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;
+	if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/*  */
+	/*  Config current HT Protection mode. */
+	/*  */
+	/* pmlmeinfo->HT_protection = pmlmeinfo->HT_info.infos[1] & 0x3; */
+
+}
+
+void start_bss_network(struct adapter *padapter, u8 *pbuf)
+{
+	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;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	bcn_interval = (u16)pnetwork->Configuration.BeaconPeriod;
+	cur_channel = pnetwork->Configuration.DSConfig;
+	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 (NULL == rtw_get_wps_ie(
+		pnetwork->IEs+_FIXED_IE_LENGTH_,
+		pnetwork->IELength-_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 != true) { /* 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->MacAddress);
+
+	/* 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 != true) { /* 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)),
+		_HT_ADD_INFO_IE_,
+		&ie_len,
+		(pnetwork->IELength - 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);
+	DBG_871X(
+		"CH =%d, BW =%d, offset =%d\n",
+		cur_channel,
+		cur_bwmode,
+		cur_ch_offset
+	);
+	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->SupportedRates);
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
+
+	/* udpate capability after cur_wireless_mode updated */
+	update_capinfo(
+		padapter,
+		rtw_get_capability((struct wlan_bssid_ex *)pnetwork)
+	);
+
+
+	if (true == pmlmeext->bstart_bss) {
+
+		update_beacon(padapter, _TIM_IE_, NULL, true);
+
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN /* other case will  tx beacon when bcn interrupt coming in. */
+		/* issue beacon frame */
+		if (send_beacon(padapter) == _FAIL)
+			DBG_871X("issue_beacon, fail!\n");
+
+#endif /* CONFIG_INTERRUPT_BASED_TXBCN */
+
+	}
+
+
+	/* 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;
+
+	/* SSID */
+	/* Supported rates */
+	/* DS Params */
+	/* WLAN_EID_COUNTRY */
+	/* ERP Information element */
+	/* Extended supported rates */
+	/* WPA/WPA2 */
+	/* Wi-Fi Wireless Multimedia Extensions */
+	/* ht_capab, ht_oper */
+	/* WPS IE */
+
+	DBG_871X("%s, len =%d\n", __func__, len);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return _FAIL;
+
+
+	if (len > MAX_IE_SZ)
+		return _FAIL;
+
+	pbss_network->IELength = len;
+
+	memset(ie, 0, MAX_IE_SZ);
+
+	memcpy(ie, pbuf, pbss_network->IELength);
+
+
+	if (pbss_network->InfrastructureMode != Ndis802_11APMode)
+		return _FAIL;
+
+	pbss_network->Rssi = 0;
+
+	memcpy(pbss_network->MacAddress, 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.BeaconPeriod = le16_to_cpu(*(unsigned short*)p); */
+	pbss_network->Configuration.BeaconPeriod = RTW_GET_LE16(p);
+
+	/* capability */
+	/* cap = *(unsigned short *)rtw_get_capability_from_ie(ie); */
+	/* cap = le16_to_cpu(cap); */
+	cap = RTW_GET_LE16(ie);
+
+	/* SSID */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_SSID_IE_,
+		&ie_len,
+		(pbss_network->IELength - _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.SsidLength = ie_len;
+	}
+
+	/* chnnel */
+	channel = 0;
+	pbss_network->Configuration.Length = 0;
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_DSSET_IE_, &ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		channel = *(p + 2);
+
+	pbss_network->Configuration.DSConfig = channel;
+
+
+	memset(supportRate, 0, NDIS_802_11_LENGTH_RATES_EX);
+	/*  get supported rates */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_SUPPORTEDRATES_IE_,
+		&ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p !=  NULL) {
+
+		memcpy(supportRate, p+2, ie_len);
+		supportRateNum = ie_len;
+	}
+
+	/* get ext_supported rates */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_EXT_SUPPORTEDRATES_IE_,
+		&ie_len,
+		pbss_network->IELength - _BEACON_IE_OFFSET_
+	);
+	if (p !=  NULL) {
+
+		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->SupportedRates, network_type);
+
+
+	/* parsing ERP_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_ERPINFO_IE_,
+		&ie_len,
+		(pbss_network->IELength - _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_,
+		_RSN_IE_2_,
+		&ie_len,
+		(pbss_network->IELength - _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,
+			_SSN_IE_1_,
+			&ie_len,
+			(pbss_network->IELength - _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 == NULL) || (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,
+				_VENDOR_SPECIFIC_IE_,
+				&ie_len,
+				(pbss_network->IELength - _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 == NULL) || (ie_len == 0))
+				break;
+
+		}
+	}
+
+	/* parsing HT_CAP_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_HT_CAPABILITY_IE_,
+		&ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0) {
+
+		u8 rf_type = 0;
+		u8 max_rx_ampdu_factor = 0;
+		struct rtw_ieee80211_ht_cap *pht_cap = (struct rtw_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 */
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		if (rf_type == RF_1T1R) {
+
+			pht_cap->supp_mcs_set[0] = 0xff;
+			pht_cap->supp_mcs_set[1] = 0x0;
+		}
+
+		memcpy(&pmlmepriv->htpriv.ht_cap, p+2, ie_len);
+
+	}
+
+	/* parsing HT_INFO_IE */
+	p = rtw_get_ie(
+		ie + _BEACON_IE_OFFSET_,
+		_HT_ADD_INFO_IE_,
+		&ie_len,
+		(pbss_network->IELength - _BEACON_IE_OFFSET_)
+	);
+	if (p && ie_len > 0)
+		pHT_info_ie = p;
+
+
+	switch (network_type) {
+
+	case WIRELESS_11B:
+		pbss_network->NetworkTypeInUse = Ndis802_11DS;
+		break;
+	case WIRELESS_11G:
+	case WIRELESS_11BG:
+	case WIRELESS_11G_24N:
+	case WIRELESS_11BG_24N:
+		pbss_network->NetworkTypeInUse = Ndis802_11OFDM24;
+		break;
+	case WIRELESS_11A:
+		pbss_network->NetworkTypeInUse = Ndis802_11OFDM5;
+		break;
+	default:
+		pbss_network->NetworkTypeInUse = 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 == true) {
+
+		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->MacAddress);
+	if (!psta) {
+
+		psta = rtw_alloc_stainfo(&padapter->stapriv, pbss_network->MacAddress);
+		if (psta == NULL)
+			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;
+
+	DBG_871X("%s, mode =%d\n", __func__, mode);
+
+	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;
+
+	DBG_871X(
+		"%s(acl_num =%d) =" MAC_FMT "\n",
+		__func__,
+		pacl_list->num,
+		MAC_ARG(addr)
+	);
+
+	if ((NUM_ACL-1) < pacl_list->num)
+		return (-1);
+
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (!memcmp(paclnode->addr, addr, ETH_ALEN)) {
+
+			if (paclnode->valid == true) {
+
+				added = true;
+				DBG_871X("%s, sta has been added\n", __func__);
+				break;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+
+	if (added == true)
+		return ret;
+
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	for (i = 0; i < NUM_ACL; i++) {
+
+		paclnode = &pacl_list->aclnode[i];
+
+		if (paclnode->valid == false) {
+
+			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;
+		}
+	}
+
+	DBG_871X("%s, acl_num =%d\n", __func__, pacl_list->num);
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	return ret;
+}
+
+int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
+{
+	struct list_head	*plist, *phead;
+	int 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;
+	u8 baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };	/* Baddr is used for clearing acl_list */
+
+	DBG_871X(
+		"%s(acl_num =%d) =" MAC_FMT "\n",
+		__func__,
+		pacl_list->num,
+		MAC_ARG(addr)
+	);
+
+	spin_lock_bh(&(pacl_node_q->lock));
+
+	phead = get_list_head(pacl_node_q);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (
+			!memcmp(paclnode->addr, addr, ETH_ALEN) ||
+			!memcmp(baddr, addr, ETH_ALEN)
+		) {
+
+			if (paclnode->valid == true) {
+
+				paclnode->valid = false;
+
+				list_del_init(&paclnode->list);
+
+				pacl_list->num--;
+			}
+		}
+	}
+
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	DBG_871X("%s, acl_num =%d\n", __func__, pacl_list->num);
+
+	return ret;
+
+}
+
+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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(
+		sizeof(struct set_stakey_parm)
+	);
+	if (psetstakey_para == NULL) {
+		kfree((u8 *) 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;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
+	if (psetkeyparm == NULL) {
+		kfree((unsigned char *)pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+	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)
+{
+	DBG_871X("%s\n", __func__);
+
+	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_;
+	}
+
+	DBG_871X("%s\n", __func__);
+
+	return rtw_ap_set_key(padapter, key, alg, keyid, set_tx);
+}
+
+static void update_bcn_fixed_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+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;
+
+	DBG_871X("%s, ERP_enable =%d\n", __func__, pmlmeinfo->ERP_enable);
+
+	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_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)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_htinfo_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_rsn_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_wpa_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+static void update_bcn_wmm_ie(struct adapter *padapter)
+{
+	DBG_871X("%s\n", __func__);
+
+}
+
+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->IELength;
+
+
+	DBG_871X("%s\n", __func__);
+
+	pwps_ie = rtw_get_wps_ie(
+		ie+_FIXED_IE_LENGTH_,
+		ielen-_FIXED_IE_LENGTH_,
+		NULL,
+		&wps_ielen
+	);
+
+	if (pwps_ie == NULL || wps_ielen == 0)
+		return;
+
+	pwps_ie_src = pmlmepriv->wps_beacon_ie;
+	if (pwps_ie_src == NULL)
+		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 IELength */
+		pnetwork->IELength = wps_offset + (wps_ielen+2) + remainder_ielen;
+	}
+
+	kfree(pbackup_remainder_ie);
+
+	/*  deal with the case without set_tx_beacon_cmd() in update_beacon() */
+#if defined(CONFIG_INTERRUPT_BASED_TXBCN)
+	if ((pmlmeinfo->state&0x03) == WIFI_FW_AP_STATE) {
+
+		u8 sr = 0;
+
+		rtw_get_wps_attr_content(
+			pwps_ie_src,
+			wps_ielen,
+			WPS_ATTR_SELECTED_REGISTRAR,
+			(u8 *)(&sr),
+			NULL
+		);
+
+		if (sr) {
+			set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
+			DBG_871X("%s, set WIFI_UNDER_WPS\n", __func__);
+		}
+	}
+#endif
+}
+
+static void update_bcn_p2p_ie(struct adapter *padapter)
+{
+
+}
+
+static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
+{
+	DBG_871X("%s\n", __func__);
+
+	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);
+
+	else
+		DBG_871X("unknown OUI type!\n");
+
+
+
+}
+
+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; */
+
+	/* DBG_871X("%s\n", __func__); */
+
+	if (!padapter)
+		return;
+
+	pmlmepriv = &(padapter->mlmepriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	/* pmlmeinfo = &(pmlmeext->mlmext_info); */
+
+	if (false == 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 _TIM_IE_:
+
+		update_BCNTIM(padapter);
+
+		break;
+
+	case _ERPINFO_IE_:
+
+		update_bcn_erpinfo_ie(padapter);
+
+		break;
+
+	case _HT_CAPABILITY_IE_:
+
+		update_bcn_htcap_ie(padapter);
+
+		break;
+
+	case _RSN_IE_2_:
+
+		update_bcn_rsn_ie(padapter);
+
+		break;
+
+	case _HT_ADD_INFO_IE_:
+
+		update_bcn_htinfo_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);
+
+#ifndef CONFIG_INTERRUPT_BASED_TXBCN
+	if (tx) {
+
+		/* send_beacon(padapter);//send_beacon must execute on TSR level */
+		set_tx_beacon_cmd(padapter);
+	}
+#endif /* CONFIG_INTERRUPT_BASED_TXBCN */
+
+}
+
+/*
+op_mode
+Set to 0 (HT pure) under the followign 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 == true)
+		return 0;
+
+	/* if (!iface->conf->ieee80211n || iface->conf->ht_op_mode_fixed) */
+	/*  return 0; */
+
+	DBG_871X("%s current operation mode = 0x%X\n",
+		   __func__, pmlmepriv->ht_op_mode);
+
+	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)
+		&& 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++;
+	}
+
+	DBG_871X("%s new operation mode = 0x%X changes =%d\n",
+		   __func__, pmlmepriv->ht_op_mode, op_mode_changes);
+
+	return op_mode_changes;
+
+}
+
+void associated_clients_update(struct adapter *padapter, u8 updated)
+{
+	/* update associcated stations cap. */
+	if (updated == true) {
+
+		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 = get_next(phead);
+
+		/* check asoc_queue */
+		while (phead != plist) {
+
+			psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+			plist = get_next(plist);
+
+			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);
+
+		DBG_871X("HT: STA " MAC_FMT " HT Capabilities "
+			   "Info: 0x%04x\n", MAC_ARG(psta->hwaddr), ht_capab);
+
+		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++;
+			}
+			DBG_871X("%s STA " MAC_FMT " - no "
+				   "greenfield, num of non-gf stations %d\n",
+				   __func__, MAC_ARG(psta->hwaddr),
+				   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++;
+			}
+			DBG_871X("%s STA " MAC_FMT " - 20 MHz HT, "
+				   "num of 20MHz HT STAs %d\n",
+				   __func__, MAC_ARG(psta->hwaddr),
+				   pmlmepriv->num_sta_ht_20mhz);
+		}
+
+	} else{
+
+
+		if (!psta->no_ht_set) {
+			psta->no_ht_set = 1;
+			pmlmepriv->num_sta_no_ht++;
+		}
+		if (pmlmepriv->htpriv.ht_option == true) {
+			DBG_871X("%s STA " MAC_FMT
+				   " - no HT, num of non-HT stations %d\n",
+				   __func__, MAC_ARG(psta->hwaddr),
+				   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 associcated stations cap. */
+	associated_clients_update(padapter,  beacon_updated);
+
+	DBG_871X("%s, updated =%d\n", __func__, 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 associcated stations cap. */
+	/* associated_clients_update(padapter,  beacon_updated); //move it to avoid deadlock */
+
+	DBG_871X("%s, updated =%d\n", __func__, beacon_updated);
+
+	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 == true) {
+
+		/* 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;
+
+}
+
+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};
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	if ((pmlmeinfo->state&0x03) != WIFI_FW_AP_STATE)
+		return ret;
+
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* free sta asoc_queue */
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+		plist = get_next(plist);
+
+		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);
+
+	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 == false)
+		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_priv *mlmepriv = &padapter->mlmepriv;
+	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, (u8 *)&mlmepriv->cur_network.network);
+
+	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;
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		int stainfo_offset;
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+
+		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 == NULL) {
+			DBG_871X(FUNC_ADPT_FMT" sta_info is null\n", FUNC_ADPT_ARG(padapter));
+		} else 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 ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+				(padapter->securitypriv.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;
+	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 = get_next(phead);
+	while (phead != plist) {
+
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		if (paclnode->valid == true) {
+
+			paclnode->valid = false;
+
+			list_del_init(&paclnode->list);
+
+			pacl_list->num--;
+		}
+	}
+	spin_unlock_bh(&(pacl_node_q->lock));
+
+	DBG_871X("%s, free acl_node_queue, num =%d\n", __func__, pacl_list->num);
+
+	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 000000000000..3c5cb78b52ea
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_btcoex.c
@@ -0,0 +1,243 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_btcoex.h>
+#include <hal_btcoex.h>
+
+
+void rtw_btcoex_Initialize(struct adapter *padapter)
+{
+	hal_btcoex_Initialize(padapter);
+}
+
+void rtw_btcoex_PowerOnSetting(struct adapter *padapter)
+{
+	hal_btcoex_PowerOnSetting(padapter);
+}
+
+void rtw_btcoex_HAL_Initialize(struct adapter *padapter, u8 bWifiOnly)
+{
+	hal_btcoex_InitHwConfig(padapter, bWifiOnly);
+}
+
+void rtw_btcoex_IpsNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_IpsNotify(padapter, type);
+}
+
+void rtw_btcoex_LpsNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_LpsNotify(padapter, type);
+}
+
+void rtw_btcoex_ScanNotify(struct adapter *padapter, u8 type)
+{
+	hal_btcoex_ScanNotify(padapter, type);
+}
+
+void rtw_btcoex_ConnectNotify(struct adapter *padapter, u8 action)
+{
+	hal_btcoex_ConnectNotify(padapter, action);
+}
+
+void rtw_btcoex_MediaStatusNotify(struct adapter *padapter, u8 mediaStatus)
+{
+	if ((RT_MEDIA_CONNECT == mediaStatus)
+		&& (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_SpecialPacketNotify(struct adapter *padapter, u8 pktType)
+{
+	hal_btcoex_SpecialPacketNotify(padapter, pktType);
+}
+
+void rtw_btcoex_IQKNotify(struct adapter *padapter, u8 state)
+{
+	hal_btcoex_IQKNotify(padapter, state);
+}
+
+void rtw_btcoex_BtInfoNotify(struct adapter *padapter, u8 length, u8 *tmpBuf)
+{
+	hal_btcoex_BtInfoNotify(padapter, length, tmpBuf);
+}
+
+void rtw_btcoex_SuspendNotify(struct adapter *padapter, u8 state)
+{
+	hal_btcoex_SuspendNotify(padapter, state);
+}
+
+void rtw_btcoex_HaltNotify(struct adapter *padapter)
+{
+	if (false == padapter->bup) {
+		DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n",
+			FUNC_ADPT_ARG(padapter), padapter->bup);
+
+		return;
+	}
+
+	if (true == padapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(padapter), padapter->bSurpriseRemoved);
+
+		return;
+	}
+
+	hal_btcoex_HaltNotify(padapter);
+}
+
+u8 rtw_btcoex_IsBtDisabled(struct adapter *padapter)
+{
+	return hal_btcoex_IsBtDisabled(padapter);
+}
+
+void rtw_btcoex_Handler(struct adapter *padapter)
+{
+	hal_btcoex_Hanlder(padapter);
+}
+
+s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter)
+{
+	s32 coexctrl;
+
+	coexctrl = hal_btcoex_IsBTCoexCtrlAMPDUSize(padapter);
+
+	return coexctrl;
+}
+
+void rtw_btcoex_SetManualControl(struct adapter *padapter, u8 manual)
+{
+	if (true == manual) {
+		hal_btcoex_SetManualControl(padapter, true);
+	} else{
+		hal_btcoex_SetManualControl(padapter, false);
+	}
+}
+
+u8 rtw_btcoex_IsBtControlLps(struct adapter *padapter)
+{
+	return hal_btcoex_IsBtControlLps(padapter);
+}
+
+u8 rtw_btcoex_IsLpsOn(struct adapter *padapter)
+{
+	return hal_btcoex_IsLpsOn(padapter);
+}
+
+u8 rtw_btcoex_RpwmVal(struct adapter *padapter)
+{
+	return hal_btcoex_RpwmVal(padapter);
+}
+
+u8 rtw_btcoex_LpsVal(struct adapter *padapter)
+{
+	return hal_btcoex_LpsVal(padapter);
+}
+
+void rtw_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist)
+{
+	hal_btcoex_SetBTCoexist(padapter, bBtExist);
+}
+
+void rtw_btcoex_SetChipType(struct adapter *padapter, u8 chipType)
+{
+	hal_btcoex_SetChipType(padapter, chipType);
+}
+
+void rtw_btcoex_SetPGAntNum(struct adapter *padapter, u8 antNum)
+{
+	hal_btcoex_SetPgAntNum(padapter, antNum);
+}
+
+void rtw_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath)
+{
+	hal_btcoex_SetSingleAntPath(padapter, singleAntPath);
+}
+
+u32 rtw_btcoex_GetRaMask(struct adapter *padapter)
+{
+	return hal_btcoex_GetRaMask(padapter);
+}
+
+void rtw_btcoex_RecordPwrMode(struct adapter *padapter, u8 *pCmdBuf, u8 cmdLen)
+{
+	hal_btcoex_RecordPwrMode(padapter, pCmdBuf, cmdLen);
+}
+
+void rtw_btcoex_DisplayBtCoexInfo(struct adapter *padapter, u8 *pbuf, u32 bufsize)
+{
+	hal_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+}
+
+void rtw_btcoex_SetDBG(struct adapter *padapter, u32 *pDbgModule)
+{
+	hal_btcoex_SetDBG(padapter, pDbgModule);
+}
+
+u32 rtw_btcoex_GetDBG(struct adapter *padapter, u8 *pStrBuf, u32 bufSize)
+{
+	return hal_btcoex_GetDBG(padapter, pStrBuf, bufSize);
+}
+
+/*  ================================================== */
+/*  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 (true == enable) {
+		pmlmeinfo->bAcceptAddbaReq = false;
+		if (psta)
+			send_delba(padapter, 0, psta->hwaddr);
+	} else{
+		pmlmeinfo->bAcceptAddbaReq = 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 = rtw_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 000000000000..080c81b9aa94
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_cmd.c
@@ -0,0 +1,2226 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_CMD_C_
+
+#include <drv_types.h>
+#include <rtw_debug.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(_LedBlink), NULL},/*60*/
+
+	{GEN_CMD_CODE(_SetChannelSwitch), NULL},/*61*/
+	{GEN_CMD_CODE(_TDLS), NULL},/*62*/
+	{GEN_CMD_CODE(_ChkBMCSleepq), NULL}, /*63*/
+
+	{GEN_CMD_CODE(_RunInThreadCMD), NULL},/*64*/
+};
+
+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*/
+	GEN_MLME_EXT_HANDLER(0, chk_bmc_sleepq_hdl) /*63*/
+	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.
+*/
+
+sint	_rtw_init_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	sint res = _SUCCESS;
+
+	sema_init(&(pcmdpriv->cmd_queue_sema), 0);
+	/* sema_init(&(pcmdpriv->cmd_done_sema), 0); */
+	sema_init(&(pcmdpriv->terminate_cmdthread_sema), 0);
+
+
+	_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 = rtw_zmalloc(MAX_CMDSZ + CMDBUFF_ALIGN_SZ);
+
+	if (pcmdpriv->cmd_allocated_buf == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	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 == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pcmdpriv->rsp_buf = pcmdpriv->rsp_allocated_buf  +  4 - ((SIZE_PTR)(pcmdpriv->rsp_allocated_buf) & 3);
+
+	pcmdpriv->cmd_issued_cnt = pcmdpriv->cmd_done_cnt = pcmdpriv->rsp_cnt = 0;
+
+	mutex_init(&pcmdpriv->sctx_mutex);
+exit:
+	return res;
+}
+
+static void c2h_wk_callback(_workitem *work);
+sint _rtw_init_evt_priv(struct evt_priv *pevtpriv)
+{
+	sint res = _SUCCESS;
+
+	/* 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);
+
+	return res;
+}
+
+void _rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+_rtw_free_evt_priv\n"));
+
+	_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 != NULL && c2h != (void *)pevtpriv) {
+			kfree(c2h);
+		}
+	}
+	kfree(pevtpriv->c2h_queue);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("-_rtw_free_evt_priv\n"));
+}
+
+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.
+
+*/
+
+sint	_rtw_enqueue_cmd(struct __queue *queue, struct cmd_obj *obj)
+{
+	_irqL irqL;
+
+	if (obj == NULL)
+		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)
+{
+	_irqL 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 = LIST_CONTAINOR(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;
+}
+
+u32 rtw_init_cmd_priv(struct cmd_priv *pcmdpriv)
+{
+	u32 res;
+
+	res = _rtw_init_cmd_priv(pcmdpriv);
+	return res;
+}
+
+u32 rtw_init_evt_priv(struct	evt_priv *pevtpriv)
+{
+	int	res;
+
+	res = _rtw_init_evt_priv(pevtpriv);
+	return res;
+}
+
+void rtw_free_evt_priv(struct	evt_priv *pevtpriv)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_evt_priv\n"));
+	_rtw_free_evt_priv(pevtpriv);
+}
+
+void rtw_free_cmd_priv(struct	cmd_priv *pcmdpriv)
+{
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("rtw_free_cmd_priv\n"));
+	_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 == false && bAllow == false)
+		|| atomic_read(&(pcmdpriv->cmdthd_running)) == false	/* com_thread not running */
+	) {
+		/* DBG_871X("%s:%s: drop cmdcode:%u, hw_init_completed:%u, cmdthd_running:%u\n", caller_func, __func__, */
+		/* 	cmd_obj->cmdcode, */
+		/* 	pcmdpriv->padapter->hw_init_completed, */
+		/* 	pcmdpriv->cmdthd_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 == NULL) {
+		goto exit;
+	}
+
+	cmd_obj->padapter = padapter;
+
+	res = rtw_cmd_filter(pcmdpriv, cmd_obj);
+	if (_FAIL == res) {
+		rtw_free_cmd_obj(cmd_obj);
+		goto exit;
+	}
+
+	res = _rtw_enqueue_cmd(&pcmdpriv->cmd_queue, cmd_obj);
+
+	if (res == _SUCCESS)
+		up(&pcmdpriv->cmd_queue_sema);
+
+exit:
+	return res;
+}
+
+struct	cmd_obj	*rtw_dequeue_cmd(struct cmd_priv *pcmdpriv)
+{
+	struct cmd_obj *cmd_obj;
+
+	cmd_obj = _rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
+
+	return cmd_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((unsigned char *)pcmd->parmbuf);
+	}
+
+	if (pcmd->rsp != NULL) {
+		if (pcmd->rspsz != 0) {
+			/* free rsp in cmd_obj */
+			kfree((unsigned char *)pcmd->rsp);
+		}
+	}
+
+	/* free cmd_obj */
+	kfree((unsigned char *)pcmd);
+}
+
+
+void rtw_stop_cmd_thread(struct adapter *adapter)
+{
+	if (adapter->cmdThread &&
+		atomic_read(&(adapter->cmdpriv.cmdthd_running)) == true &&
+		adapter->cmdpriv.stop_req == 0) {
+		adapter->cmdpriv.stop_req = 1;
+		up(&adapter->cmdpriv.cmd_queue_sema);
+		down(&adapter->cmdpriv.terminate_cmdthread_sema);
+	}
+}
+
+int rtw_cmd_thread(void *context)
+{
+	u8 ret;
+	struct cmd_obj *pcmd;
+	u8 *pcmdbuf, *prspbuf;
+	unsigned long cmd_start_time;
+	unsigned long cmd_process_time;
+	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);
+	struct drvextra_cmd_parm *extra_parm = NULL;
+
+	thread_enter("RTW_CMD_THREAD");
+
+	pcmdbuf = pcmdpriv->cmd_buf;
+	prspbuf = pcmdpriv->rsp_buf;
+
+	pcmdpriv->stop_req = 0;
+	atomic_set(&(pcmdpriv->cmdthd_running), true);
+	up(&pcmdpriv->terminate_cmdthread_sema);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("start r871x rtw_cmd_thread !!!!\n"));
+
+	while (1) {
+		if (down_interruptible(&pcmdpriv->cmd_queue_sema)) {
+			DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" down_interruptible(&pcmdpriv->cmd_queue_sema) return != 0, break\n", FUNC_ADPT_ARG(padapter));
+			break;
+		}
+
+		if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) {
+			DBG_871X_LEVEL(_drv_always_, "%s: DriverStopped(%d) SurpriseRemoved(%d) break at line %d\n",
+				__func__, padapter->bDriverStopped, padapter->bSurpriseRemoved, __LINE__);
+			break;
+		}
+
+		if (pcmdpriv->stop_req) {
+			DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" stop_req:%u, break\n", FUNC_ADPT_ARG(padapter), pcmdpriv->stop_req);
+			break;
+		}
+
+		if (list_empty(&(pcmdpriv->cmd_queue.queue))) {
+			/* DBG_871X("%s: cmd queue is empty!\n", __func__); */
+			continue;
+		}
+
+		if (rtw_register_cmd_alive(padapter) != _SUCCESS) {
+			RT_TRACE(_module_hal_xmit_c_, _drv_notice_,
+					 ("%s: wait to leave LPS_LCLK\n", __func__));
+			continue;
+		}
+
+_next:
+		if ((padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true)) {
+			DBG_871X_LEVEL(_drv_always_, "%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;
+		}
+
+		cmd_start_time = jiffies;
+
+		if (_FAIL == rtw_cmd_filter(pcmdpriv, pcmd)) {
+			pcmd->res = H2C_DROPPED;
+			goto post_process;
+		}
+
+		pcmdpriv->cmd_issued_cnt++;
+
+		pcmd->cmdsz = _RND4((pcmd->cmdsz));/* _RND4 */
+
+		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) {
+				DBG_871X_LEVEL(_drv_always_, 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));
+		}
+
+		cmd_process_time = jiffies_to_msecs(jiffies - cmd_start_time);
+		if (cmd_process_time > 1000) {
+			if (pcmd->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
+				DBG_871X(ADPT_FMT" cmd =%d process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			} else if (pcmd->cmdcode == GEN_CMD_CODE(_Set_MLME_EVT)) {
+				DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			} else {
+				DBG_871X(ADPT_FMT" cmd =%d, process_time =%lu > 1 sec\n",
+					ADPT_ARG(pcmd->padapter), pcmd->cmdcode, cmd_process_time);
+				/* rtw_warn_on(1); */
+			}
+		}
+
+		/* 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 == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("mlme_cmd_hdl(): pcmd_callback = 0x%p, cmdcode = 0x%x\n", pcmd_callback, pcmd->cmdcode));
+				rtw_free_cmd_obj(pcmd);
+			} else{
+				/* todo: !!! fill rsp_buf to pcmd->rsp if (pcmd->rsp!= NULL) */
+				pcmd_callback(pcmd->padapter, pcmd);/* need conider that free cmd_obj in rtw_cmd_callback */
+			}
+		} else{
+			RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("%s: cmdcode = 0x%x callback not defined!\n", __func__, pcmd->cmdcode));
+			rtw_free_cmd_obj(pcmd);
+		}
+
+		flush_signals_thread();
+
+		goto _next;
+
+	}
+
+	/*  free all cmd_obj resources */
+	do {
+		pcmd = rtw_dequeue_cmd(pcmdpriv);
+		if (pcmd == NULL) {
+			rtw_unregister_cmd_alive(padapter);
+			break;
+		}
+
+		/* DBG_871X("%s: leaving... drop cmdcode:%u size:%d\n", __func__, pcmd->cmdcode, pcmd->cmdsz); */
+
+		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);
+
+	up(&pcmdpriv->terminate_cmdthread_sema);
+	atomic_set(&(pcmdpriv->cmdthd_running), false);
+
+	thread_exit();
+}
+
+/*
+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) == true) {
+		rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SCAN, 1);
+	}
+
+	ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL)
+		return _FAIL;
+
+	psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+	if (psurveyPara == NULL) {
+		kfree((unsigned char *) ph2c);
+		return _FAIL;
+	}
+
+	rtw_free_network_queue(padapter, false);
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("%s: flush network queue\n", __func__));
+
+	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++;
+
+				DBG_871X(FUNC_ADPT_FMT" ssid:(%s, %d)\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ssid[i].Ssid, psurveyPara->ssid[i].SsidLength);
+			}
+		}
+	}
+
+	/* 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++;
+
+				DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter),
+					psurveyPara->ch[i].hw_value);
+			}
+		}
+	}
+
+	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;
+}
+
+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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pbsetdataratepara = (struct setdatarate_parm *)rtw_zmalloc(sizeof(struct setdatarate_parm));
+	if (pbsetdataratepara == NULL) {
+		kfree((u8 *) 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)
+{
+	/* rtw_free_cmd_obj(pcmd); */
+	kfree((unsigned char *) pcmd->parmbuf);
+	kfree((unsigned char *) pcmd);
+}
+
+u8 rtw_createbss_cmd(struct adapter  *padapter)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv 			*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_priv 		*pmlmepriv = &padapter->mlmepriv;
+	struct wlan_bssid_ex		*pdev_network = &padapter->registrypriv.dev_network;
+	u8 res = _SUCCESS;
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for Any SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, (" createbss for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		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_startbss_cmd(struct adapter  *padapter, int flags)
+{
+	struct cmd_obj *pcmd;
+	struct cmd_priv  *pcmdpriv = &padapter->cmdpriv;
+	struct submit_ctx sctx;
+	u8 res = _SUCCESS;
+
+	if (flags & RTW_CMDF_DIRECTLY) {
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		start_bss_network(padapter, (u8 *)&(padapter->mlmepriv.cur_network.network));
+	} else {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (pcmd == NULL) {
+			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, __func__);
+			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 *auth, 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.InfrastructureMode;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	u32 tmp_len;
+	u8 *ptmp = NULL;
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_, ("+Join cmd: Any SSid\n"));
+	} else {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+Join cmd: SSid =[%s]\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	pcmd = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd: memory allocate for cmd_obj fail!!!\n"));
+		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;
+	if (psecnetwork == NULL) {
+		if (pcmd != NULL)
+			kfree((unsigned char *)pcmd);
+
+		res = _FAIL;
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("rtw_joinbss_cmd :psecnetwork == NULL!!!\n"));
+
+		goto exit;
+	}
+
+	memset(psecnetwork, 0, t_len);
+
+	memcpy(psecnetwork, &pnetwork->network, get_wlan_bssid_ex_sz(&pnetwork->network));
+
+	auth = &psecuritypriv->authenticator_ie[0];
+	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;
+	/*  Added by Albert 2009/02/18 */
+	/*  If the 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 == false) {
+		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) {
+		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;
+	ptmp = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &tmp_len, pnetwork->network.IELength-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->IELength);
+
+			/* rtw_restructure_ht_ie */
+			rtw_restructure_ht_ie(padapter, &pnetwork->network.IEs[12], &psecnetwork->IEs[0],
+									pnetwork->network.IELength-12, &psecnetwork->IELength,
+									pnetwork->network.Configuration.DSConfig);
+		}
+	}
+
+	rtw_append_exented_cap(padapter, &psecnetwork->IEs[0], &psecnetwork->IELength);
+
+	pmlmeinfo->assoc_AP_vendor = check_assoc_AP(pnetwork->network.IEs, pnetwork->network.IELength);
+
+	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;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_disassoc_cmd\n"));
+
+	/* prepare cmd parameter */
+	param = (struct disconnect_parm *)rtw_zmalloc(sizeof(*param));
+	if (param == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	param->deauth_timeout_ms = deauth_timeout_ms;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		cmdobj = (struct cmd_obj *)rtw_zmalloc(sizeof(*cmdobj));
+		if (cmdobj == NULL) {
+			res = _FAIL;
+			kfree((u8 *)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 (H2C_SUCCESS != disconnect_hdl(padapter, (u8 *)param))
+			res = _FAIL;
+		kfree((u8 *)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 = (struct setopmode_parm *)rtw_zmalloc(sizeof(struct setopmode_parm));
+
+	if (psetop == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	psetop->mode = (u8)networktype;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			kfree((u8 *)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((u8 *)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 = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+	if (psetstakey_para == NULL) {
+		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 == true) {
+		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
+	} else{
+		memcpy(&psetstakey_para->key, &psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, 16);
+	}
+
+	/* jeff: set this becasue at least sw key is ready */
+	padapter->securitypriv.busetkipkey = true;
+
+	if (enqueue) {
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			kfree((u8 *) psetstakey_para);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (psetstakey_rsp == NULL) {
+			kfree((u8 *) ph2c);
+			kfree((u8 *) 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((u8 *) 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) {
+			DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(sta->hwaddr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else{
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_para = (struct set_stakey_parm *)rtw_zmalloc(sizeof(struct set_stakey_parm));
+		if (psetstakey_para == NULL) {
+			kfree((u8 *) ph2c);
+			res = _FAIL;
+			goto exit;
+		}
+
+		psetstakey_rsp = (struct set_stakey_rsp *)rtw_zmalloc(sizeof(struct set_stakey_rsp));
+		if (psetstakey_rsp == NULL) {
+			kfree((u8 *) ph2c);
+			kfree((u8 *) 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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	paddbareq_parm = (struct addBaReq_parm *)rtw_zmalloc(sizeof(struct addBaReq_parm));
+	if (paddbareq_parm == NULL) {
+		kfree((unsigned char *)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));
+
+	/* DBG_871X("rtw_addbareq_cmd, tid =%d\n", tid); */
+
+	/* 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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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;
+}
+
+u8 rtw_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue, u8 swconfig)
+{
+	struct	cmd_obj *pcmdobj;
+	struct	SetChannelPlan_param *setChannelPlan_param;
+	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
+
+	u8 res = _SUCCESS;
+
+	RT_TRACE(_module_rtl871x_cmd_c_, _drv_notice_, ("+rtw_set_chplan_cmd\n"));
+
+	/*  check if allow software config */
+	if (swconfig && rtw_hal_is_disable_sw_channel_plan(padapter) == true) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* check input parameter */
+	if (!rtw_is_channel_plan_valid(chplan)) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	/* prepare cmd parameter */
+	setChannelPlan_param = (struct	SetChannelPlan_param *)rtw_zmalloc(sizeof(struct SetChannelPlan_param));
+	if (setChannelPlan_param == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	setChannelPlan_param->channel_plan = chplan;
+
+	if (enqueue) {
+		/* need enqueue, prepare cmd_obj and enqueue */
+		pcmdobj = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (pcmdobj == NULL) {
+			kfree((u8 *)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);
+	} else{
+		/* no need to enqueue, do the cmd hdl directly and free cmd parameter */
+		if (H2C_SUCCESS != set_chplan_hdl(padapter, (unsigned char *)setChannelPlan_param))
+			res = _FAIL;
+
+		kfree((u8 *)setChannelPlan_param);
+	}
+
+	/* do something based on res... */
+	if (res == _SUCCESS)
+		padapter->mlmepriv.ChannelPlan = chplan;
+
+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) == true)
+		/*&& !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)) {
+			/* DBG_871X("(-)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
+			bEnterPS = false;
+
+			if (bBusyTraffic == true) {
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount <= 4)
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 4;
+
+				pmlmepriv->LinkDetectInfo.TrafficTransitionCount++;
+
+				/* DBG_871X("Set TrafficTransitionCount to %d\n", pmlmepriv->LinkDetectInfo.TrafficTransitionCount); */
+
+				if (pmlmepriv->LinkDetectInfo.TrafficTransitionCount > 30/*TrafficTransitionLevel*/) {
+					pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 30;
+				}
+			}
+		} else{
+			/* DBG_871X("(+)Tx = %d, Rx = %d\n", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
+
+			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) == true) {
+		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 */
+	/*  */
+	rtw_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) == true)
+		|| (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+		return;
+	}
+
+	switch (lps_ctrl_type) {
+	case LPS_CTRL_SCAN:
+		/* DBG_871X("LPS_CTRL_SCAN\n"); */
+		rtw_btcoex_ScanNotify(padapter, true);
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+			/*  connect */
+			LPS_Leave(padapter, "LPS_CTRL_SCAN");
+		}
+		break;
+	case LPS_CTRL_JOINBSS:
+		/* DBG_871X("LPS_CTRL_JOINBSS\n"); */
+		LPS_Leave(padapter, "LPS_CTRL_JOINBSS");
+		break;
+	case LPS_CTRL_CONNECT:
+		/* DBG_871X("LPS_CTRL_CONNECT\n"); */
+		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:
+		/* DBG_871X("LPS_CTRL_DISCONNECT\n"); */
+		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:
+		/* DBG_871X("LPS_CTRL_SPECIAL_PACKET\n"); */
+		pwrpriv->DelayLPSLastTimeStamp = jiffies;
+		rtw_btcoex_SpecialPacketNotify(padapter, PACKET_DHCP);
+		LPS_Leave(padapter, "LPS_CTRL_SPECIAL_PACKET");
+		break;
+	case LPS_CTRL_LEAVE:
+		/* DBG_871X("LPS_CTRL_LEAVE\n"); */
+		LPS_Leave(padapter, "LPS_CTRL_LEAVE");
+		break;
+	case LPS_CTRL_TRAFFIC_BUSY:
+		LPS_Leave(padapter, "LPS_CTRL_TRAFFIC_BUSY");
+	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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			res = _FAIL;
+			goto exit;
+		}
+
+		pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+		if (pdrvextra_cmd_parm == NULL) {
+			kfree((unsigned char *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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 (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+
+	down(&pwrpriv->lock);
+
+	if (pwrpriv->dtim != dtim) {
+		DBG_871X("change DTIM from %d to %d, bFwCurrentInPSMode =%d, ps_mode =%d\n", pwrpriv->dtim, dtim,
+			pwrpriv->bFwCurrentInPSMode, pwrpriv->pwr_mode);
+
+		pwrpriv->dtim = dtim;
+	}
+
+	if ((pwrpriv->bFwCurrentInPSMode == true) && (pwrpriv->pwr_mode > PS_MODE_ACTIVE)) {
+		u8 ps_mode = pwrpriv->pwr_mode;
+
+		/* DBG_871X("change DTIM from %d to %d, ps_mode =%d\n", pwrpriv->dtim, dtim, ps_mode); */
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+	}
+
+	up(&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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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;
+
+}
+
+static void power_saving_wk_hdl(struct adapter *padapter)
+{
+	 rtw_ps_processor(padapter);
+}
+
+/* add for CONFIG_IEEE80211W, none 11w can use it */
+static void reset_securitypriv_hdl(struct adapter *padapter)
+{
+	 rtw_reset_securitypriv(padapter);
+}
+
+static void free_assoc_resources_hdl(struct adapter *padapter)
+{
+	 rtw_free_assoc_resources(padapter, 1);
+}
+
+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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ppscmd == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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 = 0;
+
+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 (false == 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 == true)
+				update_beacon(padapter, _TIM_IE_, 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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((unsigned char *)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;
+	}
+
+/* define DBG_PROC_SET_BTINFO_EVT */
+#ifdef DBG_PROC_SET_BTINFO_EVT
+	btinfo_evt_dump(RTW_DBGDUMP, info);
+#endif
+
+	/* 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;
+	rtw_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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((u8 *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	pdrvextra_cmd_parm = (struct drvextra_cmd_parm *)rtw_zmalloc(sizeof(struct drvextra_cmd_parm));
+	if (pdrvextra_cmd_parm == NULL) {
+		kfree((u8 *)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(_workitem *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 != NULL) {
+			/* This C2H event is read, clear it */
+			c2h_evt_clear(adapter);
+		} else{
+			c2h_evt = (u8 *)rtw_malloc(16);
+			if (c2h_evt != NULL) {
+				/* This C2H event is not read, read & clear now */
+				if (rtw_hal_c2h_evt_read(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) == true) {
+			/* 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:
+		power_saving_wk_hdl(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;
+#ifdef CONFIG_INTEL_WIDI
+	case INTEl_WIDI_WK_CID:
+		intel_widi_wk_hdl(padapter, pdrvextra_cmd->type, pdrvextra_cmd->pbuf);
+		break;
+#endif /* CONFIG_INTEL_WIDI */
+	/* add for CONFIG_IEEE80211W, none 11w can use it */
+	case RESET_SECURITYPRIV:
+		reset_securitypriv_hdl(padapter);
+		break;
+	case FREE_ASSOC_RESOURCES:
+		free_assoc_resources_hdl(padapter);
+		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_DROPPED) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		/* need to make timeout handlerOS independent */
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+	} else if (pcmd->res != H2C_SUCCESS) {
+		_set_timer(&pmlmepriv->scan_to_timer, 1);
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: MgntActrtw_set_802_11_bssid_LIST_SCAN Fail ************\n\n."));
+	}
+
+	/*  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);
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ***Error: disconnect_cmd_callback Fail ***\n."));
+		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_DROPPED) {
+		/* TODO: cancel timer and do timeout handler directly... */
+		/* need to make timeout handlerOS independent */
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	} else if (pcmd->res != H2C_SUCCESS) {
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+
+	rtw_free_cmd_obj(pcmd);
+}
+
+void rtw_createbss_cmd_callback(struct adapter *padapter, struct cmd_obj *pcmd)
+{
+	u8 timer_cancelled;
+	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 == NULL)
+		goto exit;
+
+	if ((pcmd->res != H2C_SUCCESS)) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n ********Error: rtw_createbss_cmd_callback  Fail ************\n\n."));
+		_set_timer(&pmlmepriv->assoc_timer, 1);
+	}
+
+	_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+	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 == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nCan't alloc sta_info when createbss_cmd_callback\n"));
+				goto createbss_cmd_fail;
+			}
+		}
+
+		rtw_indicate_connect(padapter);
+	} else{
+		pwlan = _rtw_alloc_network(pmlmepriv);
+		spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+		if (pwlan == NULL) {
+			pwlan = rtw_get_oldest_wlan_network(&pmlmepriv->scanned_queue);
+			if (pwlan == NULL) {
+				RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\n Error:  can't get pwlan in rtw_joinbss_event_callback\n"));
+				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 DSConfig */
+		/* tgt_network->network.Configuration.DSConfig = (u32)rtw_ch2freq(pnetwork->Configuration.DSConfig); */
+
+		_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 == NULL) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: rtw_setstaKey_cmdrsp_callback => can't get sta_info\n\n"));
+		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 == NULL) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nERROR: setassocsta_cmdrsp_callbac => can't get sta_info\n\n"));
+		goto exit;
+	}
+
+	psta->aid = psta->mac_id = passocsta_rsp->cam_id;
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) && (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true))
+		_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 000000000000..3db02e9f27ab
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_debug.c
@@ -0,0 +1,1447 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_DEBUG_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+u32 GlobalDebugLevel = _drv_err_;
+
+#ifdef DEBUG_RTL871X
+
+	u64 GlobalDebugComponents = \
+			_module_rtl871x_xmit_c_ |
+			_module_xmit_osdep_c_ |
+			_module_rtl871x_recv_c_ |
+			_module_recv_osdep_c_ |
+			_module_rtl871x_mlme_c_ |
+			_module_mlme_osdep_c_ |
+			_module_rtl871x_sta_mgt_c_ |
+			_module_rtl871x_cmd_c_ |
+			_module_cmd_osdep_c_ |
+			_module_rtl871x_io_c_ |
+			_module_io_osdep_c_ |
+			_module_os_intfs_c_|
+			_module_rtl871x_security_c_|
+			_module_rtl871x_eeprom_c_|
+			_module_hal_init_c_|
+			_module_hci_hal_init_c_|
+			_module_rtl871x_ioctl_c_|
+			_module_rtl871x_ioctl_set_c_|
+			_module_rtl871x_ioctl_query_c_|
+			_module_rtl871x_pwrctrl_c_|
+			_module_hci_intfs_c_|
+			_module_hci_ops_c_|
+			_module_hci_ops_os_c_|
+			_module_rtl871x_ioctl_os_c|
+			_module_rtl8712_cmd_c_|
+			_module_hal_xmit_c_|
+			_module_rtl8712_recv_c_ |
+			_module_mp_ |
+			_module_efuse_;
+
+#endif /* DEBUG_RTL871X */
+
+#include <rtw_version.h>
+
+void dump_drv_version(void *sel)
+{
+	DBG_871X_SEL_NL(sel, "%s %s\n", "rtl8723bs", DRIVERVERSION);
+}
+
+void dump_log_level(void *sel)
+{
+	DBG_871X_SEL_NL(sel, "log_level:%d\n", GlobalDebugLevel);
+}
+
+void sd_f0_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i;
+
+	for (i = 0x0; i <= 0xff; i++) {
+		if (i%16 == 0)
+			DBG_871X_SEL_NL(sel, "0x%02x ", i);
+
+		DBG_871X_SEL(sel, "%02x ", rtw_sd_f0_read8(adapter, i));
+
+		if (i%16 == 15)
+			DBG_871X_SEL(sel, "\n");
+		else if (i%8 == 7)
+			DBG_871X_SEL(sel, "\t");
+	}
+}
+
+void mac_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i, j = 1;
+
+	DBG_871X_SEL_NL(sel, "======= MAC REG =======\n");
+
+	for (i = 0x0; i < 0x800; i += 4) {
+		if (j%4 == 1)
+			DBG_871X_SEL_NL(sel, "0x%03x", i);
+		DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++)%4 == 0)
+			DBG_871X_SEL(sel, "\n");
+	}
+}
+
+void bb_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i, j = 1;
+
+	DBG_871X_SEL_NL(sel, "======= BB REG =======\n");
+	for (i = 0x800; i < 0x1000 ; i += 4) {
+		if (j%4 == 1)
+			DBG_871X_SEL_NL(sel, "0x%03x", i);
+		DBG_871X_SEL(sel, " 0x%08x ", rtw_read32(adapter, i));
+		if ((j++)%4 == 0)
+			DBG_871X_SEL(sel, "\n");
+	}
+}
+
+void rf_reg_dump(void *sel, struct adapter *adapter)
+{
+	int i, j = 1, path;
+	u32 value;
+	u8 rf_type = 0;
+	u8 path_nums = 0;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if ((RF_1T2R == rf_type) || (RF_1T1R == rf_type))
+		path_nums = 1;
+	else
+		path_nums = 2;
+
+	DBG_871X_SEL_NL(sel, "======= RF REG =======\n");
+
+	for (path = 0; path < path_nums; path++) {
+		DBG_871X_SEL_NL(sel, "RF_Path(%x)\n", path);
+		for (i = 0; i < 0x100; i++) {
+			value = rtw_hal_read_rfreg(adapter, path, i, 0xffffffff);
+			if (j%4 == 1)
+				DBG_871X_SEL_NL(sel, "0x%02x ", i);
+			DBG_871X_SEL(sel, " 0x%08x ", value);
+			if ((j++)%4 == 0)
+				DBG_871X_SEL(sel, "\n");
+		}
+	}
+}
+
+#ifdef PROC_DEBUG
+ssize_t proc_set_write_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 addr, val, len;
+
+	if (count < 3) {
+		DBG_871X("argument size is less than 3\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x %x %x", &addr, &val, &len);
+
+		if (num !=  3) {
+			DBG_871X("invalid write_reg parameter!\n");
+			return count;
+		}
+
+		switch (len) {
+		case 1:
+			rtw_write8(padapter, addr, (u8)val);
+			break;
+		case 2:
+			rtw_write16(padapter, addr, (u16)val);
+			break;
+		case 4:
+			rtw_write32(padapter, addr, val);
+			break;
+		default:
+			DBG_871X("error write length =%d", len);
+			break;
+		}
+
+	}
+
+	return count;
+
+}
+
+static u32 proc_get_read_addr = 0xeeeeeeee;
+static u32 proc_get_read_len = 0x4;
+
+int proc_get_read_reg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (proc_get_read_addr == 0xeeeeeeee) {
+		DBG_871X_SEL_NL(m, "address not initialized\n");
+		return 0;
+	}
+
+	switch (proc_get_read_len) {
+	case 1:
+		DBG_871X_SEL_NL(m, "rtw_read8(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read8(padapter, proc_get_read_addr));
+		break;
+	case 2:
+		DBG_871X_SEL_NL(m, "rtw_read16(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read16(padapter, proc_get_read_addr));
+		break;
+	case 4:
+		DBG_871X_SEL_NL(m, "rtw_read32(0x%x) = 0x%x\n", proc_get_read_addr, rtw_read32(padapter, proc_get_read_addr));
+		break;
+	default:
+		DBG_871X_SEL_NL(m, "error read length =%d\n", proc_get_read_len);
+		break;
+	}
+
+	return 0;
+}
+
+ssize_t proc_set_read_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[16];
+	u32 addr, len;
+
+	if (count < 2) {
+		DBG_871X("argument size is less than 2\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x %x", &addr, &len);
+
+		if (num !=  2) {
+			DBG_871X("invalid read_reg parameter!\n");
+			return count;
+		}
+
+		proc_get_read_addr = addr;
+
+		proc_get_read_len = len;
+	}
+
+	return count;
+
+}
+
+int proc_get_fwstate(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(m, "fwstate = 0x%x\n", get_fwstate(pmlmepriv));
+
+	return 0;
+}
+
+int proc_get_sec_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct security_priv *sec = &padapter->securitypriv;
+
+	DBG_871X_SEL_NL(m, "auth_alg = 0x%x, enc_alg = 0x%x, auth_type = 0x%x, enc_type = 0x%x\n",
+						sec->dot11AuthAlgrthm, sec->dot11PrivacyAlgrthm,
+						sec->ndisauthtype, sec->ndisencryptstatus);
+
+	DBG_871X_SEL_NL(m, "hw_decrypted =%d\n", sec->hw_decrypted);
+
+#ifdef DBG_SW_SEC_CNT
+	DBG_871X_SEL_NL(m, "wep_sw_enc_cnt =%llu, %llu, %llu\n"
+		, sec->wep_sw_enc_cnt_bc, sec->wep_sw_enc_cnt_mc, sec->wep_sw_enc_cnt_uc);
+	DBG_871X_SEL_NL(m, "wep_sw_dec_cnt =%llu, %llu, %llu\n"
+		, sec->wep_sw_dec_cnt_bc, sec->wep_sw_dec_cnt_mc, sec->wep_sw_dec_cnt_uc);
+
+	DBG_871X_SEL_NL(m, "tkip_sw_enc_cnt =%llu, %llu, %llu\n"
+		, sec->tkip_sw_enc_cnt_bc, sec->tkip_sw_enc_cnt_mc, sec->tkip_sw_enc_cnt_uc);
+	DBG_871X_SEL_NL(m, "tkip_sw_dec_cnt =%llu, %llu, %llu\n"
+		, sec->tkip_sw_dec_cnt_bc, sec->tkip_sw_dec_cnt_mc, sec->tkip_sw_dec_cnt_uc);
+
+	DBG_871X_SEL_NL(m, "aes_sw_enc_cnt =%llu, %llu, %llu\n"
+		, sec->aes_sw_enc_cnt_bc, sec->aes_sw_enc_cnt_mc, sec->aes_sw_enc_cnt_uc);
+	DBG_871X_SEL_NL(m, "aes_sw_dec_cnt =%llu, %llu, %llu\n"
+		, sec->aes_sw_dec_cnt_bc, sec->aes_sw_dec_cnt_mc, sec->aes_sw_dec_cnt_uc);
+#endif /* DBG_SW_SEC_CNT */
+
+	return 0;
+}
+
+int proc_get_mlmext_state(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X_SEL_NL(m, "pmlmeinfo->state = 0x%x\n", pmlmeinfo->state);
+
+	return 0;
+}
+
+int proc_get_roam_flags(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m, "0x%02x\n", rtw_roam_flags(adapter));
+
+	return 0;
+}
+
+ssize_t proc_set_roam_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 flags;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhx", &flags);
+
+		if (num == 1)
+			rtw_assign_roam_flags(adapter, flags);
+	}
+
+	return count;
+
+}
+
+int proc_get_roam_param(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	DBG_871X_SEL_NL(m, "%12s %12s %11s\n", "rssi_diff_th", "scanr_exp_ms", "scan_int_ms");
+	DBG_871X_SEL_NL(m, "%-12u %-12u %-11u\n"
+		, mlme->roam_rssi_diff_th
+		, mlme->roam_scanr_exp_ms
+		, mlme->roam_scan_int_ms
+	);
+
+	return 0;
+}
+
+ssize_t proc_set_roam_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+
+	char tmp[32];
+	u8 rssi_diff_th;
+	u32 scanr_exp_ms;
+	u32 scan_int_ms;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhu %u %u", &rssi_diff_th, &scanr_exp_ms, &scan_int_ms);
+
+		if (num >= 1)
+			mlme->roam_rssi_diff_th = rssi_diff_th;
+		if (num >= 2)
+			mlme->roam_scanr_exp_ms = scanr_exp_ms;
+		if (num >= 3)
+			mlme->roam_scan_int_ms = scan_int_ms;
+	}
+
+	return count;
+
+}
+
+ssize_t proc_set_roam_tgt_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	char tmp[32];
+	u8 addr[ETH_ALEN];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", addr, addr+1, addr+2, addr+3, addr+4, addr+5);
+		if (num == 6)
+			memcpy(adapter->mlmepriv.roam_tgt_addr, addr, ETH_ALEN);
+
+		DBG_871X("set roam_tgt_addr to "MAC_FMT"\n", MAC_ARG(adapter->mlmepriv.roam_tgt_addr));
+	}
+
+	return count;
+}
+
+int proc_get_qos_option(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(m, "qos_option =%d\n", pmlmepriv->qospriv.qos_option);
+
+	return 0;
+}
+
+int proc_get_ht_option(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+	DBG_871X_SEL_NL(m, "ht_option =%d\n", pmlmepriv->htpriv.ht_option);
+
+	return 0;
+}
+
+int proc_get_rf_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	DBG_871X_SEL_NL(m, "cur_ch =%d, cur_bw =%d, cur_ch_offet =%d\n",
+					pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+
+	DBG_871X_SEL_NL(m, "oper_ch =%d, oper_bw =%d, oper_ch_offet =%d\n",
+					rtw_get_oper_ch(padapter), rtw_get_oper_bw(padapter),  rtw_get_oper_choffset(padapter));
+
+	return 0;
+}
+
+int proc_get_survey_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	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;
+	struct list_head	*plist, *phead;
+	s32 notify_signal;
+	s16 notify_noise = 0;
+	u16  index = 0;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+	phead = get_list_head(queue);
+	plist = phead ? get_next(phead) : NULL;
+	if ((!phead) || (!plist)) {
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+		return 0;
+	}
+
+	DBG_871X_SEL_NL(m, "%5s  %-17s  %3s  %-3s  %-4s  %-4s  %5s  %s\n", "index", "bssid", "ch", "RSSI", "SdBm", "Noise", "age", "ssid");
+	while (1) {
+		if (phead == plist)
+			break;
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (!pnetwork)
+			break;
+
+		if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+			is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) {
+			notify_signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);/*dbm*/
+		} else {
+			notify_signal = translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength);/*dbm*/
+		}
+
+		#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		rtw_hal_get_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &(pnetwork->network.Configuration.DSConfig), &(notify_noise));
+		#endif
+
+		DBG_871X_SEL_NL(m, "%5d  "MAC_FMT"  %3d  %3d  %4d  %4d  %5d  %s\n",
+			++index,
+			MAC_ARG(pnetwork->network.MacAddress),
+			pnetwork->network.Configuration.DSConfig,
+			(int)pnetwork->network.Rssi,
+			notify_signal,
+			notify_noise,
+			jiffies_to_msecs(jiffies - pnetwork->last_scanned),
+			/*translate_percentage_to_dbm(pnetwork->network.PhyInfo.SignalStrength),*/
+			pnetwork->network.Ssid.Ssid);
+		plist = get_next(plist);
+	}
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	return 0;
+}
+
+int proc_get_ap_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct sta_info *psta;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct wlan_network *cur_network = &(pmlmepriv->cur_network);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+	if (psta) {
+		int i;
+		struct recv_reorder_ctrl *preorder_ctrl;
+
+		DBG_871X_SEL_NL(m, "SSID =%s\n", cur_network->network.Ssid.Ssid);
+		DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+		DBG_871X_SEL_NL(m, "cur_channel =%d, cur_bwmode =%d, cur_ch_offset =%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+		DBG_871X_SEL_NL(m, "wireless_mode = 0x%x, rtsen =%d, cts2slef =%d\n", psta->wireless_mode, psta->rtsen, psta->cts2self);
+		DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+		DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+		DBG_871X_SEL_NL(m, "bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m);
+		DBG_871X_SEL_NL(m, "ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+		DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+		DBG_871X_SEL_NL(m, "ldpc_cap = 0x%x, stbc_cap = 0x%x, beamform_cap = 0x%x\n", psta->htpriv.ldpc_cap, psta->htpriv.stbc_cap, psta->htpriv.beamform_cap);
+
+		for (i = 0; i < 16; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+			if (preorder_ctrl->enable) {
+				DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n", i, preorder_ctrl->indicate_seq);
+			}
+		}
+
+	} else{
+		DBG_871X_SEL_NL(m, "can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress));
+	}
+
+	return 0;
+}
+
+int proc_get_adapter_state(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m, "name =%s, bSurpriseRemoved =%d, bDriverStopped =%d\n",
+					dev->name, padapter->bSurpriseRemoved, padapter->bDriverStopped);
+
+	return 0;
+}
+
+int proc_get_trx_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	int i;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct recv_priv  *precvpriv = &padapter->recvpriv;
+	struct hw_xmit *phwxmit;
+
+	DBG_871X_SEL_NL(m, "free_xmitbuf_cnt =%d, free_xmitframe_cnt =%d\n"
+		, pxmitpriv->free_xmitbuf_cnt, pxmitpriv->free_xmitframe_cnt);
+	DBG_871X_SEL_NL(m, "free_ext_xmitbuf_cnt =%d, free_xframe_ext_cnt =%d\n"
+		, pxmitpriv->free_xmit_extbuf_cnt, pxmitpriv->free_xframe_ext_cnt);
+	DBG_871X_SEL_NL(m, "free_recvframe_cnt =%d\n"
+		, precvpriv->free_recvframe_cnt);
+
+	for (i = 0; i < 4; i++) {
+		phwxmit = pxmitpriv->hwxmits + i;
+		DBG_871X_SEL_NL(m, "%d, hwq.accnt =%d\n", i, phwxmit->accnt);
+	}
+
+	return 0;
+}
+
+int proc_get_rate_ctl(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (adapter->fix_rate != 0xff) {
+		DBG_871X_SEL_NL(m, "FIX\n");
+		DBG_871X_SEL_NL(m, "0x%02x\n", adapter->fix_rate);
+	} else {
+		DBG_871X_SEL_NL(m, "RA\n");
+	}
+
+	return 0;
+}
+
+ssize_t proc_set_rate_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u8 fix_rate;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%hhx", &fix_rate);
+
+		if (num >= 1)
+			adapter->fix_rate = fix_rate;
+	}
+
+	return count;
+}
+
+ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%hhu %hhu", &g_fwdl_chksum_fail, &g_fwdl_wintint_rdy_fail);
+	}
+
+	return count;
+}
+
+ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%u", &g_wait_hiq_empty);
+	}
+
+	return count;
+}
+
+int proc_get_suspend_resume_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+	DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_alloc_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_alloc_irq_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_free_irq_error_cnt =%d\n", pdbgpriv->dbg_sdio_free_irq_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_init_error_cnt =%d\n", pdbgpriv->dbg_sdio_init_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_sdio_deinit_error_cnt =%d\n", pdbgpriv->dbg_sdio_deinit_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_suspend_error_cnt =%d\n", pdbgpriv->dbg_suspend_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_suspend_cnt =%d\n", pdbgpriv->dbg_suspend_cnt);
+	DBG_871X_SEL_NL(m, "dbg_resume_cnt =%d\n", pdbgpriv->dbg_resume_cnt);
+	DBG_871X_SEL_NL(m, "dbg_resume_error_cnt =%d\n", pdbgpriv->dbg_resume_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_deinit_fail_cnt =%d\n", pdbgpriv->dbg_deinit_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_carddisable_cnt =%d\n", pdbgpriv->dbg_carddisable_cnt);
+	DBG_871X_SEL_NL(m, "dbg_ps_insuspend_cnt =%d\n", pdbgpriv->dbg_ps_insuspend_cnt);
+	DBG_871X_SEL_NL(m, "dbg_dev_unload_inIPS_cnt =%d\n", pdbgpriv->dbg_dev_unload_inIPS_cnt);
+	DBG_871X_SEL_NL(m, "dbg_scan_pwr_state_cnt =%d\n", pdbgpriv->dbg_scan_pwr_state_cnt);
+	DBG_871X_SEL_NL(m, "dbg_downloadfw_pwr_state_cnt =%d\n", pdbgpriv->dbg_downloadfw_pwr_state_cnt);
+	DBG_871X_SEL_NL(m, "dbg_carddisable_error_cnt =%d\n", pdbgpriv->dbg_carddisable_error_cnt);
+	DBG_871X_SEL_NL(m, "dbg_fw_read_ps_state_fail_cnt =%d\n", pdbgpriv->dbg_fw_read_ps_state_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_leave_ips_fail_cnt =%d\n", pdbgpriv->dbg_leave_ips_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_leave_lps_fail_cnt =%d\n", pdbgpriv->dbg_leave_lps_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_h2c_leave32k_fail_cnt =%d\n", pdbgpriv->dbg_h2c_leave32k_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_diswow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_diswow_dload_fw_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_enwow_dload_fw_fail_cnt =%d\n", pdbgpriv->dbg_enwow_dload_fw_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_ips_drvopen_fail_cnt =%d\n", pdbgpriv->dbg_ips_drvopen_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_poll_fail_cnt =%d\n", pdbgpriv->dbg_poll_fail_cnt);
+	DBG_871X_SEL_NL(m, "dbg_rpwm_toogle_cnt =%d\n", pdbgpriv->dbg_rpwm_toogle_cnt);
+	DBG_871X_SEL_NL(m, "dbg_rpwm_timeout_fail_cnt =%d\n", pdbgpriv->dbg_rpwm_timeout_fail_cnt);
+
+	return 0;
+}
+
+#ifdef CONFIG_DBG_COUNTER
+
+int proc_get_rx_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct rx_logs *rx_logs = &padapter->rx_logs;
+
+	DBG_871X_SEL_NL(m,
+		"intf_rx =%d\n"
+		"intf_rx_err_recvframe =%d\n"
+		"intf_rx_err_skb =%d\n"
+		"intf_rx_report =%d\n"
+		"core_rx =%d\n"
+		"core_rx_pre =%d\n"
+		"core_rx_pre_ver_err =%d\n"
+		"core_rx_pre_mgmt =%d\n"
+		"core_rx_pre_mgmt_err_80211w =%d\n"
+		"core_rx_pre_mgmt_err =%d\n"
+		"core_rx_pre_ctrl =%d\n"
+		"core_rx_pre_ctrl_err =%d\n"
+		"core_rx_pre_data =%d\n"
+		"core_rx_pre_data_wapi_seq_err =%d\n"
+		"core_rx_pre_data_wapi_key_err =%d\n"
+		"core_rx_pre_data_handled =%d\n"
+		"core_rx_pre_data_err =%d\n"
+		"core_rx_pre_data_unknown =%d\n"
+		"core_rx_pre_unknown =%d\n"
+		"core_rx_enqueue =%d\n"
+		"core_rx_dequeue =%d\n"
+		"core_rx_post =%d\n"
+		"core_rx_post_decrypt =%d\n"
+		"core_rx_post_decrypt_wep =%d\n"
+		"core_rx_post_decrypt_tkip =%d\n"
+		"core_rx_post_decrypt_aes =%d\n"
+		"core_rx_post_decrypt_wapi =%d\n"
+		"core_rx_post_decrypt_hw =%d\n"
+		"core_rx_post_decrypt_unknown =%d\n"
+		"core_rx_post_decrypt_err =%d\n"
+		"core_rx_post_defrag_err =%d\n"
+		"core_rx_post_portctrl_err =%d\n"
+		"core_rx_post_indicate =%d\n"
+		"core_rx_post_indicate_in_oder =%d\n"
+		"core_rx_post_indicate_reoder =%d\n"
+		"core_rx_post_indicate_err =%d\n"
+		"os_indicate =%d\n"
+		"os_indicate_ap_mcast =%d\n"
+		"os_indicate_ap_forward =%d\n"
+		"os_indicate_ap_self =%d\n"
+		"os_indicate_err =%d\n"
+		"os_netif_ok =%d\n"
+		"os_netif_err =%d\n",
+		rx_logs->intf_rx,
+		rx_logs->intf_rx_err_recvframe,
+		rx_logs->intf_rx_err_skb,
+		rx_logs->intf_rx_report,
+		rx_logs->core_rx,
+		rx_logs->core_rx_pre,
+		rx_logs->core_rx_pre_ver_err,
+		rx_logs->core_rx_pre_mgmt,
+		rx_logs->core_rx_pre_mgmt_err_80211w,
+		rx_logs->core_rx_pre_mgmt_err,
+		rx_logs->core_rx_pre_ctrl,
+		rx_logs->core_rx_pre_ctrl_err,
+		rx_logs->core_rx_pre_data,
+		rx_logs->core_rx_pre_data_wapi_seq_err,
+		rx_logs->core_rx_pre_data_wapi_key_err,
+		rx_logs->core_rx_pre_data_handled,
+		rx_logs->core_rx_pre_data_err,
+		rx_logs->core_rx_pre_data_unknown,
+		rx_logs->core_rx_pre_unknown,
+		rx_logs->core_rx_enqueue,
+		rx_logs->core_rx_dequeue,
+		rx_logs->core_rx_post,
+		rx_logs->core_rx_post_decrypt,
+		rx_logs->core_rx_post_decrypt_wep,
+		rx_logs->core_rx_post_decrypt_tkip,
+		rx_logs->core_rx_post_decrypt_aes,
+		rx_logs->core_rx_post_decrypt_wapi,
+		rx_logs->core_rx_post_decrypt_hw,
+		rx_logs->core_rx_post_decrypt_unknown,
+		rx_logs->core_rx_post_decrypt_err,
+		rx_logs->core_rx_post_defrag_err,
+		rx_logs->core_rx_post_portctrl_err,
+		rx_logs->core_rx_post_indicate,
+		rx_logs->core_rx_post_indicate_in_oder,
+		rx_logs->core_rx_post_indicate_reoder,
+		rx_logs->core_rx_post_indicate_err,
+		rx_logs->os_indicate,
+		rx_logs->os_indicate_ap_mcast,
+		rx_logs->os_indicate_ap_forward,
+		rx_logs->os_indicate_ap_self,
+		rx_logs->os_indicate_err,
+		rx_logs->os_netif_ok,
+		rx_logs->os_netif_err
+	);
+
+	return 0;
+}
+
+int proc_get_tx_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct tx_logs *tx_logs = &padapter->tx_logs;
+
+	DBG_871X_SEL_NL(m,
+		"os_tx =%d\n"
+		"os_tx_err_up =%d\n"
+		"os_tx_err_xmit =%d\n"
+		"os_tx_m2u =%d\n"
+		"os_tx_m2u_ignore_fw_linked =%d\n"
+		"os_tx_m2u_ignore_self =%d\n"
+		"os_tx_m2u_entry =%d\n"
+		"os_tx_m2u_entry_err_xmit =%d\n"
+		"os_tx_m2u_entry_err_skb =%d\n"
+		"os_tx_m2u_stop =%d\n"
+		"core_tx =%d\n"
+		"core_tx_err_pxmitframe =%d\n"
+		"core_tx_err_brtx =%d\n"
+		"core_tx_upd_attrib =%d\n"
+		"core_tx_upd_attrib_adhoc =%d\n"
+		"core_tx_upd_attrib_sta =%d\n"
+		"core_tx_upd_attrib_ap =%d\n"
+		"core_tx_upd_attrib_unknown =%d\n"
+		"core_tx_upd_attrib_dhcp =%d\n"
+		"core_tx_upd_attrib_icmp =%d\n"
+		"core_tx_upd_attrib_active =%d\n"
+		"core_tx_upd_attrib_err_ucast_sta =%d\n"
+		"core_tx_upd_attrib_err_ucast_ap_link =%d\n"
+		"core_tx_upd_attrib_err_sta =%d\n"
+		"core_tx_upd_attrib_err_link =%d\n"
+		"core_tx_upd_attrib_err_sec =%d\n"
+		"core_tx_ap_enqueue_warn_fwstate =%d\n"
+		"core_tx_ap_enqueue_warn_sta =%d\n"
+		"core_tx_ap_enqueue_warn_nosta =%d\n"
+		"core_tx_ap_enqueue_warn_link =%d\n"
+		"core_tx_ap_enqueue_warn_trigger =%d\n"
+		"core_tx_ap_enqueue_mcast =%d\n"
+		"core_tx_ap_enqueue_ucast =%d\n"
+		"core_tx_ap_enqueue =%d\n"
+		"intf_tx =%d\n"
+		"intf_tx_pending_ac =%d\n"
+		"intf_tx_pending_fw_under_survey =%d\n"
+		"intf_tx_pending_fw_under_linking =%d\n"
+		"intf_tx_pending_xmitbuf =%d\n"
+		"intf_tx_enqueue =%d\n"
+		"core_tx_enqueue =%d\n"
+		"core_tx_enqueue_class =%d\n"
+		"core_tx_enqueue_class_err_sta =%d\n"
+		"core_tx_enqueue_class_err_nosta =%d\n"
+		"core_tx_enqueue_class_err_fwlink =%d\n"
+		"intf_tx_direct =%d\n"
+		"intf_tx_direct_err_coalesce =%d\n"
+		"intf_tx_dequeue =%d\n"
+		"intf_tx_dequeue_err_coalesce =%d\n"
+		"intf_tx_dump_xframe =%d\n"
+		"intf_tx_dump_xframe_err_txdesc =%d\n"
+		"intf_tx_dump_xframe_err_port =%d\n",
+		tx_logs->os_tx,
+		tx_logs->os_tx_err_up,
+		tx_logs->os_tx_err_xmit,
+		tx_logs->os_tx_m2u,
+		tx_logs->os_tx_m2u_ignore_fw_linked,
+		tx_logs->os_tx_m2u_ignore_self,
+		tx_logs->os_tx_m2u_entry,
+		tx_logs->os_tx_m2u_entry_err_xmit,
+		tx_logs->os_tx_m2u_entry_err_skb,
+		tx_logs->os_tx_m2u_stop,
+		tx_logs->core_tx,
+		tx_logs->core_tx_err_pxmitframe,
+		tx_logs->core_tx_err_brtx,
+		tx_logs->core_tx_upd_attrib,
+		tx_logs->core_tx_upd_attrib_adhoc,
+		tx_logs->core_tx_upd_attrib_sta,
+		tx_logs->core_tx_upd_attrib_ap,
+		tx_logs->core_tx_upd_attrib_unknown,
+		tx_logs->core_tx_upd_attrib_dhcp,
+		tx_logs->core_tx_upd_attrib_icmp,
+		tx_logs->core_tx_upd_attrib_active,
+		tx_logs->core_tx_upd_attrib_err_ucast_sta,
+		tx_logs->core_tx_upd_attrib_err_ucast_ap_link,
+		tx_logs->core_tx_upd_attrib_err_sta,
+		tx_logs->core_tx_upd_attrib_err_link,
+		tx_logs->core_tx_upd_attrib_err_sec,
+		tx_logs->core_tx_ap_enqueue_warn_fwstate,
+		tx_logs->core_tx_ap_enqueue_warn_sta,
+		tx_logs->core_tx_ap_enqueue_warn_nosta,
+		tx_logs->core_tx_ap_enqueue_warn_link,
+		tx_logs->core_tx_ap_enqueue_warn_trigger,
+		tx_logs->core_tx_ap_enqueue_mcast,
+		tx_logs->core_tx_ap_enqueue_ucast,
+		tx_logs->core_tx_ap_enqueue,
+		tx_logs->intf_tx,
+		tx_logs->intf_tx_pending_ac,
+		tx_logs->intf_tx_pending_fw_under_survey,
+		tx_logs->intf_tx_pending_fw_under_linking,
+		tx_logs->intf_tx_pending_xmitbuf,
+		tx_logs->intf_tx_enqueue,
+		tx_logs->core_tx_enqueue,
+		tx_logs->core_tx_enqueue_class,
+		tx_logs->core_tx_enqueue_class_err_sta,
+		tx_logs->core_tx_enqueue_class_err_nosta,
+		tx_logs->core_tx_enqueue_class_err_fwlink,
+		tx_logs->intf_tx_direct,
+		tx_logs->intf_tx_direct_err_coalesce,
+		tx_logs->intf_tx_dequeue,
+		tx_logs->intf_tx_dequeue_err_coalesce,
+		tx_logs->intf_tx_dump_xframe,
+		tx_logs->intf_tx_dump_xframe_err_txdesc,
+		tx_logs->intf_tx_dump_xframe_err_port
+	);
+
+	return 0;
+}
+
+int proc_get_int_logs(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m,
+		"all =%d\n"
+		"err =%d\n"
+		"tbdok =%d\n"
+		"tbder =%d\n"
+		"bcnderr =%d\n"
+		"bcndma =%d\n"
+		"bcndma_e =%d\n"
+		"rx =%d\n"
+		"rx_rdu =%d\n"
+		"rx_fovw =%d\n"
+		"txfovw =%d\n"
+		"mgntok =%d\n"
+		"highdok =%d\n"
+		"bkdok =%d\n"
+		"bedok =%d\n"
+		"vidok =%d\n"
+		"vodok =%d\n",
+		padapter->int_logs.all,
+		padapter->int_logs.err,
+		padapter->int_logs.tbdok,
+		padapter->int_logs.tbder,
+		padapter->int_logs.bcnderr,
+		padapter->int_logs.bcndma,
+		padapter->int_logs.bcndma_e,
+		padapter->int_logs.rx,
+		padapter->int_logs.rx_rdu,
+		padapter->int_logs.rx_fovw,
+		padapter->int_logs.txfovw,
+		padapter->int_logs.mgntok,
+		padapter->int_logs.highdok,
+		padapter->int_logs.bkdok,
+		padapter->int_logs.bedok,
+		padapter->int_logs.vidok,
+		padapter->int_logs.vodok
+	);
+
+	return 0;
+}
+
+#endif /* CONFIG_DBG_COUNTER*/
+
+int proc_get_rx_signal(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	DBG_871X_SEL_NL(m, "rssi:%d\n", padapter->recvpriv.rssi);
+	/*DBG_871X_SEL_NL(m, "rxpwdb:%d\n", padapter->recvpriv.rxpwdb);*/
+	DBG_871X_SEL_NL(m, "signal_strength:%u\n", padapter->recvpriv.signal_strength);
+	DBG_871X_SEL_NL(m, "signal_qual:%u\n", padapter->recvpriv.signal_qual);
+	DBG_871X_SEL_NL(m, "noise:%d\n", padapter->recvpriv.noise);
+	rtw_odm_get_perpkt_rssi(m, padapter);
+	#ifdef DBG_RX_SIGNAL_DISPLAY_RAW_DATA
+	rtw_get_raw_rssi_info(m, padapter);
+	#endif
+	return 0;
+}
+
+
+int proc_get_hw_status(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &dvobj->drv_dbg;
+
+	DBG_871X_SEL_NL(m, "RX FIFO full count: last_time =%lld, current_time =%lld, differential =%lld\n"
+	, pdbgpriv->dbg_rx_fifo_last_overflow, pdbgpriv->dbg_rx_fifo_curr_overflow, pdbgpriv->dbg_rx_fifo_diff_overflow);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 is_signal_dbg, signal_strength;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%u %u", &is_signal_dbg, &signal_strength);
+
+		is_signal_dbg = is_signal_dbg == 0?0:1;
+
+		if (is_signal_dbg && num != 2)
+			return count;
+
+		signal_strength = signal_strength > 100?100:signal_strength;
+
+		padapter->recvpriv.is_signal_dbg = is_signal_dbg;
+		padapter->recvpriv.signal_strength_dbg =  signal_strength;
+
+		if (is_signal_dbg)
+			DBG_871X("set %s %u\n", "DBG_SIGNAL_STRENGTH", signal_strength);
+		else
+			DBG_871X("set %s\n", "HW_SIGNAL_STRENGTH");
+
+	}
+
+	return count;
+
+}
+
+int proc_get_ht_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m, "%d\n", pregpriv->ht_enable);
+
+	return 0;
+}
+
+ssize_t proc_set_ht_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 2) {
+			pregpriv->ht_enable = mode;
+			printk("ht_enable =%d\n", pregpriv->ht_enable);
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_bw_mode(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m, "0x%02x\n", pregpriv->bw_mode);
+
+	return 0;
+}
+
+ssize_t proc_set_bw_mode(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv &&  mode < 2) {
+
+			pregpriv->bw_mode = mode;
+			printk("bw_mode =%d\n", mode);
+
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_ampdu_enable(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m, "%d\n", pregpriv->ampdu_enable);
+
+	return 0;
+}
+
+ssize_t proc_set_ampdu_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 3) {
+			pregpriv->ampdu_enable = mode;
+			printk("ampdu_enable =%d\n", mode);
+		}
+
+	}
+
+	return count;
+
+}
+
+int proc_get_rx_ampdu(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m,
+			"bAcceptAddbaReq = %d , 0:Reject AP's Add BA req, 1:Accept AP's Add BA req.\n", pmlmeinfo->bAcceptAddbaReq
+			);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_ampdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 2) {
+			pmlmeinfo->bAcceptAddbaReq = mode;
+			DBG_871X("pmlmeinfo->bAcceptAddbaReq =%d\n", pmlmeinfo->bAcceptAddbaReq);
+			if (mode == 0) {
+				/*tear down Rx AMPDU*/
+				send_delba(padapter, 0, get_my_bssid(&(pmlmeinfo->network)));/* recipient*/
+			}
+		}
+
+	}
+
+	return count;
+}
+
+int proc_get_en_fwps(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m, "check_fw_ps = %d , 1:enable get FW PS state , 0: disable get FW PS state\n"
+			, pregpriv->check_fw_ps);
+
+	return 0;
+}
+
+ssize_t proc_set_en_fwps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && mode < 2) {
+			pregpriv->check_fw_ps = mode;
+			DBG_871X("pregpriv->check_fw_ps =%d\n", pregpriv->check_fw_ps);
+		}
+	}
+	return count;
+}
+
+int proc_get_rx_stbc(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+
+	if (pregpriv)
+		DBG_871X_SEL_NL(m, "%d\n", pregpriv->rx_stbc);
+
+	return 0;
+}
+
+ssize_t proc_set_rx_stbc(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct registry_priv *pregpriv = &padapter->registrypriv;
+	char tmp[32];
+	u32 mode;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &mode);
+
+		if (pregpriv && (mode == 0 || mode == 1 ||
+		    mode == 2 || mode == 3)) {
+			pregpriv->rx_stbc = mode;
+			printk("rx_stbc =%d\n", mode);
+		}
+	}
+
+	return count;
+
+}
+
+int proc_get_rssi_disp(struct seq_file *m, void *v)
+{
+	return 0;
+}
+
+ssize_t proc_set_rssi_disp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 enable = 0;
+
+	if (count < 1) {
+		DBG_8192C("argument size is less than 1\n");
+		return -EFAULT;
+	}
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		int num = sscanf(tmp, "%x", &enable);
+
+		if (num !=  1) {
+			DBG_8192C("invalid set_rssi_disp parameter!\n");
+			return count;
+		}
+
+		if (enable) {
+			DBG_8192C("Linked info Function Enable\n");
+			padapter->bLinkInfoDump = enable;
+		} else {
+			DBG_8192C("Linked info Function Disable\n");
+			padapter->bLinkInfoDump = 0;
+		}
+	}
+	return count;
+}
+
+int proc_get_all_sta_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct sta_info *psta;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	int i, j;
+	struct list_head	*plist, *phead;
+	struct recv_reorder_ctrl *preorder_ctrl;
+
+	DBG_871X_SEL_NL(m, "sta_dz_bitmap = 0x%x, tim_bitmap = 0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	for (i = 0; i < NUM_STA; i++) {
+		phead = &(pstapriv->sta_hash[i]);
+		plist = get_next(phead);
+
+		while (phead != plist) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			DBG_871X_SEL_NL(m, "==============================\n");
+			DBG_871X_SEL_NL(m, "sta's macaddr:" MAC_FMT "\n",
+					MAC_ARG(psta->hwaddr));
+			DBG_871X_SEL_NL(m, "rtsen =%d, cts2slef =%d\n",
+					psta->rtsen, psta->cts2self);
+			DBG_871X_SEL_NL(m, "state = 0x%x, aid =%d, macid =%d, raid =%d\n",
+					psta->state, psta->aid, psta->mac_id,
+					psta->raid);
+			DBG_871X_SEL_NL(m, "qos_en =%d, ht_en =%d, init_rate =%d\n",
+					psta->qos_option,
+					psta->htpriv.ht_option,
+					psta->init_rate);
+			DBG_871X_SEL_NL(m, "bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n",
+					psta->bw_mode, psta->htpriv.ch_offset,
+					psta->htpriv.sgi_20m,
+					psta->htpriv.sgi_40m);
+			DBG_871X_SEL_NL(m, "ampdu_enable = %d\n",
+					psta->htpriv.ampdu_enable);
+			DBG_871X_SEL_NL(m, "agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n",
+					psta->htpriv.agg_enable_bitmap,
+					psta->htpriv.candidate_tid_bitmap);
+			DBG_871X_SEL_NL(m, "sleepq_len =%d\n",
+					psta->sleepq_len);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.vo_q_qcnt =%d\n",
+					psta->sta_xmitpriv.vo_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.vi_q_qcnt =%d\n",
+					psta->sta_xmitpriv.vi_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.be_q_qcnt =%d\n",
+					psta->sta_xmitpriv.be_q.qcnt);
+			DBG_871X_SEL_NL(m, "sta_xmitpriv.bk_q_qcnt =%d\n",
+					psta->sta_xmitpriv.bk_q.qcnt);
+
+			DBG_871X_SEL_NL(m, "capability = 0x%x\n",
+					psta->capability);
+			DBG_871X_SEL_NL(m, "flags = 0x%x\n", psta->flags);
+			DBG_871X_SEL_NL(m, "wpa_psk = 0x%x\n", psta->wpa_psk);
+			DBG_871X_SEL_NL(m, "wpa2_group_cipher = 0x%x\n",
+					psta->wpa2_group_cipher);
+			DBG_871X_SEL_NL(m, "wpa2_pairwise_cipher = 0x%x\n",
+					psta->wpa2_pairwise_cipher);
+			DBG_871X_SEL_NL(m, "qos_info = 0x%x\n", psta->qos_info);
+			DBG_871X_SEL_NL(m, "dot118021XPrivacy = 0x%x\n",
+					psta->dot118021XPrivacy);
+
+			for (j = 0; j < 16; j++) {
+				preorder_ctrl = &psta->recvreorder_ctrl[j];
+				if (preorder_ctrl->enable)
+					DBG_871X_SEL_NL(m, "tid =%d, indicate_seq =%d\n",
+							j, preorder_ctrl->indicate_seq);
+			}
+			DBG_871X_SEL_NL(m, "==============================\n");
+		}
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+	return 0;
+}
+
+int proc_get_btcoex_dbg(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter;
+	char buf[512] = {0};
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_btcoex_GetDBG(padapter, buf, 512);
+
+	DBG_871X_SEL(m, "%s", buf);
+
+	return 0;
+}
+
+ssize_t proc_set_btcoex_dbg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter;
+	u8 tmp[80] = {0};
+	u32 module[2] = {0};
+	u32 num;
+
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+/*	DBG_871X("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter));*/
+
+	if (NULL == buffer) {
+		DBG_871X(FUNC_ADPT_FMT ": input buffer is NULL!\n",
+			FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	if (count < 1) {
+		DBG_871X(FUNC_ADPT_FMT ": input length is 0!\n",
+			FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	num = count;
+	if (num > (sizeof(tmp) - 1))
+		num = (sizeof(tmp) - 1);
+
+	if (copy_from_user(tmp, buffer, num)) {
+		DBG_871X(FUNC_ADPT_FMT ": copy buffer from user space FAIL!\n",
+			FUNC_ADPT_ARG(padapter));
+
+		return -EFAULT;
+	}
+
+	num = sscanf(tmp, "%x %x", module, module+1);
+	if (1 == num) {
+		if (0 == module[0])
+			memset(module, 0, sizeof(module));
+		else
+			memset(module, 0xFF, sizeof(module));
+	} else if (2 != num) {
+		DBG_871X(FUNC_ADPT_FMT ": input(\"%s\") format incorrect!\n",
+			FUNC_ADPT_ARG(padapter), tmp);
+
+		if (0 == num)
+			return -EFAULT;
+	}
+
+	DBG_871X(FUNC_ADPT_FMT ": input 0x%08X 0x%08X\n",
+		FUNC_ADPT_ARG(padapter), module[0], module[1]);
+	rtw_btcoex_SetDBG(padapter, module);
+
+	return count;
+}
+
+int proc_get_btcoex_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter;
+	const u32 bufsize = 30*100;
+	u8 *pbuf = NULL;
+
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	pbuf = rtw_zmalloc(bufsize);
+	if (NULL == pbuf) {
+		return -ENOMEM;
+	}
+
+	rtw_btcoex_DisplayBtCoexInfo(padapter, pbuf, bufsize);
+
+	DBG_871X_SEL(m, "%s\n", pbuf);
+
+	kfree(pbuf);
+
+	return 0;
+}
+
+#endif
diff --git a/drivers/staging/rtl8723bs/core/rtw_eeprom.c b/drivers/staging/rtl8723bs/core/rtw_eeprom.c
new file mode 100644
index 000000000000..35031a7a5dc5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_eeprom.c
@@ -0,0 +1,369 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_EEPROM_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+
+void up_clk(_adapter *padapter,	 u16 *x)
+{
+_func_enter_;
+	*x = *x | _EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+
+_func_exit_;
+
+}
+
+void down_clk(_adapter *padapter, u16 *x)
+{
+_func_enter_;
+	*x = *x & ~_EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	udelay(CLOCK_RATE);
+_func_exit_;
+}
+
+void shift_out_bits(_adapter *padapter, u16 data, u16 count)
+{
+	u16 x, mask;
+_func_enter_;
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	mask = 0x01 << (count - 1);
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+
+	do {
+		x &= ~_EEDI;
+		if (data & mask)
+			x |= _EEDI;
+		if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+		}
+		rtw_write8(padapter, EE_9346CR, (u8)x);
+		udelay(CLOCK_RATE);
+		up_clk(padapter, &x);
+		down_clk(padapter, &x);
+		mask = mask >> 1;
+	} while (mask);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x &= ~_EEDI;
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+out:
+_func_exit_;
+}
+
+u16 shift_in_bits(_adapter *padapter)
+{
+	u16 x, d = 0, i;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+	d = 0;
+
+	for (i = 0; i < 16; i++) {
+		d = d << 1;
+		up_clk(padapter, &x);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+		x = rtw_read8(padapter, EE_9346CR);
+
+		x &= ~(_EEDI);
+		if (x & _EEDO)
+		d |= 1;
+
+		down_clk(padapter, &x);
+	}
+out:
+_func_exit_;
+
+	return d;
+}
+
+void standby(_adapter *padapter)
+{
+	u8   x;
+_func_enter_;
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EECS | _EESK);
+	rtw_write8(padapter, EE_9346CR, x);
+
+	udelay(CLOCK_RATE);
+	x |= _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+	udelay(CLOCK_RATE);
+_func_exit_;
+}
+
+u16 wait_eeprom_cmd_done(_adapter *padapter)
+{
+	u8 x;
+	u16 i, res = false;
+_func_enter_;
+	standby(padapter);
+	for (i = 0; i < 200; i++) {
+		x = rtw_read8(padapter, EE_9346CR);
+		if (x & _EEDO) {
+			res = true;
+			goto exit;
+			}
+		udelay(CLOCK_RATE);
+	}
+exit:
+_func_exit_;
+	return res;
+}
+
+void eeprom_clean(_adapter *padapter)
+{
+	u16 x;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	x &= ~(_EECS | _EEDI);
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	up_clk(padapter, &x);
+		if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	down_clk(padapter, &x);
+out:
+_func_exit_;
+}
+
+void eeprom_write16(_adapter *padapter, u16 reg, u16 data)
+{
+	u8 x;
+
+_func_enter_;
+
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+
+	shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5);
+
+	if (padapter->EepromAddressSize == 8)	/*CF+ and SDIO*/
+		shift_out_bits(padapter, 0, 6);
+	else									/*USB*/
+		shift_out_bits(padapter, 0, 4);
+
+	standby(padapter);
+
+/* Commented out by rcnjko, 2004.0
+*	 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.
+*	shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3);
+*	shift_out_bits(Adapter, reg, Adapter->EepromAddressSize);
+*
+*	if (wait_eeprom_cmd_done(Adapter ) == false)
+*	{
+*		return;
+*	}
+*/
+
+	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->EepromAddressSize);
+
+	/* write the data to the selected EEPROM word.*/
+	shift_out_bits(padapter, data, 16);
+
+	if (wait_eeprom_cmd_done(padapter) == false) {
+
+		goto exit;
+	}
+
+	standby(padapter);
+
+	shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5);
+	shift_out_bits(padapter, reg, 4);
+
+	eeprom_clean(padapter);
+exit:
+_func_exit_;
+	return;
+}
+
+u16 eeprom_read16(_adapter *padapter, u16 reg) /*ReadEEprom*/
+{
+
+	u16 x;
+	u16 data = 0;
+
+_func_enter_;
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS*/
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_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->EepromAddressSize);
+
+	/* Now read the data (16 bits) in from the selected EEPROM word*/
+	data = shift_in_bits(padapter);
+
+	eeprom_clean(padapter);
+out:
+_func_exit_;
+	return data;
+
+
+}
+
+
+
+
+/*From even offset*/
+void eeprom_read_sz(_adapter *padapter, u16 reg, u8 *data, u32 sz)
+{
+
+	u16 x, data16;
+	u32 i;
+_func_enter_;
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS*/
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (padapter->bSurpriseRemoved == true) {
+		RT_TRACE(_module_rtl871x_eeprom_c_, _drv_err_, ("padapter->bSurpriseRemoved==true"));
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_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->EepromAddressSize);
+
+
+	for (i = 0; i < sz; i += 2) {
+		data16 = shift_in_bits(padapter);
+		data[i] = data16 & 0xff;
+		data[i+1] = data16 >> 8;
+	}
+
+	eeprom_clean(padapter);
+out:
+_func_exit_;
+
+
+
+}
+
+
+/*addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg)*/
+u8 eeprom_read(_adapter *padapter, u32 addr_off, u8 sz, u8 *rbuf)
+{
+	u8 quotient, remainder, addr_2align_odd;
+	u16 reg, stmp, i = 0, idx = 0;
+_func_enter_;
+	reg = (u16)(addr_off >> 1);
+	addr_2align_odd = (u8)(addr_off & 0x1);
+
+	/*read that start at high part: e.g  1,3,5,7,9,...*/
+	if (addr_2align_odd) {
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff); /*return hogh-part of the short*/
+		reg++; sz--;
+	}
+
+	quotient = sz >> 1;
+	remainder = sz & 0x1;
+
+	for (i = 0; i < quotient; i++) {
+		stmp = eeprom_read16(padapter, reg+i);
+		rbuf[idx++] = (u8) (stmp&0xff);
+		rbuf[idx++] = (u8) ((stmp>>8)&0xff);
+	}
+
+	reg = reg+i;
+	if (remainder) { /*end of read at lower part of short : 0,2,4,6,...*/
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx] = (u8)(stmp & 0xff);
+	}
+_func_exit_;
+	return true;
+}
+
+
+
+void read_eeprom_content(_adapter *padapter)
+{
+
+_func_enter_;
+
+
+_func_exit_;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_efuse.c b/drivers/staging/rtl8723bs/core/rtw_efuse.c
new file mode 100644
index 000000000000..8e29802fc67f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_efuse.c
@@ -0,0 +1,635 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_EFUSE_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+#include <linux/jiffies.h>
+
+
+/*------------------------Define local variable------------------------------*/
+u8 fakeEfuseBank = 0;
+u32 fakeEfuseUsedBytes = 0;
+u8 fakeEfuseContent[EFUSE_MAX_HW_SIZE] = {0};
+u8 fakeEfuseInitMap[EFUSE_MAX_MAP_LEN] = {0};
+u8 fakeEfuseModifiedMap[EFUSE_MAX_MAP_LEN] = {0};
+
+u32 BTEfuseUsedBytes = 0;
+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 = 0;
+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 local variable------------------------------*/
+
+/*  */
+#define REG_EFUSE_CTRL		0x0030
+#define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
+/*  */
+
+bool
+Efuse_Read1ByteFromFakeContent(
+	struct adapter *padapter,
+	u16 	Offset,
+	u8 *Value);
+bool
+Efuse_Read1ByteFromFakeContent(
+	struct adapter *padapter,
+	u16 	Offset,
+	u8 *Value)
+{
+	if (Offset >= EFUSE_MAX_HW_SIZE) {
+		return false;
+	}
+	/* DbgPrint("Read fake content, offset = %d\n", Offset); */
+	if (fakeEfuseBank == 0)
+		*Value = fakeEfuseContent[Offset];
+	else
+		*Value = fakeBTEfuseContent[fakeEfuseBank-1][Offset];
+	return true;
+}
+
+bool
+Efuse_Write1ByteToFakeContent(
+	struct adapter *padapter,
+	u16 	Offset,
+	u8 Value);
+bool
+Efuse_Write1ByteToFakeContent(
+	struct adapter *padapter,
+	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)
+{
+	u16 ret = 0;
+
+	ret = padapter->HalFunc.EfuseGetCurrentSize(padapter, efuseType, bPseudoTest);
+
+	return ret;
+}
+
+/*  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. Refered 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 data;
+	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) {
+				k = 0;
+				break;
+			}
+		}
+		data = rtw_read8(Adapter, EFUSE_CTRL);
+		return data;
+	} 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;
+
+	/* DBG_871X("===> EFUSE_OneByteRead(), addr = %x\n", addr); */
+	/* DBG_871X("===> EFUSE_OneByteRead() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Read1ByteFromFakeContent(padapter, addr, data);
+		return bResult;
+	}
+
+	/*  <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;
+		DBG_871X("%s: [ERROR] addr = 0x%x bResult =%d time out 1s !!!\n", __func__, addr, bResult);
+		DBG_871X("%s: [ERROR] EFUSE_CTRL = 0x%08x !!!\n", __func__, rtw_read32(padapter, EFUSE_CTRL));
+	}
+
+	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;
+
+	/* DBG_871X("===> EFUSE_OneByteWrite(), addr = %x data =%x\n", addr, data); */
+	/* DBG_871X("===> EFUSE_OneByteWrite() start, 0x34 = 0x%X\n", rtw_read32(padapter, EFUSE_TEST)); */
+
+	if (bPseudoTest) {
+		bResult = Efuse_Write1ByteToFakeContent(padapter, addr, data);
+		return bResult;
+	}
+
+
+	/*  -----------------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;
+		DBG_871X("%s: [ERROR] addr = 0x%x , efuseValue = 0x%x , bResult =%d time out 1s !!!\n",
+					__func__, addr, efuseValue, bResult);
+		DBG_871X("%s: [ERROR] EFUSE_CTRL = 0x%08x !!!\n", __func__, rtw_read32(padapter, EFUSE_CTRL));
+	}
+
+	/*  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)
+{
+	int	ret = 0;
+
+	ret =  padapter->HalFunc.Efuse_PgPacketRead(padapter, offset, data, bPseudoTest);
+
+	return ret;
+}
+
+int
+Efuse_PgPacketWrite(struct adapter *padapter,
+				u8 	offset,
+				u8 	word_en,
+				u8 	*data,
+				bool		bPseudoTest)
+{
+	int ret;
+
+	ret =  padapter->HalFunc.Efuse_PgPacketWrite(padapter, offset, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * 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)
+{
+	u8 ret = 0;
+
+	ret =  padapter->HalFunc.Efuse_WordEnableDataWrite(padapter, efuse_addr, word_en, data, bPseudoTest);
+
+	return ret;
+}
+
+/*-----------------------------------------------------------------------------
+ * 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 == true) {
+		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 000000000000..7b37e085b793
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
@@ -0,0 +1,1430 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _IEEE80211_C
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/of.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;
+}
+
+uint	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;
+}
+
+uint	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) {
+		if ((rtw_is_cckrates_included(rate)) == true)
+			return WIRELESS_INVALID;
+		else
+			return WIRELESS_11A;
+	} else{ /*  could be pure B, pure G, or B/G */
+		if ((rtw_is_cckratesonly_included(rate)) == true)
+			return WIRELESS_11B;
+		else if ((rtw_is_cckrates_included(rate)) == true)
+			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,
+	sint 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, sint index, sint *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;
+		} else{
+			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;
+		} else{
+			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 scarch
+ * @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 buf[MAX_IE_SZ] = {0};
+			u8 *remain_ies = target_ie + target_ielen;
+			uint remain_len = search_len - (remain_ies - start);
+
+			memcpy(buf, remain_ies, remain_len);
+			memcpy(target_ie, buf, 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 *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:
+	case WIRELESS_11A:
+	case WIRELESS_11_5N:
+	case WIRELESS_11A_5N:/* Todo: no basic rate for ofdm ? */
+	case WIRELESS_11_5AC:
+		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 */
+	if (pregistrypriv->wireless_mode == WIRELESS_11ABGN) {
+		if (pdev_network->Configuration.DSConfig > 14)
+			wireless_mode = WIRELESS_11A_5N;
+		else
+			wireless_mode = WIRELESS_11BG_24N;
+	} else{
+		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);
+	}
+
+	/* HT Cap. */
+	if (((pregistrypriv->wireless_mode&WIRELESS_11_5N) || (pregistrypriv->wireless_mode&WIRELESS_11_24N))
+		&& (pregistrypriv->ht_enable == true)) {
+		/* todo: */
+	}
+
+	/* pdev_network->IELength =  sz; update IELength */
+
+	/* 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, _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;
+
+		} 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, _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) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+
+		return _FAIL;
+	}
+
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+		/* count = le16_to_cpu(*(u16*)pos); */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * WPA_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+						"count %u left %u", __func__, count, left));
+			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) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",   __func__));
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s : there has 802.1x auth\n", __func__));
+				*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) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie length mismatch, %u too much", __func__, left));
+		return _FAIL;
+	}
+
+	/* pairwise_cipher */
+	if (left >= 2) {
+	  /* count = le16_to_cpu(*(u16*)pos); */
+		count = RTW_GET_LE16(pos);
+		pos += 2;
+		left -= 2;
+
+		if (count == 0 || left < count * RSN_SELECTOR_LEN) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie count botch (pairwise), "
+						 "count %u left %u", __func__, count, left));
+			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) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s: ie too short (for key mgmt)",  __func__));
+
+		return _FAIL;
+	}
+
+	if (is_8021x) {
+		if (left >= 6) {
+			pos += 2;
+			if (!memcmp(pos, SUITE_1X, 4)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s (): there has 802.1x auth\n", __func__));
+				*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, i;
+	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 == _WAPI_IE_) */
+		if (authmode == _WAPI_IE_ && (!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);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+								wapi_ie[i], wapi_ie[i+1], wapi_ie[i+2], wapi_ie[i+3], wapi_ie[i+4],
+								wapi_ie[i+5], wapi_ie[i+6], wapi_ie[i+7]));
+				}
+			}
+
+			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 */
+
+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, sec_idx, i;
+	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_);
+
+	sec_idx = 0;
+
+	while (cnt < in_len) {
+		authmode = in_ie[cnt];
+
+		if ((authmode == _WPA_IE_ID_) && (!memcmp(&in_ie[cnt+2], &wpa_oui[0], 4))) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_get_wpa_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n", sec_idx, in_ie[cnt+1]+2));
+
+				if (wpa_ie) {
+				memcpy(wpa_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+						RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									wpa_ie[i], wpa_ie[i+1], wpa_ie[i+2], wpa_ie[i+3], wpa_ie[i+4],
+									wpa_ie[i+5], wpa_ie[i+6], wpa_ie[i+7]));
+					}
+				}
+
+				*wpa_len = in_ie[cnt+1]+2;
+				cnt += in_ie[cnt+1]+2;  /* get next */
+		} else{
+			if (authmode == _WPA2_IE_ID_) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n get_rsn_ie: sec_idx =%d in_ie[cnt+1]+2 =%d\n", sec_idx, in_ie[cnt+1]+2));
+
+				if (rsn_ie) {
+				memcpy(rsn_ie, &in_ie[cnt], in_ie[cnt+1]+2);
+
+				for (i = 0; i < (in_ie[cnt+1]+2); i = i+8) {
+						RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n %2x,%2x,%2x,%2x,%2x,%2x,%2x,%2x\n",
+									rsn_ie[i], rsn_ie[i+1], rsn_ie[i+2], rsn_ie[i+3], rsn_ie[i+4],
+									rsn_ie[i+5], rsn_ie[i+6], rsn_ie[i+7]));
+					}
+				}
+
+				*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 == NULL)
+		return match;
+
+	eid = ie_ptr[0];
+
+	if ((eid == _WPA_IE_ID_) && (!memcmp(&ie_ptr[2], wps_oui, 4))) {
+		/* DBG_8192C("==> found WPS_IE.....\n"); */
+		*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;
+		} else{
+			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;
+
+		/* DBG_871X("%s attr_ptr:%p, id:%u, length:%u\n", __func__, attr_ptr, attr_id, attr_data_len); */
+		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;
+		} else{
+			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) {
+		if (show_errors) {
+			DBG_871X("short vendor specific "
+				   "information element ignored (len =%lu)\n",
+				   (unsigned long) elen);
+		}
+		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) {
+				DBG_871X("short WME "
+					   "information element ignored "
+					   "(len =%lu)\n",
+					   (unsigned long) elen);
+				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:
+				DBG_871X("unknown WME "
+					   "information element ignored "
+					   "(subtype =%d len =%lu)\n",
+					   pos[4], (unsigned long) elen);
+				return -1;
+			}
+			break;
+		case 4:
+			/* Wi-Fi Protected Setup (WPS) IE */
+			elems->wps_ie = pos;
+			elems->wps_ie_len = elen;
+			break;
+		default:
+			DBG_871X("Unknown Microsoft "
+				   "information element ignored "
+				   "(type =%d len =%lu)\n",
+				   pos[3], (unsigned long) elen);
+			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:
+			DBG_871X("Unknown Broadcom "
+				   "information element ignored "
+				   "(type =%d len =%lu)\n",
+				   pos[3], (unsigned long) elen);
+			return -1;
+		}
+		break;
+
+	default:
+		DBG_871X("unknown vendor specific information "
+			   "element ignored (vendor OUI %02x:%02x:%02x "
+			   "len =%lu)\n",
+			   pos[0], pos[1], pos[2], (unsigned long) elen);
+		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
+ */
+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) {
+			if (show_errors) {
+				DBG_871X("IEEE 802.11 element "
+					   "parse failed (id =%d elen =%d "
+					   "left =%lu)\n",
+					   id, elen, (unsigned long) 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;
+		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_VHT_OP_MODE_NOTIFY:
+			elems->vht_op_mode_notify = pos;
+			elems->vht_op_mode_notify_len = elen;
+			break;
+		default:
+			unknown++;
+			if (!show_errors)
+				break;
+			DBG_871X("IEEE 802.11 element parse "
+				   "ignored unknown element (id =%d elen =%d)\n",
+				   id, elen);
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	if (left)
+		return ParseFailed;
+
+	return unknown ? ParseUnknown : ParseOK;
+
+}
+
+static u8 key_char2num(u8 ch);
+static 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 key_2char2num(u8 hch, u8 lch);
+u8 key_2char2num(u8 hch, u8 lch)
+{
+		return ((key_char2num(hch) << 4) | key_char2num(lch));
+}
+
+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 == NULL)
+		return;
+
+	if (rtw_initmac) {	/* 	Users specify the mac address */
+		int jj, kk;
+
+		for (jj = 0, kk = 0; jj < ETH_ALEN; jj++, kk += 3) {
+			mac[jj] = key_2char2num(rtw_initmac[kk], rtw_initmac[kk + 1]);
+		}
+		memcpy(mac_addr, mac, ETH_ALEN);
+	} else{	/* 	Use the mac address stored in the Efuse */
+		memcpy(mac, mac_addr, ETH_ALEN);
+	}
+
+	if (((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))) {
+	        if (np &&
+	            (addr = of_get_property(np, "local-mac-address", &len)) &&
+	            len == ETH_ALEN) {
+			memcpy(mac_addr, addr, ETH_ALEN);
+		} else {
+			mac[0] = 0x00;
+			mac[1] = 0xe0;
+			mac[2] = 0x4c;
+			mac[3] = 0x87;
+			mac[4] = 0x00;
+			mac[5] = 0x00;
+			/*  use default mac addresss */
+			memcpy(mac_addr, mac, ETH_ALEN);
+			DBG_871X("MAC Address from efuse error, assign default one !!!\n");
+		}
+	}
+
+	DBG_871X("rtw_macaddr_cfg MAC Address  = "MAC_FMT"\n", MAC_ARG(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.IELength-12);
+
+	if (pbuf && (wpa_ielen > 0)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_cipher_info: wpa_ielen: %d", wpa_ielen));
+		if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+
+			pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+			pnetwork->BcnInfo.group_cipher = group_cipher;
+			pnetwork->BcnInfo.is_8021x = is8021x;
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d, is_8021x is %d",
+						__func__, pnetwork->BcnInfo.pairwise_cipher, pnetwork->BcnInfo.is_8021x));
+			ret = _SUCCESS;
+		}
+	} else {
+
+		pbuf = rtw_get_wpa2_ie(&pnetwork->network.IEs[12], &wpa_ielen, pnetwork->network.IELength-12);
+
+		if (pbuf && (wpa_ielen > 0)) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE\n"));
+			if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("get RSN IE  OK!!!\n"));
+				pnetwork->BcnInfo.pairwise_cipher = pairwise_cipher;
+				pnetwork->BcnInfo.group_cipher = group_cipher;
+				pnetwork->BcnInfo.is_8021x = is8021x;
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s: pnetwork->pairwise_cipher: %d,"
+							"pnetwork->group_cipher is %d, is_8021x is %d",	__func__, pnetwork->BcnInfo.pairwise_cipher,
+							pnetwork->BcnInfo.group_cipher, pnetwork->BcnInfo.is_8021x));
+				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->BcnInfo.encryp_protocol = ENCRYP_PROTOCOL_OPENSYS;
+	}
+	rtw_get_sec_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &rsn_len, NULL, &wpa_len);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_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;
+	}
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_get_bcn_info: pnetwork->encryp_protocol is %x\n",
+				pnetwork->BcnInfo.encryp_protocol));
+	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 rf_type, u8 bw_40MHz, u8 short_GI, unsigned char *MCS_rate)
+{
+	u16 max_rate = 0;
+
+	if (rf_type == RF_1T1R) {
+		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);
+	} else{
+		if (MCS_rate[1]) {
+			if (MCS_rate[1] & BIT(7))
+				max_rate = (bw_40MHz) ? ((short_GI)?3000:2700):((short_GI)?1444:1300);
+			else if (MCS_rate[1] & BIT(6))
+				max_rate = (bw_40MHz) ? ((short_GI)?2700:2430):((short_GI)?1300:1170);
+			else if (MCS_rate[1] & BIT(5))
+				max_rate = (bw_40MHz) ? ((short_GI)?2400:2160):((short_GI)?1156:1040);
+			else if (MCS_rate[1] & BIT(4))
+				max_rate = (bw_40MHz) ? ((short_GI)?1800:1620):((short_GI)?867:780);
+			else if (MCS_rate[1] & BIT(3))
+				max_rate = (bw_40MHz) ? ((short_GI)?1200:1080):((short_GI)?578:520);
+			else if (MCS_rate[1] & BIT(2))
+				max_rate = (bw_40MHz) ? ((short_GI)?900:810):((short_GI)?433:390);
+			else if (MCS_rate[1] & BIT(1))
+				max_rate = (bw_40MHz) ? ((short_GI)?600:540):((short_GI)?289:260);
+			else if (MCS_rate[1] & BIT(0))
+				max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
+		} else{
+			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 & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE))
+		!= (RTW_IEEE80211_FTYPE_MGMT|RTW_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 000000000000..6bd5a4741e79
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_io.c
@@ -0,0 +1,203 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*
+
+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
+
+*/
+
+#define _RTW_IO_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+#define rtw_le16_to_cpu(val)		val
+#define rtw_le32_to_cpu(val)		val
+#define rtw_cpu_to_le16(val)		val
+#define rtw_cpu_to_le32(val)		val
+
+u8 _rtw_read8(struct adapter *adapter, u32 addr)
+{
+	u8 r_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);
+	u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read8 = pintfhdl->io_ops._read8;
+
+	r_val = _read8(pintfhdl, addr);
+	return r_val;
+}
+
+u16 _rtw_read16(struct adapter *adapter, u32 addr)
+{
+	u16 r_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);
+	u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read16 = pintfhdl->io_ops._read16;
+
+	r_val = _read16(pintfhdl, addr);
+	return rtw_le16_to_cpu(r_val);
+}
+
+u32 _rtw_read32(struct adapter *adapter, u32 addr)
+{
+	u32 r_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);
+	u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_read32 = pintfhdl->io_ops._read32;
+
+	r_val = _read32(pintfhdl, addr);
+	return rtw_le32_to_cpu(r_val);
+
+}
+
+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);
+}
+
+u8 _rtw_sd_f0_read8(struct adapter *adapter, u32 addr)
+{
+	u8 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u8 (*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_f0_read8 = pintfhdl->io_ops._sd_f0_read8;
+
+	if (_sd_f0_read8)
+		r_val = _sd_f0_read8(pintfhdl, addr);
+	else
+		DBG_871X_LEVEL(_drv_warning_, FUNC_ADPT_FMT" _sd_f0_read8 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+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);
+	u32 ret = _SUCCESS;
+
+	_write_port = pintfhdl->io_ops._write_port;
+
+	ret = _write_port(pintfhdl, addr, cnt, pmem);
+
+	return ret;
+}
+
+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 == NULL)
+		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) {
+		DBG_871X("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR);
+		ret = true;
+	} else {
+		/* DBG_871X("[dvobj:%p] continual_io_error:%d\n", dvobj, value); */
+	}
+	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 000000000000..e0793f8d329d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_ioctl_set.c
@@ -0,0 +1,698 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_SET_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+#define IS_MAC_ADDRESS_BROADCAST(addr) \
+(\
+	((addr[0] == 0xff) && (addr[1] == 0xff) && \
+		(addr[2] == 0xff) && (addr[3] == 0xff) && \
+		(addr[4] == 0xff) && (addr[5] == 0xff))  ? true : false \
+)
+
+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->SsidLength > 32) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid length >32\n"));
+		ret = false;
+		goto exit;
+	}
+
+#ifdef CONFIG_VALIDATE_SSID
+	for (i = 0; i < ssid->SsidLength; i++) {
+		/* wifi, printable ascii code must be supported */
+		if (!((ssid->Ssid[i] >= 0x20) && (ssid->Ssid[i] <= 0x7e))) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("ssid has nonprintabl ascii\n"));
+			ret = false;
+			break;
+		}
+	}
+#endif /* CONFIG_VALIDATE_SSID */
+
+exit:
+	return ret;
+}
+
+u8 rtw_do_join(struct adapter *padapter);
+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);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("\n rtw_do_join: phead = %p; plist = %p\n\n\n", phead, plist));
+
+	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
+		) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_do_join(): site survey if scanned_queue is empty\n."));
+			/*  submit site_survey_cmd */
+			ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+			if (_SUCCESS != ret) {
+				pmlmepriv->to_join = false;
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_do_join(): site survey return error\n."));
+			}
+		} 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.MacAddress;
+
+				memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+				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) {
+					RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>do_goin: rtw_createbss_cmd status FAIL***\n "));
+					ret =  false;
+					goto exit;
+				}
+
+				pmlmepriv->to_join = false;
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("***Error => rtw_select_and_join_from_scanned_queue FAIL under STA_Mode***\n "));
+
+			} 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
+				) {
+					/* DBG_871X("rtw_do_join() when   no desired bss in scanning queue\n"); */
+					ret = rtw_sitesurvey_cmd(padapter, &pmlmepriv->assoc_ssid, 1, NULL, 0);
+					if (_SUCCESS != ret) {
+						pmlmepriv->to_join = false;
+						RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("do_join(): site survey return error\n."));
+					}
+				} else{
+					ret = _FAIL;
+					pmlmepriv->to_join = false;
+				}
+			}
+
+		}
+
+	}
+
+exit:
+	return ret;
+}
+
+u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid)
+{
+	u8 status = _SUCCESS;
+
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	DBG_871X_LEVEL(_drv_always_, "set bssid:%pM\n", bssid);
+
+	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);
+
+
+	DBG_871X("Set BSSID under fw_state = 0x%08x\n", 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;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if (!memcmp(&pmlmepriv->cur_network.network.MacAddress, bssid, ETH_ALEN)) {
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)
+				goto release_mlme_lock;/* it means driver is in WIFI_ADHOC_MASTER_STATE, we needn't create bss again. */
+		} else {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set BSSID not the same bssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_bssid ="MAC_FMT"\n", MAC_ARG(bssid)));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("cur_bssid ="MAC_FMT"\n", MAC_ARG(pmlmepriv->cur_network.network.MacAddress)));
+
+			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;
+	}
+
+	memset(&pmlmepriv->assoc_ssid, 0, sizeof(struct ndis_802_11_ssid));
+	memcpy(&pmlmepriv->assoc_bssid, bssid, ETH_ALEN);
+	pmlmepriv->assoc_by_bssid = true;
+
+	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:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("rtw_set_802_11_bssid: status =%d\n", status));
+
+	return status;
+}
+
+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;
+
+	DBG_871X_LEVEL(_drv_always_, "set ssid [%s] fw_state = 0x%08x\n",
+			ssid->Ssid, get_fwstate(pmlmepriv));
+
+	if (padapter->hw_init_completed == false) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("set_ssid: hw_init_completed ==false =>exit!!!\n"));
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	DBG_871X("Set SSID under fw_state = 0x%08x\n", 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;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+			 ("set_ssid: _FW_LINKED||WIFI_ADHOC_MASTER_STATE\n"));
+
+		if ((pmlmepriv->assoc_ssid.SsidLength == ssid->SsidLength) &&
+		    (!memcmp(&pmlmepriv->assoc_ssid.Ssid, ssid->Ssid, ssid->SsidLength))) {
+			if ((check_fwstate(pmlmepriv, WIFI_STATION_STATE) == false)) {
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+					 ("Set SSID is the same ssid, fw_state = 0x%08x\n",
+					  get_fwstate(pmlmepriv)));
+
+				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{
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("Set SSID not the same ssid\n"));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_ssid =[%s] len = 0x%x\n", ssid->Ssid, (unsigned int)ssid->SsidLength));
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("assoc_ssid =[%s] len = 0x%x\n", pmlmepriv->assoc_ssid.Ssid, (unsigned int)pmlmepriv->assoc_ssid.SsidLength));
+
+			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:
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+		("-rtw_set_802_11_ssid: status =%d\n", status));
+
+	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 == false && bssid_valid == false) {
+		DBG_871X(FUNC_ADPT_FMT" ssid:%p, ssid_valid:%d, bssid:%p, bssid_valid:%d\n",
+			FUNC_ADPT_ARG(padapter), ssid, ssid_valid, bssid, bssid_valid);
+		status = _FAIL;
+		goto exit;
+	}
+
+	if (padapter->hw_init_completed == false) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_,
+			 ("set_ssid: hw_init_completed ==false =>exit!!!\n"));
+		status = _FAIL;
+		goto exit;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	DBG_871X_LEVEL(_drv_always_, 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.InfrastructureMode);
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_notice_,
+		 ("+rtw_set_802_11_infrastructure_mode: old =%d new =%d fw_state = 0x%08x\n",
+		  *pold_state, networktype, get_fwstate(pmlmepriv)));
+
+	if (*pold_state != networktype) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, (" change mode!"));
+		/* DBG_871X("change mode, old_mode =%d, new_mode =%d, fw_state = 0x%x\n", *pold_state, networktype, get_fwstate(pmlmepriv)); */
+
+		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 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);
+			start_ap_mode(padapter);
+			/* rtw_indicate_connect(padapter); */
+
+			break;
+
+		case Ndis802_11AutoUnknown:
+		case Ndis802_11InfrastructureMax:
+			break;
+		}
+
+		/* SecClearAllKeys(adapter); */
+
+		/* RT_TRACE(COMP_OID_SET, DBG_LOUD, ("set_infrastructure: fw_state:%x after changing mode\n", */
+		/* 									get_fwstate(pmlmepriv))); */
+
+		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) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_disassociate: rtw_indicate_disconnect\n"));
+
+		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);
+		if (_FAIL == rtw_pwr_wakeup(padapter))
+			DBG_871X("%s(): rtw_pwr_wakeup fail !!!\n", __func__);
+	}
+
+	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;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("+rtw_set_802_11_bssid_list_scan(), fw_state =%x\n", get_fwstate(pmlmepriv)));
+
+	if (padapter == NULL) {
+		res = false;
+		goto exit;
+	}
+	if (padapter->hw_init_completed == false) {
+		res = false;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n ===rtw_set_802_11_bssid_list_scan:hw_init_completed ==false ===\n"));
+		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. */
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("rtw_set_802_11_bssid_list_scan fail since fw_state = %x\n", get_fwstate(pmlmepriv)));
+		res = true;
+
+		if (check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING)) == true) {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###_FW_UNDER_SURVEY|_FW_UNDER_LINKING\n\n"));
+		} else {
+			RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("\n###pmlmepriv->sitesurveyctrl.traffic_busy ==true\n\n"));
+		}
+	} else {
+		if (rtw_is_scan_deny(padapter)) {
+			DBG_871X(FUNC_ADPT_FMT": scan deny\n", FUNC_ADPT_ARG(padapter));
+			indicate_wx_scan_complete_event(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;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("set_802_11_auth.mode(): mode =%x\n", authmode));
+
+	psecuritypriv->ndisauthtype = authmode;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_authentication_mode:psecuritypriv->ndisauthtype =%d", psecuritypriv->ndisauthtype));
+
+	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)
+{
+
+	u8 bdefaultkey;
+	u8 btransmitkey;
+	sint		keyid, res;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	u8 ret = _SUCCESS;
+
+	bdefaultkey = (wep->KeyIndex & 0x40000000) > 0 ? false : true;   /* for ??? */
+	btransmitkey = (wep->KeyIndex & 0x80000000) > 0 ? true  : false;	/* for ??? */
+	keyid = wep->KeyIndex & 0x3fffffff;
+
+	if (keyid >= 4) {
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("MgntActrtw_set_802_11_add_wep:keyid>4 =>fail\n"));
+		ret = false;
+		goto exit;
+	}
+
+	switch (wep->KeyLength) {
+	case 5:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength =5\n"));
+		break;
+	case 13:
+		psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength = 13\n"));
+		break;
+	default:
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("MgntActrtw_set_802_11_add_wep:wep->KeyLength!=5 or 13\n"));
+		break;
+	}
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_,
+		 ("rtw_set_802_11_add_wep:before memcpy, wep->KeyLength = 0x%x wep->KeyIndex = 0x%x  keyid =%x\n",
+		  wep->KeyLength, wep->KeyIndex, keyid));
+
+	memcpy(&(psecuritypriv->dot11DefKey[keyid].skey[0]), &(wep->KeyMaterial), wep->KeyLength);
+
+	psecuritypriv->dot11DefKeylen[keyid] = wep->KeyLength;
+
+	psecuritypriv->dot11PrivacyKeyIndex = keyid;
+
+	RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_info_, ("rtw_set_802_11_add_wep:security key material : %x %x %x %x %x %x %x %x %x %x %x %x %x\n",
+		psecuritypriv->dot11DefKey[keyid].skey[0], psecuritypriv->dot11DefKey[keyid].skey[1], psecuritypriv->dot11DefKey[keyid].skey[2],
+		psecuritypriv->dot11DefKey[keyid].skey[3], psecuritypriv->dot11DefKey[keyid].skey[4], psecuritypriv->dot11DefKey[keyid].skey[5],
+		psecuritypriv->dot11DefKey[keyid].skey[6], psecuritypriv->dot11DefKey[keyid].skey[7], psecuritypriv->dot11DefKey[keyid].skey[8],
+		psecuritypriv->dot11DefKey[keyid].skey[9], psecuritypriv->dot11DefKey[keyid].skey[10], psecuritypriv->dot11DefKey[keyid].skey[11],
+		psecuritypriv->dot11DefKey[keyid].skey[12]));
+
+	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;
+	u8 rf_type = 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 == NULL)
+		return 0;
+
+	short_GI = query_ra_short_GI(psta);
+
+	if (IsSupportedHT(psta->wireless_mode)) {
+		rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+		max_rate = rtw_mcs_rate(
+			rf_type,
+			((psta->bw_mode == CHANNEL_WIDTH_40)?1:0),
+			short_GI,
+			psta->htpriv.ht_cap.supp_mcs_set
+		);
+	} 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 = 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 000000000000..9e355734f0c0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c
@@ -0,0 +1,3150 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_MLME_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+
+extern u8 rtw_do_join(struct adapter *padapter);
+
+sint	_rtw_init_mlme_priv(struct adapter *padapter)
+{
+	sint	i;
+	u8 *pbuf;
+	struct wlan_network	*pnetwork;
+	struct mlme_priv 	*pmlmepriv = &padapter->mlmepriv;
+	sint	res = _SUCCESS;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((u8 *)pmlmepriv, 0, sizeof(struct mlme_priv)); */
+
+	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.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 == NULL) {
+		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
+		#ifdef CONFIG_LAYER2_ROAMING_ACTIVE /* FIXME */
+		| RTW_ROAM_ACTIVE
+		#endif
+		;
+
+	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)
+{
+	rtw_free_mlme_priv_ie_data(pmlmepriv);
+
+	if (pmlmepriv) {
+		if (pmlmepriv->free_bss_buf) {
+			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 = LIST_CONTAINOR(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)/* _queue *free_queue) */
+{
+	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 = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+	list_del_init(&pnetwork->list);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("_rtw_alloc_network: ptr =%p\n", plist));
+	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 == NULL)
+		return;
+
+	if (pnetwork->fixed == true)
+		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--;
+
+
+	/* DBG_871X("_rtw_free_network:SSID =%s\n", pnetwork->network.Ssid.Ssid); */
+
+	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 == NULL)
+		return;
+
+	if (pnetwork->fixed == true)
+		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 calle 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);
+	plist = get_next(phead);
+
+	while (plist != phead) {
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (!memcmp(addr, pnetwork->network.MacAddress, ETH_ALEN))
+			break;
+
+		plist = get_next(plist);
+	}
+
+	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;
+	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 = get_next(phead);
+
+	while (phead != plist) {
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		plist = get_next(plist);
+
+		_rtw_free_network(pmlmepriv, pnetwork, isfreeall);
+
+	}
+
+	spin_unlock_bh(&scanned_queue->lock);
+}
+
+
+
+
+sint rtw_if_up(struct adapter *padapter)
+{
+
+	sint res;
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved ||
+		(check_fwstate(&padapter->mlmepriv, _FW_LINKED) == false)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_if_up:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+		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]; */
+	return;
+}
+
+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);
+}
+
+
+int	rtw_init_mlme_priv(struct adapter *padapter)/* struct	mlme_priv *pmlmepriv) */
+{
+	int	res;
+
+	res = _rtw_init_mlme_priv(padapter);/*  (pmlmepriv); */
+	return res;
+}
+
+void rtw_free_mlme_priv(struct mlme_priv *pmlmepriv)
+{
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_mlme_priv\n"));
+	_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;
+}
+*/
+
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv);
+struct	wlan_network *rtw_alloc_network(struct	mlme_priv *pmlmepriv)/* _queue	*free_queue) */
+{
+	struct	wlan_network	*pnetwork;
+
+	pnetwork = _rtw_alloc_network(pmlmepriv);
+	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)
+{
+	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_network ==> ssid = %s\n\n" , pnetwork->network.Ssid.Ssid)); */
+	_rtw_free_network_nolock(&(padapter->mlmepriv), pnetwork);
+	rtw_cfg80211_unlink_bss(padapter, 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 calle 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)
+{
+	/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("(%s,%d)(%s,%d)\n", */
+	/* 		a->Ssid.Ssid, a->Ssid.SsidLength, b->Ssid.Ssid, b->Ssid.SsidLength)); */
+	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, 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.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_BSS) ==
+			(d_cap & WLAN_CAPABILITY_BSS)));
+
+}
+
+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);
+	plist = get_next(phead);
+
+	while (plist != phead) {
+		found = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (is_same_network(&network->network, &found->network, 0))
+			break;
+
+		plist = get_next(plist);
+	}
+
+	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);
+
+	plist = get_next(phead);
+
+	while (1) {
+
+		if (phead == plist)
+			break;
+
+		pwlan = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		if (pwlan->fixed != true) {
+			if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
+				oldest = pwlan;
+		}
+
+		plist = get_next(plist);
+	}
+	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;
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1
+	if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X(FUNC_ADPT_FMT" %s("MAC_FMT", ch%u) ss_ori:%3u, sq_ori:%3u, rssi_ori:%3ld, ss_smp:%3u, sq_smp:%3u, rssi_smp:%3ld\n"
+			, FUNC_ADPT_ARG(padapter)
+			, src->Ssid.Ssid, MAC_ARG(src->MacAddress), src->Configuration.DSConfig
+			, ss_ori, sq_ori, rssi_ori
+			, ss_smp, sq_smp, rssi_smp
+		);
+	}
+	#endif
+
+	/* 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->PhyInfo.SignalStrength)+(u32)(dst->PhyInfo.SignalStrength)*4)/5;
+			sq_final = ((u32)(src->PhyInfo.SignalQuality)+(u32)(dst->PhyInfo.SignalQuality)*4)/5;
+			rssi_final = (src->Rssi+dst->Rssi*4)/5;
+		} else {
+			/* bss info not receving 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;
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) && 1
+	if (strcmp(dst->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X(FUNC_ADPT_FMT" %s("MAC_FMT"), SignalStrength:%u, SignalQuality:%u, RawRSSI:%ld\n"
+			, FUNC_ADPT_ARG(padapter)
+			, dst->Ssid.Ssid, MAC_ARG(dst->MacAddress), dst->PhyInfo.SignalStrength, dst->PhyInfo.SignalQuality, dst->Rssi);
+	}
+	#endif
+}
+
+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))) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"Same Network\n"); */
+
+		/* if (pmlmepriv->cur_network.network.IELength<= pnetwork->IELength) */
+		{
+			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.IELength);
+		}
+	}
+}
+
+
+/*
+
+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);
+	plist = get_next(phead);
+
+	while (1) {
+		if (phead == plist)
+			break;
+
+		pnetwork = LIST_CONTAINOR(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 netowrk in the same ess as oldest if it's new enough*/
+		}
+
+		if (oldest == NULL || time_after(oldest->last_scanned, pnetwork->last_scanned))
+			oldest = pnetwork;
+
+		plist = get_next(plist);
+
+	}
+
+
+	/* 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 == NULL) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
+				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 receving 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 == NULL) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n\nsomething wrong here\n\n\n"));
+				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 receving 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 scaned network is a bcn frame. */
+		if ((pnetwork->network.IELength > target->IELength) && (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.IELength-_FIXED_IE_LENGTH_, NULL, &wps_ielen) != NULL)
+			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)) {
+		DBG_871X("desired_encmode: %d, privacy: %d\n", desired_encmode, privacy);
+		bselected = false;
+	}
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
+		if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+			bselected = false;
+	}
+
+
+	return bselected;
+}
+
+/* TODO: Perry : For Power Management */
+void rtw_atimdone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("receive atimdone_evet\n"));
+}
+
+
+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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_survey_event_callback, ssid =%s\n",  pnetwork->Ssid.Ssid));
+
+	len = get_wlan_bssid_ex_sz(pnetwork);
+	if (len > (sizeof(struct wlan_bssid_ex))) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n ****rtw_survey_event_callback: return a wrong bss ***\n"));
+		return;
+	}
+
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	/*  update IBSS_network 's timestamp */
+	if ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) {
+		/* RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,"rtw_survey_event_callback : WIFI_ADHOC_MASTER_STATE\n\n"); */
+		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)) == false) {
+		if (pnetwork->Ssid.Ssid[0] == 0) {
+			pnetwork->Ssid.SsidLength = 0;
+		}
+		rtw_add_network(adapter, pnetwork);
+	}
+
+exit:
+
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	return;
+}
+
+
+
+void rtw_surveydone_event_callback(struct adapter	*adapter, u8 *pbuf)
+{
+	u8 timer_cancelled = false;
+	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;
+	}
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_surveydone_event_callback: fw_state:%x\n\n", get_fwstate(pmlmepriv)));
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		/* u8 timer_cancelled; */
+
+		timer_cancelled = true;
+		/* _cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled); */
+
+		_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+	} else {
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("nic status =%x, survey done event comes too late!\n", get_fwstate(pmlmepriv)));
+	}
+	spin_unlock_bh(&pmlmepriv->lock);
+
+	if (timer_cancelled)
+		_cancel_timer(&pmlmepriv->scan_to_timer, &timer_cancelled);
+
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	rtw_set_signal_stat_timer(&adapter->recvpriv);
+
+	if (pmlmepriv->to_join == true) {
+		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{
+					struct wlan_bssid_ex    *pdev_network = &(adapter->registrypriv.dev_network);
+					u8 *pibss = adapter->registrypriv.dev_network.MacAddress;
+
+					/* pmlmepriv->fw_state ^= _FW_UNDER_SURVEY;because don't set assoc_timer */
+					_clr_fwstate_(pmlmepriv, _FW_UNDER_SURVEY);
+
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("switching to adhoc master\n"));
+
+					memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+					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;
+
+					if (rtw_createbss_cmd(adapter) != _SUCCESS) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error =>rtw_createbss_cmd status FAIL\n"));
+					}
+
+					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 (_SUCCESS == s_ret) {
+			     _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{
+				DBG_871X("try_to_join, but select scanning queue fail, to_roam:%d\n", rtw_to_roam(adapter));
+
+				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);
+#ifdef CONFIG_INTEL_WIDI
+						if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+							memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+							intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+							DBG_871X("change to widi listen\n");
+						}
+#endif /*  CONFIG_INTEL_WIDI */
+						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.MacAddress
+						, WLAN_REASON_ACTIVE_ROAM);
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("scan complete in %dms\n", jiffies_to_msecs(jiffies - pmlmepriv->scan_start_time)); */
+
+	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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+free_scanqueue\n"));
+	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.MacAddress);
+	if (pwlan)
+		pwlan->fixed = false;
+	else
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_free_assoc_resources : pwlan == NULL\n\n"));
+
+
+	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	sta_priv *pstapriv = &adapter->stapriv;
+	struct dvobj_priv *psdpriv = adapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_free_assoc_resources\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("tgt_network->network.MacAddress ="MAC_FMT" ssid =%s\n",
+		MAC_ARG(tgt_network->network.MacAddress), tgt_network->network.Ssid.Ssid));
+
+	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);
+		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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_connect\n"));
+
+	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);
+#ifdef CONFIG_INTEL_WIDI
+	if (padapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+		memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+		intel_widi_wk_cmd(padapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+		DBG_871X("change to widi listen\n");
+	}
+#endif /*  CONFIG_INTEL_WIDI */
+
+	rtw_set_scan_deny(padapter, 3000);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("-rtw_indicate_connect: fw_state = 0x%08x\n", get_fwstate(pmlmepriv)));
+}
+
+/*
+*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
+*/
+void rtw_indicate_disconnect(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_indicate_disconnect\n"));
+
+	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
+
+	/* DBG_871X("clear wps when %s\n", __func__); */
+
+	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)
+{
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	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))) {
+		struct pwrctrl_priv *pwrpriv;
+
+		pwrpriv = adapter_to_pwrctl(padapter);
+		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;
+
+		DBG_871X(FUNC_NDEV_FMT"fw_state = _FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		msleep(20);
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY)) {
+		if (!adapter->bDriverStopped && !adapter->bSurpriseRemoved)
+			DBG_871X(FUNC_NDEV_FMT"waiting for scan_abort time out!\n", FUNC_NDEV_ARG(adapter->pnetdev));
+		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.MacAddress);
+	if (psta == NULL) {
+		psta = rtw_alloc_stainfo(pstapriv, pnetwork->network.MacAddress);
+	}
+
+	if (psta) { /* update ptarget_sta */
+
+		DBG_871X("%s\n", __func__);
+
+		psta->aid  = pnetwork->join_res;
+
+		update_sta_info(padapter, psta);
+
+		/* update station supportRate */
+		psta->bssratelen = rtw_get_rateset_len(pnetwork->network.SupportedRates);
+		memcpy(psta->bssrateset, pnetwork->network.SupportedRates, psta->bssratelen);
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+
+		psta->wireless_mode = pmlmeext->cur_wireless_mode;
+		psta->raid = rtw_hal_networktype_to_raid(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 tramsmitted. */
+		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, reseting  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;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			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;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq);
+				#endif
+				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);
+
+	DBG_871X("%s\n", __func__);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\nfw_state:%x, BSSID:"MAC_FMT"\n"
+		, get_fwstate(pmlmepriv), MAC_ARG(pnetwork->network.MacAddress)));
+
+
+	/*  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);
+	#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		DBG_871X(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+			"\n"
+			, FUNC_ADPT_ARG(padapter)
+			, padapter->recvpriv.signal_strength
+			, padapter->recvpriv.rssi
+			, padapter->recvpriv.signal_qual
+	);
+	#endif
+
+	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;
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Invalid network_mode\n"));
+			break;
+	}
+
+	rtw_update_protection(padapter, (cur_network->network.IEs) + sizeof(struct ndis_802_11_fix_ie),
+									(cur_network->network.IELength));
+
+	rtw_update_ht_cap(padapter, cur_network->network.IEs, cur_network->network.IELength, (u8) cur_network->network.Configuration.DSConfig);
+}
+
+/* Notes: the fucntion 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 retry = 0;
+	u8 timer_cancelled;
+	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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("joinbss event call back received with res =%d\n", pnetwork->join_res));
+
+	rtw_get_encrypt_decrypt_from_registrypriv(adapter);
+
+
+	if (pmlmepriv->assoc_ssid.SsidLength == 0) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@   joinbss event call back  for Any SSid\n"));
+	} else{
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("@@@@@   rtw_joinbss_event_callback for SSid:%s\n", pmlmepriv->assoc_ssid.Ssid));
+	}
+
+	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)) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n\n ***joinbss_evt_callback return a wrong bss ***\n\n"));
+		return;
+	}
+
+	spin_lock_bh(&pmlmepriv->lock);
+
+	pmlmepriv->LinkDetectInfo.TrafficTransitionCount = 0;
+	pmlmepriv->LinkDetectInfo.LowPowerTransitionCount = 0;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("\n rtw_joinbss_event_callback !! spin_lock_irqsave\n"));
+
+	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 == true) {
+					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)
+						rtw_free_stainfo(adapter,  pcur_sta);
+
+					ptarget_wlan = rtw_find_network(&pmlmepriv->scanned_queue, pnetwork->network.MacAddress);
+					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{
+				DBG_871X_LEVEL(_drv_always_, "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 == NULL) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't update stainfo when joinbss_event callback\n"));
+					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);
+			} else{
+				/* adhoc mode will rtw_indicate_connect when rtw_stassoc_event_callback */
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("adhoc mode, fw_state:%x", get_fwstate(pmlmepriv)));
+			}
+
+
+			/* s5. Cancle assoc_timer */
+			_cancel_timer(&pmlmepriv->assoc_timer, &timer_cancelled);
+
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("Cancle assoc_timer\n"));
+
+		} else{
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_joinbss_event_callback err: fw_state:%x", get_fwstate(pmlmepriv)));
+			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);
+
+		/* rtw_free_assoc_resources(adapter, 1); */
+
+		if ((check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) == true) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("fail! clear _FW_UNDER_LINKING ^^^fw_state =%x\n", get_fwstate(pmlmepriv)));
+			_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);
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("rtw_select_and_join_from_scanned_queue again! res:%d\n", res));
+		}
+
+		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{
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Set Assoc_Timer = 1; can't find match ssid in scanned_q\n"));
+		#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 == NULL)
+		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);
+
+#ifndef CONFIG_AUTO_AP_MODE
+
+			ap_sta_info_defer_update(adapter, psta);
+
+			/* report to upper layer */
+			DBG_871X("indicate_sta_assoc_event to upper layer - hostapd\n");
+			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);
+			}
+#endif /* CONFIG_AUTO_AP_MODE */
+		}
+		return;
+	}
+
+	/* for AD-HOC mode */
+	psta = rtw_get_stainfo(&adapter->stapriv, pstassoc->macaddr);
+	if (psta != NULL) {
+		/* the sta have been in sta_info_queue => do nothing */
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Error: rtw_stassoc_event_callback: sta has been in sta_hash_queue\n"));
+
+		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 == NULL) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Can't alloc sta_info when rtw_stassoc_event_callback\n"));
+		return;
+	}
+
+	/* to do : init sta_info variable */
+	psta->qos_option = 0;
+	psta->mac_id = (uint)pstassoc->cam_id;
+	/* psta->aid = (uint)pstassoc->cam_id; */
+	DBG_871X("%s\n", __func__);
+	/* 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.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 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;
+
+	DBG_871X("%s(mac_id =%d) =" MAC_FMT "\n", __func__, mac_id, MAC_ARG(pstadel->macaddr));
+
+	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;
+		}
+#ifdef CONFIG_INTEL_WIDI
+		else if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_CONNECTED) {
+			roam = true;
+		}
+#endif /*  CONFIG_INTEL_WIDI */
+
+		if (roam == true) {
+			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.MacAddress);
+		if (pwlan) {
+			pwlan->fixed = false;
+			rtw_free_network_nolock(adapter, pwlan);
+		}
+		spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+#ifdef CONFIG_INTEL_WIDI
+		if (!rtw_to_roam(adapter))
+			process_intel_widi_disconnect(adapter, 1);
+#endif /*  CONFIG_INTEL_WIDI */
+
+		_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) */
+			/* 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.MacAddress);
+			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.MacAddress;
+
+			memcpy(pdev_network, &tgt_network->network, get_wlan_bssid_ex_sz(&tgt_network->network));
+
+			memset(&pdev_network->Ssid, 0, sizeof(struct ndis_802_11_ssid));
+			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);
+			}
+
+			if (rtw_createbss_cmd(adapter) != _SUCCESS) {
+
+				RT_TRACE(_module_rtl871x_ioctl_set_c_, _drv_err_, ("***Error =>stadel_event_callback: rtw_createbss_cmd status FAIL***\n "));
+
+			}
+
+
+		}
+
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+void rtw_cpwm_event_callback(struct adapter *padapter, u8 *pbuf)
+{
+	struct reportpwrstate_parm *preportpwrstate;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("+rtw_cpwm_event_callback !!!\n"));
+	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/faliure handler for CMD JoinBss
+* @adapter: pointer to struct adapter structure
+*/
+void _rtw_join_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	DBG_871X("%s, fw_state =%x\n", __func__, get_fwstate(pmlmepriv));
+
+	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;
+				DBG_871X("%s try another roaming\n", __func__);
+				do_join_r = rtw_do_join(adapter);
+				if (_SUCCESS != do_join_r) {
+					DBG_871X("%s roaming do_join return %d\n", __func__, do_join_r);
+					continue;
+				}
+				break;
+			} else {
+#ifdef CONFIG_INTEL_WIDI
+				if (adapter->mlmepriv.widi_state == INTEL_WIDI_STATE_ROAMING) {
+					memset(pmlmepriv->sa_ext, 0x00, L2SDTA_SERVICE_VE_LEN);
+					intel_widi_wk_cmd(adapter, INTEL_WIDI_LISTEN_WK, NULL, 0);
+					DBG_871X("change to widi listen\n");
+				}
+#endif /*  CONFIG_INTEL_WIDI */
+				DBG_871X("%s We've try roaming but fail\n", __func__);
+				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/Faliure handler for CMD SiteSurvey
+* @adapter: pointer to struct adapter structure
+*/
+void rtw_scan_timeout_handler (struct adapter *adapter)
+{
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+
+	DBG_871X(FUNC_ADPT_FMT" fw_state =%x\n", FUNC_ADPT_ARG(adapter), get_fwstate(pmlmepriv));
+
+	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 */
+
+	return;
+}
+
+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) {
+				DBG_871X(FUNC_ADPT_FMT" _FW_UNDER_SURVEY|_FW_UNDER_LINKING\n", FUNC_ADPT_ARG(padapter));
+				goto exit;
+			}
+
+			if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true) {
+				DBG_871X(FUNC_ADPT_FMT" exit BusyTraffic\n", FUNC_ADPT_ARG(padapter));
+				goto exit;
+			}
+		}
+
+		DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+		rtw_set_802_11_bssid_list_scan(padapter, NULL, 0);
+	}
+
+exit:
+	return;
+}
+
+void rtw_dynamic_check_timer_handlder(struct adapter *adapter)
+{
+	if (!adapter)
+		return;
+
+	if (adapter->hw_init_completed == false)
+		return;
+
+	if ((adapter->bDriverStopped == true) || (adapter->bSurpriseRemoved == true))
+		return;
+
+	if (adapter->net_closed == true)
+		return;
+
+	if (is_primary_adapter(adapter))
+		DBG_871X("IsBtDisabled =%d, IsBtControlLps =%d\n", rtw_btcoex_IsBtDisabled(adapter), rtw_btcoex_IsBtControlLps(adapter));
+
+	if ((adapter_to_pwrctl(adapter)->bFwCurrentInPSMode == true)
+		&& (rtw_btcoex_IsBtControlLps(adapter) == false)
+		) {
+		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);
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+}
+
+void rtw_set_scan_deny_timer_hdl(struct adapter *adapter)
+{
+	rtw_clear_scan_deny(adapter);
+}
+
+void rtw_set_scan_deny(struct adapter *adapter, u32 ms)
+{
+	struct mlme_priv *mlmepriv = &adapter->mlmepriv;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	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;
+
+	DBG_871X("roam candidate:%s %s("MAC_FMT", ch%3u) rssi:%d, age:%5d\n",
+		(competitor == mlme->cur_network_scanned)?"*":" ",
+		competitor->network.Ssid.Ssid,
+		MAC_ARG(competitor->network.MacAddress),
+		competitor->network.Configuration.DSConfig,
+		(int)competitor->network.Rssi,
+		jiffies_to_msecs(jiffies - competitor->last_scanned)
+	);
+
+	/* got specific addr to roam */
+	if (!is_zero_mac_addr(mlme->roam_tgt_addr)) {
+		if (!memcmp(mlme->roam_tgt_addr, competitor->network.MacAddress, 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 != NULL && (*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 adapter *adapter;
+	struct __queue	*queue	= &(mlme->scanned_queue);
+	struct	wlan_network	*pnetwork = NULL;
+	struct	wlan_network	*candidate = NULL;
+
+	if (mlme->cur_network_scanned == NULL) {
+		rtw_warn_on(1);
+		return ret;
+	}
+
+	spin_lock_bh(&(mlme->scanned_queue.lock));
+	phead = get_list_head(queue);
+	adapter = (struct adapter *)mlme->nic_hdl;
+
+	mlme->pscanned = get_next(phead);
+
+	while (phead != mlme->pscanned) {
+
+		pnetwork = LIST_CONTAINOR(mlme->pscanned, struct wlan_network, list);
+		if (pnetwork == NULL) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__));
+			ret = _FAIL;
+			goto exit;
+		}
+
+		mlme->pscanned = get_next(mlme->pscanned);
+
+		DBG_871X("%s("MAC_FMT", ch%u) rssi:%d\n"
+			, pnetwork->network.Ssid.Ssid
+			, MAC_ARG(pnetwork->network.MacAddress)
+			, pnetwork->network.Configuration.DSConfig
+			, (int)pnetwork->network.Rssi);
+
+		rtw_check_roaming_candidate(mlme, &candidate, pnetwork);
+
+	}
+
+	if (candidate == NULL) {
+		DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__);
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
+			candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress),
+			candidate->network.Configuration.DSConfig);
+
+		mlme->roam_network = candidate;
+
+		if (!memcmp(candidate->network.MacAddress, mlme->roam_tgt_addr, ETH_ALEN))
+			memset(mlme->roam_tgt_addr, 0, ETH_ALEN);
+	}
+
+	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 == true) {
+		if (memcmp(competitor->network.MacAddress, mlme->assoc_bssid, ETH_ALEN))
+			goto exit;
+	}
+
+	/* check ssid, if needed */
+	if (mlme->assoc_ssid.Ssid[0] && mlme->assoc_ssid.SsidLength) {
+		if (competitor->network.Ssid.SsidLength != mlme->assoc_ssid.SsidLength
+			|| memcmp(competitor->network.Ssid.Ssid, mlme->assoc_ssid.Ssid, mlme->assoc_ssid.SsidLength)
+		)
+			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 == NULL || (*candidate)->network.Rssi < competitor->network.Rssi) {
+		*candidate = competitor;
+		updated = true;
+	}
+
+	if (updated) {
+		DBG_871X("[by_bssid:%u][assoc_ssid:%s]"
+			"[to_roam:%u] "
+			"new candidate: %s("MAC_FMT", ch%u) rssi:%d\n",
+			mlme->assoc_by_bssid,
+			mlme->assoc_ssid.Ssid,
+			rtw_to_roam(adapter),
+			(*candidate)->network.Ssid.Ssid,
+			MAC_ARG((*candidate)->network.MacAddress),
+			(*candidate)->network.Configuration.DSConfig,
+			(int)(*candidate)->network.Rssi
+		);
+	}
+
+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);
+	pmlmepriv->pscanned = get_next(phead);
+
+	while (phead != pmlmepriv->pscanned) {
+
+		pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+		if (pnetwork == NULL) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("%s return _FAIL:(pnetwork == NULL)\n", __func__));
+			ret = _FAIL;
+			goto exit;
+		}
+
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		DBG_871X("%s("MAC_FMT", ch%u) rssi:%d\n"
+			, pnetwork->network.Ssid.Ssid
+			, MAC_ARG(pnetwork->network.MacAddress)
+			, pnetwork->network.Configuration.DSConfig
+			, (int)pnetwork->network.Rssi);
+
+		rtw_check_join_candidate(pmlmepriv, &candidate, pnetwork);
+
+	}
+
+	if (candidate == NULL) {
+		DBG_871X("%s: return _FAIL(candidate == NULL)\n", __func__);
+#ifdef CONFIG_WOWLAN
+		_clr_fwstate_(pmlmepriv, _FW_LINKED|_FW_UNDER_LINKING);
+#endif
+		ret = _FAIL;
+		goto exit;
+	} else {
+		DBG_871X("%s: candidate: %s("MAC_FMT", ch:%u)\n", __func__,
+			candidate->network.Ssid.Ssid, MAC_ARG(candidate->network.MacAddress),
+			candidate->network.Configuration.DSConfig);
+		goto candidate_exist;
+	}
+
+candidate_exist:
+
+	/*  check for situation of  _FW_LINKED */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		DBG_871X("%s: _FW_LINKED while ask_for_joinbss!!!\n", __func__);
+
+		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;
+}
+
+sint rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv)
+{
+	struct	cmd_obj *pcmd;
+	struct	setauth_parm *psetauthparm;
+	struct	cmd_priv *pcmdpriv = &(adapter->cmdpriv);
+	sint		res = _SUCCESS;
+
+	pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+	if (pcmd == NULL) {
+		res = _FAIL;  /* try again */
+		goto exit;
+	}
+
+	psetauthparm = (struct setauth_parm *)rtw_zmalloc(sizeof(struct setauth_parm));
+	if (psetauthparm == NULL) {
+		kfree((unsigned char *)pcmd);
+		res = _FAIL;
+		goto exit;
+	}
+
+	memset(psetauthparm, 0, sizeof(struct setauth_parm));
+	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);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("after enqueue set_auth_cmd, auth_mode =%x\n", psecuritypriv->dot11AuthAlgrthm));
+
+	res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+
+exit:
+	return res;
+}
+
+sint rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, sint keyid, u8 set_tx, bool enqueue)
+{
+	u8 keylen;
+	struct cmd_obj		*pcmd;
+	struct setkey_parm	*psetkeyparm;
+	struct cmd_priv 	*pcmdpriv = &(adapter->cmdpriv);
+	sint	res = _SUCCESS;
+
+	psetkeyparm = (struct setkey_parm *)rtw_zmalloc(sizeof(struct setkey_parm));
+	if (psetkeyparm == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	memset(psetkeyparm, 0, sizeof(struct setkey_parm));
+
+	if (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) {
+		psetkeyparm->algorithm = (unsigned char)psecuritypriv->dot118021XGrpPrivacy;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(unsigned char)psecuritypriv->dot118021XGrpPrivacy =%d\n", psetkeyparm->algorithm));
+	} else {
+		psetkeyparm->algorithm = (u8)psecuritypriv->dot11PrivacyAlgrthm;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =(u8)psecuritypriv->dot11PrivacyAlgrthm =%d\n", psetkeyparm->algorithm));
+
+	}
+	psetkeyparm->keyid = (u8)keyid;/* 0~3 */
+	psetkeyparm->set_tx = set_tx;
+	if (is_wep_enc(psetkeyparm->algorithm))
+		adapter->securitypriv.key_mask |= BIT(psetkeyparm->keyid);
+
+	DBG_871X("==> rtw_set_key algorithm(%x), keyid(%x), key_mask(%x)\n", psetkeyparm->algorithm, psetkeyparm->keyid, adapter->securitypriv.key_mask);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key: psetkeyparm->algorithm =%d psetkeyparm->keyid =(u8)keyid =%d\n", psetkeyparm->algorithm, 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:
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("\n rtw_set_key:psecuritypriv->dot11PrivacyAlgrthm = %x (must be 1 or 2 or 4 or 5)\n", psecuritypriv->dot11PrivacyAlgrthm));
+		res = _FAIL;
+		kfree((unsigned char *)psetkeyparm);
+		goto exit;
+	}
+
+
+	if (enqueue) {
+		pcmd = (struct	cmd_obj *)rtw_zmalloc(sizeof(struct	cmd_obj));
+		if (pcmd == NULL) {
+			kfree((unsigned char *)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);
+
+		/* sema_init(&(pcmd->cmd_sem), 0); */
+
+		res = rtw_enqueue_cmd(pcmdpriv, pcmd);
+	} else{
+		setkey_hdl(adapter, (u8 *)psetkeyparm);
+		kfree((u8 *) 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 *psecuritypriv = &Adapter->securitypriv;
+	int i = 0;
+
+	do {
+		if ((psecuritypriv->PMKIDList[i].bUsed) &&
+				(!memcmp(psecuritypriv->PMKIDList[i].Bssid, bssid, ETH_ALEN))) {
+			break;
+		} else{
+			i++;
+			/* continue; */
+		}
+
+	} while (i < NUM_PMKID_CACHE);
+
+	if (i == NUM_PMKID_CACHE) {
+		i = -1;/*  Could not find. */
+	} else {
+		/*  There is one Pre-Authentication Key for the specific BSSID. */
+	}
+
+	return i;
+
+}
+
+/*  */
+/*  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;
+}
+
+sint 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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+		 ("+rtw_restruct_sec_ie: ndisauthmode =%d ndissecuritytype =%d\n",
+		  ndisauthmode, ndissecuritytype));
+
+	/* 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);
+		/* 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 == _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_conf);
+	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;
+	/* 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->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;
+	case WIRELESS_11A:
+	case WIRELESS_11A_5N:
+		pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+		break;
+	case WIRELESS_11ABGN:
+		if (pregistrypriv->channel > 14)
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM5);
+		else
+			pdev_network->NetworkTypeInUse = (Ndis802_11OFDM24);
+		break;
+	default:
+		/*  TODO */
+		break;
+	}
+
+	pdev_network->Configuration.DSConfig = (pregistrypriv->channel);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("pregistrypriv->channel =%d, pdev_network->Configuration.DSConfig = 0x%x\n", pregistrypriv->channel, pdev_network->Configuration.DSConfig));
+
+	if (cur_network->network.InfrastructureMode == Ndis802_11IBSS)
+		pdev_network->Configuration.ATIMWindow = (0);
+
+	pdev_network->InfrastructureMode = (cur_network->network.InfrastructureMode);
+
+	/*  1. Supported rates */
+	/*  2. IE */
+
+	/* rtw_set_supported_rate(pdev_network->SupportedRates, pregistrypriv->wireless_mode) ;  will be called in rtw_generate_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); */
+}
+
+void rtw_get_encrypt_decrypt_from_registrypriv(struct adapter *adapter)
+{
+}
+
+/* the fucntion 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);
+	}
+	if (phtpriv->ldpc_cap)
+		DBG_871X("[HT] Support LDPC = 0x%02X\n", phtpriv->ldpc_cap);
+
+	/*  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);
+	}
+	if (phtpriv->stbc_cap)
+		DBG_871X("[HT] Support STBC = 0x%02X\n", phtpriv->stbc_cap);
+
+	/*  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);
+		DBG_871X("[HT] Support Beamformer\n");
+	}
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT5) && bHwSupportBeamformee) {
+		SET_FLAG(phtpriv->beamform_cap, BEAMFORMING_HT_BEAMFORMEE_ENABLE);
+		DBG_871X("[HT] Support Beamformee\n");
+	}
+}
+
+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;
+	u8 *pframe;
+
+	if (padapter->mlmepriv.qospriv.qos_option == 0) {
+		out_len = *pout_len;
+		pframe = rtw_set_ie(out_ie+out_len, _VENDOR_SPECIFIC_IE_,
+							_WMM_IE_Length_, WMM_IE, pout_len);
+
+		padapter->mlmepriv.qospriv.qos_option = 1;
+	}
+}
+
+/* the fucntion 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 HT_CAP_AMPDU_FACTOR max_rx_ampdu_factor;
+	unsigned char *p, *pframe;
+	struct rtw_ieee80211_ht_cap ht_capie;
+	u8 cbw40_enable = 0, stbc_rx_enable = 0, rf_type = 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 rtw_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 == NULL) {
+		/* 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, _HT_ADD_INFO_IE_, &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;
+			DBG_871X("declare supporting RX STBC\n");
+		}
+	}
+
+	/* fill default supported_mcs_set */
+	memcpy(ht_capie.supp_mcs_set, pmlmeext->default_supported_mcs_set, 16);
+
+	/* update default supported_mcs_set */
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	switch (rf_type) {
+	case RF_1T1R:
+		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.supp_mcs_set, MCS_RATE_1R);
+		break;
+
+	case RF_2T2R:
+	case RF_1T2R:
+	default:
+		if (stbc_rx_enable)
+			ht_capie.cap_info |= cpu_to_le16(IEEE80211_HT_CAP_RX_STBC_2R);/* RX STBC two spatial stream */
+
+		#ifdef CONFIG_DISABLE_MCS13TO15
+		if (((cbw40_enable == 1) && (operation_bw == CHANNEL_WIDTH_40)) && (pregistrypriv->wifi_spec != 1))
+				set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R_13TO15_OFF);
+		else
+				set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R);
+		#else /* CONFIG_DISABLE_MCS13TO15 */
+			set_mcs_rate_by_mask(ht_capie.supp_mcs_set, MCS_RATE_2R);
+		#endif /* CONFIG_DISABLE_MCS13TO15 */
+		break;
+	}
+
+	{
+		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 HT_CAP_AMPDU_FACTOR)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);
+
+	pframe = rtw_set_ie(out_ie+out_len, _HT_CAPABILITY_IE_,
+						sizeof(struct rtw_ieee80211_ht_cap), (unsigned char *)&ht_capie, pout_len);
+
+	phtpriv->ht_option = true;
+
+	if (in_ie != NULL) {
+		p = rtw_get_ie(in_ie, _HT_ADD_INFO_IE_, &ielen, in_len);
+		if (p && (ielen == sizeof(struct ieee80211_ht_addt_info))) {
+			out_len = *pout_len;
+			pframe = rtw_set_ie(out_ie+out_len, _HT_ADD_INFO_IE_, ielen, p+2, pout_len);
+		}
+	}
+
+	return phtpriv->ht_option;
+
+}
+
+/* the fucntion 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 rtw_ieee80211_ht_cap *pht_capie;
+	struct ieee80211_ht_addt_info *pht_addtinfo;
+	/* 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;
+
+	DBG_871X("+rtw_update_ht_cap()\n");
+
+	/* maybe needs check if ap supports rx ampdu. */
+	if ((phtpriv->ampdu_enable == false) && (pregistrypriv->ampdu_enable == 1)) {
+		if (pregistrypriv->wifi_spec == 1) {
+			/* remove this part because testbed AP should disable RX AMPDU */
+			/* phtpriv->ampdu_enable = false; */
+			phtpriv->ampdu_enable = true;
+		} else {
+			phtpriv->ampdu_enable = true;
+		}
+	} else if (pregistrypriv->ampdu_enable == 2) {
+		/* remove this part because testbed AP should disable RX AMPDU */
+		/* phtpriv->ampdu_enable = true; */
+	}
+
+
+	/* check Max Rx A-MPDU Size */
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_CAPABILITY_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		pht_capie = (struct rtw_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); */
+
+		/* DBG_871X("rtw_update_ht_cap(): max_ampdu_sz =%d\n", max_ampdu_sz); */
+		phtpriv->rx_ampdu_maxlen = max_ampdu_sz;
+
+	}
+
+
+	len = 0;
+	p = rtw_get_ie(pie+sizeof(struct ndis_802_11_fix_ie), _HT_ADD_INFO_IE_, &len, ie_len-sizeof(struct ndis_802_11_fix_ie));
+	if (p && len > 0) {
+		pht_addtinfo = (struct ieee80211_ht_addt_info *)(p+2);
+		/* 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;
+		u8 rf_type;
+
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+		/* 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 */
+		switch (rf_type) {
+		case RF_1T1R:
+		case RF_1T2R:
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+			break;
+		case RF_2T2R:
+		default:
+#ifdef CONFIG_DISABLE_MCS13TO15
+			if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1)
+				set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF);
+			else
+				set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#else /* CONFIG_DISABLE_MCS13TO15 */
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#endif /* CONFIG_DISABLE_MCS13TO15 */
+		}
+
+		/* 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;
+	if (pmlmeinfo->SM_PS == WLAN_HT_CAP_SM_PS_STATIC)
+		DBG_871X("%s(): WLAN_HT_CAP_SM_PS_STATIC\n", __func__);
+
+	/*  */
+	/*  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;
+	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) {
+		DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return;
+	}
+
+	if (psta == NULL) {
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return;
+	}
+
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) {
+		issued = (phtpriv->agg_enable_bitmap>>priority)&0x1;
+		issued |= (phtpriv->candidate_tid_bitmap>>priority)&0x1;
+
+		if (0 == issued) {
+			DBG_871X("rtw_issue_addbareq_cmd, p =%d\n", priority);
+			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};
+	u8 *pframe;
+
+
+	if (phtpriv->bss_coexist) {
+		SET_EXT_CAPABILITY_ELE_BSS_COEXIST(cap_content, 1);
+	}
+
+	pframe = rtw_set_ie(out_ie + *pout_len, EID_EXTCapability, 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;
+	int do_join_r;
+
+	if (0 < rtw_to_roam(padapter)) {
+		DBG_871X("roaming from %s("MAC_FMT"), length:%d\n",
+				cur_network->network.Ssid.Ssid, MAC_ARG(cur_network->network.MacAddress),
+				cur_network->network.Ssid.SsidLength);
+		memcpy(&pmlmepriv->assoc_ssid, &cur_network->network.Ssid, sizeof(struct ndis_802_11_ssid));
+
+		pmlmepriv->assoc_by_bssid = false;
+
+		while (1) {
+			do_join_r = rtw_do_join(padapter);
+			if (_SUCCESS == do_join_r) {
+				break;
+			} else {
+				DBG_871X("roaming do_join return %d\n", do_join_r);
+				rtw_dec_to_roam(padapter);
+
+				if (rtw_to_roam(padapter) > 0) {
+					continue;
+				} else {
+					DBG_871X("%s(%d) -to roaming fail, indicate_disconnect\n", __func__, __LINE__);
+					rtw_indicate_disconnect(padapter);
+					break;
+				}
+			}
+		}
+	}
+
+}
+
+sint 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 000000000000..17d881d66910
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
@@ -0,0 +1,6941 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_MLME_EXT_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_wifi_regd.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_VHT, "ACTION_VHT", &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 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 RT_CHANNEL_PLAN_5G	RTW_ChannelPlan5G[RT_CHANNEL_DOMAIN_5G_MAX] = {
+	{{}, 0},																					/*  0x00, RT_CHANNEL_DOMAIN_5G_NULL */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19},						/*  0x01, RT_CHANNEL_DOMAIN_5G_ETSI1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},	/*  0x02, RT_CHANNEL_DOMAIN_5G_ETSI2 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 149, 153, 157, 161, 165}, 22},			/*  0x03, RT_CHANNEL_DOMAIN_5G_ETSI3 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165}, 24},	/*  0x04, RT_CHANNEL_DOMAIN_5G_FCC1 */
+	{{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9},														/*  0x05, RT_CHANNEL_DOMAIN_5G_FCC2 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13},											/*  0x06, RT_CHANNEL_DOMAIN_5G_FCC3 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161}, 12},												/*  0x07, RT_CHANNEL_DOMAIN_5G_FCC4 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x08, RT_CHANNEL_DOMAIN_5G_FCC5 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x09, RT_CHANNEL_DOMAIN_5G_FCC6 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20},					/*  0x0A, RT_CHANNEL_DOMAIN_5G_FCC7_IC1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 149, 153, 157, 161, 165}, 20},					/*  0x0B, RT_CHANNEL_DOMAIN_5G_KCC1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 19},						/*  0x0C, RT_CHANNEL_DOMAIN_5G_MKK1 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x0D, RT_CHANNEL_DOMAIN_5G_MKK2 */
+	{{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11},											/*  0x0E, RT_CHANNEL_DOMAIN_5G_MKK3 */
+	{{56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 15},								/*  0x0F, RT_CHANNEL_DOMAIN_5G_NCC1 */
+	{{56, 60, 64, 149, 153, 157, 161, 165}, 8},															/*  0x10, RT_CHANNEL_DOMAIN_5G_NCC2 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x11, RT_CHANNEL_DOMAIN_5G_NCC3 */
+	{{36, 40, 44, 48}, 4},																			/*  0x12, RT_CHANNEL_DOMAIN_5G_ETSI4 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 136, 140, 149, 153, 157, 161, 165}, 20},					/*  0x13, RT_CHANNEL_DOMAIN_5G_ETSI5 */
+	{{149, 153, 157, 161}, 4},																		/*  0x14, RT_CHANNEL_DOMAIN_5G_FCC8 */
+	{{36, 40, 44, 48, 52, 56, 60, 64}, 8},																/*  0x15, RT_CHANNEL_DOMAIN_5G_ETSI6 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 13},											/*  0x16, RT_CHANNEL_DOMAIN_5G_ETSI7 */
+	{{36, 40, 44, 48, 149, 153, 157, 161, 165}, 9},														/*  0x17, RT_CHANNEL_DOMAIN_5G_ETSI8 */
+	{{100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140}, 11},											/*  0x18, RT_CHANNEL_DOMAIN_5G_ETSI9 */
+	{{149, 153, 157, 161, 165}, 5},																	/*  0x19, RT_CHANNEL_DOMAIN_5G_ETSI10 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 132, 136, 140, 149, 153, 157, 161, 165}, 16},									/*  0x1A, RT_CHANNEL_DOMAIN_5G_ETSI11 */
+	{{52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17},							/*  0x1B, RT_CHANNEL_DOMAIN_5G_NCC4 */
+	{{149, 153, 157, 161}, 4},																		/*  0x1C, RT_CHANNEL_DOMAIN_5G_ETSI12 */
+	{{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 17},							/*  0x1D, RT_CHANNEL_DOMAIN_5G_FCC9 */
+	{{36, 40, 44, 48, 100, 104, 108, 112, 116, 132, 136, 140}, 12},											/*  0x1E, RT_CHANNEL_DOMAIN_5G_ETSI13 */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161}, 20},					/*  0x1F, RT_CHANNEL_DOMAIN_5G_FCC10 */
+
+	/*  Driver self defined for old channel plan Compatible , Remember to modify if have new channel plan definition ===== */
+	{{36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165}, 21},				/*  0x20, RT_CHANNEL_DOMAIN_5G_FCC */
+	{{36, 40, 44, 48}, 4},																			/*  0x21, RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS */
+	{{36, 40, 44, 48, 149, 153, 157, 161}, 8},															/*  0x22, RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS */
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_ChannelPlanMap[RT_CHANNEL_DOMAIN_MAX] = {
+	/*  0x00 ~ 0x1F , Old Define ===== */
+	{0x02, 0x20},	/* 0x00, RT_CHANNEL_DOMAIN_FCC */
+	{0x02, 0x0A},	/* 0x01, RT_CHANNEL_DOMAIN_IC */
+	{0x01, 0x01},	/* 0x02, RT_CHANNEL_DOMAIN_ETSI */
+	{0x01, 0x00},	/* 0x03, RT_CHANNEL_DOMAIN_SPAIN */
+	{0x01, 0x00},	/* 0x04, RT_CHANNEL_DOMAIN_FRANCE */
+	{0x03, 0x00},	/* 0x05, RT_CHANNEL_DOMAIN_MKK */
+	{0x03, 0x00},	/* 0x06, RT_CHANNEL_DOMAIN_MKK1 */
+	{0x01, 0x09},	/* 0x07, RT_CHANNEL_DOMAIN_ISRAEL */
+	{0x03, 0x09},	/* 0x08, RT_CHANNEL_DOMAIN_TELEC */
+	{0x03, 0x00},	/* 0x09, RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN */
+	{0x00, 0x00},	/* 0x0A, RT_CHANNEL_DOMAIN_WORLD_WIDE_13 */
+	{0x02, 0x0F},	/* 0x0B, RT_CHANNEL_DOMAIN_TAIWAN */
+	{0x01, 0x08},	/* 0x0C, RT_CHANNEL_DOMAIN_CHINA */
+	{0x02, 0x06},	/* 0x0D, RT_CHANNEL_DOMAIN_SINGAPORE_INDIA_MEXICO */
+	{0x02, 0x0B},	/* 0x0E, RT_CHANNEL_DOMAIN_KOREA */
+	{0x02, 0x09},	/* 0x0F, RT_CHANNEL_DOMAIN_TURKEY */
+	{0x01, 0x01},	/* 0x10, RT_CHANNEL_DOMAIN_JAPAN */
+	{0x02, 0x05},	/* 0x11, RT_CHANNEL_DOMAIN_FCC_NO_DFS */
+	{0x01, 0x21},	/* 0x12, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x00, 0x04},	/* 0x13, RT_CHANNEL_DOMAIN_WORLD_WIDE_5G */
+	{0x02, 0x10},	/* 0x14, RT_CHANNEL_DOMAIN_TAIWAN_NO_DFS */
+	{0x00, 0x21},	/* 0x15, RT_CHANNEL_DOMAIN_ETSI_NO_DFS */
+	{0x00, 0x22},	/* 0x16, RT_CHANNEL_DOMAIN_KOREA_NO_DFS */
+	{0x03, 0x21},	/* 0x17, RT_CHANNEL_DOMAIN_JAPAN_NO_DFS */
+	{0x06, 0x08},	/* 0x18, RT_CHANNEL_DOMAIN_PAKISTAN_NO_DFS */
+	{0x02, 0x08},	/* 0x19, RT_CHANNEL_DOMAIN_TAIWAN2_NO_DFS */
+	{0x00, 0x00},	/* 0x1A, */
+	{0x00, 0x00},	/* 0x1B, */
+	{0x00, 0x00},	/* 0x1C, */
+	{0x00, 0x00},	/* 0x1D, */
+	{0x00, 0x00},	/* 0x1E, */
+	{0x06, 0x04},	/* 0x1F, RT_CHANNEL_DOMAIN_WORLD_WIDE_ONLY_5G */
+	/*  0x20 ~ 0x7F , New Define ===== */
+	{0x00, 0x00},	/* 0x20, RT_CHANNEL_DOMAIN_WORLD_NULL */
+	{0x01, 0x00},	/* 0x21, RT_CHANNEL_DOMAIN_ETSI1_NULL */
+	{0x02, 0x00},	/* 0x22, RT_CHANNEL_DOMAIN_FCC1_NULL */
+	{0x03, 0x00},	/* 0x23, RT_CHANNEL_DOMAIN_MKK1_NULL */
+	{0x04, 0x00},	/* 0x24, RT_CHANNEL_DOMAIN_ETSI2_NULL */
+	{0x02, 0x04},	/* 0x25, RT_CHANNEL_DOMAIN_FCC1_FCC1 */
+	{0x00, 0x01},	/* 0x26, RT_CHANNEL_DOMAIN_WORLD_ETSI1 */
+	{0x03, 0x0C},	/* 0x27, RT_CHANNEL_DOMAIN_MKK1_MKK1 */
+	{0x00, 0x0B},	/* 0x28, RT_CHANNEL_DOMAIN_WORLD_KCC1 */
+	{0x00, 0x05},	/* 0x29, RT_CHANNEL_DOMAIN_WORLD_FCC2 */
+	{0x00, 0x00},	/* 0x2A, */
+	{0x00, 0x00},	/* 0x2B, */
+	{0x00, 0x00},	/* 0x2C, */
+	{0x00, 0x00},	/* 0x2D, */
+	{0x00, 0x00},	/* 0x2E, */
+	{0x00, 0x00},	/* 0x2F, */
+	{0x00, 0x06},	/* 0x30, RT_CHANNEL_DOMAIN_WORLD_FCC3 */
+	{0x00, 0x07},	/* 0x31, RT_CHANNEL_DOMAIN_WORLD_FCC4 */
+	{0x00, 0x08},	/* 0x32, RT_CHANNEL_DOMAIN_WORLD_FCC5 */
+	{0x00, 0x09},	/* 0x33, RT_CHANNEL_DOMAIN_WORLD_FCC6 */
+	{0x02, 0x0A},	/* 0x34, RT_CHANNEL_DOMAIN_FCC1_FCC7 */
+	{0x00, 0x02},	/* 0x35, RT_CHANNEL_DOMAIN_WORLD_ETSI2 */
+	{0x00, 0x03},	/* 0x36, RT_CHANNEL_DOMAIN_WORLD_ETSI3 */
+	{0x03, 0x0D},	/* 0x37, RT_CHANNEL_DOMAIN_MKK1_MKK2 */
+	{0x03, 0x0E},	/* 0x38, RT_CHANNEL_DOMAIN_MKK1_MKK3 */
+	{0x02, 0x0F},	/* 0x39, RT_CHANNEL_DOMAIN_FCC1_NCC1 */
+	{0x00, 0x00},	/* 0x3A, */
+	{0x00, 0x00},	/* 0x3B, */
+	{0x00, 0x00},	/* 0x3C, */
+	{0x00, 0x00},	/* 0x3D, */
+	{0x00, 0x00},	/* 0x3E, */
+	{0x00, 0x00},	/* 0x3F, */
+	{0x02, 0x10},	/* 0x40, RT_CHANNEL_DOMAIN_FCC1_NCC2 */
+	{0x05, 0x00},	/* 0x41, RT_CHANNEL_DOMAIN_GLOBAL_NULL */
+	{0x01, 0x12},	/* 0x42, RT_CHANNEL_DOMAIN_ETSI1_ETSI4 */
+	{0x02, 0x05},	/* 0x43, RT_CHANNEL_DOMAIN_FCC1_FCC2 */
+	{0x02, 0x11},	/* 0x44, RT_CHANNEL_DOMAIN_FCC1_NCC3 */
+	{0x00, 0x13},	/* 0x45, RT_CHANNEL_DOMAIN_WORLD_ETSI5 */
+	{0x02, 0x14},	/* 0x46, RT_CHANNEL_DOMAIN_FCC1_FCC8 */
+	{0x00, 0x15},	/* 0x47, RT_CHANNEL_DOMAIN_WORLD_ETSI6 */
+	{0x00, 0x16},	/* 0x48, RT_CHANNEL_DOMAIN_WORLD_ETSI7 */
+	{0x00, 0x17},	/* 0x49, RT_CHANNEL_DOMAIN_WORLD_ETSI8 */
+	{0x00, 0x18},	/* 0x50, RT_CHANNEL_DOMAIN_WORLD_ETSI9 */
+	{0x00, 0x19},	/* 0x51, RT_CHANNEL_DOMAIN_WORLD_ETSI10 */
+	{0x00, 0x1A},	/* 0x52, RT_CHANNEL_DOMAIN_WORLD_ETSI11 */
+	{0x02, 0x1B},	/* 0x53, RT_CHANNEL_DOMAIN_FCC1_NCC4 */
+	{0x00, 0x1C},	/* 0x54, RT_CHANNEL_DOMAIN_WORLD_ETSI12 */
+	{0x02, 0x1D},	/* 0x55, RT_CHANNEL_DOMAIN_FCC1_FCC9 */
+	{0x00, 0x1E},	/* 0x56, RT_CHANNEL_DOMAIN_WORLD_ETSI13 */
+	{0x02, 0x1F},	/* 0x57, RT_CHANNEL_DOMAIN_FCC1_FCC10 */
+};
+
+static RT_CHANNEL_PLAN_MAP	RTW_CHANNEL_PLAN_MAP_REALTEK_DEFINE = {0x03, 0x02}; /* use the conbination for max channel numbers */
+
+/*
+ * 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(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;
+}
+
+/*
+ * Check the @param ch is fit with setband setting of @param adapter
+ * @adapter: the given adapter
+ * @ch: the given channel number
+ *
+ * return true when check valid, false not valid
+ */
+bool rtw_mlme_band_check(struct adapter *adapter, const u32 ch)
+{
+	if (adapter->setband == GHZ24_50 /* 2.4G and 5G */
+		|| (adapter->setband == GHZ_24 && ch < 35) /* 2.4G only */
+		|| (adapter->setband == GHZ_50 && ch > 35) /* 5G only */
+	) {
+		return true;
+	}
+	return false;
+}
+
+/****************************************************************************
+
+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);
+
+	if (pmlmeext->cur_channel > 14)
+		pmlmeext->tx_rate = IEEE80211_OFDM_RATE_6MB;
+	else
+		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(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, 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 },
+		{ 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;
+		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 ((0 == padapter->registrypriv.ht_enable) && (8 == o->inc))
+				continue;
+
+			if ((0 < (padapter->registrypriv.bw_mode & 0xf0)) &&
+				((BW40MINUS == o->bw) || (BW40PLUS == o->bw)))
+				continue;
+
+			if (reg == NULL) {
+				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, RT_CHANNEL_INFO *channel_set)
+{
+	u8 index, chanset_size = 0;
+	u8 b5GBand = false, b2_4GBand = false;
+	u8 Index2G = 0, Index5G = 0;
+
+	memset(channel_set, 0, sizeof(RT_CHANNEL_INFO)*MAX_CHANNEL_NUM);
+
+	if (ChannelPlan >= RT_CHANNEL_DOMAIN_MAX && ChannelPlan != RT_CHANNEL_DOMAIN_REALTEK_DEFINE) {
+		DBG_871X("ChannelPlan ID %x error !!!!!\n", ChannelPlan);
+		return chanset_size;
+	}
+
+	if (IsSupported24G(padapter->registrypriv.wireless_mode)) {
+		b2_4GBand = true;
+		if (RT_CHANNEL_DOMAIN_REALTEK_DEFINE == ChannelPlan)
+			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 ((RT_CHANNEL_DOMAIN_GLOBAL_DOAMIN == ChannelPlan) ||/* Channel 1~11 is active, and 12~14 is passive */
+				(RT_CHANNEL_DOMAIN_GLOBAL_NULL == ChannelPlan)) {
+				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 (RT_CHANNEL_DOMAIN_WORLD_WIDE_13 == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan ||
+				RT_CHANNEL_DOMAIN_2G_WORLD == Index2G) { /*  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++;
+		}
+	}
+
+	if (b5GBand) {
+		for (index = 0; index < RTW_ChannelPlan5G[Index5G].Len; index++) {
+			if (RTW_ChannelPlan5G[Index5G].Channel[index] <= 48
+				|| RTW_ChannelPlan5G[Index5G].Channel[index] >= 149) {
+				channel_set[chanset_size].ChannelNum = RTW_ChannelPlan5G[Index5G].Channel[index];
+				if (RT_CHANNEL_DOMAIN_WORLD_WIDE_5G == ChannelPlan)/* passive scan for all 5G channels */
+					channel_set[chanset_size].ScanType = SCAN_PASSIVE;
+				else
+					channel_set[chanset_size].ScanType = SCAN_ACTIVE;
+				DBG_871X("%s(): channel_set[%d].ChannelNum = %d\n", __func__, chanset_size, channel_set[chanset_size].ChannelNum);
+				chanset_size++;
+			}
+		}
+	}
+
+	DBG_871X("%s ChannelPlan ID %x Chan num:%d \n", __func__, ChannelPlan, chanset_size);
+	return chanset_size;
+}
+
+int	init_mlme_ext_priv(struct adapter *padapter)
+{
+	int	res = _SUCCESS;
+	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);
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((u8 *)pmlmeext, 0, sizeof(struct mlme_ext_priv)); */
+
+	pmlmeext->padapter = padapter;
+
+	/* fill_fwpriv(padapter, &(pmlmeext->fwpriv)); */
+
+	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->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
+
+	return res;
+
+}
+
+void free_mlme_ext_priv(struct mlme_ext_priv *pmlmeext)
+{
+	struct adapter *padapter = pmlmeext->padapter;
+
+	if (!padapter)
+		return;
+
+	if (padapter->bDriverStopped == true) {
+		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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+		 ("+mgt_dispatcher: type(0x%x) subtype(0x%x)\n",
+		  GetFrameType(pframe), GetFrameSubType(pframe)));
+
+	if (GetFrameType(pframe) != WIFI_MGT_TYPE) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("mgt_dispatcher: type(0x%x) error!\n", GetFrameType(pframe)));
+		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 >= (sizeof(mlme_sta_tbl) / sizeof(struct mlme_handler))) {
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("Currently we do not support reserved sub-fr-type =%d\n", index));
+		return;
+	}
+	ptable += index;
+
+	if (psta != NULL) {
+		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++;
+				DBG_871X("Drop duplicate management frame with seq_num = %d.\n", precv_frame->u.hdr.attrib.seq_num);
+				return;
+			}
+		}
+		psta->RxMgmtFrameSeqNum = precv_frame->u.hdr.attrib.seq_num;
+	}
+
+	switch (GetFrameSubType(pframe)) {
+	case WIFI_AUTH:
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+			ptable->func = &OnAuth;
+		else
+			ptable->func = &OnAuthClient;
+		/* pass through */
+	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;
+	}
+
+
+	/* DBG_871X("+OnProbeReq\n"); */
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+			pmlmepriv->cur_network.join_res == true) {
+		struct sta_info *psta;
+		u8 *mac_addr, *peer_addr;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+		u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A};
+		/* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */
+
+		p = rtw_get_ie(pframe + WLAN_HDR_A3_LEN + _PROBEREQ_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, (int *)&ielen,
+			len - WLAN_HDR_A3_LEN - _PROBEREQ_IE_OFFSET_);
+
+		if (!p || ielen != 14)
+			goto _non_rc_device;
+
+		if (memcmp(p+2, RC_OUI, sizeof(RC_OUI)))
+			goto _non_rc_device;
+
+		if (memcmp(p+6, get_sa(pframe), ETH_ALEN)) {
+			DBG_871X("%s, do rc pairing ("MAC_FMT"), but mac addr mismatch!("MAC_FMT")\n", __func__,
+				MAC_ARG(get_sa(pframe)), MAC_ARG(p+6));
+
+			goto _non_rc_device;
+		}
+
+		DBG_871X("%s, got the pairing device("MAC_FMT")\n", __func__,  MAC_ARG(get_sa(pframe)));
+
+		/* new a station */
+		psta = rtw_get_stainfo(pstapriv, get_sa(pframe));
+		if (psta == NULL) {
+			/*  allocate a new one */
+			DBG_871X("going to alloc stainfo for rc ="MAC_FMT"\n",  MAC_ARG(get_sa(pframe)));
+			psta = rtw_alloc_stainfo(pstapriv, get_sa(pframe));
+			if (psta == NULL) {
+				/* TODO: */
+				DBG_871X(" Exceed the upper limit of supported clients...\n");
+				return _SUCCESS;
+			}
+
+			spin_lock_bh(&pstapriv->asoc_list_lock);
+			if (list_empty(&psta->asoc_list)) {
+				psta->expire_to = pstapriv->expire_to;
+				list_add_tail(&psta->asoc_list, &pstapriv->asoc_list);
+				pstapriv->asoc_list_cnt++;
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+			/* generate pairing ID */
+			mac_addr = myid(&(padapter->eeprompriv));
+			peer_addr = psta->hwaddr;
+			psta->pid = (u16)(((mac_addr[4]<<8) + mac_addr[5]) + ((peer_addr[4]<<8) + peer_addr[5]));
+
+			/* update peer stainfo */
+			psta->isrc = true;
+			/* psta->aid = 0; */
+			/* psta->mac_id = 2; */
+
+			/* get a unique AID */
+			if (psta->aid > 0) {
+				DBG_871X("old AID %d\n", psta->aid);
+			} else {
+				for (psta->aid = 1; psta->aid <= NUM_STA; psta->aid++)
+					if (pstapriv->sta_aid[psta->aid - 1] == NULL)
+						break;
+
+				if (psta->aid > pstapriv->max_num_sta) {
+					psta->aid = 0;
+					DBG_871X("no room for more AIDs\n");
+					return _SUCCESS;
+				} else {
+					pstapriv->sta_aid[psta->aid - 1] = psta;
+					DBG_871X("allocate new AID = (%d)\n", psta->aid);
+				}
+			}
+
+			psta->qos_option = 1;
+			psta->bw_mode = CHANNEL_WIDTH_20;
+			psta->ieee8021x_blocked = false;
+			psta->htpriv.ht_option = true;
+			psta->htpriv.ampdu_enable = false;
+			psta->htpriv.sgi_20m = false;
+			psta->htpriv.sgi_40m = false;
+			psta->htpriv.ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+			psta->htpriv.agg_enable_bitmap = 0x0;/* reset */
+			psta->htpriv.candidate_tid_bitmap = 0x0;/* reset */
+
+			rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
+
+			memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+
+			spin_lock_bh(&psta->lock);
+			psta->state |= _FW_LINKED;
+			spin_unlock_bh(&psta->lock);
+
+			report_add_sta_event(padapter, psta->hwaddr, psta->aid);
+
+		}
+
+		issue_probersp(padapter, get_sa(pframe), false);
+
+		return _SUCCESS;
+
+	}
+
+_non_rc_device:
+
+	return _SUCCESS;
+
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	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 != NULL) {
+		if (is_valid_p2p_probereq == true)
+			goto _issue_probersp;
+
+		if ((ielen != 0 && false == !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) == true &&
+			pmlmepriv->cur_network.join_res == true)) || check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+			/* DBG_871X("+issue_probersp during ap mode\n"); */
+			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_, _EXT_SUPPORTEDRATES_IE_, &ielen, precv_frame->u.hdr.len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
+	if ((p != NULL) && (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. */
+			DBG_871X("[WIFIDBG] Error in ESR IE is detected in Beacon of BSSID:"MAC_FMT". Fix the length of ESR IE to avoid failed Beacon parsing.\n", MAC_ARG(GetAddr3Ptr(pframe)));
+			*(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 = (struct wlan_bssid_ex *)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((u8 *)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 != NULL) {
+				ret = rtw_check_bcn_info(padapter, pframe, len);
+				if (!ret) {
+						DBG_871X_LEVEL(_drv_always_, "ap has changed, disconnect now\n ");
+						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)
+					/* DBG_871X("update_bcn_info\n"); */
+					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 != NULL) {
+				/* 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) {
+					/* DBG_871X("update_bcn_info\n"); */
+					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;
+
+	DBG_871X("+OnAuth\n");
+
+	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));
+
+	DBG_871X("auth alg =%x, seq =%X\n", algorithm, seq);
+
+	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 */
+		DBG_871X("auth rejected due to bad alg [alg =%d, auth_mib =%d] %02X%02X%02X%02X%02X%02X\n",
+			algorithm, auth_mode, sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+
+		status = _STATS_NO_SUPP_ALG_;
+
+		goto auth_fail;
+	}
+
+	if (rtw_access_ctrl(padapter, sa) == false) {
+		status = _STATS_UNABLE_HANDLE_STA_;
+		goto auth_fail;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, sa);
+	if (pstat == NULL) {
+
+		/*  allocate a new one */
+		DBG_871X("going to alloc stainfo for sa ="MAC_FMT"\n",  MAC_ARG(sa));
+		pstat = rtw_alloc_stainfo(pstapriv, sa);
+		if (pstat == NULL) {
+			DBG_871X(" Exceed the upper limit of supported clients...\n");
+			status = _STATS_UNABLE_HANDLE_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) {
+		DBG_871X("(1)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+			seq, pstat->auth_seq+1);
+		status = _STATS_OUT_OF_AUTH_SEQ_;
+		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{
+			DBG_871X("(2)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+				seq, pstat->auth_seq+1);
+			status = _STATS_OUT_OF_AUTH_SEQ_;
+			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;
+			pstat->auth_seq = 2;
+		} else if (seq == 3) {
+			/* checking for challenging txt... */
+			DBG_871X("checking for challenging txt...\n");
+
+			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 == NULL) || (ie_len <= 0)) {
+				DBG_871X("auth rejected because challenge failure!(1)\n");
+				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{
+				DBG_871X("auth rejected because challenge failure!\n");
+				status = _STATS_CHALLENGE_FAIL_;
+				goto auth_fail;
+			}
+		} else{
+			DBG_871X("(3)auth rejected because out of seq [rx_seq =%d, exp_seq =%d]!\n",
+				seq, pstat->auth_seq+1);
+			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, union recv_frame *precv_frame)
+{
+	unsigned int	seq, len, status, algthm, 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;
+
+	DBG_871X("%s\n", __func__);
+
+	/* 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;
+
+	algthm	= 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));
+	status	= le16_to_cpu(*(__le16 *)((SIZE_PTR)pframe + WLAN_HDR_A3_LEN + offset + 4));
+
+	if (status != 0) {
+		DBG_871X("clnt auth fail, status: %d\n", status);
+		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_, _CHLGETXT_IE_, (int *)&len,
+				pkt_len - WLAN_HDR_A3_LEN - _AUTH_IE_OFFSET_);
+
+			if (p == NULL) {
+				/* DBG_871X("marc: no challenge text?\n"); */
+				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 */
+		/* DBG_871X("marc: clnt auth failed due to illegal seq =%x\n", seq); */
+		goto authclnt_fail;
+	}
+
+	if (go2asoc) {
+		DBG_871X_LEVEL(_drv_always_, "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, listen_interval;
+	struct rtw_ieee802_11_elems elems;
+	struct sta_info *pstat;
+	unsigned char 	reassoc, *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->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) {
+		reassoc = 0;
+		ie_offset = _ASOCREQ_IE_OFFSET_;
+	} else{ /*  WIFI_REASSOCREQ */
+		reassoc = 1;
+		ie_offset = _REASOCREQ_IE_OFFSET_;
+	}
+
+
+	if (pkt_len < IEEE80211_3ADDR_LEN + ie_offset) {
+		DBG_871X("handle_assoc(reassoc =%d) - too short payload (len =%lu)"
+		       "\n", reassoc, (unsigned long)pkt_len);
+		return _FAIL;
+	}
+
+	pstat = rtw_get_stainfo(pstapriv, GetAddr2Ptr(pframe));
+	if (pstat == (struct sta_info *)NULL) {
+		status = _RSON_CLS2_;
+		goto asoc_class2_error;
+	}
+
+	capab_info = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN);
+	/* capab_info = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN)); */
+	/* listen_interval = le16_to_cpu(*(unsigned short *)(pframe + WLAN_HDR_A3_LEN+2)); */
+	listen_interval = RTW_GET_LE16(pframe + WLAN_HDR_A3_LEN+2);
+
+	left = pkt_len - (IEEE80211_3ADDR_LEN + ie_offset);
+	pos = pframe + (IEEE80211_3ADDR_LEN + ie_offset);
+
+
+	DBG_871X("%s\n", __func__);
+
+	/*  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) {
+		DBG_871X("STA " MAC_FMT " sent invalid association request\n",
+		       MAC_ARG(pstat->hwaddr));
+		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 == NULL) {
+		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 (_STATS_SUCCESSFUL_ != status)
+		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 == NULL) {
+		DBG_871X("Rx a sta assoc-req which supported rate is empty!\n");
+		/*  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 !=  NULL) {
+
+			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 (_STATS_SUCCESSFUL_ != status)
+		goto OnAssocReqFail;
+
+	pstat->flags &= ~(WLAN_STA_WPS | WLAN_STA_MAYBE_WPS);
+	if (wpa_ie == NULL) {
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - assume WPS is "
+				   "used\n");
+			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 {
+			DBG_871X("STA did not include WPA/RSN IE "
+				   "in (Re)Association Request - possible WPS "
+				   "use\n");
+			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) {
+					DBG_871X("selected_registrar is false , or AP is not ready to do WPS\n");
+
+					status = _STATS_UNABLE_HANDLE_STA_;
+
+					goto OnAssocReqFail;
+				}
+			}
+		}
+
+	} else{
+		int copy_len;
+
+		if (psecuritypriv->wpa_psk == 0) {
+			DBG_871X("STA " MAC_FMT ": WPA/RSN IE in association "
+			"request, but AP don't support WPA/RSN\n", MAC_ARG(pstat->hwaddr));
+
+			status = WLAN_STATUS_INVALID_IE;
+
+			goto OnAssocReqFail;
+
+		}
+
+		if (elems.wps_ie) {
+			DBG_871X("STA included WPS IE in "
+				   "(Re)Association Request - WPS is "
+				   "used\n");
+			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 != NULL) {
+				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 rtw_ieee80211_ht_cap));
+	if (elems.ht_capabilities && elems.ht_capabilities_len >= sizeof(struct rtw_ieee80211_ht_cap)) {
+		pstat->flags |= WLAN_STA_HT;
+
+		pstat->flags |= WLAN_STA_WME;
+
+		memcpy(&pstat->htpriv.ht_cap, elems.ht_capabilities, sizeof(struct rtw_ieee80211_ht_cap));
+
+	} else
+		pstat->flags &= ~WLAN_STA_HT;
+
+
+	if ((pmlmepriv->htpriv.ht_option == false) && (pstat->flags&WLAN_STA_HT)) {
+		status = _STATS_FAILURE_;
+		goto OnAssocReqFail;
+	}
+
+
+	if ((pstat->flags & WLAN_STA_HT) &&
+		    ((pstat->wpa2_pairwise_cipher&WPA_CIPHER_TKIP) ||
+		      (pstat->wpa_pairwise_cipher&WPA_CIPHER_TKIP))) {
+		DBG_871X("HT: " MAC_FMT " tried to "
+				   "use TKIP with HT association\n", MAC_ARG(pstat->hwaddr));
+
+		/* status = WLAN_STATUS_CIPHER_REJECTED_PER_POLICY; */
+		/* 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;
+
+	/* 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) {
+		DBG_871X("  old AID %d\n", pstat->aid);
+	} else {
+		for (pstat->aid = 1; pstat->aid < NUM_STA; pstat->aid++)
+			if (pstapriv->sta_aid[pstat->aid - 1] == NULL)
+				break;
+
+		/* if (pstat->aid > NUM_STA) { */
+		if (pstat->aid > pstapriv->max_num_sta) {
+
+			pstat->aid = 0;
+
+			DBG_871X("  no room for more AIDs\n");
+
+			status = WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA;
+
+			goto OnAssocReqFail;
+
+
+		} else {
+			pstapriv->sta_aid[pstat->aid - 1] = pstat;
+			DBG_871X("allocate new AID = (%d)\n", pstat->aid);
+		}
+	}
+
+
+	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) && (_STATS_SUCCESSFUL_ == status)) {
+		/* 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);
+		if (pstat->passoc_req) {
+			kfree(pstat->passoc_req);
+			pstat->passoc_req = NULL;
+			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;
+
+	DBG_871X("%s\n", __func__);
+
+	/* 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) {
+		DBG_871X("assoc reject, status code: %d\n", status);
+		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->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);
+
+		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.SupportedRates);
+
+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;
+
+	/* 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));
+
+	DBG_871X("%s Reason code(%d)\n", __func__, reason);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+		DBG_871X_LEVEL(_drv_always_, "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;
+	} else{
+		int	ignore_received_deauth = 0;
+
+		/* 	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 (WLAN_REASON_PREV_AUTH_NOT_VALID == reason) {
+				/*  TODO: 802.11r */
+				ignore_received_deauth = 1;
+			}
+		}
+
+		DBG_871X_LEVEL(_drv_always_, "sta recv deauth reason code(%d) sta:%pM, ignore = %d\n",
+				reason, GetAddr3Ptr(pframe), ignore_received_deauth);
+
+		if (0 == ignore_received_deauth) {
+			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));
+
+	DBG_871X("%s Reason code(%d)\n", __func__, reason);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+		struct sta_info *psta;
+		struct sta_priv *pstapriv = &padapter->stapriv;
+
+		/* spin_lock_bh(&(pstapriv->sta_hash_lock)); */
+		/* rtw_free_stainfo(padapter, psta); */
+		/* spin_unlock_bh(&(pstapriv->sta_hash_lock)); */
+
+		DBG_871X_LEVEL(_drv_always_, "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;
+	} else{
+		DBG_871X_LEVEL(_drv_always_, "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)
+{
+	DBG_871X("%s\n", __func__);
+	return _SUCCESS;
+}
+
+unsigned int on_action_spct(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	unsigned int ret = _FAIL;
+	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;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(padapter->pnetdev));
+
+	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 RTW_WLAN_ACTION_SPCT_MSR_REQ:
+	case RTW_WLAN_ACTION_SPCT_MSR_RPRT:
+	case RTW_WLAN_ACTION_SPCT_TPC_REQ:
+	case RTW_WLAN_ACTION_SPCT_TPC_RPRT:
+	case RTW_WLAN_ACTION_SPCT_CHL_SWITCH:
+		break;
+	default:
+		break;
+	}
+
+exit:
+	return ret;
+}
+
+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, reason_code = 0;
+	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;
+
+	DBG_871X("%s\n", __func__);
+
+	/* 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 == NULL)
+		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];
+		DBG_871X("%s, action =%d\n", __func__, action);
+		switch (action) {
+		case RTW_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->bAcceptAddbaReq == true) {
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 0);
+			} else{
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_ADDBA_RESP, 37);/* reject ADDBA Req */
+			}
+
+			break;
+
+		case RTW_WLAN_ACTION_ADDBA_RESP: /* ADDBA response */
+			status = RTW_GET_LE16(&frame_body[3]);
+			tid = ((frame_body[5] >> 2) & 0x7);
+
+			if (status == 0) {
+				/* successful */
+				DBG_871X("agg_enable for TID =%d\n", tid);
+				psta->htpriv.agg_enable_bitmap |= 1 << tid;
+				psta->htpriv.candidate_tid_bitmap &= ~BIT(tid);
+			} else{
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+			}
+
+			if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
+				DBG_871X("%s alive check - rx ADDBA response\n", __func__);
+				psta->htpriv.agg_enable_bitmap &= ~BIT(tid);
+				psta->expire_to = pstapriv->expire_to;
+				psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
+			}
+
+			/* DBG_871X("marc: ADDBA RSP: %x\n", pmlmeinfo->agg_enable_bitmap); */
+			break;
+
+		case RTW_WLAN_ACTION_DELBA: /* DELBA */
+			if ((frame_body[3] & BIT(3)) == 0) {
+				psta->htpriv.agg_enable_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+				psta->htpriv.candidate_tid_bitmap &= ~(1 << ((frame_body[3] >> 4) & 0xf));
+
+				/* reason_code = frame_body[4] | (frame_body[5] << 8); */
+				reason_code = RTW_GET_LE16(&frame_body[4]);
+			} 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;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq);
+				#endif
+			}
+
+			DBG_871X("%s(): DELBA: %x(%x)\n", __func__, pmlmeinfo->agg_enable_bitmap, reason_code);
+			/* 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)) {
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x, token:%d\n",
+					FUNC_ADPT_ARG(adapter), seq_ctrl, mlmeext->action_public_rxseq, token);
+				return _FAIL;
+			}
+		} else {
+			if (seq_ctrl == mlmeext->action_public_rxseq) {
+				DBG_871X(FUNC_ADPT_FMT" seq_ctrl = 0x%x, rxseq = 0x%x\n",
+					FUNC_ADPT_ARG(adapter), 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;
+	int cnt = 0;
+	char msg[64];
+
+	token = frame_body[2];
+
+	if (rtw_action_public_decache(precv_frame, token) == _FAIL)
+		goto exit;
+
+	cnt += sprintf((msg+cnt), "%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 RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING:
+		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;
+	/* Baron */
+
+	DBG_871X("OnAction_sa_query\n");
+
+	switch (pframe[WLAN_HDR_A3_LEN+1]) {
+	case 0: /* SA Query req */
+		memcpy(&tid, &pframe[WLAN_HDR_A3_LEN+2], sizeof(unsigned short));
+		DBG_871X("OnAction_sa_query request, action =%d, tid =%04x\n", pframe[WLAN_HDR_A3_LEN+1], tid);
+		issue_action_SA_Query(padapter, GetAddr2Ptr(pframe), 1, tid);
+		break;
+
+	case 1: /* SA Query rsp */
+		del_timer_sync(&pmlmeext->sa_query_timer);
+		DBG_871X("OnAction_sa_query response, action =%d, tid =%04x, cancel timer\n", pframe[WLAN_HDR_A3_LEN+1], pframe[WLAN_HDR_A3_LEN+2]);
+		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 < sizeof(OnAction_tbl)/sizeof(struct action_handler); 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)
+{
+
+	/* DBG_871X("rcvd mgt frame(%x, %x)\n", (GetFrameSubType(pframe) >> 4), *(unsigned int *)GetAddr1Ptr(pframe)); */
+	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 == NULL) {
+		DBG_871X(FUNC_ADPT_FMT" alloc xmitframe fail, once:%d\n", FUNC_ADPT_ARG(pxmitpriv->adapter), once);
+		goto exit;
+	}
+
+	pxmitbuf = rtw_alloc_xmitbuf_ext(pxmitpriv);
+	if (pxmitbuf == NULL) {
+		DBG_871X(FUNC_ADPT_FMT" alloc xmitbuf fail\n", FUNC_ADPT_ARG(pxmitpriv->adapter));
+		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 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;
+	/* DBG_871X("%s(): rate = %x\n", __func__, 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 == true ||
+		padapter->bDriverStopped == true) {
+		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;
+	_irqL irqL;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_buf *pxmitbuf = pmgntframe->pxmitbuf;
+	struct submit_ctx sctx;
+
+	if (padapter->bSurpriseRemoved == true ||
+		padapter->bDriverStopped == true) {
+		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, __func__);
+
+	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 = 0;
+	s32 ret = _FAIL;
+	u32 timeout_ms = 500;/*   500ms */
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter->bSurpriseRemoved == true ||
+		padapter->bDriverStopped == true) {
+		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;
+	sint ssid_len_ori;
+	int len_diff = 0;
+
+	ssid_ie = rtw_get_ie(ies,  WLAN_EID_SSID, &ssid_len_ori, ies_len);
+
+	/* DBG_871X("%s hidden_ssid_mode:%u, ssid_ie:%p, ssid_len_ori:%d\n", __func__, hidden_ssid_mode, ssid_ie, ssid_len_ori); */
+
+	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);
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%s, alloc mgnt frame fail\n", __func__);
+		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;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	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) {
+		/* DBG_871X("ie len =%d\n", cur_network->IELength); */
+		{
+			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);
+		}
+
+		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);
+
+	/* 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 */
+
+_issue_bcn:
+
+	pmlmepriv->update_bcn = false;
+
+	spin_unlock_bh(&pmlmepriv->bcn_update_lock);
+
+	if ((pattrib->pktlen + TXDESC_SIZE) > 512) {
+		DBG_871X("beacon frame too large\n");
+		return;
+	}
+
+	pattrib->last_txcmdsz = pattrib->pktlen;
+
+	/* DBG_871X("issue bcn_sz =%d\n", pattrib->last_txcmdsz); */
+	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;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	if (da == NULL)
+		return;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%s, alloc mgnt frame fail\n", __func__);
+		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 != NULL) && 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;
+		}
+
+		/* retrieve SSID IE from cur_network->Ssid */
+		{
+			u8 *ssid_ie;
+			sint ssid_ielen;
+			sint ssid_ielen_diff;
+			u8 buf[MAX_IE_SZ];
+			u8 *ies = pmgntframe->buf_addr+TXDESC_OFFSET+sizeof(struct ieee80211_hdr_3addr);
+
+			ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
+				(pframe-ies)-_FIXED_IE_LENGTH_);
+
+			ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
+
+			if (ssid_ie &&  cur_network->Ssid.SsidLength) {
+				uint remainder_ielen;
+				u8 *remainder_ie;
+				remainder_ie = ssid_ie+2;
+				remainder_ielen = (pframe-remainder_ie);
+
+				if (remainder_ielen > MAX_IE_SZ) {
+					DBG_871X_LEVEL(_drv_warning_, 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.SsidLength;
+				memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
+
+				pframe += ssid_ielen_diff;
+				pattrib->pktlen += ssid_ielen_diff;
+			}
+		}
+	} 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 */
+
+	}
+
+#ifdef CONFIG_AUTO_AP_MODE
+{
+	struct sta_info *psta;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("(%s)\n", __func__);
+
+	/* check rc station */
+	psta = rtw_get_stainfo(pstapriv, da);
+	if (psta && psta->isrc && psta->pid > 0) {
+		u8 RC_OUI[4] = {0x00, 0xE0, 0x4C, 0x0A};
+		u8 RC_INFO[14] = {0};
+		/* EID[1] + EID_LEN[1] + RC_OUI[4] + MAC[6] + PairingID[2] + ChannelNum[2] */
+		u16 cu_ch = (u16)cur_network->Configuration.DSConfig;
+
+		DBG_871X("%s, reply rc(pid = 0x%x) device "MAC_FMT" in ch =%d\n", __func__,
+			psta->pid, MAC_ARG(psta->hwaddr), cu_ch);
+
+		/* append vendor specific ie */
+		memcpy(RC_INFO, RC_OUI, sizeof(RC_OUI));
+		memcpy(&RC_INFO[4], mac, ETH_ALEN);
+		memcpy(&RC_INFO[10], (u8 *)&psta->pid, 2);
+		memcpy(&RC_INFO[12], (u8 *)&cu_ch, 2);
+
+		pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, sizeof(RC_INFO), RC_INFO, &pattrib->pktlen);
+	}
+}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+
+	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, 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;
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+issue_probereq\n"));
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		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 */
+		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+	}
+
+	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));
+	}
+
+	if (ch)
+		pframe = rtw_set_ie(pframe, _DSSET_IE_, 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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("issuing probe_req, tx_len =%d\n", pattrib->last_txcmdsz));
+
+	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?true:false);
+
+		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
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+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;
+	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 == NULL)
+		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)
+			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, _CHLGETXT_IE_, 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);
+		/* DBG_871X("%s auth_algo = %s auth_seq =%d\n", __func__, (pmlmeinfo->auth_algo == 0)?"OPEN":"SHARED", pmlmeinfo->auth_seq); */
+
+		/* setting IV for auth seq #3 */
+		if ((pmlmeinfo->auth_seq == 3) && (pmlmeinfo->state & WIFI_FW_AUTH_STATE) && (use_shared_key == 1)) {
+			__le32 le_tmp32;
+
+			/* DBG_871X("==> iv(%d), key_index(%d)\n", pmlmeinfo->iv, pmlmeinfo->key_index); */
+			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, _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, (u8 *)pmgntframe);
+	DBG_871X("%s\n", __func__);
+	dump_mgntframe(padapter, pmgntframe);
+
+	return;
+}
+
+
+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;
+
+	DBG_871X("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		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, _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 */
+		/* p = hostapd_eid_ht_capabilities_info(hapd, p); */
+		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 */
+		/* p = hostapd_eid_ht_operation(hapd, p); */
+		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 == NULL) || (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;
+	}
+
+	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 == NULL)
+		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, _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);
+	/* DBG_871X("sta_bssrate_len =%d\n", 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++) { */
+	/* 	DBG_871X("sta_bssrate[%d]=%02X\n", i, sta_bssrate[i]); */
+	/*  */
+
+	for (i = 0; i < NDIS_802_11_LENGTH_RATES_EX; i++) {
+		if (pmlmeinfo->network.SupportedRates[i] == 0)
+			break;
+		DBG_871X("network.SupportedRates[%d]=%02X\n", i, pmlmeinfo->network.SupportedRates[i]);
+	}
+
+
+	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)) {
+				/* DBG_871X("match i = %d, j =%d\n", i, j); */
+				break;
+			} else {
+				/* DBG_871X("not match: %02X != %02X\n", (pmlmeinfo->network.SupportedRates[i]|IEEE80211_BASIC_RATE_MASK), (sta_bssrate[j]|IEEE80211_BASIC_RATE_MASK)); */
+			}
+		}
+
+		if (j == sta_bssrate_len) {
+			/*  the rate is not supported by STA */
+			DBG_871X("%s(): the rate[%d]=%02X is not supported by STA!\n", __func__, i, pmlmeinfo->network.SupportedRates[i]);
+		} else {
+			/*  the rate is supported by STA */
+			bssrate[index++] = pmlmeinfo->network.SupportedRates[i];
+		}
+	}
+
+	bssrate_len = index;
+	DBG_871X("bssrate_len = %d\n", bssrate_len);
+
+	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));
+
+	/* vendor specific IE, such as WPA, WMM, WPS */
+	for (i = sizeof(struct ndis_802_11_fix_ie); i < pmlmeinfo->network.IELength;) {
+		pIE = (struct ndis_80211_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))) {
+				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 vender extensions informations to AP */
+
+					vs_ie_length = 14;
+				}
+
+				pframe = rtw_set_ie(pframe, _VENDOR_SPECIFIC_IE_, vs_ie_length, pIE->data, &(pattrib->pktlen));
+			}
+			break;
+
+		case EID_WPA2:
+			pframe = rtw_set_ie(pframe, EID_WPA2, pIE->Length, pIE->data, &(pattrib->pktlen));
+			break;
+		case EID_HTCapability:
+			if (padapter->mlmepriv.htpriv.ht_option == true) {
+				if (!(is_ap_in_tkip(padapter))) {
+					memcpy(&(pmlmeinfo->HT_caps), pIE->data, sizeof(struct HT_caps_element));
+					pframe = rtw_set_ie(pframe, EID_HTCapability, pIE->Length, (u8 *)(&(pmlmeinfo->HT_caps)), &(pattrib->pktlen));
+				}
+			}
+			break;
+
+		case EID_EXTCapability:
+			if (padapter->mlmepriv.htpriv.ht_option == true)
+				pframe = rtw_set_ie(pframe, EID_EXTCapability, 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));
+
+
+	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);
+
+	return;
+}
+
+/* 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;
+
+	/* DBG_871X("%s:%d\n", __func__, power_mode); */
+
+	if (!padapter)
+		goto exit;
+
+	pxmitpriv = &(padapter->xmitpriv);
+	pmlmeext = &(padapter->mlmeextpriv);
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		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 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);
+	struct sta_info *psta;
+
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		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 {
+		DBG_871X(FUNC_ADPT_FMT ": Can't find sta info for " MAC_FMT ", skip macid %s!!\n",
+			FUNC_ADPT_ARG(padapter), MAC_ARG(da), power_mode?"sleep":"wakeup");
+		rtw_warn_on(1);
+	}
+
+	do {
+		ret = _issue_nulldata(padapter, da, power_mode, wait_ms > 0?true:false);
+
+		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
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+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)
+{
+	int ret;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+
+
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	/* da == NULL, assum it's null data for sta to ap*/
+	if (da == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	ret = _issue_nulldata(padapter, da, 0, false);
+
+	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;
+	u16 *qc;
+	struct xmit_priv 		*pxmitpriv = &(padapter->xmitpriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	DBG_871X("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		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);
+
+	if (pattrib->mdata)
+		SetMData(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 == NULL)
+		da = get_my_bssid(&(pmlmeinfo->network));
+
+	do {
+		ret = _issue_qos_nulldata(padapter, da, tid, wait_ms > 0?true:false);
+
+		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
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+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;
+
+	/* DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da)); */
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		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)
+{
+	DBG_871X("%s to "MAC_FMT"\n", __func__, MAC_ARG(da));
+	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?true:false);
+
+		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
+	}
+
+	if (try_cnt && wait_ms) {
+		if (da)
+			DBG_871X(FUNC_ADPT_FMT" to "MAC_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), MAC_ARG(da), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+		else
+			DBG_871X(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
+				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
+				ret == _SUCCESS?", acked":"", i, try_cnt, (i + 1) * wait_ms);
+	}
+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;
+
+	DBG_871X("%s\n", __func__);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		DBG_871X("%s: alloc_mgtxmitframe fail\n", __func__);
+		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 HT_CAP_AMPDU_FACTOR 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;
+
+	DBG_871X("%s, category =%d, action =%d, status =%d\n", __func__, category, action, status);
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL)
+		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 (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter)) {
+				/*  A-MSDU NOT Supported */
+				BA_para_set = 0;
+				/*  immediate Block Ack */
+				BA_para_set |= (1 << 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) & RTW_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.MacAddress)) != NULL) */
+			psta = rtw_get_stainfo(pstapriv, raddr);
+			if (psta != NULL) {
+				start_seq = (psta->sta_xmitpriv.txseq_tid[status & 0x07]&0xfff) + 1;
+
+				DBG_871X("BA_starting_seqctrl = %d for TID =%d\n", start_seq, status & 0x07);
+
+				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 HT_CAP_AMPDU_FACTOR)padapter->driver_rx_ampdu_factor;
+			else
+				rtw_hal_get_def_var(padapter,
+						    HW_VAR_MAX_RX_AMPDU_FACTOR, &max_rx_ampdu_factor);
+
+			if (MAX_AMPDU_FACTOR_64K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x1000); /* 64 buffer size */
+			else if (MAX_AMPDU_FACTOR_32K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0800); /* 32 buffer size */
+			else if (MAX_AMPDU_FACTOR_16K == max_rx_ampdu_factor)
+				BA_para_set = ((le16_to_cpu(pmlmeinfo->ADDBA_req.BA_para_set) & 0x3f) | 0x0400); /* 16 buffer size */
+			else if (MAX_AMPDU_FACTOR_8K == max_rx_ampdu_factor)
+				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 (rtw_btcoex_IsBTCoexCtrlAMPDUSize(padapter) &&
+			    padapter->driver_rx_ampdu_factor == 0xFF) {
+				/*  max buffer size is 8 MSDU */
+				BA_para_set &= ~RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK;
+				BA_para_set |= (8 << 6) & RTW_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;
+
+
+	DBG_871X("%s\n", __func__);
+
+
+	category = RTW_WLAN_CATEGORY_PUBLIC;
+	action = ACT_PUBLIC_BSSCOEXIST;
+
+	pmgntframe = alloc_mgtxmitframe(pxmitpriv);
+	if (pmgntframe == NULL) {
+		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 = get_next(phead);
+
+		while (1) {
+			int len;
+			u8 *p;
+			struct wlan_bssid_ex *pbss_network;
+
+			if (phead == plist)
+				break;
+
+			pnetwork = LIST_CONTAINOR(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_, _HT_CAPABILITY_IE_, &len, pbss_network->IELength - _FIXED_IE_LENGTH_);
+			if ((p == NULL) || (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 == NULL)
+		return _SUCCESS;
+
+	/* DBG_871X("%s:%s\n", __func__, (initiator == 0)?"RX_DIR":"TX_DIR"); */
+
+	if (initiator == 0) {/*  recipient */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->recvreorder_ctrl[tid].enable == true) {
+				DBG_871X("rx agg disable tid(%d)\n", tid);
+				issue_action_BA(padapter, addr, RTW_WLAN_ACTION_DELBA, (((tid << 1) | initiator)&0x1F));
+				psta->recvreorder_ctrl[tid].enable = false;
+				psta->recvreorder_ctrl[tid].indicate_seq = 0xffff;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d indicate_seq:%u\n", __func__, __LINE__,
+					psta->recvreorder_ctrl[tid].indicate_seq);
+				#endif
+			}
+		}
+	} else if (initiator == 1) {/*  originator */
+		/* DBG_871X("tx agg_enable_bitmap(0x%08x)\n", psta->htpriv.agg_enable_bitmap); */
+		for (tid = 0; tid < MAXTID; tid++) {
+			if (psta->htpriv.agg_enable_bitmap & BIT(tid)) {
+				DBG_871X("tx agg disable tid(%d)\n", tid);
+				issue_action_BA(padapter, addr, RTW_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;
+	unsigned long start = jiffies;
+
+	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 {
+			yield();
+			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 (false == bxmitok) {
+		DBG_871X("%s fail! %u ms\n", __func__, jiffies_to_msecs(jiffies - start));
+		return _FAIL;
+	} else{
+		unsigned long passing_time = jiffies_to_msecs(jiffies - start);
+
+		if (passing_time > 100 || issue > 3)
+			DBG_871X("%s success, issue:%d, poll:%d, %lu ms\n", __func__, issue, poll, passing_time);
+		/* else */
+		/* 	DBG_871X("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, passing_time); */
+
+		return _SUCCESS;
+	}
+}
+
+/****************************************************************************
+
+Following are some utitity fuctions for WiFi MLME
+
+*****************************************************************************/
+
+void site_survey(struct adapter *padapter)
+{
+	unsigned char 	survey_channel = 0, val8;
+	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;
+		}
+	}
+
+	DBG_871X(FUNC_ADPT_FMT" ch:%u (cnt:%u) at %dms, %c%c%c\n"
+		 , FUNC_ADPT_ARG(padapter)
+		 , survey_channel
+		 , pmlmeext->sitesurvey_res.channel_idx
+		 , jiffies_to_msecs(jiffies - padapter->mlmepriv.scan_start_time)
+		 , ScanType?'A':'P', pmlmeext->sitesurvey_res.scan_mode?'A':'P'
+		 , pmlmeext->sitesurvey_res.ssid[0].SsidLength?'S':' '
+		);
+#ifdef DBG_FIXED_CHAN
+	DBG_871X(FUNC_ADPT_FMT" fixed_chan:%u\n", pmlmeext->fixed_chan);
+#endif
+
+	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].SsidLength) {
+						/* 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);
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		{
+			struct noise_info info;
+			info.bPauseDIG = false;
+			info.IGIValue = 0;
+			info.max_time = channel_scan_time_ms/2;/* ms */
+			info.chan = survey_channel;
+			rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR, &info, false);
+		}
+#endif
+
+	} 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) == true)
+				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) {
+		/* DBG_871X("IE too long for survey event\n"); */
+		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->IELength = len;
+	memcpy(bssid->IEs, (pframe + sizeof(struct ieee80211_hdr_3addr)), bssid->IELength);
+
+	/* get the signal strength */
+	bssid->Rssi = precv_frame->u.hdr.attrib.phy_info.RecvSignalPower; /*  in dBM.raw data */
+	bssid->PhyInfo.SignalQuality = precv_frame->u.hdr.attrib.phy_info.SignalQuality;/* in percentage */
+	bssid->PhyInfo.SignalStrength = precv_frame->u.hdr.attrib.phy_info.SignalStrength;/* in percentage */
+
+	/*  checking SSID */
+	p = rtw_get_ie(bssid->IEs + ie_offset, _SSID_IE_, &len, bssid->IELength - ie_offset);
+	if (p == NULL) {
+		DBG_871X("marc: cannot find SSID for survey event\n");
+		return _FAIL;
+	}
+
+	if (*(p + 1)) {
+		if (len > NDIS_802_11_LENGTH_SSID) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			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 != NULL) {
+		if (len > NDIS_802_11_LENGTH_RATES_EX) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			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 != NULL) {
+		if (len > (NDIS_802_11_LENGTH_RATES_EX-i)) {
+			DBG_871X("%s()-%d: IE too long (%d) for survey event\n", __func__, __LINE__, len);
+			return _FAIL;
+		}
+		memcpy(bssid->SupportedRates + i, (p + 2), len);
+	}
+
+	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) && (false == 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++;
+	}
+
+#ifdef CONFIG_INTEL_WIDI
+	/* process_intel_widi_query_or_tigger(padapter, bssid); */
+	if (process_intel_widi_query_or_tigger(padapter, bssid))
+		return _FAIL;
+#endif /*  CONFIG_INTEL_WIDI */
+
+	#if defined(DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) & 1
+	if (strcmp(bssid->Ssid.Ssid, DBG_RX_SIGNAL_DISPLAY_SSID_MONITORED) == 0) {
+		DBG_871X("Receiving %s("MAC_FMT", DSConfig:%u) from ch%u with ss:%3u, sq:%3u, RawRSSI:%3ld\n"
+			, bssid->Ssid.Ssid, MAC_ARG(bssid->MacAddress), bssid->Configuration.DSConfig
+			, rtw_get_oper_ch(padapter)
+			, bssid->PhyInfo.SignalStrength, bssid->PhyInfo.SignalQuality, bssid->Rssi
+		);
+	}
+	#endif
+
+	/*  mark bss info receving from nearby channel as SignalQuality 101 */
+	if (bssid->Configuration.DSConfig != rtw_get_oper_ch(padapter))
+		bssid->PhyInfo.SignalQuality = 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.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 */
+		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) {
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_, ("issuing beacon frame fail....\n"));
+
+			report_join_res(padapter, -1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+		} else{
+			rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, padapter->registrypriv.dev_network.MacAddress);
+			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{
+		DBG_871X("start_create_ibss, invalid cap:%x\n", caps);
+		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);
+
+	/* 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_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->MacAddress, 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&cap_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{
+		/* DBG_871X("marc: invalid cap:%x\n", caps); */
+		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;
+
+
+	DBG_871X_LEVEL(_drv_always_, "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;
+
+	DBG_871X("%s\n", __func__);
+
+	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;
+	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;
+		RT_CHANNEL_PLAN chplan_ap;
+		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;
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+				("%s: 802.11d country =%s\n", __func__, country));
+
+		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;
+
+#ifdef DEBUG_RTL871X
+		i = 0;
+		DBG_871X("%s: AP[%s] channel plan {", __func__, bssid->Ssid.Ssid);
+		while ((i < chplan_ap.Len) && (chplan_ap.Channel[i] != 0)) {
+			DBG_8192C("%02d,", chplan_ap.Channel[i]);
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+
+		memcpy(chplan_sta, pmlmeext->channel_set, sizeof(chplan_sta));
+#ifdef DEBUG_RTL871X
+		i = 0;
+		DBG_871X("%s: STA channel plan {", __func__);
+		while ((i < MAX_CHANNEL_NUM) && (chplan_sta[i].ChannelNum != 0)) {
+			DBG_871X("%02d(%c),", chplan_sta[i].ChannelNum, chplan_sta[i].ScanType == SCAN_PASSIVE?'p':'a');
+			i++;
+		}
+		DBG_871X("}\n");
+#endif
+
+		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++;
+			}
+		}
+
+		if (pregistrypriv->wireless_mode & WIRELESS_11A) {
+			do {
+				if ((i == MAX_CHANNEL_NUM) ||
+					(chplan_sta[i].ChannelNum == 0))
+					break;
+
+				if ((j == chplan_ap.Len) || (chplan_ap.Channel[j] == 0))
+					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_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] != 0)) {
+				chplan_new[k].ChannelNum = chplan_ap.Channel[j];
+				chplan_new[k].ScanType = SCAN_ACTIVE;
+				j++;
+				k++;
+			}
+		} else{
+			/*  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;
+
+#ifdef DEBUG_RTL871X
+		k = 0;
+		DBG_871X("%s: new STA channel plan {", __func__);
+		while ((k < MAX_CHANNEL_NUM) && (chplan_new[k].ChannelNum != 0)) {
+			DBG_871X("%02d(%c),", chplan_new[k].ChannelNum, chplan_new[k].ScanType == SCAN_PASSIVE?'p':'c');
+			k++;
+		}
+		DBG_871X("}\n");
+#endif
+	}
+
+	/*  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) {
+				/* 5G Bnad 2, 3 (DFS) doesn't change to active scan */
+				if (channel >= 52 && channel <= 144)
+					break;
+
+				chplan_new[i].ScanType = SCAN_ACTIVE;
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
+						 ("%s: change channel %d scan type from passive to active\n",
+						  __func__, channel));
+			}
+			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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct survey_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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((u8 *)pcmd_obj);
+		kfree((u8 *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct surveydone_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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;
+
+	DBG_871X("survey done event(%x) band:%d for "ADPT_FMT"\n", psurveydone_evt->bss_cnt, padapter->setband, ADPT_ARG(padapter));
+
+	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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct joinbss_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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;
+
+	DBG_871X("report_join_res(%d)\n", 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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct wmm_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL) {
+		return;
+	}
+
+	cmdsz = (sizeof(struct stadel_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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;
+
+	DBG_871X("report_del_sta_event: delete STA, mac_id =%d\n", mac_id);
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (pcmd_obj == NULL)
+		return;
+
+	cmdsz = (sizeof(struct stassoc_event) + sizeof(struct C2HEvent_Header));
+	pevtcmd = (u8 *)rtw_zmalloc(cmdsz);
+	if (pevtcmd == NULL) {
+		kfree((u8 *)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;
+
+	DBG_871X("report_add_sta_event: add STA\n");
+
+	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
+
+	return;
+}
+
+
+bool rtw_port_switch_chk(struct adapter *adapter)
+{
+	bool switch_needed = false;
+	return switch_needed;
+}
+
+/****************************************************************************
+
+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 rtw_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));
+	u8 state_backup = (pmlmeinfo->state&0x03);
+
+	/* 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;
+
+	if (state_backup == WIFI_FW_STATION_STATE) {
+		if (rtw_port_switch_chk(padapter) == true) {
+			rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+			{
+				struct adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
+				if (port0_iface)
+					rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
+			}
+		}
+	}
+
+	/* 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);
+
+		goto exit_mlmeext_joinbss_event_callback;
+	}
+
+	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-releated issue */
+	update_IOT_info(padapter);
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, cur_network->SupportedRates);
+
+	/* BCN interval */
+	rtw_hal_set_hwreg(padapter, HW_VAR_BEACON_INTERVAL, (u8 *)(&pmlmeinfo->bcn_interval));
+
+	/* udpate 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->MacAddress);
+	if (psta) { /* only for infra. mode */
+
+		pmlmeinfo->FW_sta_info[psta->mac_id].psta = psta;
+
+		/* DBG_871X("set_sta_rate\n"); */
+
+		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);
+	}
+
+	if (rtw_port_switch_chk(padapter) == true)
+		rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+
+	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);
+
+exit_mlmeext_joinbss_event_callback:
+
+	DBG_871X("=>%s\n", __func__);
+
+}
+
+/* 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;
+
+	DBG_871X("%s\n", __func__);
+
+	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 = rtw_hal_networktype_to_raid(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) {
+
+		DBG_871X("\n ============["ADPT_FMT"] linked status check ===================\n", ADPT_ARG(padapter));
+
+		if ((pmlmeinfo->state&0x03) == WIFI_FW_STATION_STATE) {
+			rtw_hal_get_def_var(padapter, HAL_DEF_UNDERCORATEDSMOOTHEDPWDB, &UndecoratedSmoothedPWDB);
+
+			DBG_871X("AP[" MAC_FMT "] - UndecoratedSmoothedPWDB:%d\n",
+				MAC_ARG(padapter->mlmepriv.cur_network.network.MacAddress), UndecoratedSmoothedPWDB);
+		} else if ((pmlmeinfo->state&0x03) == _HW_STATE_AP_) {
+			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 = get_next(phead);
+			while (phead != plist) {
+				psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+				plist = get_next(plist);
+
+				DBG_871X("STA[" MAC_FMT "]:UndecoratedSmoothedPWDB:%d\n",
+					MAC_ARG(psta->hwaddr), psta->rssi_stat.UndecoratedSmoothedPWDB);
+			}
+			spin_unlock_bh(&pstapriv->asoc_list_lock);
+
+		}
+		for (i = 0; i < NUM_STA; i++) {
+			if (pdvobj->macid[i] == true) {
+				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;
+
+	#ifdef DBG_EXPIRATION_CHK
+	DBG_871X(FUNC_ADPT_FMT" rx:"STA_PKTS_FMT", beacon:%llu, probersp_to_self:%llu"
+				/*", probersp_bm:%llu, probersp_uo:%llu, probereq:%llu, BI:%u"*/
+				", retry:%u\n"
+		, FUNC_ADPT_ARG(padapter)
+		, STA_RX_PKTS_DIFF_ARG(psta)
+		, psta->sta_stats.rx_beacon_pkts - psta->sta_stats.last_rx_beacon_pkts
+		, psta->sta_stats.rx_probersp_pkts - psta->sta_stats.last_rx_probersp_pkts
+		/*, psta->sta_stats.rx_probersp_bm_pkts - psta->sta_stats.last_rx_probersp_bm_pkts
+		, psta->sta_stats.rx_probersp_uo_pkts - psta->sta_stats.last_rx_probersp_uo_pkts
+		, psta->sta_stats.rx_probereq_pkts - psta->sta_stats.last_rx_probereq_pkts
+		, pmlmeinfo->bcn_interval*/
+		, pmlmeext->retry
+	);
+
+	DBG_871X(FUNC_ADPT_FMT" tx_pkts:%llu, link_count:%u\n", FUNC_ADPT_ARG(padapter)
+		, padapter->xmitpriv.tx_pkts
+		, pmlmeinfo->link_count
+	);
+	#endif
+
+	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 */
+		/* ifdef CONFIG_INTEL_WIDI */
+		/* if (padapter->mlmepriv.widi_state != INTEL_WIDI_STATE_NONE) */
+		/* 	rx_chk_limit = 1; */
+		/* endif */
+
+		psta = rtw_get_stainfo(pstapriv, pmlmeinfo->network.MacAddress);
+		if (psta != NULL) {
+			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) {
+						#ifdef DBG_EXPIRATION_CHK
+						DBG_871X("issue_probereq to trigger probersp, retry =%d\n", pmlmeext->retry);
+						#endif
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+						issue_probereq_ex(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress, 0, 0, 0, 0);
+					}
+				}
+
+				if (tx_chk != _SUCCESS && pmlmeinfo->link_count++ == link_count_limit) {
+					#ifdef DBG_EXPIRATION_CHK
+					DBG_871X("%s issue_nulldata 0\n", __func__);
+					#endif
+					tx_chk = issue_nulldata_in_interrupt(padapter, NULL);
+				}
+			}
+
+			if (rx_chk == _FAIL) {
+				pmlmeext->retry++;
+				if (pmlmeext->retry > rx_chk_limit) {
+					DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" disconnect or roaming\n",
+						FUNC_ADPT_ARG(padapter));
+					receive_disconnect(padapter, pmlmeinfo->network.MacAddress
+						, 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.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 (NULL == psta)
+					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 adapter *padapter)
+{
+	struct cmd_obj	*ph2c;
+	struct sitesurvey_parm	*psurveyPara;
+	struct cmd_priv 				*pcmdpriv = &padapter->cmdpriv;
+	struct mlme_ext_priv 	*pmlmeext = &padapter->mlmeextpriv;
+
+	/* DBG_871X("marc: survey timer\n"); */
+
+	/* 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 == true) {
+			{
+				pmlmeext->sitesurvey_res.channel_idx = pmlmeext->sitesurvey_res.ch_num;
+				DBG_871X("%s idx:%d\n", __func__
+					, pmlmeext->sitesurvey_res.channel_idx
+				);
+			}
+
+			pmlmeext->scan_abort = false;/* reset */
+		}
+
+		ph2c = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+		if (ph2c == NULL) {
+			goto exit_survey_timer_hdl;
+		}
+
+		psurveyPara = (struct sitesurvey_parm *)rtw_zmalloc(sizeof(struct sitesurvey_parm));
+		if (psurveyPara == NULL) {
+			kfree((unsigned char *)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)
+{
+	/* 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) {
+		DBG_871X("link_timer_hdl:no beacon while connecting\n");
+		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) {
+			/* if (pmlmeinfo->auth_algo != dot11AuthAlgrthm_Auto) */
+			/*  */
+				pmlmeinfo->state = 0;
+				report_join_res(padapter, -1);
+				return;
+			/*  */
+			/* else */
+			/*  */
+			/* 	pmlmeinfo->auth_algo = dot11AuthAlgrthm_Shared; */
+			/* 	pmlmeinfo->reauth_count = 0; */
+			/*  */
+		}
+
+		DBG_871X("link_timer_hdl: auth timeout and try again\n");
+		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;
+		}
+
+		DBG_871X("link_timer_hdl: assoc timeout and try again\n");
+		issue_assocreq(padapter);
+		set_link_timer(pmlmeext, REASSOC_TO);
+	}
+
+	return;
+}
+
+void addba_timer_hdl(struct sta_info *psta)
+{
+	struct ht_priv *phtpriv;
+
+	if (!psta)
+		return;
+
+	phtpriv = &psta->htpriv;
+
+	if ((phtpriv->ht_option == true) && (phtpriv->ampdu_enable == true)) {
+		if (phtpriv->candidate_tid_bitmap)
+			phtpriv->candidate_tid_bitmap = 0x0;
+
+	}
+}
+
+void sa_query_timer_hdl(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	/* disconnect */
+	spin_lock_bh(&pmlmepriv->lock);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		rtw_disassoc_cmd(padapter, 0, true);
+		rtw_indicate_disconnect(padapter);
+		rtw_free_assoc_resources(padapter, 1);
+	}
+
+	spin_unlock_bh(&pmlmepriv->lock);
+	DBG_871X("SA query timeout disconnect\n");
+}
+
+u8 NULL_hdl(struct adapter *padapter, u8 *pbuf)
+{
+	return H2C_SUCCESS;
+}
+
+#ifdef CONFIG_AUTO_AP_MODE
+static int rtw_auto_ap_start_beacon(struct adapter *adapter)
+{
+	int ret = 0;
+	u8 *pbuf = NULL;
+	uint len;
+	u8 supportRate[16];
+	int	sz = 0, rateLen;
+	u8 *ie;
+	u8 wireless_mode, oper_channel;
+	u8 ssid[3] = {0}; /* hidden ssid */
+	u32 ssid_len = sizeof(ssid);
+	struct mlme_priv *pmlmepriv = &(adapter->mlmepriv);
+
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+
+	len = 128;
+	pbuf = rtw_zmalloc(len);
+	if (!pbuf)
+		return -ENOMEM;
+
+
+	/* generate beacon */
+	ie = pbuf;
+
+	/* timestamp will be inserted by hardware */
+	sz += 8;
+	ie += sz;
+
+	/* beacon interval : 2bytes */
+	*(u16 *)ie = cpu_to_le16((u16)100);/* BCN_INTERVAL = 100; */
+	sz += 2;
+	ie += 2;
+
+	/* capability info */
+	*(u16 *)ie = 0;
+	*(u16 *)ie |= cpu_to_le16(cap_ESS);
+	*(u16 *)ie |= cpu_to_le16(cap_ShortPremble);
+	/* u16*)ie |= cpu_to_le16(cap_Privacy); */
+	sz += 2;
+	ie += 2;
+
+	/* SSID */
+	ie = rtw_set_ie(ie, _SSID_IE_, ssid_len, ssid, &sz);
+
+	/* supported rates */
+	wireless_mode = WIRELESS_11BG_24N;
+	rtw_set_supported_rate(supportRate, wireless_mode);
+	rateLen = rtw_get_rateset_len(supportRate);
+	if (rateLen > 8) {
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, 8, supportRate, &sz);
+	} else{
+		ie = rtw_set_ie(ie, _SUPPORTEDRATES_IE_, rateLen, supportRate, &sz);
+	}
+
+
+	/* DS parameter set */
+	if (check_buddy_fwstate(adapter, _FW_LINKED) &&
+		check_buddy_fwstate(adapter, WIFI_STATION_STATE)) {
+		struct adapter *pbuddystruct adapter = adapter->pbuddystruct adapter;
+		struct mlme_ext_priv *pbuddy_mlmeext  = &pbuddystruct adapter->mlmeextpriv;
+
+		oper_channel = pbuddy_mlmeext->cur_channel;
+	} else{
+		oper_channel = adapter_to_dvobj(adapter)->oper_channel;
+	}
+	ie = rtw_set_ie(ie, _DSSET_IE_, 1, &oper_channel, &sz);
+
+	/* ext supported rates */
+	if (rateLen > 8) {
+		ie = rtw_set_ie(ie, _EXT_SUPPORTEDRATES_IE_, (rateLen - 8), (supportRate + 8), &sz);
+	}
+
+	DBG_871X("%s, start auto ap beacon sz =%d\n", __func__, sz);
+
+	/* lunch ap mode & start to issue beacon */
+	if (rtw_check_beacon_data(adapter, pbuf,  sz) == _SUCCESS) {
+
+	} else{
+		ret = -EINVAL;
+	}
+
+
+	kfree(pbuf);
+
+	return ret;
+
+}
+#endif/* CONFIG_AUTO_AP_MODE */
+
+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_NETYPE0_MSR(padapter, type); */
+
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psetop->mode == Ndis802_11APMode)
+		rtw_auto_ap_start_beacon(padapter);
+#endif
+
+	if (rtw_port_switch_chk(padapter) == true) {
+		rtw_hal_set_hwreg(padapter, HW_VAR_PORT_SWITCH, NULL);
+
+		if (psetop->mode == Ndis802_11APMode)
+			adapter_to_pwrctl(padapter)->fw_psmode_iface_id = 0xff; /* ap mode won't dowload rsvd pages */
+		else if (psetop->mode == Ndis802_11Infrastructure) {
+			struct adapter *port0_iface = dvobj_get_port0_adapter(adapter_to_dvobj(padapter));
+			if (port0_iface)
+				rtw_lps_ctrl_wk_cmd(port0_iface, LPS_CTRL_CONNECT, 0);
+		}
+	}
+
+	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) {
+		struct wlan_bssid_ex *network = &padapter->mlmepriv.cur_network.network;
+		start_bss_network(padapter, (u8 *)network);
+		return H2C_SUCCESS;
+	}
+
+	/* below is for ad-hoc master */
+	if (pparm->network.InfrastructureMode == 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, 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)
+{
+	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->MacAddress, 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, 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);
+
+	pmlmeext->cur_channel = (u8)pnetwork->Configuration.DSConfig;
+	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->IELength); */
+
+	/* sizeof(struct ndis_802_11_fix_ie) */
+	for (i = _FIXED_IE_LENGTH_; i < pnetwork->IELength;) {
+		pIE = (struct ndis_80211_var_ie *)(pnetwork->IEs + i);
+
+		switch (pIE->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:/* Get WMM IE. */
+			if (!memcmp(pIE->data, WMM_OUI, 4))
+				WMM_param_handler(padapter, pIE);
+			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 (pnetwork->Configuration.DSConfig > 14) {
+					if ((pregpriv->bw_mode >> 4) > CHANNEL_WIDTH_20)
+						cbw40_enable = 1;
+				} else {
+					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;
+					}
+
+					DBG_871X("set HT ch/bw before connected\n");
+				}
+			}
+			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.MacAddress);
+	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->MacAddress, 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++) {
+
+		DBG_871X(FUNC_ADPT_FMT" "CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(&in[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
+			&& rtw_mlme_band_check(padapter, in[i].hw_value) == true
+		) {
+			if (j >= out_num) {
+				DBG_871X_LEVEL(_drv_always_, 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++) {
+
+			DBG_871X(FUNC_ADPT_FMT" ch:%u\n", FUNC_ADPT_ARG(padapter), pmlmeext->channel_set[i].ChannelNum);
+
+			if (rtw_mlme_band_check(padapter, pmlmeext->channel_set[i].ChannelNum) == true) {
+
+				if (j >= out_num) {
+					DBG_871X_LEVEL(_drv_always_, 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].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) == true) {
+			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 scaning, 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);
+		DBG_871X_LEVEL(_drv_always_, "set group key camid:%d, addr:"MAC_FMT", 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) {
+		DBG_871X_LEVEL(_drv_always_, "%s sta:"MAC_FMT" 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) {
+			DBG_871X_LEVEL(_drv_always_, "clear key for addr:"MAC_FMT", camid:%d\n", MAC_ARG(pparm->addr), cam_id);
+			clear_cam_entry(padapter, cam_id);
+			rtw_camid_free(padapter, cam_id);
+		}
+	} else {
+		DBG_871X_LEVEL(_drv_always_, "set pairwise key camid:%d, addr:"MAC_FMT", 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, RTW_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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		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 = (struct cmd_obj *)rtw_zmalloc(sizeof(struct cmd_obj));
+	if (ph2c == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+
+	ptxBeacon_parm = (struct Tx_Beacon_param *)rtw_zmalloc(sizeof(struct Tx_Beacon_param));
+	if (ptxBeacon_parm == NULL) {
+		kfree((unsigned char *)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, evt_seq;
+	u16 evt_sz;
+	uint	*peventbuf;
+	void (*event_callback)(struct adapter *dev, u8 *pbuf);
+	struct evt_priv *pevt_priv = &(padapter->evtpriv);
+
+	if (pbuf == NULL)
+		goto _abort_event_;
+
+	peventbuf = (uint *)pbuf;
+	evt_sz = (u16)(*peventbuf&0xffff);
+	evt_seq = (u8)((*peventbuf>>24)&0x7f);
+	evt_code = (u8)((*peventbuf>>16)&0xff);
+
+
+	#ifdef CHECK_EVENT_SEQ
+	/*  checking event sequence... */
+	if (evt_seq != (atomic_read(&pevt_priv->event_seq) & 0x7f)) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_info_,
+			 ("Event Seq Error! %d vs %d\n", (evt_seq & 0x7f),
+			  (atomic_read(&pevt_priv->event_seq) & 0x7f)));
+
+		pevt_priv->event_seq = (evt_seq+1)&0x7f;
+
+		goto _abort_event_;
+	}
+	#endif
+
+	/*  checking if event code is valid */
+	if (evt_code >= MAX_C2HEVT) {
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent Code(%d) mismatch!\n", evt_code));
+		goto _abort_event_;
+	}
+
+	/*  checking if event size match the event parm size */
+	if ((wlanevents[evt_code].parmsize != 0) &&
+			(wlanevents[evt_code].parmsize != evt_sz)) {
+
+		RT_TRACE(_module_rtl871x_cmd_c_, _drv_err_, ("\nEvent(%d) Parm Size mismatch (%d vs %d)!\n",
+			evt_code, 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;
+	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);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while (xmitframe_phead != xmitframe_plist) {
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			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) == true)
+				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) {
+		DBG_871X("issue_beacon, fail!\n");
+		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 == true) {
+		DBG_871X("start_join_set_ch_bw: ch =%d, bwmode =%d, ch_offset =%d\n", cur_ch, cur_bw, cur_ch_offset);
+		*ch = cur_ch;
+		*bw = cur_bw;
+		*offset = cur_ch_offset;
+	}
+
+	return connect_allow == true ? _SUCCESS : _FAIL;
+}
+
+/* Find union about ch, bw, ch_offset of all linked/linking interfaces */
+int rtw_get_ch_setting_union(struct adapter *adapter, u8 *ch, u8 *bw, u8 *offset)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct adapter *iface;
+	struct mlme_ext_priv *mlmeext;
+	u8 ch_ret = 0;
+	u8 bw_ret = CHANNEL_WIDTH_20;
+	u8 offset_ret = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	if (ch)
+		*ch = 0;
+	if (bw)
+		*bw = CHANNEL_WIDTH_20;
+	if (offset)
+		*offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+
+	iface = dvobj->padapters;
+	mlmeext = &iface->mlmeextpriv;
+
+	if (!check_fwstate(&iface->mlmepriv, _FW_LINKED|_FW_UNDER_LINKING))
+		return 0;
+
+	ch_ret = mlmeext->cur_channel;
+	bw_ret = mlmeext->cur_bwmode;
+	offset_ret = mlmeext->cur_ch_offset;
+
+	return 1;
+}
+
+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;
+
+	DBG_871X(FUNC_NDEV_FMT" ch:%u, bw:%u, ch_offset:%u\n",
+		FUNC_NDEV_ARG(padapter->pnetdev),
+		set_ch_parm->ch, set_ch_parm->bw, set_ch_parm->ch_offset);
+
+	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 != NULL) && (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 led_blink_hdl(struct adapter *padapter, unsigned char *pbuf)
+{
+	struct LedBlink_param *ledBlink_param;
+
+	if (!pbuf)
+		return H2C_PARAMETERS_ERROR;
+
+	ledBlink_param = (struct LedBlink_param *)pbuf;
+	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 (NULL == pbuf)
+		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_odm.c b/drivers/staging/rtl8723bs/core/rtw_odm.c
new file mode 100644
index 000000000000..3144e8ec2fa2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_odm.c
@@ -0,0 +1,197 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_odm.h>
+#include <hal_data.h>
+
+static const char *odm_comp_str[] = {
+	/* BIT0 */"ODM_COMP_DIG",
+	/* BIT1 */"ODM_COMP_RA_MASK",
+	/* BIT2 */"ODM_COMP_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_COMP_FA_CNT",
+	/* BIT4 */"ODM_COMP_RSSI_MONITOR",
+	/* BIT5 */"ODM_COMP_CCK_PD",
+	/* BIT6 */"ODM_COMP_ANT_DIV",
+	/* BIT7 */"ODM_COMP_PWR_SAVE",
+	/* BIT8 */"ODM_COMP_PWR_TRAIN",
+	/* BIT9 */"ODM_COMP_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_COMP_PATH_DIV",
+	/* BIT11 */"ODM_COMP_PSD",
+	/* BIT12 */"ODM_COMP_DYNAMIC_PRICCA",
+	/* BIT13 */"ODM_COMP_RXHP",
+	/* BIT14 */"ODM_COMP_MP",
+	/* BIT15 */"ODM_COMP_DYNAMIC_ATC",
+	/* BIT16 */"ODM_COMP_EDCA_TURBO",
+	/* BIT17 */"ODM_COMP_EARLY_MODE",
+	/* BIT18 */NULL,
+	/* BIT19 */NULL,
+	/* BIT20 */NULL,
+	/* BIT21 */NULL,
+	/* BIT22 */NULL,
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_COMP_TX_PWR_TRACK",
+	/* BIT25 */"ODM_COMP_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_COMP_CALIBRATION",
+	/* BIT27 */NULL,
+	/* BIT28 */NULL,
+	/* BIT29 */NULL,
+	/* BIT30 */"ODM_COMP_COMMON",
+	/* BIT31 */"ODM_COMP_INIT",
+};
+
+#define RTW_ODM_COMP_MAX 32
+
+static const char *odm_ability_str[] = {
+	/* BIT0 */"ODM_BB_DIG",
+	/* BIT1 */"ODM_BB_RA_MASK",
+	/* BIT2 */"ODM_BB_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_BB_FA_CNT",
+	/* BIT4 */"ODM_BB_RSSI_MONITOR",
+	/* BIT5 */"ODM_BB_CCK_PD",
+	/* BIT6 */"ODM_BB_ANT_DIV",
+	/* BIT7 */"ODM_BB_PWR_SAVE",
+	/* BIT8 */"ODM_BB_PWR_TRAIN",
+	/* BIT9 */"ODM_BB_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_BB_PATH_DIV",
+	/* BIT11 */"ODM_BB_PSD",
+	/* BIT12 */"ODM_BB_RXHP",
+	/* BIT13 */"ODM_BB_ADAPTIVITY",
+	/* BIT14 */"ODM_BB_DYNAMIC_ATC",
+	/* BIT15 */NULL,
+	/* BIT16 */"ODM_MAC_EDCA_TURBO",
+	/* BIT17 */"ODM_MAC_EARLY_MODE",
+	/* BIT18 */NULL,
+	/* BIT19 */NULL,
+	/* BIT20 */NULL,
+	/* BIT21 */NULL,
+	/* BIT22 */NULL,
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_RF_TX_PWR_TRACK",
+	/* BIT25 */"ODM_RF_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_RF_CALIBRATION",
+};
+
+#define RTW_ODM_ABILITY_MAX 27
+
+static const char *odm_dbg_level_str[] = {
+	NULL,
+	"ODM_DBG_OFF",
+	"ODM_DBG_SERIOUS",
+	"ODM_DBG_WARNING",
+	"ODM_DBG_LOUD",
+	"ODM_DBG_TRACE",
+};
+
+#define RTW_ODM_DBG_LEVEL_NUM 6
+
+void rtw_odm_dbg_comp_msg(void *sel, struct adapter *adapter)
+{
+	u64 dbg_comp;
+	int i;
+
+	rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &dbg_comp);
+	DBG_871X_SEL_NL(sel, "odm.DebugComponents = 0x%016llx\n", dbg_comp);
+	for (i = 0; i < RTW_ODM_COMP_MAX; i++) {
+		if (odm_comp_str[i])
+			DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n",
+					(BIT0 << i) & dbg_comp ? '+' : ' ',
+					i, odm_comp_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_comp_set(struct adapter *adapter, u64 comps)
+{
+	rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_FLAG, &comps);
+}
+
+void rtw_odm_dbg_level_msg(void *sel, struct adapter *adapter)
+{
+	u32 dbg_level;
+	int i;
+
+	rtw_hal_get_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &dbg_level);
+	DBG_871X_SEL_NL(sel, "odm.DebugLevel = %u\n", dbg_level);
+	for (i = 0; i < RTW_ODM_DBG_LEVEL_NUM; i++) {
+		if (odm_dbg_level_str[i])
+			DBG_871X_SEL_NL(sel, "%u %s\n", i, odm_dbg_level_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_level_set(struct adapter *adapter, u32 level)
+{
+	rtw_hal_set_def_var(adapter, HW_DEF_ODM_DBG_LEVEL, &level);
+}
+
+void rtw_odm_ability_msg(void *sel, struct adapter *adapter)
+{
+	u32 ability = 0;
+	int i;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability);
+	DBG_871X_SEL_NL(sel, "odm.SupportAbility = 0x%08x\n", ability);
+	for (i = 0; i < RTW_ODM_ABILITY_MAX; i++) {
+		if (odm_ability_str[i])
+			DBG_871X_SEL_NL(sel, "%cBIT%-2d %s\n",
+					(BIT0 << i) & ability ? '+' : ' ', i,
+					odm_ability_str[i]);
+	}
+}
+
+inline void rtw_odm_ability_set(struct adapter *adapter, u32 ability)
+{
+	rtw_hal_set_hwreg(adapter, HW_VAR_DM_FLAG, (u8 *)&ability);
+}
+
+void rtw_odm_adaptivity_parm_msg(void *sel, struct adapter *adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	DBG_871X_SEL_NL(sel, "%10s %16s %8s %10s %11s %14s\n"
+		, "TH_L2H_ini", "TH_EDCCA_HL_diff", "IGI_Base", "ForceEDCCA", "AdapEn_RSSI", "IGI_LowerBound");
+	DBG_871X_SEL_NL(sel, "0x%-8x %-16d 0x%-6x %-10d %-11u %-14u\n"
+		, (u8)odm->TH_L2H_ini
+		, odm->TH_EDCCA_HL_diff
+		, odm->IGI_Base
+		, odm->ForceEDCCA
+		, odm->AdapEn_RSSI
+		, odm->IGI_LowerBound
+	);
+}
+
+void rtw_odm_adaptivity_parm_set(struct adapter *adapter, s8 TH_L2H_ini, s8 TH_EDCCA_HL_diff,
+	s8 IGI_Base, bool ForceEDCCA, u8 AdapEn_RSSI, u8 IGI_LowerBound)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	odm->TH_L2H_ini = TH_L2H_ini;
+	odm->TH_EDCCA_HL_diff = TH_EDCCA_HL_diff;
+	odm->IGI_Base = IGI_Base;
+	odm->ForceEDCCA = ForceEDCCA;
+	odm->AdapEn_RSSI = AdapEn_RSSI;
+	odm->IGI_LowerBound = IGI_LowerBound;
+}
+
+void rtw_odm_get_perpkt_rssi(void *sel, struct adapter *adapter)
+{
+	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+
+	DBG_871X_SEL_NL(sel, "RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+	HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B);
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
new file mode 100644
index 000000000000..f708dbf5bfd4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
@@ -0,0 +1,1421 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_PWRCTRL_C_
+
+#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++;
+	DBG_871X("==>ips_enter cnts:%d\n", pwrpriv->ips_enter_cnts);
+
+	if (rf_off == pwrpriv->change_rfpwrstate) {
+		pwrpriv->bpower_saving = true;
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save enter\n");
+
+		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);
+
+
+	rtw_btcoex_IpsNotify(padapter, pwrpriv->ips_mode_req);
+
+	down(&pwrpriv->lock);
+	_ips_enter(padapter);
+	up(&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++;
+		DBG_871X("==>ips_leave cnts:%d\n", pwrpriv->ips_leave_cnts);
+
+		result = rtw_ips_pwr_up(padapter);
+		if (result == _SUCCESS) {
+			pwrpriv->rf_pwrstate = rf_on;
+		}
+		DBG_871X_LEVEL(_drv_always_, "nolinked power save leave\n");
+
+		DBG_871X("==> ips_leave.....LED(0x%08x)...\n", rtw_read32(padapter, 0x4c));
+		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;
+
+	down(&pwrpriv->lock);
+	ret = _ips_leave(padapter);
+	up(&pwrpriv->lock);
+
+	if (_SUCCESS == ret)
+		rtw_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 == true) {
+		/* DBG_871X("%s: already in LPS or IPS mode\n", __func__); */
+		goto exit;
+	}
+
+	if (time_before(jiffies, adapter_to_pwrctl(adapter)->ips_deny_time)) {
+		/* DBG_871X("%s ips_deny_time\n", __func__); */
+		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) {
+		DBG_871X_LEVEL(_drv_always_, "There are some pkts to transmit\n");
+		DBG_871X_LEVEL(_drv_always_, "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;
+
+	down(&adapter_to_pwrctl(padapter)->lock);
+	ps_deny = rtw_ps_deny_get(padapter);
+	up(&adapter_to_pwrctl(padapter)->lock);
+	if (ps_deny != 0) {
+		DBG_871X(FUNC_ADPT_FMT ": ps_deny = 0x%08X, skip power save!\n",
+			FUNC_ADPT_ARG(padapter), ps_deny);
+		goto exit;
+	}
+
+	if (pwrpriv->bInSuspend == true) {/* system suspend or autosuspend */
+		pdbgpriv->dbg_ps_insuspend_cnt++;
+		DBG_871X("%s, pwrpriv->bInSuspend == true ignore this process\n", __func__);
+		return;
+	}
+
+	pwrpriv->ps_processing = true;
+
+	if (pwrpriv->ips_mode_req == IPS_NONE)
+		goto exit;
+
+	if (rtw_pwr_unassociated_idle(padapter) == false)
+		goto exit;
+
+	if ((pwrpriv->rf_pwrstate == rf_on) && ((pwrpriv->pwr_state_check_cnts%4) == 0)) {
+		DBG_871X("==>%s\n", __func__);
+		pwrpriv->change_rfpwrstate = rf_off;
+		{
+			ips_enter(padapter);
+		}
+	}
+exit:
+	pwrpriv->ps_processing = false;
+	return;
+}
+
+void pwr_state_check_handler(RTW_TIMER_HDL_ARGS);
+void pwr_state_check_handler(RTW_TIMER_HDL_ARGS)
+{
+	struct adapter *padapter = (struct adapter *)FunctionContext;
+	rtw_ps_cmd(padapter);
+}
+
+void traffic_check_for_leave_lps(struct adapter *padapter, u8 tx, u32 tx_packets)
+{
+	static unsigned long start_time = 0;
+	static u32 xmit_cnt = 0;
+	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)
+					&& (rtw_btcoex_IsBtControlLps(padapter) == false)
+					) {
+					DBG_871X("leave lps via Tx = %d\n", xmit_cnt);
+					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)
+				&& (rtw_btcoex_IsBtControlLps(padapter) == false)
+				) {
+				DBG_871X("leave lps via Rx = %d\n", pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod);
+				bLeaveLPS = true;
+			}
+		}
+	}
+
+	if (bLeaveLPS)
+		/* DBG_871X("leave lps via %s, Tx = %d, Rx = %d\n", tx?"Tx":"Rx", pmlmepriv->LinkDetectInfo.NumTxOkInPeriod, pmlmepriv->LinkDetectInfo.NumRxUnicastOkInPeriod); */
+		/* 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 == true) {
+		DBG_871X("%s: RPWM timeout, force to set RPWM(0x%02X) again!\n", __func__, pslv);
+	} else{
+		if ((pwrpriv->rpwm == pslv)
+			|| ((pwrpriv->rpwm >= PS_STATE_S2) && (pslv >= PS_STATE_S2))) {
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				("%s: Already set rpwm[0x%02X], new = 0x%02X!\n", __func__, pwrpriv->rpwm, pslv));
+			return;
+		}
+	}
+
+	if ((padapter->bSurpriseRemoved == true) ||
+		(padapter->hw_init_completed == false)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: SurpriseRemoved(%d) hw_init_completed(%d)\n",
+				  __func__, padapter->bSurpriseRemoved, padapter->hw_init_completed));
+
+		pwrpriv->cpwm = PS_STATE_S4;
+
+		return;
+	}
+
+	if (padapter->bDriverStopped == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+				 ("%s: change power state(0x%02X) when DriverStopped\n", __func__, pslv));
+
+		if (pslv < PS_STATE_S2) {
+			RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_,
+					 ("%s: Reject to enter PS_STATE(0x%02X) lower than S2 when DriverStopped!!\n", __func__, pslv));
+			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;
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("rtw_set_rpwm: rpwm = 0x%02x cpwm = 0x%02x\n", rpwm, pwrpriv->cpwm));
+
+	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) {
+				DBG_871X("%s: polling cpwm timeout! poll_cnt =%d, cpwm_orig =%02x, cpwm_now =%02x\n", __func__, poll_cnt, cpwm_orig, cpwm_now);
+				_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 defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_mode)
+		return true;
+	else if (true == pwrpriv->bInSuspend && pwrpriv->wowlan_ap_mode)
+		return true;
+	else if (true == pwrpriv->bInSuspend)
+		return false;
+#else
+	if (true == pwrpriv->bInSuspend)
+		return false;
+#endif
+
+	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 == false)) {
+		DBG_871X("Group handshake still in progress !!!\n");
+		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 defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	struct debug_priv *pdbgpriv = &padapter->dvobj->drv_dbg;
+#endif
+
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("%s: PowerMode =%d Smart_PS =%d\n",
+			  __func__, ps_mode, smart_ps));
+
+	if (ps_mode > PM_Card_Disable) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_err_, ("ps_mode:%d error\n", ps_mode));
+		return;
+	}
+
+	if (pwrpriv->pwr_mode == ps_mode)
+		if (PS_MODE_ACTIVE == ps_mode)
+			return;
+
+
+	down(&pwrpriv->lock);
+
+	/* if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) */
+	if (ps_mode == PS_MODE_ACTIVE) {
+		if (1
+			&& (((rtw_btcoex_IsBtControlLps(padapter) == false)
+					)
+				|| ((rtw_btcoex_IsBtControlLps(padapter) == true)
+					&& (rtw_btcoex_IsLpsOn(padapter) == false))
+				)
+			) {
+			DBG_871X(FUNC_ADPT_FMT" Leave 802.11 power save - %s\n",
+				FUNC_ADPT_ARG(padapter), msg);
+
+			pwrpriv->pwr_mode = ps_mode;
+			rtw_set_rpwm(padapter, PS_STATE_S4);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+			if (pwrpriv->wowlan_mode == true ||
+					pwrpriv->wowlan_ap_mode == true) {
+				unsigned long start_time;
+				u32 delay_ms;
+				u8 val8;
+				delay_ms = 20;
+				start_time = jiffies;
+				do {
+					rtw_hal_get_hwreg(padapter, HW_VAR_SYS_CLKR, &val8);
+					if (!(val8 & BIT(4))) { /* 0x08 bit4 = 1 --> in 32k, bit4 = 0 --> leave 32k */
+						pwrpriv->cpwm = PS_STATE_S4;
+						break;
+					}
+					if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
+						DBG_871X("%s: Wait for FW 32K leave more than %u ms!!!\n",
+								__func__, delay_ms);
+						pdbgpriv->dbg_wow_leave_ps_fail_cnt++;
+						break;
+					}
+					msleep(1);
+				} while (1);
+			}
+#endif
+			rtw_hal_set_hwreg(padapter, HW_VAR_H2C_FW_PWRMODE, (u8 *)(&ps_mode));
+			pwrpriv->bFwCurrentInPSMode = false;
+
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+		}
+	} else{
+		if ((PS_RDY_CHECK(padapter) && check_fwstate(&padapter->mlmepriv, WIFI_ASOC_STATE))
+			|| ((rtw_btcoex_IsBtControlLps(padapter) == true)
+				&& (rtw_btcoex_IsLpsOn(padapter) == true))
+			) {
+			u8 pslv;
+
+			DBG_871X(FUNC_ADPT_FMT" Enter 802.11 power save - %s\n",
+				FUNC_ADPT_ARG(padapter), msg);
+
+			rtw_btcoex_LpsNotify(padapter, ps_mode);
+
+			pwrpriv->bFwCurrentInPSMode = 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 ((rtw_btcoex_IsBtDisabled(padapter) == false)
+				&& (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+				u8 val8;
+
+				val8 = rtw_btcoex_LpsVal(padapter);
+				if (val8 & BIT(4))
+					pslv = PS_STATE_S2;
+			}
+
+			rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&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 (true == bAwake)
+			break;
+
+		if (true == padapter->bSurpriseRemoved) {
+			err = -2;
+			DBG_871X("%s: device surprise removed!!\n", __func__);
+			break;
+		}
+
+		if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
+			err = -1;
+			DBG_871X("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
+			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 (rtw_btcoex_IsBtControlLps(padapter) == true)
+		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) == false)
+			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) {
+				sprintf(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++;
+	}
+
+/* 	DBG_871X("-LeisurePSEnter\n"); */
+}
+
+/*  */
+/* 	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};
+
+/* 	DBG_871X("+LeisurePSLeave\n"); */
+
+	if (rtw_btcoex_IsBtControlLps(padapter) == true)
+		return;
+
+	if (pwrpriv->bLeisurePs) {
+		if (pwrpriv->pwr_mode != PS_MODE_ACTIVE) {
+			sprintf(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;
+/* 	DBG_871X("-LeisurePSLeave\n"); */
+
+}
+
+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);
+
+	DBG_871X("%s.....\n", __func__);
+
+	if (true == Adapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), Adapter->bSurpriseRemoved);
+		return;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true)) { /* connect */
+
+		if (pwrpriv->pwr_mode == PS_MODE_ACTIVE) {
+			DBG_871X("%s: Driver Already Leave LPS\n", __func__);
+			return;
+		}
+
+		down(&pwrpriv->lock);
+
+		rtw_set_rpwm(Adapter, PS_STATE_S4);
+
+		up(&pwrpriv->lock);
+
+		rtw_lps_ctrl_wk_cmd(pri_padapter, LPS_CTRL_LEAVE, 0);
+	} else{
+		if (pwrpriv->rf_pwrstate == rf_off)
+			if (false == ips_leave(pri_padapter))
+				DBG_871X("======> ips_leave fail.............\n");
+	}
+}
+
+/*  */
+/*  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) {
+		DBG_871X(FUNC_ADPT_FMT ": bup =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), Adapter->bup);
+		return;
+	}
+
+	if (Adapter->bSurpriseRemoved) {
+		DBG_871X(FUNC_ADPT_FMT ": bSurpriseRemoved =%d Skip!\n",
+			FUNC_ADPT_ARG(Adapter), 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) {
+			if (false == ips_leave(Adapter))
+				DBG_871X("======> ips_leave fail.............\n");
+		}
+	}
+}
+
+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;
+
+	yield();
+
+	while (1) {
+		down(&pwrpriv->lock);
+
+		if ((padapter->bSurpriseRemoved == true)
+			|| (padapter->hw_init_completed == false)
+			|| (pwrpriv->pwr_mode == PS_MODE_ACTIVE)
+			)
+			bReady = true;
+
+		up(&pwrpriv->lock);
+
+		if (true == bReady)
+			break;
+
+		if (jiffies_to_msecs(jiffies - start_time) > 100) {
+			DBG_871X("Wait for cpwm event  than 100 ms!!!\n");
+			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);
+
+	down(&pwrpriv->lock);
+
+	if (pwrpriv->rpwm < PS_STATE_S2) {
+		DBG_871X("%s: Redundant CPWM Int. RPWM = 0x%02X CPWM = 0x%02x\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		up(&pwrpriv->lock);
+		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)
+			up(&padapter->cmdpriv.cmd_queue_sema);
+
+		if (pwrpriv->alives & XMIT_ALIVE)
+			up(&padapter->xmitpriv.xmit_sema);
+	}
+
+	up(&pwrpriv->lock);
+
+exit:
+	RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+			 ("cpwm_int_hdl: cpwm = 0x%02x\n", pwrpriv->cpwm));
+}
+
+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;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	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;
+/* 	DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm); */
+
+	down(&pwrpriv->lock);
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("%s: rpwm = 0x%02X cpwm = 0x%02X CPWM done!\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+		goto exit;
+	}
+	up(&pwrpriv->lock);
+
+	if (rtw_read8(padapter, 0x100) != 0xEA) {
+		struct reportpwrstate_parm report;
+
+		report.state = PS_STATE_S2;
+		DBG_871X("\n%s: FW already leave 32K!\n\n", __func__);
+		cpwm_int_hdl(padapter, &report);
+
+		return;
+	}
+
+	down(&pwrpriv->lock);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		goto exit;
+	}
+	pwrpriv->brpwmtimeout = true;
+	rtw_set_rpwm(padapter, pwrpriv->rpwm);
+	pwrpriv->brpwmtimeout = false;
+
+exit:
+	up(&pwrpriv->lock);
+}
+
+/*
+ * This function is a timer handler, can't do any IO in it.
+ */
+static void pwr_rpwm_timeout_handler(void *FunctionContext)
+{
+	struct adapter *padapter;
+	struct pwrctrl_priv *pwrpriv;
+
+
+	padapter = (struct adapter *)FunctionContext;
+	pwrpriv = adapter_to_pwrctl(padapter);
+	DBG_871X("+%s: rpwm = 0x%02X cpwm = 0x%02X\n", __func__, pwrpriv->rpwm, pwrpriv->cpwm);
+
+	if ((pwrpriv->rpwm == pwrpriv->cpwm) || (pwrpriv->cpwm >= PS_STATE_S2)) {
+		DBG_871X("+%s: cpwm =%d, nothing to do!\n", __func__, pwrpriv->cpwm);
+		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;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, task);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: task = 0x%x cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, task, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		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 ((rtw_btcoex_IsBtDisabled(padapter) == false)
+		&& (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, task);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+
+	}
+
+	up(&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;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("rtw_register_tx_alive: cpwm = 0x%02x alives = 0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		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;
+
+	down(&pwrctrl->lock);
+
+	register_task_alive(pwrctrl, CMD_ALIVE);
+
+	if (pwrctrl->bFwCurrentInPSMode == true) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("rtw_register_cmd_alive: cpwm = 0x%02x alives = 0x%08x\n",
+				  pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm < pslv) {
+			if (pwrctrl->cpwm < PS_STATE_S2)
+				res = _FAIL;
+			if (pwrctrl->rpwm < pslv)
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+
+	if (_FAIL == res)
+		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 ((rtw_btcoex_IsBtDisabled(padapter) == false)
+		&& (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, XMIT_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_notice_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv)
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+	}
+
+	up(&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 ((rtw_btcoex_IsBtDisabled(padapter) == false)
+		&& (rtw_btcoex_IsBtControlLps(padapter) == true)) {
+		u8 val8;
+
+		val8 = rtw_btcoex_LpsVal(padapter);
+		if (val8 & BIT(4))
+			pslv = PS_STATE_S2;
+	}
+
+	down(&pwrctrl->lock);
+
+	unregister_task_alive(pwrctrl, CMD_ALIVE);
+
+	if ((pwrctrl->pwr_mode != PS_MODE_ACTIVE)
+		&& (pwrctrl->bFwCurrentInPSMode == true)) {
+		RT_TRACE(_module_rtl871x_pwrctrl_c_, _drv_info_,
+				 ("%s: cpwm = 0x%02x alives = 0x%08x\n",
+				  __func__, pwrctrl->cpwm, pwrctrl->alives));
+
+		if (pwrctrl->cpwm > pslv) {
+			if ((pslv >= PS_STATE_S2) || (pwrctrl->alives == 0))
+				rtw_set_rpwm(padapter, pslv);
+		}
+	}
+
+	up(&pwrctrl->lock);
+}
+
+void rtw_init_pwrctrl_priv(struct adapter *padapter)
+{
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	sema_init(&pwrctrlpriv->lock, 1);
+	sema_init(&pwrctrlpriv->check_32k_lock, 1);
+	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 = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+
+	pwrctrlpriv->bFwCurrentInPSMode = 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;
+	_init_workitem(&pwrctrlpriv->rpwmtimeoutwi, rpwmtimeout_workitem_callback, NULL);
+	_init_timer(&pwrctrlpriv->pwr_rpwm_timer, padapter->pnetdev, pwr_rpwm_timeout_handler, padapter);
+
+	rtw_init_timer(&pwrctrlpriv->pwr_state_check_timer, padapter, pwr_state_check_handler);
+
+	pwrctrlpriv->wowlan_mode = false;
+	pwrctrlpriv->wowlan_ap_mode = false;
+
+#ifdef CONFIG_PNO_SUPPORT
+	pwrctrlpriv->pno_inited = false;
+	pwrctrlpriv->pnlo_info = NULL;
+	pwrctrlpriv->pscan_info = NULL;
+	pwrctrlpriv->pno_ssid_list = NULL;
+	pwrctrlpriv->pno_in_resume = true;
+#endif
+}
+
+
+void rtw_free_pwrctrl_priv(struct adapter *adapter)
+{
+	/* memset((unsigned char *)pwrctrlpriv, 0, sizeof(struct pwrctrl_priv)); */
+
+#ifdef CONFIG_PNO_SUPPORT
+	if (pwrctrlpriv->pnlo_info != NULL)
+		printk("****** pnlo_info memory leak********\n");
+
+	if (pwrctrlpriv->pscan_info != NULL)
+		printk("****** pscan_info memory leak********\n");
+
+	if (pwrctrlpriv->pno_ssid_list != NULL)
+		printk("****** pno_ssid_list memory leak********\n");
+#endif
+}
+
+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 wiil 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) {
+		DBG_871X("%s wait ps_processing...\n", __func__);
+		while (pwrpriv->ps_processing && jiffies_to_msecs(jiffies - start) <= 3000)
+			msleep(10);
+		if (pwrpriv->ps_processing)
+			DBG_871X("%s wait ps_processing timeout\n", __func__);
+		else
+			DBG_871X("%s wait ps_processing done\n", __func__);
+	}
+
+	if (pwrpriv->bInternalAutoSuspend == false && pwrpriv->bInSuspend) {
+		DBG_871X("%s wait bInSuspend...\n", __func__);
+		while (pwrpriv->bInSuspend
+			&& jiffies_to_msecs(jiffies - start) <= 3000
+		) {
+			msleep(10);
+		}
+		if (pwrpriv->bInSuspend)
+			DBG_871X("%s wait bInSuspend timeout\n", __func__);
+		else
+			DBG_871X("%s wait bInSuspend done\n", __func__);
+	}
+
+	/* System suspend is not allowed to wakeup */
+	if ((pwrpriv->bInternalAutoSuspend == false) && (true == pwrpriv->bInSuspend)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* block??? */
+	if ((pwrpriv->bInternalAutoSuspend == true)  && (padapter->net_closed == true)) {
+		ret = _FAIL;
+		goto exit;
+	}
+
+	/* I think this should be check in IPS, LPS, autosuspend functions... */
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true) {
+		ret = _SUCCESS;
+		goto exit;
+	}
+
+	if (rf_off == pwrpriv->rf_pwrstate) {
+		{
+			DBG_8192C("%s call ips_leave....\n", __func__);
+			if (_FAIL ==  ips_leave(padapter)) {
+				DBG_8192C("======> ips_leave fail.............\n");
+				ret = _FAIL;
+				goto exit;
+			}
+		}
+	}
+
+	/* TODO: the following checking need to be merged... */
+	if (padapter->bDriverStopped
+		|| !padapter->bup
+		|| !padapter->hw_init_completed
+	) {
+		DBG_8192C("%s: bDriverStopped =%d, bup =%d, hw_init_completed =%u\n"
+			, caller
+			, 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 (PS_MODE_ACTIVE == mode)
+				LeaveAllPowerSaveMode(padapter);
+			else
+				pwrctrlpriv->LpsIdleCount = 2;
+
+			pwrctrlpriv->power_mgnt = mode;
+			pwrctrlpriv->bLeisurePs = (PS_MODE_ACTIVE != pwrctrlpriv->power_mgnt)?true:false;
+		}
+	} 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);
+		DBG_871X("%s %s\n", __func__, mode == IPS_NORMAL?"IPS_NORMAL":"IPS_LEVEL_2");
+		return 0;
+	} else if (mode == IPS_NONE) {
+		rtw_ips_mode_req(pwrctrlpriv, mode);
+		DBG_871X("%s %s\n", __func__, "IPS_NONE");
+		if ((padapter->bSurpriseRemoved == 0) && (_FAIL == rtw_pwr_wakeup(padapter)))
+			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;
+
+/* 	DBG_871X("+" FUNC_ADPT_FMT ": Request PS deny for %d (0x%08X)\n", */
+/* 		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	down(&pwrpriv->lock);
+	if (pwrpriv->ps_deny & BIT(reason)) {
+		DBG_871X(FUNC_ADPT_FMT ": [WARNING] Reason %d had been set before!!\n",
+			FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny |= BIT(reason);
+	up(&pwrpriv->lock);
+
+/* 	DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */
+/* 		FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
+}
+
+/*
+ * ATTENTION:
+ *This function will request pwrctrl LOCK!
+ */
+void rtw_ps_deny_cancel(struct adapter *padapter, enum PS_DENY_REASON reason)
+{
+	struct pwrctrl_priv *pwrpriv;
+
+
+/* 	DBG_871X("+" FUNC_ADPT_FMT ": Cancel PS deny for %d(0x%08X)\n", */
+/* 		FUNC_ADPT_ARG(padapter), reason, BIT(reason)); */
+
+	pwrpriv = adapter_to_pwrctl(padapter);
+
+	down(&pwrpriv->lock);
+	if ((pwrpriv->ps_deny & BIT(reason)) == 0) {
+		DBG_871X(FUNC_ADPT_FMT ": [ERROR] Reason %d had been canceled before!!\n",
+			FUNC_ADPT_ARG(padapter), reason);
+	}
+	pwrpriv->ps_deny &= ~BIT(reason);
+	up(&pwrpriv->lock);
+
+/* 	DBG_871X("-" FUNC_ADPT_FMT ": Now PS deny for 0x%08X\n", */
+/* 		FUNC_ADPT_ARG(padapter), pwrpriv->ps_deny); */
+}
+
+/*
+ * 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)
+{
+	u32 deny;
+
+
+	deny = adapter_to_pwrctl(padapter)->ps_deny;
+
+	return deny;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_recv.c b/drivers/staging/rtl8723bs/core/rtw_recv.c
new file mode 100644
index 000000000000..695a5c958c80
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_recv.c
@@ -0,0 +1,2689 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_RECV_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.h>
+#include <rtw_recv.h>
+
+static u8 SNAP_ETH_TYPE_IPX[2] = {0x81, 0x37};
+static u8 SNAP_ETH_TYPE_APPLETALK_AARP[2] = {0x80, 0xf3};
+
+u8 rtw_rfc1042_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+u8 rtw_bridge_tunnel_header[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS);
+
+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]); */
+
+	_rtw_init_queue(&psta_recvpriv->defrag_q);
+}
+
+sint _rtw_init_recv_priv(struct recv_priv *precvpriv, struct adapter *padapter)
+{
+	sint i;
+	union recv_frame *precvframe;
+	sint	res = _SUCCESS;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((unsigned char *)precvpriv, 0, sizeof (struct  recv_priv)); */
+
+	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(union recv_frame) + RXFRAME_ALIGN_SZ);
+
+	if (precvpriv->pallocated_frame_buf == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+	/* memset(precvpriv->pallocated_frame_buf, 0, NR_RECVFRAME * sizeof(union recv_frame) + RXFRAME_ALIGN_SZ); */
+
+	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));
+
+		res = 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);
+
+	rtw_init_timer(&precvpriv->signal_stat_timer, padapter, rtw_signal_stat_timer_hdl);
+
+	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);
+
+	if (precvpriv->pallocated_frame_buf)
+		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 = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		list_del_init(&precvframe->u.hdr.list);
+		padapter = precvframe->u.hdr.adapter;
+		if (padapter != NULL) {
+			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 != NULL) {
+		if (pfree_recv_queue == &precvpriv->free_recv_queue)
+				precvpriv->free_recvframe_cnt++;
+	}
+	spin_unlock_bh(&pfree_recv_queue->lock);
+	return _SUCCESS;
+}
+
+
+
+
+sint _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 != NULL)
+		if (queue == &precvpriv->free_recv_queue)
+			precvpriv->free_recvframe_cnt++;
+
+	return _SUCCESS;
+}
+
+sint rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue)
+{
+	sint 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;
+}
+
+/*
+sint	rtw_enqueue_recvframe(union recv_frame *precvframe, struct __queue *queue)
+{
+	return rtw_free_recvframe(precvframe, queue);
+}
+*/
+
+
+
+
+/*
+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 = LIST_CONTAINOR(plist, union recv_frame, u);
+
+		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++;
+	}
+
+	if (cnt)
+		DBG_871X(FUNC_ADPT_FMT" dequeue %d\n", FUNC_ADPT_ARG(adapter), cnt);
+
+	return cnt;
+}
+
+
+sint 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;
+}
+
+sint 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 = LIST_CONTAINOR(plist, struct recv_buf, list);
+
+		list_del_init(&precvbuf->list);
+
+	}
+
+	spin_unlock_bh(&queue->lock);
+
+	return precvbuf;
+
+}
+
+sint recvframe_chkmic(struct adapter *adapter,  union recv_frame *precvframe);
+sint recvframe_chkmic(struct adapter *adapter,  union recv_frame *precvframe)
+{
+
+	sint	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_) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:prxattrib->encrypt == _TKIP_\n"));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic:da = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+			prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2], prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5]));
+
+		/* calculate mic code */
+		if (stainfo != NULL) {
+			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];
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n recvframe_chkmic: bcmc key\n"));
+				/* DBG_871X("\n recvframe_chkmic: bcmc key psecuritypriv->dot118021XGrpKeyid(%d), pmlmeinfo->key_index(%d) , recv key_id(%d)\n", */
+				/* 								psecuritypriv->dot118021XGrpKeyid, pmlmeinfo->key_index, rxdata_key_idx); */
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					res = _FAIL;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n"));
+					DBG_871X("\n recvframe_chkmic:didn't install group key!!!!!!!!!!\n");
+					goto exit;
+				}
+			} else {
+				mickey = &stainfo->dot11tkiprxmickey.skey[0];
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n recvframe_chkmic: unicast key\n"));
+			}
+
+			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;
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n prxattrib->iv_len =%d prxattrib->icv_len =%d\n", prxattrib->iv_len, prxattrib->icv_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)) {
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic:miccode[%d](%02x) != *(pframemic+%d)(%02x) ", i, miccode[i], i, *(pframemic+i)));
+					bmic_err = true;
+				}
+			}
+
+
+			if (bmic_err == true) {
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-8)-*(pframemic-1) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-8), *(pframemic-7), *(pframemic-6), *(pframemic-5), *(pframemic-4), *(pframemic-3), *(pframemic-2), *(pframemic-1)));
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n *(pframemic-16)-*(pframemic-9) = 0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x\n",
+					*(pframemic-16), *(pframemic-15), *(pframemic-14), *(pframemic-13), *(pframemic-12), *(pframemic-11), *(pframemic-10), *(pframemic-9)));
+
+				{
+					uint i;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet (len =%d) ======\n", precvframe->u.hdr.len));
+					for (i = 0; i < precvframe->u.hdr.len; i = i+8) {
+						RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x:0x%02x",
+							*(precvframe->u.hdr.rx_data+i), *(precvframe->u.hdr.rx_data+i+1),
+							*(precvframe->u.hdr.rx_data+i+2), *(precvframe->u.hdr.rx_data+i+3),
+							*(precvframe->u.hdr.rx_data+i+4), *(precvframe->u.hdr.rx_data+i+5),
+							*(precvframe->u.hdr.rx_data+i+6), *(precvframe->u.hdr.rx_data+i+7)));
+					}
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n ======demp packet end [len =%d]======\n", precvframe->u.hdr.len));
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("\n hrdlen =%d,\n", prxattrib->hdrlen));
+				}
+
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ra = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x psecuritypriv->binstallGrpkey =%d ",
+					prxattrib->ra[0], prxattrib->ra[1], prxattrib->ra[2],
+					prxattrib->ra[3], prxattrib->ra[4], prxattrib->ra[5], psecuritypriv->binstallGrpkey));
+
+				/*  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 == true) && (brpt_micerror == true)) {
+					rtw_handle_tkip_mic_err(adapter, (u8)IS_MCAST(prxattrib->ra));
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted =%d\n", prxattrib->bdecrypted);
+				} else{
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" mic error :prxattrib->bdecrypted =%d ", prxattrib->bdecrypted));
+					DBG_871X(" mic error :prxattrib->bdecrypted =%d\n", prxattrib->bdecrypted);
+				}
+
+				res = _FAIL;
+
+			} else {
+				/* mic checked ok */
+				if ((psecuritypriv->bcheck_grpkey == false) && (IS_MCAST(prxattrib->ra) == true)) {
+					psecuritypriv->bcheck_grpkey = true;
+					RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("psecuritypriv->bcheck_grpkey =true"));
+				}
+			}
+
+		} else
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic: rtw_get_stainfo == NULL!!!\n"));
+
+		recvframe_pull_tail(precvframe, 8);
+
+	}
+
+exit:
+	return res;
+
+}
+
+/* decrypt and set the ivlen, icvlen of the recv_frame */
+union recv_frame *decryptor(struct adapter *padapter, union recv_frame *precv_frame);
+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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("prxstat->decrypted =%x prxattrib->encrypt = 0x%03x\n", prxattrib->bdecrypted, prxattrib->encrypt));
+
+	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) {
+			DBG_871X("prxattrib->key_index(%d) > WEP_KEYS\n", prxattrib->key_index);
+
+			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;
+
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+			__func__,
+			__LINE__,
+			prxattrib->bdecrypted,
+			prxattrib->encrypt,
+			psecuritypriv->hw_decrypted);
+		#endif
+
+		switch (prxattrib->encrypt) {
+		case _WEP40_:
+		case _WEP104_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_wep);
+			rtw_wep_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _TKIP_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_tkip);
+			res = rtw_tkip_decrypt(padapter, (u8 *)precv_frame);
+			break;
+		case _AES_:
+			DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_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_)
+		) {
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_hw);
+
+		psecuritypriv->hw_decrypted = true;
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+			__func__,
+			__LINE__,
+			prxattrib->bdecrypted,
+			prxattrib->encrypt,
+			psecuritypriv->hw_decrypted);
+
+		#endif
+	} else {
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_unknown);
+		#ifdef DBG_RX_DECRYPTOR
+		DBG_871X("[%s] %d:prxstat->bdecrypted:%d,  prxattrib->encrypt:%d,  Setting psecuritypriv->hw_decrypted = %d\n",
+			__func__,
+			__LINE__,
+			prxattrib->bdecrypted,
+			prxattrib->encrypt,
+			psecuritypriv->hw_decrypted);
+		#endif
+	}
+
+	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 */
+union recv_frame *portctrl(struct adapter *adapter, union recv_frame *precv_frame);
+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 = get_recvframe_data(precv_frame);
+	pfhdr = &precv_frame->u.hdr;
+	pattrib = &pfhdr->attrib;
+	psta_addr = pattrib->ta;
+
+	prtnframe = NULL;
+
+	psta = rtw_get_stainfo(pstapriv, psta_addr);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:adapter->securitypriv.dot11AuthAlgrthm =%d\n", adapter->securitypriv.dot11AuthAlgrthm));
+
+	if (auth_alg == 2) {
+		if ((psta != NULL) && (psta->ieee8021x_blocked)) {
+			__be16 be_tmp;
+
+			/* blocked */
+			/* only accept EAPOL frame */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 1\n"));
+
+			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 */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:psta->ieee8021x_blocked == 0\n"));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:precv_frame->hdr.attrib.privacy =%x\n", precv_frame->u.hdr.attrib.privacy));
+
+			if (pattrib->bdecrypted == 0)
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("portctrl:prxstat->decrypted =%x\n", pattrib->bdecrypted));
+
+			prtnframe = precv_frame;
+			/* check is the EAPOL frame or not (Rekey) */
+			/* if (ether_type == eapol_type) { */
+			/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("########portctrl:ether_type == 0x888e\n")); */
+				/* check Rekey */
+
+			/* 	prtnframe =precv_frame; */
+			/*  */
+			/* else { */
+			/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("########portctrl:ether_type = 0x%04x\n", ether_type)); */
+			/*  */
+		}
+	} else
+		prtnframe = precv_frame;
+
+	return prtnframe;
+}
+
+sint recv_decache(union recv_frame *precv_frame, u8 bretry, struct stainfo_rxcache *prxcache);
+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) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, (tid>15)! seq_ctrl = 0x%x, tid = 0x%x\n", seq_ctrl, tid));
+
+		return _FAIL;
+	}
+
+	if (1) { /* if (bretry) */
+		if (seq_ctrl == prxcache->tid_rxseq[tid]) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_decache, seq_ctrl = 0x%x, tid = 0x%x, tid_rxseq = 0x%x\n", seq_ctrl, tid, prxcache->tid_rxseq[tid]));
+
+			return _FAIL;
+		}
+	}
+
+	prxcache->tid_rxseq[tid] = seq_ctrl;
+
+	return _SUCCESS;
+
+}
+
+void process_pwrbit_data(struct adapter *padapter, union recv_frame *precv_frame);
+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);
+
+				/* DBG_871X("to sleep, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		} 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);
+
+				/* DBG_871X("to wakeup, sta_dz_bitmap =%x\n", pstapriv->sta_dz_bitmap); */
+			}
+		}
+
+	}
+}
+
+void process_wmmps_data(struct adapter *padapter, union recv_frame *precv_frame);
+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);
+		}
+	}
+}
+
+void count_rx_stats(struct adapter *padapter, union recv_frame *prframe, struct sta_info *sta);
+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);
+}
+
+sint sta2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta
+);
+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;
+	sint bmcast = IS_MCAST(pattrib->dst);
+
+	/* DBG_871X("[%s] %d, seqnum:%d\n", __func__, __LINE__, pattrib->seq_num); */
+
+	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)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n"));
+			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)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("bssid != TA under STATION_MODE; drop pkt\n"));
+			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 == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under sta2sta_data_frame ; drop pkt\n"));
+		ret = _FAIL;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+sint ap2sta_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+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;
+	sint ret = _SUCCESS;
+	struct	sta_priv 	*pstapriv = &adapter->stapriv;
+	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *mybssid  = get_bssid(pmlmepriv);
+	u8 *myhwaddr = myid(&adapter->eeprompriv);
+	sint 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)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" SA ==myself\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s SA ="MAC_FMT", myhwaddr ="MAC_FMT"\n",
+				__func__, MAC_ARG(pattrib->src), MAC_ARG(myhwaddr));
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+		/*  da should be for me */
+		if ((memcmp(myhwaddr, pattrib->dst, ETH_ALEN)) && (!bmcast)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+				(" ap2sta_data_frame:  compare DA fail; DA ="MAC_FMT"\n", MAC_ARG(pattrib->dst)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s DA ="MAC_FMT"\n", __func__, MAC_ARG(pattrib->dst));
+			#endif
+			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))) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_,
+				(" ap2sta_data_frame:  compare BSSID fail ; BSSID ="MAC_FMT"\n", MAC_ARG(pattrib->bssid)));
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("mybssid ="MAC_FMT"\n", MAC_ARG(mybssid)));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s BSSID ="MAC_FMT", mybssid ="MAC_FMT"\n",
+				__func__, MAC_ARG(pattrib->bssid), MAC_ARG(mybssid));
+			DBG_871X("this adapter = %d, buddy adapter = %d\n", adapter->adapter_type, adapter->pbuddystruct adapter->adapter_type);
+			#endif
+
+			if (!bmcast) {
+				DBG_871X("issue_deauth to the nonassociated ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+				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 == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("ap2sta: can't get psta under STATION_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under STATION_MODE ; drop pkt\n", __func__);
+			#endif
+			ret = _FAIL;
+			goto exit;
+		}
+
+		if ((GetFrameSubType(ptr) & WIFI_QOS_DATA_TYPE) == WIFI_QOS_DATA_TYPE) {
+		}
+
+		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 == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under MP_MODE ; drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s can't get psta under WIFI_MP_STATE ; drop pkt\n", __func__);
+			#endif
+			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 == NULL) {
+
+				/* for AP multicast issue , modify by yiwei */
+				static unsigned long send_issue_deauth_time = 0;
+
+				/* DBG_871X("After send deauth , %u ms has elapsed.\n", jiffies_to_msecs(jiffies - send_issue_deauth_time)); */
+
+				if (jiffies_to_msecs(jiffies - send_issue_deauth_time) > 10000 || send_issue_deauth_time == 0) {
+					send_issue_deauth_time = jiffies;
+
+					DBG_871X("issue_deauth to the ap =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->bssid));
+
+					issue_deauth(adapter, pattrib->bssid, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+				}
+			}
+		}
+
+		ret = _FAIL;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s fw_state:0x%x\n", __func__, get_fwstate(pmlmepriv));
+		#endif
+	}
+
+exit:
+	return ret;
+}
+
+sint sta2ap_data_frame(
+	struct adapter *adapter,
+	union recv_frame *precv_frame,
+	struct sta_info **psta);
+sint 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);
+	sint 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 == NULL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("can't get psta under AP_MODE; drop pkt\n"));
+			DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+
+			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;
+		}
+		DBG_871X("issue_deauth to sta =" MAC_FMT " for the reason(7)\n", MAC_ARG(pattrib->src));
+		issue_deauth(adapter, pattrib->src, WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA);
+		ret = RTW_RX_HANDLED;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+sint validate_recv_ctrl_frame(struct adapter *padapter, union recv_frame *precv_frame);
+sint 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; */
+
+	/* DBG_871X("+validate_recv_ctrl_frame\n"); */
+
+	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 == NULL)
+		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) {
+			DBG_871X("%s alive check-rx ps-poll\n", __func__);
+			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 = LIST_CONTAINOR(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;
+
+				/* DBG_871X("handling ps-poll, q_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+				rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+				if (psta->sleepq_len == 0) {
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* DBG_871X("after handling ps-poll, tim =%x\n", pstapriv->tim_bitmap); */
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, _TIM_IE_, 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);
+
+				/* DBG_871X("no buffered packets to xmit\n"); */
+				if (pstapriv->tim_bitmap&BIT(psta->aid)) {
+					if (psta->sleepq_len == 0) {
+						DBG_871X("no buffered packets to xmit\n");
+
+						/* issue nulldata with More data bit = 0 to indicate we have no buffered packets */
+						issue_nulldata_in_interrupt(padapter, psta->hwaddr);
+					} else{
+						DBG_871X("error!psta->sleepq_len =%d\n", psta->sleepq_len);
+						psta->sleepq_len = 0;
+					}
+
+					pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+					/* upate BCN for TIM IE */
+					/* update_BCNTIM(padapter); */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+				}
+			}
+		}
+	}
+
+	return _FAIL;
+
+}
+
+union recv_frame *recvframe_chk_defrag(struct adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame);
+sint validate_recv_mgnt_frame(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	/* struct mlme_priv *pmlmepriv = &adapter->mlmepriv; */
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+validate_recv_mgnt_frame\n"));
+
+	precv_frame = recvframe_chk_defrag(padapter, precv_frame);
+	if (precv_frame == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("%s: fragment packet\n", __func__));
+		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;
+
+}
+
+sint validate_recv_data_frame(struct adapter *adapter, union recv_frame *precv_frame);
+sint 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;
+	sint ret = _SUCCESS;
+
+	bretry = GetRetry(ptr);
+	pda = get_da(ptr);
+	psa = get_sa(ptr);
+	pbssid = get_hdr_bssid(ptr);
+
+	if (pbssid == NULL) {
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s pbssid == NULL\n", __func__);
+		#endif
+		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;
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" case 3\n"));
+		break;
+
+	default:
+		ret = _FAIL;
+		break;
+
+	}
+
+	if (ret == _FAIL) {
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s case:%d, res:%d\n", __func__, pattrib->to_fr_ds, ret);
+		#endif
+		goto exit;
+	} else if (ret == RTW_RX_HANDLED) {
+		goto exit;
+	}
+
+
+	if (psta == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, (" after to_fr_ds_chk; psta == NULL\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s psta == NULL\n", __func__);
+		#endif
+		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) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decache : drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recv_decache return _FAIL\n", __func__);
+		#endif
+		ret = _FAIL;
+		goto exit;
+	}
+
+	if (pattrib->privacy) {
+
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("validate_recv_data_frame:pattrib->privacy =%x\n", pattrib->privacy));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ^^^^^^^^^^^IS_MCAST(pattrib->ra(0x%02x)) =%d^^^^^^^^^^^^^^^6\n", pattrib->ra[0], IS_MCAST(pattrib->ra)));
+
+		GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, IS_MCAST(pattrib->ra));
+
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n pattrib->encrypt =%d\n", pattrib->encrypt));
+
+		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 sint 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 type;
+	u8 subtype;
+
+	type =  GetFrameType(ptr);
+	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 *ppp, *mgmt_DATA;
+			u32 data_len = 0;
+			ppp = GetAddr2Ptr(ptr);
+
+			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 == NULL) {
+				DBG_871X("%s mgmt allocate fail  !!!!!!!!!\n", __func__);
+				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) {
+				DBG_871X("%s mgmt descrypt fail  !!!!!!!!!\n", __func__);
+				goto validate_80211w_fail;
+			}
+		} else if (IS_MCAST(GetAddr1Ptr(ptr)) &&
+			(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC)) {
+			sint 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) {
+				/* DBG_871X("802.11w BIP verify fail\n"); */
+				goto validate_80211w_fail;
+			} else if (BIP_ret == RTW_RX_HANDLED) {
+				/* DBG_871X("802.11w recv none protected packet\n"); */
+				/* 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) {
+					DBG_871X("action frame category =%d should robust\n", ptr[WLAN_HDR_A3_LEN]);
+					goto validate_80211w_fail;
+				}
+			} else if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC) {
+				DBG_871X("802.11w recv none protected packet\n");
+				/* issue sa query request */
+				issue_action_SA_Query(adapter, NULL, 0, 0);
+				goto validate_80211w_fail;
+			}
+		}
+	}
+	return _SUCCESS;
+
+validate_80211w_fail:
+	return _FAIL;
+
+}
+
+static inline void dump_rx_packet(u8 *ptr)
+{
+	int i;
+
+	DBG_871X("#############################\n");
+	for (i = 0; i < 64; i = i+8)
+		DBG_871X("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X:\n", *(ptr+i),
+		*(ptr+i+1), *(ptr+i+2), *(ptr+i+3), *(ptr+i+4), *(ptr+i+5), *(ptr+i+6), *(ptr+i+7));
+	DBG_871X("#############################\n");
+}
+
+sint validate_recv_frame(struct adapter *adapter, union recv_frame *precv_frame);
+sint 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;
+	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) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! (ver!= 0)\n"));
+		retval = _FAIL;
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_ver_err);
+		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));
+	if (bDumpRxPkt == 1) /* dump all rx packets */
+		dump_rx_packet(ptr);
+	else if ((bDumpRxPkt == 2) && (type == WIFI_MGT_TYPE))
+		dump_rx_packet(ptr);
+	else if ((bDumpRxPkt == 3) && (type == WIFI_DATA_TYPE))
+		dump_rx_packet(ptr);
+
+	switch (type) {
+	case WIFI_MGT_TYPE: /* mgnt */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt);
+		if (validate_80211w_mgmt(adapter, precv_frame) == _FAIL) {
+			retval = _FAIL;
+			DBG_COUNTER(padapter->rx_logs.core_rx_pre_mgmt_err_80211w);
+			break;
+		}
+
+		retval = validate_recv_mgnt_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_mgnt_frame fail\n"));
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_mgmt_err);
+		}
+		retval = _FAIL; /*  only data frame return _SUCCESS */
+		break;
+	case WIFI_CTRL_TYPE: /* ctrl */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl);
+		retval = validate_recv_ctrl_frame(adapter, precv_frame);
+		if (retval == _FAIL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_ctrl_frame fail\n"));
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_ctrl_err);
+		}
+		retval = _FAIL; /*  only data frame return _SUCCESS */
+		break;
+	case WIFI_DATA_TYPE: /* data */
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_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;
+			/* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail\n")); */
+			precvpriv->rx_drop++;
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_err);
+		} 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_SIZE, 2);
+			eth_type = ntohs((unsigned short) eth_type);
+			if ((bDumpRxPkt == 4) && (eth_type == 0x888e))
+				dump_rx_packet(ptr);
+#endif
+		} else
+			DBG_COUNTER(adapter->rx_logs.core_rx_pre_data_handled);
+		break;
+	default:
+		DBG_COUNTER(adapter->rx_logs.core_rx_pre_unknown);
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("validate_recv_data_frame fail! type = 0x%x\n", type));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME validate_recv_data_frame fail! type = 0x%x\n", type);
+		#endif
+		retval = _FAIL;
+		break;
+	}
+
+exit:
+	return retval;
+}
+
+
+/* remove the wlanhdr and add the eth_hdr */
+sint wlanhdr_to_ethhdr(union recv_frame *precvframe);
+sint wlanhdr_to_ethhdr(union recv_frame *precvframe)
+{
+	sint	rmv_len;
+	u16 eth_type, len;
+	u8 bsnaphdr;
+	u8 *psnap_type;
+	struct ieee80211_snap_hdr	*psnap;
+	__be16 be_tmp;
+	sint ret = _SUCCESS;
+	struct adapter			*adapter = precvframe->u.hdr.adapter;
+	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
+	u8 *ptr = get_recvframe_data(precvframe) ; /*  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, rtw_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, 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->u.hdr.len - rmv_len;
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("\n ===pattrib->hdrlen: %x,  pattrib->iv_len:%x ===\n\n", pattrib->hdrlen,  pattrib->iv_len));
+
+	memcpy(&be_tmp, ptr+rmv_len, 2);
+	eth_type = ntohs(be_tmp); /* pattrib->ether_type */
+	pattrib->eth_type = eth_type;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (0x8899 == pattrib->eth_type) {
+		struct sta_info *psta = precvframe->u.hdr.psta;
+
+		DBG_871X("wlan rx: got eth_type = 0x%x\n", pattrib->eth_type);
+
+		if (psta && psta->isrc && psta->pid > 0) {
+			u16 rx_pid;
+
+			rx_pid = *(u16 *)(ptr+rmv_len+2);
+
+			DBG_871X("wlan rx(pid = 0x%x): sta("MAC_FMT") pid = 0x%x\n",
+				rx_pid, MAC_ARG(psta->hwaddr), psta->pid);
+
+			if (rx_pid == psta->pid) {
+				int i;
+				u16 len = *(u16 *)(ptr+rmv_len+4);
+				/* u16 ctrl_type = *(u16*)(ptr+rmv_len+6); */
+
+				/* DBG_871X("RC: len = 0x%x, ctrl_type = 0x%x\n", len, ctrl_type); */
+				DBG_871X("RC: len = 0x%x\n", len);
+
+				for (i = 0; i < len ; i++)
+					DBG_871X("0x%x\n", *(ptr+rmv_len+6+i));
+					/* DBG_871X("0x%x\n", *(ptr+rmv_len+8+i)); */
+
+				DBG_871X("RC-end\n");
+			}
+		}
+	}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	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);
+		memcpy(ptr, get_rxmem(precvframe), 24);
+		ptr += 24;
+	} else
+		ptr = recvframe_pull(precvframe, (rmv_len-sizeof(struct ethhdr) + (bsnaphdr?2:0)));
+
+	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 union recv_frame *recvframe_defrag(struct adapter *adapter,
+					  struct __queue *defrag_q)
+{
+	struct list_head	 *plist, *phead;
+	u8 *data, 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 = LIST_CONTAINOR(plist, union recv_frame, u);
+	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);
+
+	data = get_recvframe_data(prframe);
+
+	while (phead != plist) {
+		pnextrframe = LIST_CONTAINOR(plist, union recv_frame, u);
+		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);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Performance defrag!!!!!\n"));
+
+	return prframe;
+}
+
+/* check if need to defrag, if needed queue the frame to defrag_q */
+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 == NULL) {
+		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 != NULL) {
+			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); */
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("Enqueuq: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+
+			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;
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+		}
+
+	}
+
+	if ((ismfrag == 0) && (fragnum != 0)) {
+		/* the last fragment frame */
+		/* enqueue the last fragment */
+		if (pdefrag_q != NULL) {
+			/* 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 */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("defrag: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+			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;
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("Free because pdefrag_q == NULL: ismfrag = %d, fragnum = %d\n", ismfrag, fragnum));
+		}
+
+	}
+
+
+	if ((prtnframe != NULL) && (prtnframe->u.hdr.attrib.privacy)) {
+		/* after defrag we must check tkip mic code */
+		if (recvframe_chkmic(padapter,  prtnframe) == _FAIL) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chkmic(padapter,  prtnframe) == _FAIL\n"));
+			rtw_free_recvframe(prtnframe, pfree_recv_queue);
+			prtnframe = NULL;
+		}
+	}
+	return prtnframe;
+}
+
+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;
+	_pkt *sub_pkt, *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;
+
+	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 = RTW_GET_BE16(pdata + 12);
+
+		if (a_len < (ETHERNET_HEADER_SIZE + nSubframe_Length)) {
+			DBG_871X("nRemain_Length is %d and nSubframe_Length is : %d\n", a_len, nSubframe_Length);
+			break;
+		}
+
+		sub_pkt = rtw_os_alloc_msdu_pkt(prframe, nSubframe_Length, pdata);
+		if (sub_pkt == NULL) {
+			DBG_871X("%s(): allocate sub packet fail !!!\n", __func__);
+			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) {
+			DBG_871X("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) & (4-1));
+			if (padding_len == 4) {
+				padding_len = 0;
+			}
+
+			if (a_len < padding_len) {
+				DBG_871X("ParseSubframe(): a_len < padding_len !\n");
+				break;
+			}
+			pdata += padding_len;
+			a_len -= padding_len;
+		}
+	}
+
+	for (i = 0; i < nr_subframes; i++) {
+		sub_pkt = subframes[i];
+
+		/* Indicat 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 ret;
+}
+
+int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num);
+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;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d init IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+
+		/* DbgPrint("check_indicate_seq, 1st->indicate_seq =%d\n", precvpriv->indicate_seq); */
+	}
+
+	/* DbgPrint("enter->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+	/*  Drop out the packet which SeqNum is smaller than WinStart */
+	if (SN_LESS(seq_num, preorder_ctrl->indicate_seq)) {
+		/* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */
+		/* DbgPrint("CheckRxTsIndicateSeq(): Packet Drop! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("%s IndicateSeq: %d > NewSeq: %d\n", __func__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+
+
+		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;
+
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_EQUAL IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	} else if (SN_LESS(wend, seq_num)) {
+		/* RT_TRACE(COMP_RX_REORDER, DBG_LOUD, ("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, NewSeqNum)); */
+		/* DbgPrint("CheckRxTsIndicateSeq(): Window Shift! IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, 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++;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d SN_LESS(wend, seq_num) IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, seq_num);
+		#endif
+	}
+
+	/* DbgPrint("exit->check_indicate_seq(): IndicateSeq: %d, NewSeq: %d\n", precvpriv->indicate_seq, seq_num); */
+
+	return true;
+}
+
+int enqueue_reorder_recvframe(struct recv_reorder_ctrl *preorder_ctrl, union recv_frame *prframe);
+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;
+
+	/* DbgPrint("+enqueue_reorder_recvframe()\n"); */
+
+	/* 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 = LIST_CONTAINOR(plist, union recv_frame, u);
+		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. */
+			/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Duplicate packet is dropped!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
+			/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+			return false;
+		else
+			break;
+
+		/* DbgPrint("enqueue_reorder_recvframe():while\n"); */
+
+	}
+
+
+	/* 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); */
+
+
+	/* RT_TRACE(COMP_RX_REORDER, DBG_TRACE, ("InsertRxReorderList(): Pkt insert into buffer!! IndicateSeq: %d, NewSeq: %d\n", pTS->RxIndicateSeq, SeqNum)); */
+	return true;
+
+}
+
+void recv_indicatepkts_pkt_loss_cnt(struct debug_priv *pdbgpriv, u64 prev_seq, u64 current_seq);
+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);
+
+}
+int recv_indicatepkts_in_order(struct adapter *padapter, struct recv_reorder_ctrl *preorder_ctrl, int bforced);
+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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_in_oder);
+
+	/* DbgPrint("+recv_indicatepkts_in_order\n"); */
+
+	/* 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 = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, pattrib->seq_num);
+		#endif
+		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 = LIST_CONTAINOR(plist, union recv_frame, u);
+		pattrib = &prframe->u.hdr.attrib;
+
+		if (!SN_LESS(preorder_ctrl->indicate_seq, pattrib->seq_num)) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+				 ("recv_indicatepkts_in_order: indicate =%d seq =%d amsdu =%d\n",
+				  preorder_ctrl->indicate_seq, pattrib->seq_num, pattrib->amsdu));
+
+			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;
+				#ifdef DBG_RX_SEQ
+				DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+					preorder_ctrl->indicate_seq, pattrib->seq_num);
+				#endif
+			}
+
+			/* Set this as a lock to make sure that only one thread is indicating packet. */
+			/* pTS->RxIndicateState = RXTS_INDICATE_PROCESSING; */
+
+			/*  Indicate packets */
+			/* RT_ASSERT((index<=REORDER_WIN_SIZE), ("RxReorderIndicatePacket(): Rx Reorder buffer full!!\n")); */
+
+
+			/* indicate this recv_frame */
+			/* DbgPrint("recv_indicatepkts_in_order, indicate_seq =%d, seq_num =%d\n", precvpriv->indicate_seq, pattrib->seq_num); */
+			if (!pattrib->amsdu) {
+				/* DBG_871X("recv_indicatepkts_in_order, amsdu!= 1, indicate_seq =%d, seq_num =%d\n", preorder_ctrl->indicate_seq, pattrib->seq_num); */
+
+				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;
+		}
+
+		/* DbgPrint("recv_indicatepkts_in_order():while\n"); */
+
+	}
+
+	/* spin_unlock(&ppending_recvframe_queue->lock); */
+	/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+
+	return bPktInBuf;
+}
+
+int recv_indicatepkt_reorder(struct adapter *padapter, union recv_frame *prframe);
+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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_reoder);
+
+	if (!pattrib->amsdu) {
+		/* s1. */
+		wlanhdr_to_ethhdr(prframe);
+
+		if (pattrib->qos != 1) {
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@  recv_indicatepkt_reorder -recv_func recv_indicatepkt\n"));
+
+				rtw_recv_indicatepkt(padapter, prframe);
+				return _SUCCESS;
+
+			}
+
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s pattrib->qos != 1\n", __func__);
+			#endif
+
+			return _FAIL;
+
+		}
+
+		if (preorder_ctrl->enable == false) {
+			/* indicate this recv_frame */
+			preorder_ctrl->indicate_seq = pattrib->seq_num;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			rtw_recv_indicatepkt(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			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;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			retval = amsdu_to_msdu(padapter, prframe);
+
+			preorder_ctrl->indicate_seq = (preorder_ctrl->indicate_seq + 1)%4096;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, NewSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq, pattrib->seq_num);
+			#endif
+
+			if (retval != _SUCCESS) {
+				#ifdef DBG_RX_DROP_FRAME
+				DBG_871X("DBG_RX_DROP_FRAME %s amsdu_to_msdu fail\n", __func__);
+				#endif
+			}
+
+			return retval;
+		}
+	}
+
+	spin_lock_bh(&ppending_recvframe_queue->lock);
+
+	RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_,
+		 ("recv_indicatepkt_reorder: indicate =%d seq =%d\n",
+		  preorder_ctrl->indicate_seq, pattrib->seq_num));
+
+	/* 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++;
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s check_indicate_seq fail\n", __func__);
+		#endif
+		goto _err_exit;
+	}
+
+
+	/* s3. Insert all packet into Reorder Queue to maintain its ordering. */
+	if (!enqueue_reorder_recvframe(preorder_ctrl, prframe)) {
+		/* DbgPrint("recv_indicatepkt_reorder, enqueue_reorder_recvframe fail!\n"); */
+		/* spin_unlock_irqrestore(&ppending_recvframe_queue->lock, irql); */
+		/* return _FAIL; */
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s enqueue_reorder_recvframe fail\n", __func__);
+		#endif
+		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(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;
+
+	/* DBG_871X("+rtw_reordering_ctrl_timeout_handler() =>\n"); */
+
+	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);
+
+}
+
+int process_recv_indicatepkts(struct adapter *padapter, union recv_frame *prframe);
+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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate);
+
+	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 */
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s recv_indicatepkt_reorder error!\n", __func__);
+			#endif
+
+			if ((padapter->bDriverStopped == false) &&
+			    (padapter->bSurpriseRemoved == false)) {
+				retval = _FAIL;
+				return retval;
+			}
+		}
+	} else { /* B/G mode */
+		retval = wlanhdr_to_ethhdr(prframe);
+		if (retval != _SUCCESS) {
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("wlanhdr_to_ethhdr: drop pkt\n"));
+			#ifdef DBG_RX_DROP_FRAME
+			DBG_871X("DBG_RX_DROP_FRAME %s wlanhdr_to_ethhdr error!\n", __func__);
+			#endif
+			return retval;
+		}
+
+		if ((padapter->bDriverStopped == false) && (padapter->bSurpriseRemoved == false)) {
+			/* indicate this recv_frame */
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func recv_indicatepkt\n"));
+			rtw_recv_indicatepkt(padapter, prframe);
+
+
+		} else{
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("@@@@ process_recv_indicatepkts- recv_func free_indicatepkt\n"));
+
+			RT_TRACE(_module_rtl871x_recv_c_, _drv_notice_, ("recv_func:bDriverStopped(%d) OR bSurpriseRemoved(%d)", padapter->bDriverStopped, padapter->bSurpriseRemoved));
+			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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_pre);
+
+	/* check the frame crtl field and decache */
+	ret = validate_recv_frame(padapter, rframe);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("recv_func: validate_recv_frame fail! drop pkt\n"));
+		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;
+
+	DBG_COUNTER(padapter->rx_logs.core_rx_post);
+
+	prframe = decryptor(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("decryptor: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s decryptor: drop pkt\n", __func__);
+		#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_decrypt_err);
+		goto _recv_data_drop;
+	}
+
+	prframe = recvframe_chk_defrag(padapter, prframe);
+	if (prframe == NULL)	{
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recvframe_chk_defrag: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s recvframe_chk_defrag: drop pkt\n", __func__);
+		#endif
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_defrag_err);
+		goto _recv_data_drop;
+	}
+
+	prframe = portctrl(padapter, prframe);
+	if (prframe == NULL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("portctrl: drop pkt\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s portctrl: drop pkt\n", __func__);
+		#endif
+		ret = _FAIL;
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_portctrl_err);
+		goto _recv_data_drop;
+	}
+
+	count_rx_stats(padapter, prframe, NULL);
+
+	ret = process_recv_indicatepkts(padapter, prframe);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("recv_func: process_recv_indicatepkts fail!\n"));
+		#ifdef DBG_RX_DROP_FRAME
+		DBG_871X("DBG_RX_DROP_FRAME %s process_recv_indicatepkts fail!\n", __func__);
+		#endif
+		rtw_free_recvframe(orig_prframe, pfree_recv_queue);/* free this recv_frame */
+		DBG_COUNTER(padapter->rx_logs.core_rx_post_indicate_err);
+		goto _recv_data_drop;
+	}
+
+_recv_data_drop:
+	precvpriv->rx_drop++;
+	return ret;
+}
+
+
+int recv_func(struct adapter *padapter, union recv_frame *rframe);
+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++;
+			DBG_COUNTER(padapter->rx_logs.core_rx_dequeue);
+			recv_func_posthandle(padapter, pending_frame);
+		}
+
+		if (cnt)
+			DBG_871X(FUNC_ADPT_FMT" dequeue %d from uc_swdec_pending_queue\n",
+				FUNC_ADPT_ARG(padapter), cnt);
+	}
+
+	DBG_COUNTER(padapter->rx_logs.core_rx);
+	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) {
+			DBG_COUNTER(padapter->rx_logs.core_rx_enqueue);
+			rtw_enqueue_recvframe(rframe, &padapter->recvpriv.uc_swdec_pending_queue);
+			/* DBG_871X("%s: no key, enqueue uc_swdec_pending_queue\n", __func__); */
+
+			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;
+
+/* 	RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("+rtw_recv_entry\n")); */
+
+	padapter = precvframe->u.hdr.adapter;
+
+	precvpriv = &padapter->recvpriv;
+
+	ret = recv_func(padapter, precvframe);
+	if (ret == _FAIL) {
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_info_, ("rtw_recv_entry: recv_func return fail!!!\n"));
+		goto _recv_entry_drop;
+	}
+
+
+	precvpriv->rx_pkts++;
+
+	return ret;
+
+_recv_entry_drop:
+
+	/* RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("_recv_entry_drop\n")); */
+
+	return ret;
+}
+
+void rtw_signal_stat_timer_hdl(RTW_TIMER_HDL_ARGS)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	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 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 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;
+			num_signal_qual = recvpriv->signal_qual_data.total_num;
+			/*  after avg_vals are accquired, 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;
+
+		#if defined(DBG_RX_SIGNAL_DISPLAY_PROCESSING) && 1
+		DBG_871X(FUNC_ADPT_FMT" signal_strength:%3u, rssi:%3d, signal_qual:%3u"
+			", num_signal_strength:%u, num_signal_qual:%u"
+			", on_cur_ch_ms:%d"
+			"\n"
+			, FUNC_ADPT_ARG(adapter)
+			, recvpriv->signal_strength
+			, recvpriv->rssi
+			, recvpriv->signal_qual
+			, num_signal_strength, num_signal_qual
+			, rtw_get_on_cur_ch_time(adapter) ? jiffies_to_msecs(jiffies - rtw_get_on_cur_ch_time(adapter)) : 0
+		);
+		#endif
+	}
+
+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 000000000000..b87ea4e388c0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_rf.c
@@ -0,0 +1,64 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_RF_C_
+
+#include <drv_types.h>
+
+
+struct ch_freq {
+	u32 channel;
+	u32 frequency;
+};
+
+static struct ch_freq ch_freq_map[] = {
+	{1, 2412}, {2, 2417}, {3, 2422}, {4, 2427}, {5, 2432},
+	{6, 2437}, {7, 2442}, {8, 2447}, {9, 2452}, {10, 2457},
+	{11, 2462}, {12, 2467}, {13, 2472}, {14, 2484},
+	/*  UNII */
+	{36, 5180}, {40, 5200}, {44, 5220}, {48, 5240}, {52, 5260},
+	{56, 5280}, {60, 5300}, {64, 5320}, {149, 5745}, {153, 5765},
+	{157, 5785}, {161, 5805}, {165, 5825}, {167, 5835}, {169, 5845},
+	{171, 5855}, {173, 5865},
+	/* HiperLAN2 */
+	{100, 5500}, {104, 5520}, {108, 5540}, {112, 5560}, {116, 5580},
+	{120, 5600}, {124, 5620}, {128, 5640}, {132, 5660}, {136, 5680},
+	{140, 5700},
+	/* Japan MMAC */
+	{34, 5170}, {38, 5190}, {42, 5210}, {46, 5230},
+	/*  Japan */
+	{184, 4920}, {188, 4940}, {192, 4960}, {196, 4980},
+	{208, 5040},/* Japan, means J08 */
+	{212, 5060},/* Japan, means J12 */
+	{216, 5080},/* Japan, means J16 */
+};
+
+static int ch_freq_map_num = (sizeof(ch_freq_map) / sizeof(struct ch_freq));
+
+u32 rtw_ch2freq(u32 channel)
+{
+	u8 i;
+	u32 freq = 0;
+
+	for (i = 0; i < ch_freq_map_num; i++) {
+		if (channel == ch_freq_map[i].channel) {
+			freq = ch_freq_map[i].frequency;
+				break;
+		}
+	}
+	if (i == ch_freq_map_num)
+		freq = 2412;
+
+	return freq;
+}
diff --git a/drivers/staging/rtl8723bs/core/rtw_security.c b/drivers/staging/rtl8723bs/core/rtw_security.c
new file mode 100644
index 000000000000..e832f16997b7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_security.c
@@ -0,0 +1,2437 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define  _RTW_SECURITY_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+static const char *_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;
+}
+
+#ifdef DBG_SW_SEC_CNT
+#define WEP_SW_ENC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->wep_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->wep_sw_enc_cnt_mc++; \
+	else \
+		sec->wep_sw_enc_cnt_uc++;
+
+#define WEP_SW_DEC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->wep_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->wep_sw_dec_cnt_mc++; \
+	else \
+		sec->wep_sw_dec_cnt_uc++;
+
+#define TKIP_SW_ENC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->tkip_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->tkip_sw_enc_cnt_mc++; \
+	else \
+		sec->tkip_sw_enc_cnt_uc++;
+
+#define TKIP_SW_DEC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->tkip_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->tkip_sw_dec_cnt_mc++; \
+	else \
+		sec->tkip_sw_dec_cnt_uc++;
+
+#define AES_SW_ENC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->aes_sw_enc_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->aes_sw_enc_cnt_mc++; \
+	else \
+		sec->aes_sw_enc_cnt_uc++;
+
+#define AES_SW_DEC_CNT_INC(sec, ra) \
+	if (is_broadcast_mac_addr(ra)) \
+		sec->aes_sw_dec_cnt_bc++; \
+	else if (is_multicast_mac_addr(ra)) \
+		sec->aes_sw_dec_cnt_mc++; \
+	else \
+		sec->aes_sw_dec_cnt_uc++;
+#else
+#define WEP_SW_ENC_CNT_INC(sec, ra)
+#define WEP_SW_DEC_CNT_INC(sec, ra)
+#define TKIP_SW_ENC_CNT_INC(sec, ra)
+#define TKIP_SW_DEC_CNT_INC(sec, ra)
+#define AES_SW_ENC_CNT_INC(sec, ra)
+#define AES_SW_DEC_CNT_INC(sec, ra)
+#endif /* DBG_SW_SEC_CNT */
+
+/* WEP related ===== */
+
+#define CRC32_POLY 0x04c11db7
+
+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 = 0;
+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)
+{
+	if (bcrc32initialized == 1)
+		return;
+	else {
+		sint i, j;
+		u32 c;
+		u8 *p = (u8 *)&c, *p1;
+		u8 k;
+
+		c = 0x12340000;
+
+		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 : (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 __le32 getcrc32(u8 *buf, sint len)
+{
+	u8 *p;
+	u32  crc;
+
+	if (bcrc32initialized == 0)
+		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 cpu_to_le32(~crc);    /* transmit complement, per CRC-32 spec */
+}
+
+
+/*
+	Need to consider the fragment  situation
+*/
+void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{																	/*  exclude ICV */
+
+	unsigned char crc[4];
+	struct arc4context	 mycontext;
+
+	sint	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;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		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;
+
+				*((__le32 *)crc) = 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) = 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((SIZE_PTR)(pframe));
+			}
+		}
+
+		WEP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+	}
+}
+
+void rtw_wep_decrypt(struct adapter  *padapter, u8 *precvframe)
+{
+	/*  exclude ICV */
+	u8 crc[4];
+	struct arc4context	 mycontext;
+	sint	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;
+
+	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 */
+		arcfour_init(&mycontext, wepkey, 3+keylength);
+		arcfour_encrypt(&mycontext, payload, payload,  length);
+
+		/* calculate icv and compare the icv */
+		*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
+
+		if (crc[3] != payload[length-1] || crc[2] != payload[length-2] || crc[1] != payload[length-3] || crc[0] != payload[length-4]) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+						crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
+		}
+
+		WEP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+	}
+	return;
+}
+
+/* 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)
+{
+	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 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)
+{
+	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 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
+{																	/*  exclude ICV */
+	u16 pnl;
+	u32 pnh;
+	u8 rc4key[16];
+	u8   ttkey[16];
+	u8 crc[4];
+	u8   hw_hdr_offset = 0;
+	struct arc4context mycontext;
+	sint			curfragnum, length;
+	u32 prwskeylen;
+
+	u8 *pframe, *payload, *iv, *prwskey;
+	union pn48 dot11txpn;
+	/* struct	sta_info 	*stainfo; */
+	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 == NULL)
+		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 (pattrib->psta)
+		{
+			stainfo = pattrib->psta;
+		}
+		else
+		{
+			DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+			stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+		}
+*/
+		/* if (stainfo!= NULL) */
+		{
+/*
+			if (!(stainfo->state &_FW_LINKED))
+			{
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+				return _FAIL;
+			}
+*/
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo!= NULL!!!\n"));
+
+			if (IS_MCAST(pattrib->ra))
+				prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+			else
+				/* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */
+				prwskey = pattrib->dot118021x_UncstKey.skey;
+
+			prwskeylen = 16;
+
+			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;
+					RT_TRACE(_module_rtl871x_security_c_, _drv_info_, ("pattrib->iv_len =%x, pattrib->icv_len =%x\n", pattrib->iv_len, pattrib->icv_len));
+					*((__le32 *)crc) = getcrc32(payload, length);/* modified by Amy*/
+
+					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) = getcrc32(payload, length);/* modified by Amy*/
+					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((SIZE_PTR)(pframe));
+				}
+			}
+
+			TKIP_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+		}
+/*
+		else {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt: stainfo == NULL!!!\n"));
+			DBG_871X("%s, psta ==NUL\n", __func__);
+			res = _FAIL;
+		}
+*/
+
+	}
+	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];
+	struct arc4context mycontext;
+	sint			length;
+	u32 prwskeylen;
+
+	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	recv_priv 	*precvpriv =&padapter->recvpriv; */
+	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 != NULL) {
+			if (IS_MCAST(prxattrib->ra)) {
+				static unsigned long start = 0;
+				static u32 no_gkey_bc_cnt = 0;
+				static u32 no_gkey_mc_cnt = 0;
+
+				if (psecuritypriv->binstallGrpkey == false) {
+					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) {
+							DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X_LEVEL(_drv_always_, 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;
+
+				/* DBG_871X("rx bc/mc packets, to perform sw rtw_tkip_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				prwskey = psecuritypriv->dot118021XGrpKey[prxattrib->key_index].skey;
+				prwskeylen = 16;
+			} else{
+				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
+				prwskeylen = 16;
+			}
+
+			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 */
+
+			arcfour_init(&mycontext, rc4key, 16);
+			arcfour_encrypt(&mycontext, payload, payload, length);
+
+			*((u32 *)crc) = le32_to_cpu(getcrc32(payload, length-4));
+
+			if (crc[3] != payload[length-1] || crc[2] != payload[length-2] || crc[1] != payload[length-3] || crc[0] != payload[length-4]) {
+				RT_TRACE(_module_rtl871x_security_c_, _drv_err_,
+					 ("rtw_wep_decrypt:icv error crc[3](%x)!=payload[length-1](%x) || crc[2](%x)!=payload[length-2](%x) || crc[1](%x)!=payload[length-3](%x) || crc[0](%x)!=payload[length-4](%x)\n",
+					 crc[3], payload[length-1], crc[2], payload[length-2], crc[1], payload[length-3], crc[0], payload[length-4]));
+				res = _FAIL;
+			}
+
+			TKIP_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_decrypt: stainfo == NULL!!!\n"));
+			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,
+	sint qc_exists,
+	sint 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,
+	sint header_length,
+	u8 *mpdu,
+	uint frtype
+);/*  add for CONFIG_IEEE80211W, none 11w also can use */
+static void construct_mic_header2(
+	u8 *mic_header2,
+	u8 *mpdu,
+	sint a4_exists,
+	sint qc_exists
+);
+static void construct_ctr_preload(
+	u8 *ctr_preload,
+	sint a4_exists,
+	sint qc_exists,
+	u8 *mpdu,
+	u8 *pn_vector,
+	sint c,
+	uint frtype
+);/*  add for CONFIG_IEEE80211W, none 11w also can use */
+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, sint 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)
+{
+		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];
+		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_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 halfs */
+		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)
+{
+		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  */
+/* Baron think the function is construct CCM    */
+/* nonce                                        */
+/************************************************/
+static void construct_mic_iv(
+	u8 *mic_iv,
+	sint qc_exists,
+	sint a4_exists,
+	u8 *mpdu,
+	uint payload_length,
+	u8 *pn_vector,
+	uint frtype/*  add for CONFIG_IEEE80211W, none 11w also can use */
+)
+{
+		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;
+
+		/* 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,
+	sint header_length,
+	u8 *mpdu,
+	uint frtype/*  add 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,
+	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.                               */
+/* Baron think the function is construct CCM    */
+/* nonce                                        */
+/************************************************/
+static void construct_ctr_preload(
+	u8 *ctr_preload,
+	sint a4_exists,
+	sint qc_exists,
+	u8 *mpdu,
+	u8 *pn_vector,
+	sint c,
+	uint frtype /*  add for CONFIG_IEEE80211W, none 11w also can use */
+)
+{
+	sint 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)
+{
+		sint i;
+
+		for (i = 0; i < 16; i++) {
+			out[i] = ina[i] ^ inb[i];
+		}
+}
+
+
+static sint 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);/* 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,	/* 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);/* bitwise_xor(aes_out, &message[payload_index], chain_buffer); */
+		for (j = 0; j < 16; j++)
+			pframe[payload_index++] = chain_buffer[j];/* 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,
+			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];/* 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++)
+			pframe[payload_index++] = chain_buffer[j];/* 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,
+		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];/* padded_buffer[j] = message[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];/* for (j = 0; j<8;j++) message[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 */
+	sint	curfragnum, length;
+	u32 prwskeylen;
+	u8 *pframe, *prwskey;	/*  *payload,*iv */
+	u8   hw_hdr_offset = 0;
+	/* struct	sta_info 	*stainfo = NULL; */
+	struct	pkt_attrib	 *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct	xmit_priv 	*pxmitpriv = &padapter->xmitpriv;
+
+/* 	uint	offset = 0; */
+	u32 res = _SUCCESS;
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		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_)) {
+		RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_encrypt: stainfo!= NULL!!!\n"));
+
+		if (IS_MCAST(pattrib->ra))
+			prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey;
+		else
+			/* prwskey =&stainfo->dot118021x_UncstKey.skey[0]; */
+			prwskey = pattrib->dot118021x_UncstKey.skey;
+
+		prwskeylen = 16;
+
+		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 *)RND4((SIZE_PTR)(pframe));
+			}
+		}
+
+		AES_SW_ENC_CNT_INC(psecuritypriv, pattrib->ra);
+	}
+	return res;
+}
+
+static sint 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;
+	sint 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|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]) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+					i, pframe[hdrlen+8+plen-8+i], message[hdrlen+8+plen-8+i]));
+			DBG_871X("aes_decipher:mic check error mic[%d]: pframe(%x) != message(%x)\n",
+					i, 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 */
+
+
+	sint		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;
+/* 	struct	recv_priv 	*precvpriv =&padapter->recvpriv; */
+	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 != NULL) {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo!= NULL!!!\n"));
+
+			if (IS_MCAST(prxattrib->ra)) {
+				static unsigned long start = 0;
+				static u32 no_gkey_bc_cnt = 0;
+				static u32 no_gkey_mc_cnt = 0;
+
+				/* DBG_871X("rx bc/mc packets, to perform sw rtw_aes_decrypt\n"); */
+				/* prwskey = psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey; */
+				if (psecuritypriv->binstallGrpkey == false) {
+					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) {
+							DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X_LEVEL(_drv_always_, 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) {
+					DBG_871X("not match packet_index =%d, install_index =%d\n"
+					, prxattrib->key_index, psecuritypriv->dot118021XGrpKeyid);
+					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);
+
+			AES_SW_DEC_CNT_INC(psecuritypriv, prxattrib->ra);
+		} else {
+			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_aes_decrypt: stainfo == NULL!!!\n"));
+			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 == NULL) {
+		DBG_871X("BIP AAD allocate fail\n");
+		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, _MME_IE_, &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) {
+			DBG_871X("replay BIP packet\n");
+			goto BIP_exit;
+		}
+		/* copy key index */
+		memcpy(&le_tmp, p+2, 2);
+		keyid = le16_to_cpu(le_tmp);
+		if (keyid != padapter->securitypriv.dot11wBIPKeyid) {
+			DBG_871X("BIP key index error!\n");
+			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
+			DBG_871X("BIP MIC error!\n");
+
+	} else
+		res = RTW_RX_HANDLED;
+BIP_exit:
+
+	kfree(BIP_AAD);
+	return res;
+}
+
+/* AES tables*/
+const u32 Te0[256] = {
+	0xc66363a5U, 0xf87c7c84U, 0xee777799U, 0xf67b7b8dU,
+	0xfff2f20dU, 0xd66b6bbdU, 0xde6f6fb1U, 0x91c5c554U,
+	0x60303050U, 0x02010103U, 0xce6767a9U, 0x562b2b7dU,
+	0xe7fefe19U, 0xb5d7d762U, 0x4dababe6U, 0xec76769aU,
+	0x8fcaca45U, 0x1f82829dU, 0x89c9c940U, 0xfa7d7d87U,
+	0xeffafa15U, 0xb25959ebU, 0x8e4747c9U, 0xfbf0f00bU,
+	0x41adadecU, 0xb3d4d467U, 0x5fa2a2fdU, 0x45afafeaU,
+	0x239c9cbfU, 0x53a4a4f7U, 0xe4727296U, 0x9bc0c05bU,
+	0x75b7b7c2U, 0xe1fdfd1cU, 0x3d9393aeU, 0x4c26266aU,
+	0x6c36365aU, 0x7e3f3f41U, 0xf5f7f702U, 0x83cccc4fU,
+	0x6834345cU, 0x51a5a5f4U, 0xd1e5e534U, 0xf9f1f108U,
+	0xe2717193U, 0xabd8d873U, 0x62313153U, 0x2a15153fU,
+	0x0804040cU, 0x95c7c752U, 0x46232365U, 0x9dc3c35eU,
+	0x30181828U, 0x379696a1U, 0x0a05050fU, 0x2f9a9ab5U,
+	0x0e070709U, 0x24121236U, 0x1b80809bU, 0xdfe2e23dU,
+	0xcdebeb26U, 0x4e272769U, 0x7fb2b2cdU, 0xea75759fU,
+	0x1209091bU, 0x1d83839eU, 0x582c2c74U, 0x341a1a2eU,
+	0x361b1b2dU, 0xdc6e6eb2U, 0xb45a5aeeU, 0x5ba0a0fbU,
+	0xa45252f6U, 0x763b3b4dU, 0xb7d6d661U, 0x7db3b3ceU,
+	0x5229297bU, 0xdde3e33eU, 0x5e2f2f71U, 0x13848497U,
+	0xa65353f5U, 0xb9d1d168U, 0x00000000U, 0xc1eded2cU,
+	0x40202060U, 0xe3fcfc1fU, 0x79b1b1c8U, 0xb65b5bedU,
+	0xd46a6abeU, 0x8dcbcb46U, 0x67bebed9U, 0x7239394bU,
+	0x944a4adeU, 0x984c4cd4U, 0xb05858e8U, 0x85cfcf4aU,
+	0xbbd0d06bU, 0xc5efef2aU, 0x4faaaae5U, 0xedfbfb16U,
+	0x864343c5U, 0x9a4d4dd7U, 0x66333355U, 0x11858594U,
+	0x8a4545cfU, 0xe9f9f910U, 0x04020206U, 0xfe7f7f81U,
+	0xa05050f0U, 0x783c3c44U, 0x259f9fbaU, 0x4ba8a8e3U,
+	0xa25151f3U, 0x5da3a3feU, 0x804040c0U, 0x058f8f8aU,
+	0x3f9292adU, 0x219d9dbcU, 0x70383848U, 0xf1f5f504U,
+	0x63bcbcdfU, 0x77b6b6c1U, 0xafdada75U, 0x42212163U,
+	0x20101030U, 0xe5ffff1aU, 0xfdf3f30eU, 0xbfd2d26dU,
+	0x81cdcd4cU, 0x180c0c14U, 0x26131335U, 0xc3ecec2fU,
+	0xbe5f5fe1U, 0x359797a2U, 0x884444ccU, 0x2e171739U,
+	0x93c4c457U, 0x55a7a7f2U, 0xfc7e7e82U, 0x7a3d3d47U,
+	0xc86464acU, 0xba5d5de7U, 0x3219192bU, 0xe6737395U,
+	0xc06060a0U, 0x19818198U, 0x9e4f4fd1U, 0xa3dcdc7fU,
+	0x44222266U, 0x542a2a7eU, 0x3b9090abU, 0x0b888883U,
+	0x8c4646caU, 0xc7eeee29U, 0x6bb8b8d3U, 0x2814143cU,
+	0xa7dede79U, 0xbc5e5ee2U, 0x160b0b1dU, 0xaddbdb76U,
+	0xdbe0e03bU, 0x64323256U, 0x743a3a4eU, 0x140a0a1eU,
+	0x924949dbU, 0x0c06060aU, 0x4824246cU, 0xb85c5ce4U,
+	0x9fc2c25dU, 0xbdd3d36eU, 0x43acacefU, 0xc46262a6U,
+	0x399191a8U, 0x319595a4U, 0xd3e4e437U, 0xf279798bU,
+	0xd5e7e732U, 0x8bc8c843U, 0x6e373759U, 0xda6d6db7U,
+	0x018d8d8cU, 0xb1d5d564U, 0x9c4e4ed2U, 0x49a9a9e0U,
+	0xd86c6cb4U, 0xac5656faU, 0xf3f4f407U, 0xcfeaea25U,
+	0xca6565afU, 0xf47a7a8eU, 0x47aeaee9U, 0x10080818U,
+	0x6fbabad5U, 0xf0787888U, 0x4a25256fU, 0x5c2e2e72U,
+	0x381c1c24U, 0x57a6a6f1U, 0x73b4b4c7U, 0x97c6c651U,
+	0xcbe8e823U, 0xa1dddd7cU, 0xe874749cU, 0x3e1f1f21U,
+	0x964b4bddU, 0x61bdbddcU, 0x0d8b8b86U, 0x0f8a8a85U,
+	0xe0707090U, 0x7c3e3e42U, 0x71b5b5c4U, 0xcc6666aaU,
+	0x904848d8U, 0x06030305U, 0xf7f6f601U, 0x1c0e0e12U,
+	0xc26161a3U, 0x6a35355fU, 0xae5757f9U, 0x69b9b9d0U,
+	0x17868691U, 0x99c1c158U, 0x3a1d1d27U, 0x279e9eb9U,
+	0xd9e1e138U, 0xebf8f813U, 0x2b9898b3U, 0x22111133U,
+	0xd26969bbU, 0xa9d9d970U, 0x078e8e89U, 0x339494a7U,
+	0x2d9b9bb6U, 0x3c1e1e22U, 0x15878792U, 0xc9e9e920U,
+	0x87cece49U, 0xaa5555ffU, 0x50282878U, 0xa5dfdf7aU,
+	0x038c8c8fU, 0x59a1a1f8U, 0x09898980U, 0x1a0d0d17U,
+	0x65bfbfdaU, 0xd7e6e631U, 0x844242c6U, 0xd06868b8U,
+	0x824141c3U, 0x299999b0U, 0x5a2d2d77U, 0x1e0f0f11U,
+	0x7bb0b0cbU, 0xa85454fcU, 0x6dbbbbd6U, 0x2c16163aU,
+};
+const u32 Td0[256] = {
+	0x51f4a750U, 0x7e416553U, 0x1a17a4c3U, 0x3a275e96U,
+	0x3bab6bcbU, 0x1f9d45f1U, 0xacfa58abU, 0x4be30393U,
+	0x2030fa55U, 0xad766df6U, 0x88cc7691U, 0xf5024c25U,
+	0x4fe5d7fcU, 0xc52acbd7U, 0x26354480U, 0xb562a38fU,
+	0xdeb15a49U, 0x25ba1b67U, 0x45ea0e98U, 0x5dfec0e1U,
+	0xc32f7502U, 0x814cf012U, 0x8d4697a3U, 0x6bd3f9c6U,
+	0x038f5fe7U, 0x15929c95U, 0xbf6d7aebU, 0x955259daU,
+	0xd4be832dU, 0x587421d3U, 0x49e06929U, 0x8ec9c844U,
+	0x75c2896aU, 0xf48e7978U, 0x99583e6bU, 0x27b971ddU,
+	0xbee14fb6U, 0xf088ad17U, 0xc920ac66U, 0x7dce3ab4U,
+	0x63df4a18U, 0xe51a3182U, 0x97513360U, 0x62537f45U,
+	0xb16477e0U, 0xbb6bae84U, 0xfe81a01cU, 0xf9082b94U,
+	0x70486858U, 0x8f45fd19U, 0x94de6c87U, 0x527bf8b7U,
+	0xab73d323U, 0x724b02e2U, 0xe31f8f57U, 0x6655ab2aU,
+	0xb2eb2807U, 0x2fb5c203U, 0x86c57b9aU, 0xd33708a5U,
+	0x302887f2U, 0x23bfa5b2U, 0x02036abaU, 0xed16825cU,
+	0x8acf1c2bU, 0xa779b492U, 0xf307f2f0U, 0x4e69e2a1U,
+	0x65daf4cdU, 0x0605bed5U, 0xd134621fU, 0xc4a6fe8aU,
+	0x342e539dU, 0xa2f355a0U, 0x058ae132U, 0xa4f6eb75U,
+	0x0b83ec39U, 0x4060efaaU, 0x5e719f06U, 0xbd6e1051U,
+	0x3e218af9U, 0x96dd063dU, 0xdd3e05aeU, 0x4de6bd46U,
+	0x91548db5U, 0x71c45d05U, 0x0406d46fU, 0x605015ffU,
+	0x1998fb24U, 0xd6bde997U, 0x894043ccU, 0x67d99e77U,
+	0xb0e842bdU, 0x07898b88U, 0xe7195b38U, 0x79c8eedbU,
+	0xa17c0a47U, 0x7c420fe9U, 0xf8841ec9U, 0x00000000U,
+	0x09808683U, 0x322bed48U, 0x1e1170acU, 0x6c5a724eU,
+	0xfd0efffbU, 0x0f853856U, 0x3daed51eU, 0x362d3927U,
+	0x0a0fd964U, 0x685ca621U, 0x9b5b54d1U, 0x24362e3aU,
+	0x0c0a67b1U, 0x9357e70fU, 0xb4ee96d2U, 0x1b9b919eU,
+	0x80c0c54fU, 0x61dc20a2U, 0x5a774b69U, 0x1c121a16U,
+	0xe293ba0aU, 0xc0a02ae5U, 0x3c22e043U, 0x121b171dU,
+	0x0e090d0bU, 0xf28bc7adU, 0x2db6a8b9U, 0x141ea9c8U,
+	0x57f11985U, 0xaf75074cU, 0xee99ddbbU, 0xa37f60fdU,
+	0xf701269fU, 0x5c72f5bcU, 0x44663bc5U, 0x5bfb7e34U,
+	0x8b432976U, 0xcb23c6dcU, 0xb6edfc68U, 0xb8e4f163U,
+	0xd731dccaU, 0x42638510U, 0x13972240U, 0x84c61120U,
+	0x854a247dU, 0xd2bb3df8U, 0xaef93211U, 0xc729a16dU,
+	0x1d9e2f4bU, 0xdcb230f3U, 0x0d8652ecU, 0x77c1e3d0U,
+	0x2bb3166cU, 0xa970b999U, 0x119448faU, 0x47e96422U,
+	0xa8fc8cc4U, 0xa0f03f1aU, 0x567d2cd8U, 0x223390efU,
+	0x87494ec7U, 0xd938d1c1U, 0x8ccaa2feU, 0x98d40b36U,
+	0xa6f581cfU, 0xa57ade28U, 0xdab78e26U, 0x3fadbfa4U,
+	0x2c3a9de4U, 0x5078920dU, 0x6a5fcc9bU, 0x547e4662U,
+	0xf68d13c2U, 0x90d8b8e8U, 0x2e39f75eU, 0x82c3aff5U,
+	0x9f5d80beU, 0x69d0937cU, 0x6fd52da9U, 0xcf2512b3U,
+	0xc8ac993bU, 0x10187da7U, 0xe89c636eU, 0xdb3bbb7bU,
+	0xcd267809U, 0x6e5918f4U, 0xec9ab701U, 0x834f9aa8U,
+	0xe6956e65U, 0xaaffe67eU, 0x21bccf08U, 0xef15e8e6U,
+	0xbae79bd9U, 0x4a6f36ceU, 0xea9f09d4U, 0x29b07cd6U,
+	0x31a4b2afU, 0x2a3f2331U, 0xc6a59430U, 0x35a266c0U,
+	0x744ebc37U, 0xfc82caa6U, 0xe090d0b0U, 0x33a7d815U,
+	0xf104984aU, 0x41ecdaf7U, 0x7fcd500eU, 0x1791f62fU,
+	0x764dd68dU, 0x43efb04dU, 0xccaa4d54U, 0xe49604dfU,
+	0x9ed1b5e3U, 0x4c6a881bU, 0xc12c1fb8U, 0x4665517fU,
+	0x9d5eea04U, 0x018c355dU, 0xfa877473U, 0xfb0b412eU,
+	0xb3671d5aU, 0x92dbd252U, 0xe9105633U, 0x6dd64713U,
+	0x9ad7618cU, 0x37a10c7aU, 0x59f8148eU, 0xeb133c89U,
+	0xcea927eeU, 0xb761c935U, 0xe11ce5edU, 0x7a47b13cU,
+	0x9cd2df59U, 0x55f2733fU, 0x1814ce79U, 0x73c737bfU,
+	0x53f7cdeaU, 0x5ffdaa5bU, 0xdf3d6f14U, 0x7844db86U,
+	0xcaaff381U, 0xb968c43eU, 0x3824342cU, 0xc2a3405fU,
+	0x161dc372U, 0xbce2250cU, 0x283c498bU, 0xff0d9541U,
+	0x39a80171U, 0x080cb3deU, 0xd8b4e49cU, 0x6456c190U,
+	0x7bcb8461U, 0xd532b670U, 0x486c5c74U, 0xd0b85742U,
+};
+const u8 Td4s[256] = {
+	0x52U, 0x09U, 0x6aU, 0xd5U, 0x30U, 0x36U, 0xa5U, 0x38U,
+	0xbfU, 0x40U, 0xa3U, 0x9eU, 0x81U, 0xf3U, 0xd7U, 0xfbU,
+	0x7cU, 0xe3U, 0x39U, 0x82U, 0x9bU, 0x2fU, 0xffU, 0x87U,
+	0x34U, 0x8eU, 0x43U, 0x44U, 0xc4U, 0xdeU, 0xe9U, 0xcbU,
+	0x54U, 0x7bU, 0x94U, 0x32U, 0xa6U, 0xc2U, 0x23U, 0x3dU,
+	0xeeU, 0x4cU, 0x95U, 0x0bU, 0x42U, 0xfaU, 0xc3U, 0x4eU,
+	0x08U, 0x2eU, 0xa1U, 0x66U, 0x28U, 0xd9U, 0x24U, 0xb2U,
+	0x76U, 0x5bU, 0xa2U, 0x49U, 0x6dU, 0x8bU, 0xd1U, 0x25U,
+	0x72U, 0xf8U, 0xf6U, 0x64U, 0x86U, 0x68U, 0x98U, 0x16U,
+	0xd4U, 0xa4U, 0x5cU, 0xccU, 0x5dU, 0x65U, 0xb6U, 0x92U,
+	0x6cU, 0x70U, 0x48U, 0x50U, 0xfdU, 0xedU, 0xb9U, 0xdaU,
+	0x5eU, 0x15U, 0x46U, 0x57U, 0xa7U, 0x8dU, 0x9dU, 0x84U,
+	0x90U, 0xd8U, 0xabU, 0x00U, 0x8cU, 0xbcU, 0xd3U, 0x0aU,
+	0xf7U, 0xe4U, 0x58U, 0x05U, 0xb8U, 0xb3U, 0x45U, 0x06U,
+	0xd0U, 0x2cU, 0x1eU, 0x8fU, 0xcaU, 0x3fU, 0x0fU, 0x02U,
+	0xc1U, 0xafU, 0xbdU, 0x03U, 0x01U, 0x13U, 0x8aU, 0x6bU,
+	0x3aU, 0x91U, 0x11U, 0x41U, 0x4fU, 0x67U, 0xdcU, 0xeaU,
+	0x97U, 0xf2U, 0xcfU, 0xceU, 0xf0U, 0xb4U, 0xe6U, 0x73U,
+	0x96U, 0xacU, 0x74U, 0x22U, 0xe7U, 0xadU, 0x35U, 0x85U,
+	0xe2U, 0xf9U, 0x37U, 0xe8U, 0x1cU, 0x75U, 0xdfU, 0x6eU,
+	0x47U, 0xf1U, 0x1aU, 0x71U, 0x1dU, 0x29U, 0xc5U, 0x89U,
+	0x6fU, 0xb7U, 0x62U, 0x0eU, 0xaaU, 0x18U, 0xbeU, 0x1bU,
+	0xfcU, 0x56U, 0x3eU, 0x4bU, 0xc6U, 0xd2U, 0x79U, 0x20U,
+	0x9aU, 0xdbU, 0xc0U, 0xfeU, 0x78U, 0xcdU, 0x5aU, 0xf4U,
+	0x1fU, 0xddU, 0xa8U, 0x33U, 0x88U, 0x07U, 0xc7U, 0x31U,
+	0xb1U, 0x12U, 0x10U, 0x59U, 0x27U, 0x80U, 0xecU, 0x5fU,
+	0x60U, 0x51U, 0x7fU, 0xa9U, 0x19U, 0xb5U, 0x4aU, 0x0dU,
+	0x2dU, 0xe5U, 0x7aU, 0x9fU, 0x93U, 0xc9U, 0x9cU, 0xefU,
+	0xa0U, 0xe0U, 0x3bU, 0x4dU, 0xaeU, 0x2aU, 0xf5U, 0xb0U,
+	0xc8U, 0xebU, 0xbbU, 0x3cU, 0x83U, 0x53U, 0x99U, 0x61U,
+	0x17U, 0x2bU, 0x04U, 0x7eU, 0xbaU, 0x77U, 0xd6U, 0x26U,
+	0xe1U, 0x69U, 0x14U, 0x63U, 0x55U, 0x21U, 0x0cU, 0x7dU,
+};
+const u8 rcons[] = {
+	0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1B, 0x36
+	/* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+};
+
+/**
+ * Expand the cipher key into the encryption key schedule.
+ *
+ * @return	the number of rounds for the given cipher key size.
+ */
+static void rijndaelKeySetupEnc(u32 rk[/*44*/], const u8 cipherKey[])
+{
+	int i;
+	u32 temp;
+
+	rk[0] = GETU32(cipherKey);
+	rk[1] = GETU32(cipherKey +  4);
+	rk[2] = GETU32(cipherKey +  8);
+	rk[3] = GETU32(cipherKey + 12);
+	for (i = 0; i < 10; i++) {
+		temp  = rk[3];
+		rk[4] = rk[0] ^
+			TE421(temp) ^ TE432(temp) ^ TE443(temp) ^ TE414(temp) ^
+			RCON(i);
+		rk[5] = rk[1] ^ rk[4];
+		rk[6] = rk[2] ^ rk[5];
+		rk[7] = rk[3] ^ rk[6];
+		rk += 4;
+	}
+}
+
+static void rijndaelEncrypt(u32 rk[/*44*/], u8 pt[16], u8 ct[16])
+{
+	u32 s0, s1, s2, s3, t0, t1, t2, t3;
+	int Nr = 10;
+	int r;
+
+	/*
+	 * map byte array block to cipher state
+	 * and add initial round key:
+	 */
+	s0 = GETU32(pt) ^ rk[0];
+	s1 = GETU32(pt +  4) ^ rk[1];
+	s2 = GETU32(pt +  8) ^ rk[2];
+	s3 = GETU32(pt + 12) ^ rk[3];
+
+#define ROUND(i, d, s) \
+d##0 = TE0(s##0) ^ TE1(s##1) ^ TE2(s##2) ^ TE3(s##3) ^ rk[4 * i]; \
+d##1 = TE0(s##1) ^ TE1(s##2) ^ TE2(s##3) ^ TE3(s##0) ^ rk[4 * i + 1]; \
+d##2 = TE0(s##2) ^ TE1(s##3) ^ TE2(s##0) ^ TE3(s##1) ^ rk[4 * i + 2]; \
+d##3 = TE0(s##3) ^ TE1(s##0) ^ TE2(s##1) ^ TE3(s##2) ^ rk[4 * i + 3]
+
+	/* Nr - 1 full rounds: */
+	r = Nr >> 1;
+	for (;;) {
+		ROUND(1, t, s);
+		rk += 8;
+		if (--r == 0)
+			break;
+		ROUND(0, s, t);
+	}
+
+#undef ROUND
+
+	/*
+	 * apply last round and
+	 * map cipher state to byte array block:
+	 */
+	s0 = TE41(t0) ^ TE42(t1) ^ TE43(t2) ^ TE44(t3) ^ rk[0];
+	PUTU32(ct, s0);
+	s1 = TE41(t1) ^ TE42(t2) ^ TE43(t3) ^ TE44(t0) ^ rk[1];
+	PUTU32(ct +  4, s1);
+	s2 = TE41(t2) ^ TE42(t3) ^ TE43(t0) ^ TE44(t1) ^ rk[2];
+	PUTU32(ct +  8, s2);
+	s3 = TE41(t3) ^ TE42(t0) ^ TE43(t1) ^ TE44(t2) ^ rk[3];
+	PUTU32(ct + 12, s3);
+}
+
+static void *aes_encrypt_init(u8 *key, size_t len)
+{
+	u32 *rk;
+	if (len != 16)
+		return NULL;
+	rk = (u32 *)rtw_malloc(AES_PRIV_SIZE);
+	if (rk == NULL)
+		return NULL;
+	rijndaelKeySetupEnc(rk, key);
+	return rk;
+}
+
+static void aes_128_encrypt(void *ctx, u8 *plain, u8 *crypt)
+{
+	rijndaelEncrypt(ctx, plain, crypt);
+}
+
+
+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;
+}
+
+static void aes_encrypt_deinit(void *ctx)
+{
+	memset(ctx, 0, AES_PRIV_SIZE);
+	kfree(ctx);
+}
+
+
+/**
+ * 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)
+{
+	void *ctx;
+	u8 cbc[AES_BLOCK_SIZE], pad[AES_BLOCK_SIZE];
+	u8 *pos, *end;
+	size_t i, e, left, total_len;
+
+	ctx = aes_encrypt_init(key, 16);
+	if (ctx == NULL)
+		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_128_encrypt(ctx, cbc, cbc);
+		left -= AES_BLOCK_SIZE;
+	}
+
+	memset(pad, 0, AES_BLOCK_SIZE);
+	aes_128_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_128_encrypt(ctx, pad, mac);
+	aes_encrypt_deinit(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);
+	sint 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 == true) {
+		unsigned long passing_ms = jiffies_to_msecs(jiffies - securitypriv->btkip_countermeasure_time);
+		if (passing_ms > 60*1000) {
+			DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") countermeasure time:%lus > 60s\n",
+				caller, ADPT_ARG(adapter), passing_ms/1000);
+			securitypriv->btkip_countermeasure = false;
+			securitypriv->btkip_countermeasure_time = 0;
+		} else {
+			DBG_871X_LEVEL(_drv_always_, "%s("ADPT_FMT") 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 000000000000..cb43ec90a648
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
@@ -0,0 +1,641 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_STA_MGT_C_
+
+#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); */
+
+	_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_PTR)(pstapriv->pallocated_stainfo_buf) & 3);
+
+	_rtw_init_queue(&pstapriv->free_sta_queue);
+
+	spin_lock_init(&pstapriv->sta_hash_lock);
+
+	/* _rtw_init_queue(&pstapriv->asoc_q); */
+	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)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf)/sizeof(struct sta_info);
+
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	if (!stainfo_offset_valid(offset))
+		DBG_871X("%s invalid offset(%d), out of range!!!", __func__, 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;
+	struct sta_info *psta = NULL;
+
+	spin_lock_bh(&pstapriv->sta_hash_lock);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+		psta = LIST_CONTAINOR(plist, struct sta_info, list);
+		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]);
+			plist = get_next(phead);
+
+			while (phead != plist) {
+				int i;
+				psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+				plist = get_next(plist);
+
+				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);
+
+		if (pstapriv->pallocated_stainfo_buf)
+			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)
+{
+	uint tmp_aid;
+	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));
+		psta = NULL;
+		return psta;
+	} else{
+		psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+
+		list_del_init(&(psta->list));
+
+		/* spin_unlock_bh(&(pfree_sta_queue->lock)); */
+
+		tmp_aid = psta->aid;
+
+		_rtw_init_stainfo(psta);
+
+		psta->padapter = pstapriv->padapter;
+
+		memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+		index = wifi_mac_hash(hwaddr);
+
+		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_info_, ("rtw_alloc_stainfo: index  = %x", index));
+
+		if (index >= NUM_STA) {
+			RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("ERROR => rtw_alloc_stainfo: 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);
+		}
+
+		RT_TRACE(
+			_module_rtl871x_sta_mgt_c_,
+			_drv_info_, (
+				"alloc number_%d stainfo  with hwaddr = %x %x %x %x %x %x \n",
+				pstapriv->asoc_sta_count,
+				hwaddr[0],
+				hwaddr[1],
+				hwaddr[2],
+				hwaddr[3],
+				hwaddr[4],
+				hwaddr[5]
+			)
+		);
+
+		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;
+			#ifdef DBG_RX_SEQ
+			DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __func__, __LINE__,
+				preorder_ctrl->indicate_seq);
+			#endif
+			preorder_ctrl->wend_b = 0xffff;
+			/* preorder_ctrl->wsize_b = (NR_RECVBUFF-2); */
+			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;
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+		/* alloc mac id for non-bc/mc station, */
+		rtw_alloc_macid(pstapriv->padapter, psta);
+
+	}
+
+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;
+	struct hw_xmit *phwxmit;
+
+	if (psta == NULL)
+		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);
+
+	list_del_init(&psta->hash_list);
+	RT_TRACE(
+		_module_rtl871x_sta_mgt_c_,
+		_drv_err_, (
+			"\n free number_%d stainfo  with hwaddr = 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x \n",
+			pstapriv->asoc_sta_count,
+			psta->hwaddr[0],
+			psta->hwaddr[1],
+			psta->hwaddr[2],
+			psta->hwaddr[3],
+			psta->hwaddr[4],
+			psta->hwaddr[5]
+		)
+	);
+	pstapriv->asoc_sta_count--;
+
+
+	/*  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 = LIST_CONTAINOR(plist, union recv_frame, u);
+
+			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;
+	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 = get_next(phead);
+
+		while (phead != plist) {
+			psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+			plist = get_next(plist);
+
+			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 == NULL)
+		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 = get_next(phead);
+
+
+	while (phead != plist) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+		if ((!memcmp(psta->hwaddr, addr, ETH_ALEN)))
+		 /*  if found the matched address */
+			break;
+
+		psta = NULL;
+		plist = get_next(plist);
+	}
+
+	spin_unlock_bh(&pstapriv->sta_hash_lock);
+	return psta;
+}
+
+u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
+{
+
+	struct sta_info *psta;
+	struct tx_servq	*ptxservq;
+	u32 res = _SUCCESS;
+	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 == NULL) {
+		res = _FAIL;
+		RT_TRACE(_module_rtl871x_sta_mgt_c_, _drv_err_, ("rtw_alloc_stainfo fail"));
+		goto exit;
+	}
+
+	/*  default broadcast & multicast use macid 1 */
+	psta->mac_id = 1;
+
+	ptxservq = &(psta->sta_xmitpriv.be_q);
+exit:
+	return _SUCCESS;
+}
+
+
+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 = get_next(phead);
+	while (phead != plist) {
+		paclnode = LIST_CONTAINOR(plist, struct rtw_wlan_acl_node, list);
+		plist = get_next(plist);
+
+		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 == true) ?  false:true;
+
+	else if (pacl_list->mode == 2)/* deny unless in accept list */
+		res = (match == true) ?  true:false;
+	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 000000000000..f485f541e36d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
@@ -0,0 +1,2328 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_WLAN_UTIL_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_com_h2c.h>
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+#include <linux/inetdevice.h>
+#endif
+
+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};
+
+extern unsigned char RTW_WPA_OUI[];
+extern unsigned char WPA_TKIP_CIPHER[4];
+
+#define R2T_PHY_DELAY	(0)
+
+/* 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
+};
+
+int 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;
+
+}
+
+int 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;
+}
+
+u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta)
+{
+	u8 raid, cur_rf_type, rf_type = RF_1T1R;
+
+	rtw_hal_get_hwreg(adapter, HW_VAR_RF_TYPE, (u8 *)(&cur_rf_type));
+
+	if (cur_rf_type == RF_1T1R) {
+		rf_type = RF_1T1R;
+	} else if (IsSupportedVHT(psta->wireless_mode)) {
+		if (psta->ra_mask & 0xffc00000)
+			rf_type = RF_2T2R;
+	} else if (IsSupportedHT(psta->wireless_mode)) {
+		if (psta->ra_mask & 0xfff00000)
+			rf_type = RF_2T2R;
+	}
+
+	switch (psta->wireless_mode) {
+	case WIRELESS_11B:
+		raid = RATEID_IDX_B;
+		break;
+	case WIRELESS_11A:
+	case WIRELESS_11G:
+		raid = RATEID_IDX_G;
+		break;
+	case WIRELESS_11BG:
+		raid = RATEID_IDX_BG;
+		break;
+	case WIRELESS_11_24N:
+	case WIRELESS_11_5N:
+	case WIRELESS_11A_5N:
+	case WIRELESS_11G_24N:
+		if (rf_type == RF_2T2R)
+			raid = RATEID_IDX_GN_N2SS;
+		else
+			raid = RATEID_IDX_GN_N1SS;
+		break;
+	case WIRELESS_11B_24N:
+	case WIRELESS_11BG_24N:
+		if (psta->bw_mode == CHANNEL_WIDTH_20) {
+			if (rf_type == RF_2T2R)
+				raid = RATEID_IDX_BGN_20M_2SS_BN;
+			else
+				raid = RATEID_IDX_BGN_20M_1SS_BN;
+		} else {
+			if (rf_type == RF_2T2R)
+				raid = RATEID_IDX_BGN_40M_2SS;
+			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)
+{
+	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;
+
+}
+
+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));
+}
+
+static void Set_NETYPE0_MSR(struct adapter *padapter, u8 type)
+{
+	rtw_hal_set_hwreg(padapter, HW_VAR_MEDIA_STATUS, (u8 *)(&type));
+}
+
+void Set_MSR(struct adapter *padapter, u8 type)
+{
+	Set_NETYPE0_MSR(padapter, 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 += snprintf(msg+cnt, len-cnt, "switch to ch %3u", ch);
+
+		for (i = 0; i < dvobj->iface_nums; i++) {
+			struct adapter *iface = dvobj->padapters[i];
+			cnt += snprintf(msg+cnt, len-cnt, " ["ADPT_FMT":", ADPT_ARG(iface));
+			if (iface->mlmeextpriv.cur_channel == ch)
+				cnt += snprintf(msg+cnt, len-cnt, "C");
+			else
+				cnt += snprintf(msg+cnt, len-cnt, "_");
+			if (iface->wdinfo.listen_channel == ch && !rtw_p2p_chk_state(&iface->wdinfo, P2P_STATE_NONE))
+				cnt += snprintf(msg+cnt, len-cnt, "L");
+			else
+				cnt += snprintf(msg+cnt, len-cnt, "_");
+			cnt += snprintf(msg+cnt, len-cnt, "]");
+		}
+
+		DBG_871X(FUNC_ADPT_FMT" %s\n", FUNC_ADPT_ARG(adapter), msg);
+#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_80) {
+		if ((channel == 36) || (channel == 40) || (channel == 44) || (channel == 48))
+			center_ch = 42;
+		if ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+			center_ch = 58;
+		if ((channel == 100) || (channel == 104) || (channel == 108) || (channel == 112))
+			center_ch = 106;
+		if ((channel == 116) || (channel == 120) || (channel == 124) || (channel == 128))
+			center_ch = 122;
+		if ((channel == 132) || (channel == 136) || (channel == 140) || (channel == 144))
+			center_ch = 138;
+		if ((channel == 149) || (channel == 153) || (channel == 157) || (channel == 161))
+			center_ch = 155;
+		else if (channel <= 14)
+			center_ch = 7;
+	} else 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;
+
+	if (padapter->bNotifyChannelChange)
+		DBG_871X("[%s] ch = %d, offset = %d, bwmode = %d\n", __func__, channel, channel_offset, bwmode);
+
+	center_ch = rtw_get_center_ch(channel, bwmode, channel_offset);
+
+	if (bwmode == CHANNEL_WIDTH_80) {
+		if (center_ch > channel)
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_LOWER;
+		else if (center_ch < channel)
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_UPPER;
+		else
+			chnl_offset80 = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
+	}
+
+	/* 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->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)
+{
+	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;
+
+	/* DBG_8192C("********* DUMP CAM Entry_#%02d***************\n", entry); */
+	for (j = 0; j < 6; j++) {
+		cmd = _ReadCAM(padapter, addr+j);
+		/* DBG_8192C("offset:0x%02x => 0x%08x\n", addr+j, cmd); */
+		if (j > 1) /* get key from cam */
+			memcpy(get_key+(j-2)*4, &cmd, 4);
+	}
+	/* DBG_8192C("*********************************\n"); */
+}
+
+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);
+}
+
+inline void write_cam_from_cache(struct adapter *adapter, u8 id)
+{
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	struct cam_ctl_t *cam_ctl = &dvobj->cam_ctl;
+	struct cam_entry_cache cache;
+
+	spin_lock_bh(&cam_ctl->lock);
+	memcpy(&cache, &dvobj->cam_cache[id], sizeof(struct cam_entry_cache));
+	spin_unlock_bh(&cam_ctl->lock);
+
+	_write_cam(adapter, id, cache.ctrl, cache.mac, cache.key);
+}
+
+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;
+	}
+
+	if (addr)
+		DBG_871X(FUNC_ADPT_FMT" addr:"MAC_FMT" kid:%d, return cam_id:%d\n"
+			 , FUNC_ADPT_ARG(adapter), MAC_ARG(addr), kid, cam_id);
+	else
+		DBG_871X(FUNC_ADPT_FMT" addr:%p kid:%d, return cam_id:%d\n"
+			 , FUNC_ADPT_ARG(adapter), addr, kid, cam_id);
+
+	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) {
+			DBG_871X_LEVEL(_drv_always_, 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) == true)
+				cam_id = i;
+			else
+				DBG_871X_LEVEL(_drv_always_, 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)
+				DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT" pairwise key with "MAC_FMT" id:%u no room\n"
+				, FUNC_ADPT_ARG(adapter), MAC_ARG(sta->hwaddr), kid);
+			else
+				DBG_871X_LEVEL(_drv_always_, 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;
+}
+
+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 | WIRELESS_11A)) {
+			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;
+			}
+
+			DBG_871X("WMM(%x): %x, %x\n", ACI, ACM, acParm);
+		}
+
+		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) {
+			u32 j, tmp, 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) {
+						tmp = edca[i];
+						edca[i] = edca[j];
+						edca[j] = tmp;
+
+						tmp = inx[i];
+						inx[i] = inx[j];
+						inx[j] = tmp;
+
+						change_inx = false;
+					}
+				}
+			}
+		}
+
+		for (i = 0; i < 4; i++) {
+			pxmitpriv->wmm_para_seq[i] = inx[i];
+			DBG_871X("wmm_para_seq(%d): %d\n", i, pxmitpriv->wmm_para_seq[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 (pmlmeext->cur_bwmode >= CHANNEL_WIDTH_80)
+		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->MacAddress);
+		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 rf_type;
+	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 == NULL)
+		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;
+		}
+	}
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	/* 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 */
+	switch (rf_type) {
+	case RF_1T1R:
+	case RF_1T2R:
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_1R);
+		break;
+	case RF_2T2R:
+	default:
+#ifdef CONFIG_DISABLE_MCS13TO15
+		if (pmlmeext->cur_bwmode == CHANNEL_WIDTH_40 && pregistrypriv->wifi_spec != 1)
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R_13TO15_OFF);
+		else
+			set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#else /* CONFIG_DISABLE_MCS13TO15 */
+		set_mcs_rate_by_mask(pmlmeinfo->HT_caps.u.HT_cap_element.MCS_rate, MCS_RATE_2R);
+#endif /* CONFIG_DISABLE_MCS13TO15 */
+	}
+
+	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);
+			DBG_871X("Enable HT Tx STBC !\n");
+		}
+		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));
+			DBG_871X("Enable HT Tx LDPC!\n");
+		}
+		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));
+			DBG_871X("Enable HT Tx STBC!\n");
+		}
+		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 == NULL)
+		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);
+
+	return;
+}
+
+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);
+
+	DBG_871X("%s\n", __func__);
+
+	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);
+	u32 hidden_ssid = 0;
+	struct HT_info_element *pht_info = NULL;
+	struct rtw_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;
+
+	if (is_client_associated_to_ap(Adapter) == false)
+		return true;
+
+	len = packet_len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (len > MAX_IE_SZ) {
+		DBG_871X("%s IE too long for survey event\n", __func__);
+		return _FAIL;
+	}
+
+	if (memcmp(cur_network->network.MacAddress, pbssid, 6)) {
+		DBG_871X("Oops: rtw_check_network_encrypt linked but recv other bssid bcn\n" MAC_FMT MAC_FMT,
+				MAC_ARG(pbssid), MAC_ARG(cur_network->network.MacAddress));
+		return true;
+	}
+
+	bssid = (struct wlan_bssid_ex *)rtw_zmalloc(sizeof(struct wlan_bssid_ex));
+	if (bssid == NULL) {
+		DBG_871X("%s rtw_zmalloc fail !!!\n", __func__);
+		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->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 rtw_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))) {
+			DBG_871X("%s bcn now: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+							ht_cap_info, ht_info_infos_0);
+			DBG_871X("%s bcn link: ht_cap_info:%x ht_info_infos_0:%x\n", __func__,
+							cur_network->BcnInfo.ht_cap_info, cur_network->BcnInfo.ht_info_infos_0);
+			DBG_871X("%s bw mode change\n", __func__);
+			{
+				/* 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 */
+			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 */
+					/* DBG_871X("Oops: %s we don't find channel IE, so don't check it\n", __func__); */
+					bcn_channel = Adapter->mlmeextpriv.cur_channel;
+			}
+	}
+	if (bcn_channel != Adapter->mlmeextpriv.cur_channel) {
+			DBG_871X("%s beacon channel:%d cur channel:%d disconnect\n", __func__,
+						   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 == NULL) {
+		DBG_871X("%s marc: cannot find SSID for survey event\n", __func__);
+		hidden_ssid = true;
+	} else {
+		hidden_ssid = false;
+	}
+
+	if ((NULL != p) && (false == 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';
+	}
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("%s bssid.Ssid.Ssid:%s bssid.Ssid.SsidLength:%d "
+				"cur_network->network.Ssid.Ssid:%s len:%d\n", __func__, bssid->Ssid.Ssid,
+				bssid->Ssid.SsidLength, cur_network->network.Ssid.Ssid,
+				cur_network->network.Ssid.SsidLength));
+
+	if (memcmp(bssid->Ssid.Ssid, cur_network->network.Ssid.Ssid, 32) ||
+			bssid->Ssid.SsidLength != cur_network->network.Ssid.SsidLength) {
+		if (bssid->Ssid.Ssid[0] != '\0' && bssid->Ssid.SsidLength != 0) { /* not hidden ssid */
+			DBG_871X("%s(), SSID is not match\n", __func__);
+			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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+			("%s(): cur_network->network.Privacy is %d, bssid.Privacy is %d\n",
+			 __func__, cur_network->network.Privacy, bssid->Privacy));
+	if (cur_network->network.Privacy != bssid->Privacy) {
+		DBG_871X("%s(), privacy is not match\n", __func__);
+		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) {
+		DBG_871X("%s(): enctyp is not match\n", __func__);
+		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)) {
+			if (_SUCCESS == rtw_parse_wpa_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+						("%s pnetwork->pairwise_cipher: %d, group_cipher is %d, is_8021x is %d\n", __func__,
+						 pairwise_cipher, group_cipher, is_8021x));
+			}
+		} else {
+			pbuf = rtw_get_wpa2_ie(&bssid->IEs[12], &wpa_ielen, bssid->IELength-12);
+
+			if (pbuf && (wpa_ielen > 0)) {
+				if (_SUCCESS == rtw_parse_wpa2_ie(pbuf, wpa_ielen+2, &group_cipher, &pairwise_cipher, &is_8021x)) {
+					RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
+							("%s pnetwork->pairwise_cipher: %d, pnetwork->group_cipher is %d, is_802x is %d\n",
+							 __func__, pairwise_cipher, group_cipher, is_8021x));
+				}
+			}
+		}
+
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+				("%s cur_network->group_cipher is %d: %d\n", __func__, cur_network->BcnInfo.group_cipher, group_cipher));
+		if (pairwise_cipher != cur_network->BcnInfo.pairwise_cipher || group_cipher != cur_network->BcnInfo.group_cipher) {
+			DBG_871X("%s pairwise_cipher(%x:%x) or group_cipher(%x:%x) is not match\n", __func__,
+					pairwise_cipher, cur_network->BcnInfo.pairwise_cipher,
+					group_cipher, cur_network->BcnInfo.group_cipher);
+			goto _mismatch;
+		}
+
+		if (is_8021x != cur_network->BcnInfo.is_8021x) {
+			DBG_871X("%s authentication is not match\n", __func__);
+			goto _mismatch;
+		}
+	}
+
+	kfree((u8 *)bssid);
+	return _SUCCESS;
+
+_mismatch:
+	kfree((u8 *)bssid);
+
+	if (pmlmepriv->NumOfBcnInfoChkFail == 0)
+		pmlmepriv->timeBcnInfoChkStart = jiffies;
+
+	pmlmepriv->NumOfBcnInfoChkFail++;
+	DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d (SeqNum of this Beacon frame = %d).\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail, GetSequence(pframe));
+
+	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)) {
+		DBG_871X("%s by "ADPT_FMT" - NumOfChkFail = %d >= threshold : %d (in %d ms), return FAIL.\n", __func__, ADPT_ARG(Adapter), pmlmepriv->NumOfBcnInfoChkFail,
+			DISCONNECT_BY_CHK_BCN_FAIL_THRESHOLD, jiffies_to_msecs(jiffies - pmlmepriv->timeBcnInfoChkStart));
+		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->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			/* to update WMM paramter 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 _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);
+	}
+}
+
+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.IELength;) {
+			pIE = (struct ndis_80211_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;
+
+			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 enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta);
+void enable_rate_adaptive(struct adapter *padapter, struct sta_info *psta)
+{
+	Update_RA_Entry(padapter, psta);
+}
+
+void set_sta_rate(struct adapter *padapter, struct sta_info *psta)
+{
+	/* rate adaptive */
+	enable_rate_adaptive(padapter, psta);
+}
+
+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->ElementID) {
+		case _VENDOR_SPECIFIC_IE_:
+			if ((!memcmp(pIE->data, ARTHEROS_OUI1, 3)) || (!memcmp(pIE->data, ARTHEROS_OUI2, 3))) {
+				DBG_871X("link to Artheros AP\n");
+				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))) {
+				DBG_871X("link to Broadcom AP\n");
+				return HT_IOT_PEER_BROADCOM;
+			} else if (!memcmp(pIE->data, MARVELL_OUI, 3)) {
+				DBG_871X("link to Marvell AP\n");
+				return HT_IOT_PEER_MARVELL;
+			} else if (!memcmp(pIE->data, RALINK_OUI, 3)) {
+				DBG_871X("link to Ralink AP\n");
+				return HT_IOT_PEER_RALINK;
+			} else if (!memcmp(pIE->data, CISCO_OUI, 3)) {
+				DBG_871X("link to Cisco AP\n");
+				return HT_IOT_PEER_CISCO;
+			} else if (!memcmp(pIE->data, REALTEK_OUI, 3)) {
+				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;
+							DBG_871X("link to Realtek JAGUAR_BCUTAP\n");
+						}
+						if (pIE->data[6] & RT_HT_CAP_USE_JAGUAR_CCUT) {
+							Vender = HT_IOT_PEER_REALTEK_JAGUAR_CCUTAP;
+							DBG_871X("link to Realtek JAGUAR_CCUTAP\n");
+						}
+					}
+				}
+
+				DBG_871X("link to Realtek AP\n");
+				return Vender;
+			} else if (!memcmp(pIE->data, AIRGOCAP_OUI, 3)) {
+				DBG_871X("link to Airgo Cap\n");
+				return HT_IOT_PEER_AIRGO;
+			} else
+				break;
+
+		default:
+			break;
+		}
+
+		i += (pIE->Length + 2);
+	}
+
+	DBG_871X("link to new AP\n");
+	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 | WIRELESS_11A | WIRELESS_11_5N | WIRELESS_11AC))
+			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 ratelen, 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->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) {
+		if (pmlmeinfo->VHT_enable)
+			network_type = WIRELESS_11AC;
+		else if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_5N;
+
+		network_type |= WIRELESS_11A;
+	} else{
+		if (pmlmeinfo->VHT_enable)
+			network_type = WIRELESS_11AC;
+		else if (pmlmeinfo->HT_enable)
+			network_type = WIRELESS_11_24N;
+
+		if ((cckratesonly_included(rate, ratelen)) == true)
+			network_type |= WIRELESS_11B;
+		else if ((cckrates_included(rate, ratelen)) == true)
+			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 (IsSupportedTxCCK(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, _SUPPORTEDRATES_IE_, &ie_len, var_ie_len);
+	if (pIE == NULL)
+		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, _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 process_addba_req(struct adapter *padapter, u8 *paddba_req, u8 *addr)
+{
+	struct sta_info *psta;
+	u16 tid, start_seq, 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) {
+		start_seq = le16_to_cpu(preq->BA_starting_seqctrl) >> 4;
+
+		param = le16_to_cpu(preq->BA_para_set);
+		tid = (param>>2)&0x0f;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[tid];
+
+		#ifdef CONFIG_UPDATE_INDICATE_SEQ_WHILE_PROCESS_ADDBA_REQ
+		preorder_ctrl->indicate_seq = start_seq;
+		#ifdef DBG_RX_SEQ
+		DBG_871X("DBG_RX_SEQ %s:%d IndicateSeq: %d, start_seq: %d\n", __func__, __LINE__,
+			preorder_ctrl->indicate_seq, start_seq);
+		#endif
+		#else
+		preorder_ctrl->indicate_seq = 0xffff;
+		#endif
+
+		preorder_ctrl->enable = (pmlmeinfo->bAcceptAddbaReq == true) ? true : false;
+	}
+
+}
+
+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);
+
+	/* DBG_871X("%s(): tsf_upper = 0x%08x, tsf_lower = 0x%08x\n", __func__, (u32)(tsf>>32), (u32)tsf); */
+
+	/* delay = (timestamp mod 1024*100)/1000 (unit: ms) */
+	/* delay_ms = do_div(tsf, (pmlmeinfo->bcn_interval*1024))/1000; */
+	delay_ms = rtw_modular64(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; */
+
+/*
+	DBG_871X("%s(): (a)bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+
+	for (i = 0; i<9; i++)
+	{
+		DBG_871X("%s():bcn_delay_cnt[%d]=%d,  bcn_delay_ratio[%d]=%d\n", __func__, 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;
+
+		DBG_871X("%s(): bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+		for (i = 0; i < 9; i++) {
+			pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i] * 100) / pmlmeext->bcn_cnt;
+
+
+			DBG_871X("%s():bcn_delay_cnt[%d]=%d,  bcn_delay_ratio[%d]=%d\n", __func__, i,
+				pmlmeext->bcn_delay_cnt[i], i, pmlmeext->bcn_delay_ratio[i]);
+
+			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;
+				DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, DrvBcnEarly);
+			}
+
+			if (ratio_80_delay > 80 && DrvBcnTimeOut == 0xff) {
+				DrvBcnTimeOut = i;
+				DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, DrvBcnTimeOut);
+			}
+
+			/* 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 beacon_timing_control(struct adapter *padapter)
+{
+	rtw_hal_bcn_related_reg_setting(padapter);
+}
+
+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;
+		DBG_871X("  no room for more MACIDs\n");
+	} else{
+		psta->mac_id = i;
+		DBG_871X("%s = %d\n", __func__, psta->mac_id);
+	}
+
+}
+
+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) {
+			DBG_871X("%s = %d\n", __func__, psta->mac_id);
+			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;
+}
+
+#ifdef CONFIG_GPIO_API
+int rtw_get_gpio(struct net_device *netdev, int gpio_num)
+{
+	u8 value;
+	u8 direction;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(adapter);
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	DBG_871X("rf_pwrstate = 0x%02x\n", pwrpriv->rf_pwrstate);
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	/* Read GPIO Direction */
+	direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num;
+
+	/* According the direction to read register value */
+	if (direction)
+		value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & BIT(gpio_num)) >> gpio_num;
+	else
+		value = (rtw_read8(adapter, REG_GPIO_PIN_CTRL) & BIT(gpio_num)) >> gpio_num;
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+	DBG_871X("%s direction =%d value =%d\n", __func__, direction, value);
+
+	return value;
+}
+EXPORT_SYMBOL(rtw_get_gpio);
+
+int  rtw_set_gpio_output_value(struct net_device *netdev, int gpio_num, bool isHigh)
+{
+	u8 direction = 0;
+	u8 res = -1;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+
+	/* Check GPIO is 4~7 */
+	if (gpio_num > 7 || gpio_num < 4) {
+		DBG_871X("%s The gpio number does not included 4~7.\n", __func__);
+		return -1;
+	}
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	/* Read GPIO direction */
+	direction = (rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & BIT(gpio_num)) >> gpio_num;
+
+	/* If GPIO is output direction, setting value. */
+	if (direction) {
+		if (isHigh)
+			rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) | BIT(gpio_num));
+		else
+			rtw_write8(adapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 1) & ~BIT(gpio_num));
+
+		DBG_871X("%s Set gpio %x[%d]=%d\n", __func__, REG_GPIO_PIN_CTRL+1, gpio_num, isHigh);
+		res = 0;
+	} else{
+		DBG_871X("%s The gpio is input, not be set!\n", __func__);
+		res = -1;
+	}
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+	return res;
+}
+EXPORT_SYMBOL(rtw_set_gpio_output_value);
+
+int rtw_config_gpio(struct net_device *netdev, int gpio_num, bool isOutput)
+{
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(netdev);
+
+	if (gpio_num > 7 || gpio_num < 4) {
+		DBG_871X("%s The gpio number does not included 4~7.\n", __func__);
+		return -1;
+	}
+
+	DBG_871X("%s gpio_num =%d direction =%d\n", __func__, gpio_num, isOutput);
+
+	rtw_ps_deny(adapter, PS_DENY_IOCTL);
+
+	LeaveAllPowerSaveModeDirect(adapter);
+
+	if (isOutput)
+		rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) | BIT(gpio_num));
+	else
+		rtw_write8(adapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(adapter, REG_GPIO_PIN_CTRL + 2) & ~BIT(gpio_num));
+
+	rtw_ps_deny_cancel(adapter, PS_DENY_IOCTL);
+
+	return 0;
+}
+EXPORT_SYMBOL(rtw_config_gpio);
+#endif
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+void rtw_get_current_ip_address(struct adapter *padapter, u8 *pcurrentip)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct in_device *my_ip_ptr = padapter->pnetdev->ip_ptr;
+	u8 ipaddress[4];
+
+	if ((pmlmeinfo->state & WIFI_FW_LINKING_STATE) ||
+			pmlmeinfo->state & WIFI_FW_AP_STATE) {
+		if (my_ip_ptr != NULL) {
+			struct in_ifaddr *my_ifa_list = my_ip_ptr->ifa_list;
+			if (my_ifa_list != NULL) {
+				ipaddress[0] = my_ifa_list->ifa_address & 0xFF;
+				ipaddress[1] = (my_ifa_list->ifa_address >> 8) & 0xFF;
+				ipaddress[2] = (my_ifa_list->ifa_address >> 16) & 0xFF;
+				ipaddress[3] = my_ifa_list->ifa_address >> 24;
+				DBG_871X("%s: %d.%d.%d.%d ==========\n", __func__,
+						ipaddress[0], ipaddress[1], ipaddress[2], ipaddress[3]);
+				memcpy(pcurrentip, ipaddress, 4);
+			}
+		}
+	}
+}
+#endif
+#ifdef CONFIG_WOWLAN
+void rtw_get_sec_iv(struct adapter *padapter, u8 *pcur_dot11txpn, u8 *StaAddr)
+{
+	struct sta_info 	*psta;
+	struct security_priv *psecpriv = &padapter->securitypriv;
+
+	memset(pcur_dot11txpn, 0, 8);
+	if (NULL == StaAddr)
+		return;
+	psta = rtw_get_stainfo(&padapter->stapriv, StaAddr);
+	DBG_871X("%s(): StaAddr: %02x %02x %02x %02x %02x %02x\n",
+		__func__, StaAddr[0], StaAddr[1], StaAddr[2],
+		StaAddr[3], StaAddr[4], StaAddr[5]);
+
+	if (psta) {
+		if (psecpriv->dot11PrivacyAlgrthm != _NO_PRIVACY_ && psta->dot11txpn.val > 0)
+			psta->dot11txpn.val--;
+		AES_IV(pcur_dot11txpn, psta->dot11txpn, 0);
+
+		DBG_871X("%s(): CurrentIV: %02x %02x %02x %02x %02x %02x %02x %02x\n"
+		, __func__, pcur_dot11txpn[0], pcur_dot11txpn[1],
+		pcur_dot11txpn[2], pcur_dot11txpn[3], pcur_dot11txpn[4],
+		pcur_dot11txpn[5], pcur_dot11txpn[6], pcur_dot11txpn[7]);
+	}
+}
+void rtw_set_sec_pn(struct adapter *padapter)
+{
+		struct sta_info         *psta;
+		struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
+		struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
+		struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+		struct security_priv *psecpriv = &padapter->securitypriv;
+
+		psta = rtw_get_stainfo(&padapter->stapriv,
+		get_my_bssid(&pmlmeinfo->network));
+
+		if (psta) {
+			if (pwrpriv->wowlan_fw_iv > psta->dot11txpn.val) {
+				if (psecpriv->dot11PrivacyAlgrthm != _NO_PRIVACY_)
+					psta->dot11txpn.val = pwrpriv->wowlan_fw_iv + 2;
+			} else {
+				DBG_871X("%s(): FW IV is smaller than driver\n", __func__);
+				psta->dot11txpn.val += 2;
+			}
+			DBG_871X("%s: dot11txpn: 0x%016llx\n", __func__, psta->dot11txpn.val);
+		}
+}
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_PNO_SUPPORT
+#define	CSCAN_TLV_TYPE_SSID_IE	'S'
+#define CIPHER_IE "key_mgmt ="
+#define CIPHER_NONE "NONE"
+#define CIPHER_WPA_PSK "WPA-PSK"
+#define CIPHER_WPA_EAP "WPA-EAP IEEE8021X"
+
+#endif /* CONFIG_PNO_SUPPORT */
diff --git a/drivers/staging/rtl8723bs/core/rtw_xmit.c b/drivers/staging/rtl8723bs/core/rtw_xmit.c
new file mode 100644
index 000000000000..8f2c9a6658bf
--- /dev/null
+++ b/drivers/staging/rtl8723bs/core/rtw_xmit.c
@@ -0,0 +1,3100 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTW_XMIT_C_
+
+#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);
+	_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);
+
+	/* for (i = 0 ; i < MAX_NUMBLKS; i++) */
+	/* 	_init_txservq(&(psta_xmitpriv->blk_q[i])); */
+
+	_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;
+	sint	res = _SUCCESS;
+
+	/*  We don't need to memset padapter->XXX to zero, because adapter is allocated by vzalloc(). */
+	/* memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv)); */
+
+	spin_lock_init(&pxmitpriv->lock);
+	spin_lock_init(&pxmitpriv->lock_sctx);
+	sema_init(&pxmitpriv->xmit_sema, 0);
+	sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
+
+	/*
+	Please insert all the queue initializaiton using _rtw_init_queue below
+	*/
+
+	pxmitpriv->adapter = padapter;
+
+	/* for (i = 0 ; i < MAX_NUMBLKS; i++) */
+	/* 	_rtw_init_queue(&pxmitpriv->blk_strms[i]); */
+
+	_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->legacy_dz_queue); */
+	/* _rtw_init_queue(&pxmitpriv->apsd_queue); */
+
+	_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  == NULL) {
+		pxmitpriv->pxmit_frame_buf = NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+	pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
+	/* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
+	/* 						((SIZE_PTR) (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  == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_buf fail!\n"));
+		res = _FAIL;
+		goto exit;
+	}
+
+	pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
+	/* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
+	/* 						((SIZE_PTR) (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->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  */
+	_rtw_init_queue(&pxmitpriv->free_xframe_ext_queue);
+
+	pxmitpriv->xframe_ext_alloc_addr = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
+
+	if (pxmitpriv->xframe_ext_alloc_addr  == NULL) {
+		pxmitpriv->xframe_ext = NULL;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n"));
+		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 */
+	_rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
+
+	pxmitpriv->pallocated_xmit_extbuf = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
+
+	if (pxmitpriv->pallocated_xmit_extbuf  == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n"));
+		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;
+		}
+	}
+
+	rtw_alloc_hwxmits(padapter);
+	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 == NULL)
+		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++;
+	}
+
+	if (pxmitpriv->pallocated_frame_buf)
+		vfree(pxmitpriv->pallocated_frame_buf);
+
+
+	if (pxmitpriv->pallocated_xmitbuf)
+		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++;
+		}
+	}
+	if (pxmitpriv->xframe_ext_alloc_addr)
+		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++;
+	}
+
+	if (pxmitpriv->pallocated_xmit_extbuf) {
+		vfree(pxmitpriv->pallocated_xmit_extbuf);
+	}
+
+	for (i = 0; i < CMDBUF_MAX; i++) {
+		pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
+		if (pxmitbuf != NULL)
+			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_80m = false;
+
+	sgi_20m = psta->htpriv.sgi_20m;
+	sgi_40m = psta->htpriv.sgi_40m;
+
+	switch (psta->bw_mode) {
+	case CHANNEL_WIDTH_80:
+		sgi = sgi_80m;
+		break;
+	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;
+
+	if (mlmeext->cur_bwmode < psta->bw_mode)
+		pattrib->bwmode = mlmeext->cur_bwmode;
+	else
+		pattrib->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;
+
+	/* if (pattrib->ht_en && psta->htpriv.ampdu_enable) */
+	/*  */
+	/* 	if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */
+	/* 		pattrib->ampdu_en = true; */
+	/*  */
+
+
+	pattrib->retry_ctrl = false;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (psta->isrc && psta->pid > 0)
+		pattrib->pctrl = true;
+#endif
+
+}
+
+static s32 update_attrib_sec_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
+{
+	sint res = _SUCCESS;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	sint 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) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\n psta->ieee8021x_blocked == true\n"));
+
+		pattrib->encrypt = 0;
+
+		if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\npsta->ieee8021x_blocked == true,  pattrib->ether_type(%.4x) != 0x888e\n", pattrib->ether_type));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == true,  pattrib->ether_type(%04x) != 0x888e\n", __func__, pattrib->ether_type);
+			#endif
+			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) {
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s psecuritypriv->busetkipkey(%d) == _FAIL drop packet\n", __func__, psecuritypriv->busetkipkey);
+			#endif
+			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);
+
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
+		("update_attrib: encrypt =%d  securitypriv.sw_encrypt =%d\n",
+		pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+
+	if (pattrib->encrypt &&
+		((padapter->securitypriv.sw_encrypt == true) || (psecuritypriv->hw_decrypted == false))) {
+		pattrib->bswenc = true;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			("update_attrib: encrypt =%d securitypriv.hw_decrypted =%d bswenc =true\n",
+			pattrib->encrypt, padapter->securitypriv.sw_encrypt));
+	} else {
+		pattrib->bswenc = false;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc =false\n"));
+	}
+
+exit:
+
+	return res;
+
+}
+
+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:
+		DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
+		break;
+	}
+
+	return change_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 = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
+		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, _pkt *pkt, struct pkt_attrib *pattrib)
+{
+	uint i;
+	struct pkt_file pktfile;
+	struct sta_info *psta = NULL;
+	struct ethhdr etherhdr;
+
+	sint bmcast;
+	struct sta_priv 	*pstapriv = &padapter->stapriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct qos_priv 	*pqospriv = &pmlmepriv->qospriv;
+	sint res = _SUCCESS;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib);
+
+	_rtw_open_pktfile(pkt, &pktfile);
+	i = _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);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc);
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
+		memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
+		memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_ap);
+	} else
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown);
+
+	pattrib->pktlen = pktfile.pkt_len;
+
+	if (ETH_P_IP == pattrib->ether_type) {
+		/*  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 (ETH_P_IP == pattrib->ether_type) {/*  IP header */
+				if (((tmp[21] == 68) && (tmp[23] == 67)) ||
+					((tmp[21] == 67) && (tmp[23] == 68))) {
+					/*  68 : UDP BOOTP client */
+					/*  67 : UDP BOOTP server */
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======================update_attrib: get DHCP Packet\n"));
+					pattrib->dhcp_pkt = 1;
+					DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp);
+				}
+			}
+		}
+
+		/* 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;
+				DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp);
+			}
+		}
+
+
+	} else if (0x888e == pattrib->ether_type) {
+		DBG_871X_LEVEL(_drv_always_, "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) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active);
+		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 == NULL)	{ /*  if we cannot get psta => drop the pkt */
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta);
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra)));
+			#ifdef DBG_TX_DROP_FRAME
+			DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+			#endif
+			res = _FAIL;
+			goto exit;
+		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) {
+			DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link);
+			res = _FAIL;
+			goto exit;
+		}
+	}
+
+	if (psta == NULL) {
+		/*  if we cannot get psta => drop the pkt */
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta);
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra)));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
+		#endif
+		res = _FAIL;
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link);
+		DBG_871X("%s, psta("MAC_FMT")->state(0x%x) != _FW_LINKED\n", __func__, MAC_ARG(psta->hwaddr), psta->state);
+		return _FAIL;
+	}
+
+
+
+	/* TODO:_lock */
+	if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sec);
+		res = _FAIL;
+		goto exit;
+	}
+
+	update_attrib_phy_info(padapter, pattrib, psta);
+
+	/* DBG_8192C("%s ==> mac_id(%d)\n", __func__, pattrib->mac_id); */
+
+	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 */
+
+	rtw_set_tx_chksum_offload(pkt, pattrib);
+
+exit:
+	return res;
+}
+
+static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint			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;
+	sint bmcst = IS_MCAST(pattrib->ra);
+
+/*
+	if (pattrib->psta)
+	{
+		stainfo = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
+	}
+
+	if (stainfo == NULL)
+	{
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return _FAIL;
+	}
+
+	if (!(stainfo->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
+		return _FAIL;
+	}
+*/
+
+	hw_hdr_offset = TXDESC_OFFSET;
+
+	if (pattrib->encrypt == _TKIP_) { /* if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_PRIVACY_) */
+		/* encode mic code */
+		/* if (stainfo!= NULL) */
+		{
+			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)) {
+					/* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */
+					/* msleep(10); */
+					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)) {
+					/* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */
+					/* msleep(10); */
+					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 (pqospriv->qos_option == 1) */
+			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 *)RND4((SIZE_PTR)(payload));
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("===curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
+					curfragnum, *payload, *(payload+1), *(payload+2), *(payload+3), *(payload+4), *(payload+5), *(payload+6), *(payload+7)));
+
+				payload = payload+pattrib->hdrlen+pattrib->iv_len;
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d", curfragnum, 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;
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d length =%d pattrib->icv_len =%d", curfragnum, length, pattrib->icv_len));
+				}
+			}
+			rtw_secgetmic(&micdata, &(mic[0]));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: before add mic code!!!\n"));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n", pattrib->last_txcmdsz));
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: mic[0]= 0x%.2x , mic[1]= 0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\n\
+  mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
+				mic[0], mic[1], mic[2], mic[3], mic[4], mic[5], mic[6], mic[7]));
+			/* add mic code  and add the mic code length in last_txcmdsz */
+
+			memcpy(payload, &(mic[0]), 8);
+			pattrib->last_txcmdsz += 8;
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("\n ========last pkt ========\n"));
+			payload = payload-pattrib->last_txcmdsz+8;
+			for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz; curfragnum = curfragnum+8)
+					RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, (" %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x ",
+					*(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2), *(payload+curfragnum+3),
+					*(payload+curfragnum+4), *(payload+curfragnum+5), *(payload+curfragnum+6), *(payload+curfragnum+7)));
+			}
+/*
+			else {
+				RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: rtw_get_stainfo == NULL!!!\n"));
+			}
+*/
+	}
+	return _SUCCESS;
+}
+
+static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+
+	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
+	/* struct	security_priv *psecuritypriv =&padapter->securitypriv; */
+
+	/* if ((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */
+	if (pattrib->bswenc) {
+		/* DBG_871X("start xmitframe_swencrypt\n"); */
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("### xmitframe_swencrypt\n"));
+		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;
+		}
+
+	} else
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, ("### xmitframe_hwencrypt\n"));
+
+	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;
+	sint 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 {
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
+			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) {
+				DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+				return _FAIL;
+			}
+
+			if (psta == NULL) {
+				DBG_871X("%s, psta ==NUL\n", __func__);
+				return _FAIL;
+			}
+
+			if (!(psta->state & _FW_LINKED)) {
+				DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+				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)) {
+						/* DBG_871X("tx ampdu seqnum(%d) < tx_seq(%d)\n", 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{
+						/* DBG_871X("tx ampdu over run\n"); */
+						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, _pkt *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 sta_info 	*psta; */
+	/* struct sta_priv 	*pstapriv = &padapter->stapriv; */
+	/* struct mlme_priv *pmlmepriv = &padapter->mlmepriv; */
+	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 (pattrib->psta)
+	{
+		psta = pattrib->psta;
+	} else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	}
+
+	if (psta == NULL)
+  {
+
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return _FAIL;
+	}
+
+
+	if (!(psta->state &_FW_LINKED))
+	{
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+*/
+	if (pxmitframe->buf_addr == NULL) {
+		DBG_8192C("==> %s buf_addr == NULL\n", __func__);
+		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) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
+		DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
+		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);
+
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
+				 ("rtw_xmitframe_coalesce: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
+				  padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
+
+			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 broadcat/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;
+		} else
+			RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
+
+		addr = (SIZE_PTR)(pframe);
+
+		mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
+		memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
+
+	}
+
+	if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
+		DBG_8192C("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
+		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, _pkt *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 == NULL)
+		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 != true) {
+		DBG_871X("no instll BIP key\n");
+		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, _MME_IE_, 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 == NULL) {
+
+				DBG_871X("%s, psta ==NUL\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			if (!(psta->state & _FW_LINKED) || pxmitframe->buf_addr == NULL) {
+				DBG_871X("%s, not _FW_LINKED or addr null\n", __func__);
+				goto xmitframe_coalesce_fail;
+			}
+
+			/* DBG_871X("%s, action frame category =%d\n", __func__, pframe[WLAN_HDR_A3_LEN]); */
+			/* 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;
+	sint	 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 == NULL)
+			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 !=  NULL) {
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 0;
+		pxmitbuf->pg_num = 0;
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	} else
+		DBG_871X("%s fail, no xmitbuf available !!!\n", __func__);
+
+	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 == NULL) {
+		DBG_871X("%s, alloc xmitframe fail\n", __func__);
+		return NULL;
+	}
+
+	pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type);
+	if (pxmitbuf == NULL) {
+		DBG_871X("%s, alloc xmitbuf fail\n", __func__);
+		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)
+{
+	_irqL 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 = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		list_del_init(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL) {
+		pxmitpriv->free_xmit_extbuf_cnt--;
+		#ifdef DBG_XMIT_BUF_EXT
+		DBG_871X("DBG_XMIT_BUF_EXT ALLOC no =%d,  free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+		#endif
+
+
+		pxmitbuf->priv_data = NULL;
+
+		pxmitbuf->len = 0;
+		pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
+		pxmitbuf->agg_num = 1;
+
+		if (pxmitbuf->sctx) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			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)
+{
+	_irqL irqL;
+	struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
+
+	if (pxmitbuf == NULL)
+		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++;
+	#ifdef DBG_XMIT_BUF_EXT
+	DBG_871X("DBG_XMIT_BUF_EXT FREE no =%d, free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
+	#endif
+
+	spin_unlock_irqrestore(&pfree_queue->lock, irqL);
+
+	return _SUCCESS;
+}
+
+struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
+{
+	_irqL irqL;
+	struct xmit_buf *pxmitbuf =  NULL;
+	struct list_head *plist, *phead;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* DBG_871X("+rtw_alloc_xmitbuf\n"); */
+
+	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 = LIST_CONTAINOR(plist, struct xmit_buf, list);
+
+		list_del_init(&(pxmitbuf->list));
+	}
+
+	if (pxmitbuf !=  NULL) {
+		pxmitpriv->free_xmitbuf_cnt--;
+		#ifdef DBG_XMIT_BUF
+		DBG_871X("DBG_XMIT_BUF ALLOC no =%d,  free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		/* DBG_871X("alloc, free_xmitbuf_cnt =%d\n", 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) {
+			DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+			rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
+		}
+	}
+	#ifdef DBG_XMIT_BUF
+	else
+		DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
+	#endif
+
+	spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+
+	return pxmitbuf;
+}
+
+s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
+{
+	_irqL irqL;
+	struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
+
+	/* DBG_871X("+rtw_free_xmitbuf\n"); */
+
+	if (pxmitbuf == NULL)
+		return _FAIL;
+
+	if (pxmitbuf->sctx) {
+		DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
+		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++;
+		/* DBG_871X("FREE, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */
+		#ifdef DBG_XMIT_BUF
+		DBG_871X("DBG_XMIT_BUF FREE no =%d, free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
+		#endif
+		spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
+	}
+	return _SUCCESS;
+}
+
+static void rtw_init_xmitframe(struct xmit_frame *pxframe)
+{
+	if (pxframe !=  NULL) { /* default value setting */
+		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->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)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(pfree_xmit_queue);
+
+		plist = get_next(phead);
+
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xmitframe_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n", 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)) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
+		pxframe =  NULL;
+	} else {
+		phead = get_list_head(queue);
+		plist = get_next(phead);
+		pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		list_del_init(&(pxframe->list));
+		pxmitpriv->free_xframe_ext_cnt--;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext():free_xmitframe_cnt =%d\n", 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 == NULL)
+		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);
+
+	DBG_871X("################## %s ##################\n", __func__);
+
+exit:
+	return pxframe;
+}
+
+s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
+{
+	struct __queue *queue = NULL;
+	struct adapter *padapter = pxmitpriv->adapter;
+	_pkt *pndis_pkt = NULL;
+
+	if (pxmitframe == NULL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe == NULL!!!!!!!!!!\n"));
+		goto exit;
+	}
+
+	if (pxmitframe->pkt) {
+		pndis_pkt = pxmitframe->pkt;
+		pxmitframe->pkt = NULL;
+	}
+
+	if (pxmitframe->alloc_addr) {
+		DBG_871X("################## %s with alloc_addr ##################\n", __func__);
+		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++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
+	} else if (pxmitframe->ext_tag == 1) {
+		pxmitpriv->free_xframe_ext_cnt++;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
+	} else {
+	}
+
+	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;
+	struct	xmit_frame	*pxmitframe;
+
+	spin_lock_bh(&(pframequeue->lock));
+
+	phead = get_list_head(pframequeue);
+	plist = get_next(phead);
+
+	while (phead != plist) {
+
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);
+
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+	}
+	spin_unlock_bh(&(pframequeue->lock));
+}
+
+s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	DBG_COUNTER(padapter->tx_logs.core_tx_enqueue);
+	if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
+			 ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
+/* 		pxmitframe->pkt = NULL; */
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, sint up, u8 *ac)
+{
+	struct tx_servq *ptxservq = NULL;
+
+	switch (up) {
+	case 1:
+	case 2:
+		ptxservq = &(psta->sta_xmitpriv.bk_q);
+		*(ac) = 3;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BK\n"));
+		break;
+
+	case 4:
+	case 5:
+		ptxservq = &(psta->sta_xmitpriv.vi_q);
+		*(ac) = 1;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VI\n"));
+		break;
+
+	case 6:
+	case 7:
+		ptxservq = &(psta->sta_xmitpriv.vo_q);
+		*(ac) = 0;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VO\n"));
+		break;
+
+	case 0:
+	case 3:
+	default:
+		ptxservq = &(psta->sta_xmitpriv.be_q);
+		*(ac) = 2;
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BE\n"));
+	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)
+{
+	/* _irqL irqL0; */
+	u8 ac_index;
+	struct sta_info *psta;
+	struct tx_servq	*ptxservq;
+	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
+	struct hw_xmit	*phwxmits =  padapter->xmitpriv.hwxmits;
+	sint res = _SUCCESS;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class);
+
+/*
+	if (pattrib->psta) {
+		psta = pattrib->psta;
+	} else {
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta = rtw_get_stainfo(pstapriv, pattrib->ra);
+	}
+*/
+
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta);
+		DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return _FAIL;
+	}
+
+	if (psta == NULL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta);
+		res = _FAIL;
+		DBG_8192C("rtw_xmit_classifier: psta == NULL\n");
+		RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmit_classifier: psta == NULL\n"));
+		goto exit;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink);
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return _FAIL;
+	}
+
+	ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
+
+	/* spin_lock_irqsave(&pstapending->lock, irqL0); */
+
+	if (list_empty(&ptxservq->tx_pending)) {
+		list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
+	}
+
+	/* spin_lock_irqsave(&ptxservq->sta_pending.lock, irqL1); */
+
+	list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
+	ptxservq->qcnt++;
+	phwxmits[ac_index].accnt++;
+
+	/* spin_unlock_irqrestore(&ptxservq->sta_pending.lock, irqL1); */
+
+	/* spin_unlock_irqrestore(&pstapending->lock, irqL0); */
+
+exit:
+
+	return res;
+}
+
+void 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 = (struct hw_xmit *)rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry);
+
+	if (pxmitpriv->hwxmits == NULL) {
+		DBG_871X("alloc hwxmits fail!...\n");
+		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;
+	} else {
+
+	}
+
+
+}
+
+void rtw_free_hwxmits(struct adapter *padapter)
+{
+	struct hw_xmit *hwxmits;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	hwxmits = pxmitpriv->hwxmits;
+	if (hwxmits)
+		kfree((u8 *)hwxmits);
+}
+
+void rtw_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;
+	}
+}
+
+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;
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority =%d , qsel = %d\n", pattrib->priority, qsel));
+
+	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, _pkt **ppkt)
+{
+	static unsigned long start = 0;
+	static u32 drop_cnt = 0;
+
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	struct xmit_frame *pxmitframe = NULL;
+
+	s32 res;
+
+	DBG_COUNTER(padapter->tx_logs.core_tx);
+
+	if (start == 0)
+		start = jiffies;
+
+	pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
+
+	if (jiffies_to_msecs(jiffies - start) > 2000) {
+		if (drop_cnt)
+			DBG_871X("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __func__, drop_cnt);
+		start = jiffies;
+		drop_cnt = 0;
+	}
+
+	if (pxmitframe == NULL) {
+		drop_cnt++;
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
+		DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe);
+		return -1;
+	}
+
+	res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
+
+	if (res == _FAIL) {
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __func__);
+		#endif
+		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);
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue);
+		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
+		) {
+			DBG_871X(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter)
+				, attrib->ether_type, attrib->dhcp_pkt?" DHCP":"");
+			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;
+}
+
+sint xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
+{
+	sint 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;
+	sint bmcst = IS_MCAST(pattrib->ra);
+	bool update_tim = false;
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate);
+	    return ret;
+	}
+/*
+	if (pattrib->psta)
+	{
+		psta = pattrib->psta;
+	}
+	else
+	{
+		DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
+		psta =rtw_get_stainfo(pstapriv, pattrib->ra);
+	}
+*/
+	psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
+	if (pattrib->psta != psta) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_sta);
+		DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
+		return false;
+	}
+
+	if (psta == NULL) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta);
+		DBG_871X("%s, psta ==NUL\n", __func__);
+		return false;
+	}
+
+	if (!(psta->state & _FW_LINKED)) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link);
+		DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
+		return false;
+	}
+
+	if (pattrib->triggered == 1) {
+		DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger);
+		/* DBG_871X("directly xmit pspoll_triggered packet\n"); */
+
+		/* pattrib->triggered = 0; */
+		if (bmcst && xmitframe_hiq_filter(pxmitframe) == true)
+			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);
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+
+			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);
+
+			/* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+			if (update_tim == true) {
+				update_beacon(padapter, _TIM_IE_, NULL, true);
+			} else {
+				chk_bmc_sleepq_cmd(padapter);
+			}
+
+			/* spin_unlock_bh(&psta->sleep_q.lock); */
+
+			ret = true;
+
+			DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast);
+
+		}
+
+		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);
+
+			/* spin_lock_bh(&psta->sleep_q.lock); */
+
+			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);
+
+				/* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
+
+				if (update_tim == true)
+					/* DBG_871X("sleepq_len == 1, update BCNTIM\n"); */
+					/* upate BCN for TIM IE */
+					update_beacon(padapter, _TIM_IE_, NULL, true);
+			}
+
+			/* spin_unlock_bh(&psta->sleep_q.lock); */
+
+			/* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */
+			/*  */
+			/* 	wakeup_sta_to_xmit(padapter, psta); */
+			/*  */
+
+			ret = true;
+
+			DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast);
+		}
+
+	}
+
+	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)
+{
+	sint ret;
+	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 = get_next(phead);
+
+	while (phead != plist) {
+		pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
+
+		plist = get_next(plist);
+
+		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 {
+			/* DBG_871X("xmitframe_enqueue_for_sleeping_sta return false\n"); */
+		}
+
+	}
+
+}
+
+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;
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+
+	psta_bmc = rtw_get_bcmc_stainfo(padapter);
+
+
+	/* 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);
+
+	while (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+		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 (rtw_hal_xmit(padapter, pxmitframe) == true)
+		{
+			rtw_os_xmit_complete(padapter, pxmitframe);
+		}
+		spin_lock_bh(&psta->sleep_q.lock);
+*/
+		rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+
+	}
+
+	if (psta->sleepq_len == 0) {
+		if (pstapriv->tim_bitmap & BIT(psta->aid)) {
+			/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			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) {
+			DBG_871X("%s alive check\n", __func__);
+			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);
+		xmitframe_plist = get_next(xmitframe_phead);
+
+		while (xmitframe_phead != xmitframe_plist) {
+			pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+			xmitframe_plist = get_next(xmitframe_plist);
+
+			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 (rtw_hal_xmit(padapter, pxmitframe) == true)
+			{
+				rtw_os_xmit_complete(padapter, pxmitframe);
+			}
+			spin_lock_bh(&psta_bmc->sleep_q.lock);
+
+*/
+			rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
+
+		}
+
+		if (psta_bmc->sleepq_len == 0) {
+			if (pstapriv->tim_bitmap & BIT(0)) {
+				/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+				/* upate BCN for TIM IE */
+				/* update_BCNTIM(padapter); */
+				update_mask |= BIT(1);
+			}
+			pstapriv->tim_bitmap &= ~BIT(0);
+			pstapriv->sta_dz_bitmap &= ~BIT(0);
+		}
+
+	}
+
+_exit:
+
+	/* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	if (update_mask)
+		/* update_BCNTIM(padapter); */
+		/* printk("%s => call update_beacon\n", __func__); */
+		update_beacon(padapter, _TIM_IE_, 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;
+	struct xmit_frame *pxmitframe = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	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);
+
+	while (xmitframe_phead != xmitframe_plist) {
+		pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
+
+		xmitframe_plist = get_next(xmitframe_plist);
+
+		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);
+
+			/* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
+			/* upate BCN for TIM IE */
+			/* update_BCNTIM(padapter); */
+			update_beacon(padapter, _TIM_IE_, NULL, true);
+			/* update_mask = BIT(0); */
+		}
+
+	}
+
+	/* spin_unlock_bh(&psta->sleep_q.lock); */
+	spin_unlock_bh(&pxmitpriv->lock);
+
+	return;
+}
+
+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);
+
+	up(&(pri_adapter->xmitpriv.xmit_sema));
+}
+
+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 = LIST_CONTAINOR(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 = LIST_CONTAINOR(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;
+}
+
+sint check_pending_xmitbuf(
+	struct xmit_priv *pxmitpriv)
+{
+	struct __queue *pqueue;
+	sint	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 = (struct adapter *)context;
+
+	thread_enter("RTW_XMIT_THREAD");
+
+	do {
+		err = rtw_hal_xmit_thread_handler(padapter);
+		flush_signals_thread();
+	} while (_SUCCESS == err);
+
+	up(&padapter->xmitpriv.terminate_xmitthread_sema);
+
+	thread_exit();
+}
+
+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, const char *msg)
+{
+	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;
+		DBG_871X("%s timeout: %s\n", __func__, msg);
+	} else {
+		status = sctx->status;
+	}
+
+	if (status == RTW_SCTX_DONE_SUCCESS) {
+		ret = _SUCCESS;
+	}
+
+	return ret;
+}
+
+static bool rtw_sctx_chk_waring_status(int status)
+{
+	switch (status) {
+	case RTW_SCTX_DONE_UNKNOWN:
+	case RTW_SCTX_DONE_BUF_ALLOC:
+	case RTW_SCTX_DONE_BUF_FREE:
+
+	case RTW_SCTX_DONE_DRV_STOP:
+	case RTW_SCTX_DONE_DEV_REMOVE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
+{
+	if (*sctx) {
+		if (rtw_sctx_chk_waring_status(status))
+			DBG_871X("%s status:%d\n", __func__, status);
+		(*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, __func__);
+}
+
+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);
+	} else {
+		DBG_871X("%s ack_tx not set\n", __func__);
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/Hal8723BPwrSeq.c b/drivers/staging/rtl8723bs/hal/Hal8723BPwrSeq.c
new file mode 100644
index 000000000000..0376806335d3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/Hal8723BPwrSeq.c
@@ -0,0 +1,138 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 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 "Hal8723BPwrSeq.h"
+
+/* drivers should parse below arrays and do the corresponding actions */
+/* 3 Power on  Array */
+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 */
+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 */
+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 */
+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 */
+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 */
+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 */
+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 */
+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 */
+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 */
+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 */
+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/Hal8723BReg.h b/drivers/staging/rtl8723bs/hal/Hal8723BReg.h
new file mode 100644
index 000000000000..152a198c8f17
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/Hal8723BReg.h
@@ -0,0 +1,442 @@
+/*****************************************************************************
+ *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 Interrup 7 */
+#define	IMR_BCNDOK6_8723B		BIT19	/*  Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5_8723B		BIT18	/*  Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4_8723B		BIT17	/*  Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3_8723B		BIT16	/*  Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2_8723B		BIT15	/*  Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1_8723B		BIT14	/*  Beacon Queue DMA OK Interrup 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 000000000000..86040adb436c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.c
@@ -0,0 +1,3779 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "Mp_Precomp.h"
+
+/*  Global variables, these are static variables */
+static COEX_DM_8723B_1ANT GLCoexDm8723b1Ant;
+static PCOEX_DM_8723B_1ANT pCoexDm = &GLCoexDm8723b1Ant;
+static COEX_STA_8723B_1ANT GLCoexSta8723b1Ant;
+static PCOEX_STA_8723B_1ANT	pCoexSta = &GLCoexSta8723b1Ant;
+
+static const char *const GLBtInfoSrc8723b1Ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 GLCoexVerDate8723b1Ant = 20140507;
+static u32 GLCoexVer8723b1Ant = 0x4e;
+
+/*  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;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to High\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Low\n")
+				);
+			}
+		} else {
+			if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Low\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at High\n")
+				);
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_BT_RSSI_STATE,
+				("[BTCoex], BT Rssi thresh error!!\n")
+			);
+			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;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Medium\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Low\n")
+				);
+			}
+		} 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;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to High\n")
+				);
+			} else if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Low\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at Medium\n")
+				);
+			}
+		} else {
+			if (btRssi < rssiThresh1) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state switch to Medium\n")
+				);
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_BT_RSSI_STATE,
+					("[BTCoex], BT Rssi state stay at High\n")
+				);
+			}
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static void halbtc8723b1ant_UpdateRaMask(
+	PBTC_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(
+	PBTC_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(
+	PBTC_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(
+	PBTC_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(
+	PBTC_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(
+	PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_MonitorBtCtr(PBTC_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++;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		(
+			"[BTCoex], Hi-Pri Rx/Tx: %d/%d, Lo-Pri Rx/Tx: %d/%d\n",
+			regHPRx,
+			regHPTx,
+			regLPRx,
+			regLPTx
+		)
+	);
+
+	/*  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(PBTC_COEXIST pBtCoexist)
+{
+	s32	wifiRssi = 0;
+	bool bWifiBusy = false, bWifiUnderBMode = false;
+	static u8 nCCKLockCounter = 0;
+
+	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(PBTC_COEXIST pBtCoexist)
+{
+	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], No BT link exists!!!\n")
+		);
+		return algorithm;
+	}
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], BT Profile = SCO only\n")
+			);
+			algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = HID only\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = A2DP only\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = PAN(HS) only\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = PAN(EDR) only\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + HID\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + A2DP ==> SCO\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = HID + A2DP\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+			} else if (pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = A2DP + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = A2DP + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist && pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n")
+				);
+				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
+			} else if (
+				pBtLinkInfo->bHidExist && pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + HID + PAN(HS)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (pBtLinkInfo->bPanExist && pBtLinkInfo->bA2dpExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n")
+					);
+					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) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n")
+					);
+
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n")
+					);
+					algorithm = BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b1ant_SetSwPenaltyTxRateAdaptive(
+	PBTC_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 */
+	}
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set WiFi Low-Penalty Retry: %s",
+			(bLowPenaltyRa ? "ON!!" : "OFF!!")
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_LowPenaltyRa(
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc)
+	);
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b1ant_CoexTable(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6cc = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			val0x6c0, val0x6c4, 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(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 type
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		("[BTCoex], ********** CoexTable(%d) **********\n", 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(
+	PBTC_COEXIST pBtCoexist, bool bEnable
+)
+{
+	u8 H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0; /* function enable */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b1ant_IgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn Ignore WlanAct %s\n",
+			(bForceExec ? "force to" : ""),
+			(bEnable ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+				pCoexDm->bPreIgnoreWlanAct,
+				pCoexDm->bCurIgnoreWlanAct
+			)
+		);
+
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b1ant_SetLpsRpwm(
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 lpsVal, u8 rpwmVal
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
+			(bForceExec ? "force to" : ""),
+			lpsVal,
+			rpwmVal
+		)
+	);
+	pCoexDm->curLps = lpsVal;
+	pCoexDm->curRpwm = rpwmVal;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], LPS-RxBeaconMode = 0x%x , LPS-RPWM = 0x%x!!\n",
+				pCoexDm->curLps,
+				pCoexDm->curRpwm
+			)
+		);
+
+		if (
+			(pCoexDm->preLps == pCoexDm->curLps) &&
+			(pCoexDm->preRpwm == pCoexDm->curRpwm)
+		) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				(
+					"[BTCoex], LPS-RPWM_Last = 0x%x , LPS-RPWM_Now = 0x%x!!\n",
+					pCoexDm->preRpwm,
+					pCoexDm->curRpwm
+				)
+			);
+
+			return;
+		}
+	}
+	halbtc8723b1ant_SetLpsRpwm(pBtCoexist, lpsVal, rpwmVal);
+
+	pCoexDm->preLps = pCoexDm->curLps;
+	pCoexDm->preRpwm = pCoexDm->curRpwm;
+}
+
+static void halbtc8723b1ant_SwMechanism(
+	PBTC_COEXIST pBtCoexist, bool bLowPenaltyRA
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		("[BTCoex], SM[LpRA] = %d\n", bLowPenaltyRA)
+	);
+
+	halbtc8723b1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
+}
+
+static void halbtc8723b1ant_SetAntPath(
+	PBTC_COEXIST pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	PBTC_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) {
+					BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ########### BT is calibrating (wait cnt =%d) ###########\n", cntBtCalChk));
+					mdelay(50);
+				} else {
+					BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ********** BT is NOT calibrating (wait cnt =%d)**********\n", cntBtCalChk));
+					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(
+	PBTC_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)) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], FW for 1Ant AP mode\n")
+			);
+			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;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
+			H2C_Parameter[0],
+			H2C_Parameter[1]<<24|
+			H2C_Parameter[2]<<16|
+			H2C_Parameter[3]<<8|
+			H2C_Parameter[4]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+
+static void halbtc8723b1ant_PsTdma(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	PBTC_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; */
+
+	/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type =%d\n", */
+	/* 	(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type)); */
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+
+	if (pCoexDm->bCurPsTdmaOn) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			(
+				"[BTCoex], ********** TDMA(on, %d) **********\n",
+				pCoexDm->curPsTdma
+			)
+		);
+	} else {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			(
+				"[BTCoex], ********** TDMA(off, %d) **********\n",
+				pCoexDm->curPsTdma
+			)
+		);
+	}
+
+	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 == true)
+			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(PBTC_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 &&
+		BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi connected + BT non connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
+	) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else if (
+		!bWifiConnected &&
+		(BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
+	) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], Wifi non connected-idle + BT Busy!!\n")
+		);
+
+		/* halbtc8723b1ant_SwMechanism(pBtCoexist, false); */
+
+		bCommon = true;
+	} else {
+		if (bWifiBusy) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], Wifi Connected-Busy + BT Busy!!\n")
+			);
+		} else {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], Wifi Connected-Idle + BT Busy!!\n")
+			);
+		}
+
+		bCommon = false;
+	}
+
+	return bCommon;
+}
+
+
+static void halbtc8723b1ant_TdmaDurationAdjustForAcl(
+	PBTC_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;
+	bool bWifiBusy = false;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		("[BTCoex], TdmaDurationAdjustForAcl()\n")
+	);
+
+	if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY == wifiStatus)
+		bWifiBusy = true;
+	else
+		bWifiBusy = false;
+
+	if (
+		(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 == wifiStatus)
+	) {
+		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;
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			("[BTCoex], first run TdmaDurationAdjust()!!\n")
+		);
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+		pCoexDm->psTdmaDuAdjType = 2;
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		btInfoExt = pCoexSta->btInfoExt;
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount)); */
+		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up =%d, dn =%d, m =%d, n =%d, WaitCount =%d\n", */
+		/* 	up, dn, m, n, WaitCount)); */
+
+		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;
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE_FW_DETAIL,
+					("[BTCoex], Increase wifi duration!!\n")
+				);
+			}
+		} 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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		} 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;
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				("[BTCoex], Decrease wifi duration for retryCounter>3!!\n")
+			);
+		}
+
+		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;
+			}
+		} else {	  /* no change */
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE_FW_DETAIL,
+				(
+					"[BTCoex], ********** TDMA(on, %d) **********\n",
+					pCoexDm->curPsTdma
+				)
+			);
+		}
+
+		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(
+	PBTC_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(
+	PBTC_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(PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	halbtc8723b1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+	halbtc8723b1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
+}
+
+static void halbtc8723b1ant_ActionBtInquiry(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, u8 wifiStatus
+)
+{
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, u8 wifiStatus
+)
+{
+	u8 btRssiState;
+
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	btRssiState = 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 (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE == wifiStatus) {
+			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(PBTC_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(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+		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 (
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		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(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+
+	halbtc8723b1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+
+	/*  tdma and coex table */
+	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+		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 (
+		(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+		(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+	) {
+		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(
+	PBTC_COEXIST pBtCoexist
+)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	bool bWifiBusy = false;
+	bool bScan = false, bLink = false, bRoam = false;
+	bool bUnder4way = false, bApEnable = false;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE,
+		("[BTCoex], CoexForWifiConnect() ===>\n")
+	);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
+	if (bUnder4way) {
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n")
+		);
+		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);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE,
+			("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n")
+		);
+		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 &&
+		BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus &&
+		!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 == false) &&
+			(pCoexSta->bA2dpExist == false) &&
+			(pCoexSta->bHidExist == false)
+		)
+			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 (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE
+			);
+		} else if (
+			(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+		) {
+			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 (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) {
+			halbtc8723b1ant_ActionWifiConnectedBtAclBusy(
+				pBtCoexist,
+				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY
+			);
+		} else if (
+			(BT_8723B_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
+			(BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus)
+		) {
+			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(PBTC_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:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
+			/* halbtc8723b1ant_ActionSco(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
+			/* halbtc8723b1ant_ActionHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
+			/* halbtc8723b1ant_ActionA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
+			/* halbtc8723b1ant_ActionA2dpPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
+			/* halbtc8723b1ant_ActionPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
+			/* halbtc8723b1ant_ActionPanHs(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
+			/* halbtc8723b1ant_ActionPanEdrA2dp(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
+			/* halbtc8723b1ant_ActionPanEdrHid(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
+			/* halbtc8723b1ant_ActionHidA2dpPanEdr(pBtCoexist); */
+			break;
+		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
+			/* halbtc8723b1ant_ActionHidA2dp(pBtCoexist); */
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b1ant_RunCoexistMechanism(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism() ===>\n"));
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if (pBtCoexist->bStopCoexDm) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
+		return;
+	}
+
+	if (pCoexSta->bUnderIps) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		return;
+	}
+
+	if (
+		(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 == pCoexDm->btStatus)
+	){
+		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)) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			(
+				"############# [BTCoex],  Multi-Port numOfWifiLink = %d, wifiLinkStatus = 0x%x\n",
+				numOfWifiLink,
+				wifiLinkStatus
+			)
+		);
+		halbtc8723b1ant_LimitedTx(pBtCoexist, NORMAL_EXEC, 0, 0, 0, 0);
+		halbtc8723b1ant_LimitedRx(pBtCoexist, NORMAL_EXEC, false, bBtCtrlAggBufSize, aggBufSize);
+
+		if ((pBtLinkInfo->bA2dpExist) && (pCoexSta->bC2hBtInquiryPage)) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("############# [BTCoex],  BT Is Inquirying\n")
+			);
+			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) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			("############# [BTCoex],  BT Is Inquirying\n")
+		);
+		halbtc8723b1ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else if (bBtHsOn) {
+		halbtc8723b1ant_ActionHs(pBtCoexist);
+		return;
+	}
+
+
+	if (!bWifiConnected) {
+		bool bScan = false, bLink = false, bRoam = false;
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
+
+		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(PBTC_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(
+	PBTC_COEXIST pBtCoexist,
+	bool bBackUp,
+	bool bWifiOnly
+)
+{
+	u32 u4Tmp = 0;/*  fwVer; */
+	u8 u1Tmpa = 0, u1Tmpb = 0;
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_INIT,
+		("[BTCoex], 1Ant Init HW Config!!\n")
+	);
+
+	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);
+
+	u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmpa = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+	u1Tmpb = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_NOTIFY,
+		(
+			"############# [BTCoex], 0x948 = 0x%x, 0x765 = 0x%x, 0x67 = 0x%x\n",
+			u4Tmp,
+			u1Tmpa,
+			u1Tmpb
+		)
+	);
+}
+
+/*  */
+/*  work around function start with wa_halbtc8723b1ant_ */
+/*  */
+/*  */
+/*  extern function start with EXhalbtc8723b1ant_ */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b1ant_InitHwConfig(pBtCoexist, true, bWifiOnly);
+}
+
+void EXhalbtc8723b1ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_INIT,
+		("[BTCoex], Coex Mechanism Init!!\n")
+	);
+
+	pBtCoexist->bStopCoexDm = false;
+
+	halbtc8723b1ant_InitCoexDm(pBtCoexist);
+
+	halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	PBTC_STACK_INFO pStackInfo = &pBtCoexist->stackInfo;
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	u8 *cliBuf = pBtCoexist->cliBuf;
+	u8 u1Tmp[4], i, btInfoExt, psTdmaCase = 0;
+	u16 u2Tmp[4];
+	u32 u4Tmp[4];
+	bool bRoam = false;
+	bool bScan = false;
+	bool bLink = false;
+	bool bWifiUnder5G = false;
+	bool bWifiUnderBMode = false;
+	bool bBtHsOn = false;
+	bool bWifiBusy = false;
+	s32 wifiRssi = 0, btHsRssi = 0;
+	u32 wifiBw, wifiTrafficDir, faOfdm, faCck, wifiLinkStatus;
+	u8 wifiDot11Chnl, wifiHsChnl;
+	u32 fwVer = 0, btPatchVer = 0;
+	static u8 PopReportIn10s = 0;
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n ============[BT Coexist info]============"
+	);
+	CL_PRINTF(cliBuf);
+
+	if (pBtCoexist->bManualControl) {
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n ============[Under Manual Control]============"
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n =========================================="
+		);
+		CL_PRINTF(cliBuf);
+	}
+	if (pBtCoexist->bStopCoexDm) {
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n ============[Coex is STOPPED]============"
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n =========================================="
+		);
+		CL_PRINTF(cliBuf);
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d", "Ant PG Num/ Ant Mech/ Ant Pos:", \
+		pBoardInfo->pgAntNum,
+		pBoardInfo->btdmAntNum,
+		pBoardInfo->btdmAntPos
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		((pStackInfo->bProfileNotified) ? "Yes" : "No"),
+		pStackInfo->hciVersion
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
+		GLCoexVerDate8723b1Ant,
+		GLCoexVer8723b1Ant,
+		fwVer,
+		btPatchVer,
+		btPatchVer
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl,
+		wifiHsChnl,
+		bBtHsOn
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0],
+		pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
+		wifiRssi-100, btHsRssi-100
+	);
+	CL_PRINTF(cliBuf);
+
+	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);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %s", "Wifi bLink/ bRoam/ bScan/ bHi-Pri", \
+		bLink, bRoam, bScan, ((pCoexSta->bWiFiIsHighPriTask) ? "1" : "0")
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir
+	);
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_BL_WIFI_UNDER_B_MODE, &bWifiUnderBMode
+	);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %s/ %s/ AP =%d/ %s ", "Wifi status", \
+		(bWifiUnder5G ? "5G" : "2.4G"),
+		((bWifiUnderBMode) ? "11b" : ((BTC_WIFI_BW_LEGACY == wifiBw) ? "11bg" : (((BTC_WIFI_BW_HT40 == wifiBw) ? "HT40" : "HT20")))),
+		((!bWifiBusy) ? "idle" : ((BTC_WIFI_TRAFFIC_TX == wifiTrafficDir) ? "uplink" : "downlink")),
+		pCoexSta->nScanAPNum,
+		(pCoexSta->bCCKLock) ? "Lock" : "noLock"
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(
+		pBtCoexist, BTC_GET_U4_WIFI_LINK_STATUS, &wifiLinkStatus
+	);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d/ %d", "sta/vwifi/hs/p2pGo/p2pGc", \
+		((wifiLinkStatus&WIFI_STA_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_AP_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_HS_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_P2P_GO_CONNECTED) ? 1 : 0),
+		((wifiLinkStatus&WIFI_P2P_GC_CONNECTED) ? 1 : 0)
+	);
+	CL_PRINTF(cliBuf);
+
+
+	PopReportIn10s++;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = [%s/ %d/ %d/ %d] ", "BT [status/ rssi/ retryCnt/ popCnt]", \
+		((pBtCoexist->btInfo.bBtDisabled) ? ("disabled") : ((pCoexSta->bC2hBtInquiryPage) ? ("inquiry/page scan") : ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) ? "non-connected idle" :
+		((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) ? "connected-idle" : "busy")))),
+		pCoexSta->btRssi, pCoexSta->btRetryCnt, pCoexSta->popEventCnt
+	);
+	CL_PRINTF(cliBuf);
+
+	if (PopReportIn10s >= 5) {
+		pCoexSta->popEventCnt = 0;
+		PopReportIn10s = 0;
+	}
+
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+		pBtLinkInfo->bScoExist,
+		pBtLinkInfo->bHidExist,
+		pBtLinkInfo->bPanExist,
+		pBtLinkInfo->bA2dpExist
+	);
+	CL_PRINTF(cliBuf);
+
+	if (pStackInfo->bProfileNotified) {
+		pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+	} else {
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Role", \
+		(pBtLinkInfo->bSlaveRole) ? "Slave" : "Master");
+		CL_PRINTF(cliBuf);
+	}
+
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0) ? "Basic rate" : "EDR rate"
+	);
+	CL_PRINTF(cliBuf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
+		if (pCoexSta->btInfoC2hCnt[i]) {
+			CL_SPRINTF(
+				cliBuf,
+				BT_TMP_BUF_SIZE,
+				"\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723b1Ant[i], \
+				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]
+			);
+			CL_PRINTF(cliBuf);
+		}
+	}
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
+		(pCoexSta->bUnderIps ? "IPS ON" : "IPS OFF"),
+		(pCoexSta->bUnderLps ? "LPS ON" : "LPS OFF"),
+		pBtCoexist->btInfo.lpsVal,
+		pBtCoexist->btInfo.rpwmVal
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	if (!pBtCoexist->bManualControl) {
+		/*  Sw mechanism */
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s", "============[Sw mechanism]============"
+		);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = %d", "SM[LowPenaltyRA]", \
+			pCoexDm->bCurLowPenaltyRa
+		);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize", \
+			(pBtCoexist->btInfo.bRejectAggPkt ? "Yes" : "No"),
+			(pBtCoexist->btInfo.bBtCtrlAggBufSize ? "Yes" : "No"),
+			pBtCoexist->btInfo.aggBufSize
+		);
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(
+			cliBuf,
+			BT_TMP_BUF_SIZE,
+			"\r\n %-35s = 0x%x ", "Rate Mask", \
+			pBtCoexist->btInfo.raMask
+		);
+		CL_PRINTF(cliBuf);
+
+		/*  Fw mechanism */
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+		CL_PRINTF(cliBuf);
+
+		psTdmaCase = pCoexDm->curPsTdma;
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
+			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
+			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
+			pCoexDm->psTdmaPara[4], psTdmaCase, pCoexDm->bAutoTdmaAdjust);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "Coex Table Type", \
+			pCoexSta->nCoexTableType);
+		CL_PRINTF(cliBuf);
+
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "IgnWlanAct", \
+			pCoexDm->bCurIgnoreWlanAct);
+		CL_PRINTF(cliBuf);
+
+		/*
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
+			pCoexDm->errorCondition);
+		CL_PRINTF(cliBuf);
+		*/
+	}
+
+	/*  Hw setting */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x", "backup ARFR1/ARFR2/RL/AMaxTime", \
+		pCoexDm->backupArfrCnt1, pCoexDm->backupArfrCnt2, pCoexDm->backupRetryLimit, pCoexDm->backupAmpduMaxTime);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x430);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x434);
+	u2Tmp[0] = pBtCoexist->fBtcRead2Byte(pBtCoexist, 0x42a);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x456);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x", "0x430/0x434/0x42a/0x456", \
+		u4Tmp[0], u4Tmp[1], u2Tmp[0], u1Tmp[0]);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x778);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6cc);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x880);
+	CL_SPRINTF(
+		cliBuf, BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/0x6cc/0x880[29:25]", \
+		u1Tmp[0], u4Tmp[0],  (u4Tmp[1]&0x3e000000) >> 25
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x764);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x76e);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x948/ 0x67[5] / 0x764 / 0x76e", \
+		u4Tmp[0], ((u1Tmp[0]&0x20) >> 5), (u4Tmp[1] & 0xffff), u1Tmp[1]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x92c);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x930);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x944);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]", \
+		u4Tmp[0]&0x3, u4Tmp[1]&0xff, u4Tmp[2]&0x3
+	);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x39);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x40);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+	u1Tmp[2] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x64);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x38[11]/0x40/0x4c[24:23]/0x64[0]", \
+		((u1Tmp[0] & 0x8)>>3),
+		u1Tmp[1],
+		((u4Tmp[0]&0x01800000)>>23),
+		u1Tmp[2]&0x1
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x522);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0], u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xc50);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49c);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0xc50(dig)/0x49c(null-drop)", \
+		u4Tmp[0]&0xff, u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xcf0);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5b);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5c);
+
+	faOfdm =
+		((u4Tmp[0]&0xffff0000) >> 16) +
+		((u4Tmp[1]&0xffff0000) >> 16) +
+		(u4Tmp[1] & 0xffff) +  (u4Tmp[2] & 0xffff) + \
+		((u4Tmp[3]&0xffff0000) >> 16) + (u4Tmp[3] & 0xffff);
+	faCck = (u1Tmp[0] << 8) + u1Tmp[1];
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "OFDM-CCA/OFDM-FA/CCK-FA", \
+		u4Tmp[0]&0xffff, faOfdm, faCck
+	);
+	CL_PRINTF(cliBuf);
+
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d", "CRC_OK CCK/11g/11n/11n-Agg", \
+		pCoexSta->nCRCOK_CCK,
+		pCoexSta->nCRCOK_11g,
+		pCoexSta->nCRCOK_11n,
+		pCoexSta->nCRCOK_11nAgg
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d/ %d", "CRC_Err CCK/11g/11n/11n-Agg", \
+		pCoexSta->nCRCErr_CCK,
+		pCoexSta->nCRCErr_11g,
+		pCoexSta->nCRCErr_11n,
+		pCoexSta->nCRCErr_11nAgg
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c8);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8(coexTable)", \
+		u4Tmp[0], u4Tmp[1], u4Tmp[2]);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x770(high-pri rx/tx)", \
+		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)", \
+		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void EXhalbtc8723b1ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl ||	pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n")
+		);
+		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 (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n")
+		);
+		pCoexSta->bUnderIps = false;
+
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n")
+		);
+		pCoexSta->bUnderLps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n")
+		);
+		pCoexSta->bUnderLps = false;
+	}
+}
+
+void EXhalbtc8723b1ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	bool bWifiConnected = false, bBtHsOn = false;
+	u32 wifiLinkStatus = 0;
+	u32 numOfWifiLink = 0;
+	bool bBtCtrlAggBufSize = false;
+	u8 aggBufSize = 5;
+
+	u8 u1Tmpa, u1Tmpb;
+	u32 u4Tmp;
+
+	if (pBtCoexist->bManualControl || pBtCoexist->bStopCoexDm)
+		return;
+
+	if (BTC_SCAN_START == type) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n")
+		);
+
+		halbtc8723b1ant_PsTdma(pBtCoexist, FORCE_EXEC, false, 8);  /* Force antenna setup for no scan result issue */
+		u4Tmp = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+		u1Tmpa = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+		u1Tmpb = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+
+
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			(
+				"[BTCoex], 0x948 = 0x%x, 0x765 = 0x%x, 0x67 = 0x%x\n",
+				u4Tmp,
+				u1Tmpa,
+				u1Tmpb
+			)
+		);
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n")
+		);
+
+		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 (BTC_SCAN_START == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n")); */
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnectedScan(pBtCoexist);
+		else	/*  wifi is connected */
+			halbtc8723b1ant_ActionWifiConnectedScan(pBtCoexist);
+	} else if (BTC_SCAN_FINISH == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n")); */
+		if (!bWifiConnected)	/*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_ConnectNotify(PBTC_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 (BTC_ASSOCIATE_START == type) {
+		pCoexSta->bWiFiIsHighPriTask = true;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+		 pCoexDm->nArpCnt = 0;
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+		/* 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 (BTC_ASSOCIATE_START == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n")); */
+		halbtc8723b1ant_ActionWifiNotConnectedAssoAuth(pBtCoexist);
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		/* BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n")); */
+
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
+		if (!bWifiConnected) /*  non-connected scan */
+			halbtc8723b1ant_ActionWifiNotConnected(pBtCoexist);
+		else
+			halbtc8723b1ant_ActionWifiConnected(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_MediaStatusNotify(PBTC_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 (BTC_MEDIA_CONNECT == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+
+		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 {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+		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 ((BTC_MEDIA_CONNECT == type) && (wifiCentralChnl <= 14)) {
+		/* H2C_Parameter[0] = 0x1; */
+		H2C_Parameter[0] = 0x0;
+		H2C_Parameter[1] = wifiCentralChnl;
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+
+		if (BTC_WIFI_BW_HT40 == wifiBw)
+			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];
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x66 = 0x%x\n",
+			H2C_Parameter[0]<<16 | H2C_Parameter[1]<<8 | H2C_Parameter[2]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b1ant_SpecialPacketNotify(PBTC_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 (
+		BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type ||
+		BTC_PACKET_ARP == type
+	) {
+		if (BTC_PACKET_ARP == type) {
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], special Packet ARP notify\n")
+			);
+
+			pCoexDm->nArpCnt++;
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], ARP Packet Count = %d\n", 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;
+			BTC_PRINT(
+				BTC_MSG_INTERFACE,
+				INTF_NOTIFY,
+				("[BTCoex], special Packet DHCP or EAPOL notify\n")
+			);
+		}
+	} else {
+		pCoexSta->bWiFiIsHighPriTask = false;
+		BTC_PRINT(
+			BTC_MSG_INTERFACE,
+			INTF_NOTIFY,
+			("[BTCoex], special Packet [Type = %d] notify\n", type)
+		);
+	}
+
+	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 (
+		BTC_PACKET_DHCP == type ||
+		BTC_PACKET_EAPOL == type ||
+		((BTC_PACKET_ARP == type) && (pCoexSta->bWiFiIsHighPriTask))
+	)
+		halbtc8723b1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_BtInfoNotify(
+	PBTC_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]++;
+
+	BTC_PRINT(
+		BTC_MSG_INTERFACE,
+		INTF_NOTIFY,
+		("[BTCoex], Bt info[%d], length =%d, hex data =[",
+		rspSource,
+		length)
+	);
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+		if (i == length-1)
+			BTC_PRINT(
+				BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i])
+			);
+		else
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+	}
+
+	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rspSource) {
+		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 */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x15\n"));
+			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) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n")
+			);
+			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) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n")
+				);
+				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;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n"));
+	} else if (btInfo == BT_INFO_8723B_1ANT_B_CONNECTION)	{
+		/*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n"));
+	} 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;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT SCO busy!!!\n"));
+	} else if (btInfo&BT_INFO_8723B_1ANT_B_ACL_BUSY) {
+		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY != pCoexDm->btStatus)
+			pCoexDm->bAutoTdmaAdjust = false;
+
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT ACL busy!!!\n"));
+	} else {
+		pCoexDm->btStatus = BT_8723B_1ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n"));
+	}
+
+	if (
+		(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 == pCoexDm->btStatus)
+	)
+		bBtBusy = true;
+	else
+		bBtBusy = false;
+	pBtCoexist->fBtcSet(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
+
+	halbtc8723b1ant_RunCoexistMechanism(pBtCoexist);
+}
+
+void EXhalbtc8723b1ant_HaltNotify(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	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(PBTC_COEXIST pBtCoexist, u8 pnpState)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if (BTC_WIFI_PNP_SLEEP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to SLEEP\n"));
+
+		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 (BTC_WIFI_PNP_WAKE_UP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to WAKE UP\n"));
+		pBtCoexist->bStopCoexDm = false;
+		halbtc8723b1ant_InitHwConfig(pBtCoexist, false, false);
+		halbtc8723b1ant_InitCoexDm(pBtCoexist);
+		halbtc8723b1ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b1ant_Periodical(PBTC_COEXIST pBtCoexist)
+{
+	static u8 disVerInfoCnt = 0;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			GLCoexVerDate8723b1Ant, GLCoexVer8723b1Ant, fwVer, btPatchVer, btPatchVer));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+	}
+
+	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 000000000000..880bd63d36a5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b1Ant.h
@@ -0,0 +1,193 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*  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 */
+
+typedef enum _BT_INFO_SRC_8723B_1ANT {
+	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
+} BT_INFO_SRC_8723B_1ANT, *PBT_INFO_SRC_8723B_1ANT;
+
+typedef enum _BT_8723B_1ANT_BT_STATUS {
+	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
+} BT_8723B_1ANT_BT_STATUS, *PBT_8723B_1ANT_BT_STATUS;
+
+typedef enum _BT_8723B_1ANT_WIFI_STATUS {
+	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
+} BT_8723B_1ANT_WIFI_STATUS, *PBT_8723B_1ANT_WIFI_STATUS;
+
+typedef enum _BT_8723B_1ANT_COEX_ALGO {
+	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,
+} BT_8723B_1ANT_COEX_ALGO, *PBT_8723B_1ANT_COEX_ALGO;
+
+typedef 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;
+} COEX_DM_8723B_1ANT, *PCOEX_DM_8723B_1ANT;
+
+typedef 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;
+} COEX_STA_8723B_1ANT, *PCOEX_STA_8723B_1ANT;
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b1ant_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b1ant_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b1ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b1ant_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtc8723b1ant_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b1ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist);
diff --git a/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
new file mode 100644
index 000000000000..f5bc511d02f2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.c
@@ -0,0 +1,3729 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "Mp_Precomp.h"
+
+/*  Global variables, these are static variables */
+static COEX_DM_8723B_2ANT GLCoexDm8723b2Ant;
+static PCOEX_DM_8723B_2ANT pCoexDm = &GLCoexDm8723b2Ant;
+static COEX_STA_8723B_2ANT GLCoexSta8723b2Ant;
+static PCOEX_STA_8723B_2ANT pCoexSta = &GLCoexSta8723b2Ant;
+
+static const char *const GLBtInfoSrc8723b2Ant[] = {
+	"BT Info[wifi fw]",
+	"BT Info[bt rsp]",
+	"BT Info[bt auto report]",
+};
+
+static u32 GLCoexVerDate8723b2Ant = 20131211;
+static u32 GLCoexVer8723b2Ant = 0x40;
+
+/*  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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		} else {
+			if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
+			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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
+			}
+		} 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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
+			} else if (btRssi < rssiThresh) {
+				btRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
+			}
+		} else {
+			if (btRssi < rssiThresh1) {
+				btRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
+			} else {
+				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
+			}
+		}
+	}
+
+	pCoexSta->preBtRssiState = btRssiState;
+
+	return btRssiState;
+}
+
+static u8 halbtc8723b2ant_WifiRssiState(
+	PBTC_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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		} else {
+			if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	} else if (levelNum == 3) {
+		if (rssiThresh > rssiThresh1) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
+			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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
+			}
+		} 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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
+			} else if (wifiRssi < rssiThresh) {
+				wifiRssiState = BTC_RSSI_STATE_LOW;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
+			}
+		} else {
+			if (wifiRssi < rssiThresh1) {
+				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
+			} else {
+				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
+			}
+		}
+	}
+
+	pCoexSta->preWifiRssiState[index] = wifiRssiState;
+
+	return wifiRssiState;
+}
+
+static void halbtc8723b2ant_LimitedRx(
+	PBTC_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_MonitorBtCtr(PBTC_COEXIST pBtCoexist)
+{
+	u32 regHPTxRx, regLPTxRx, u4Tmp;
+	u32 regHPTx = 0, regHPRx = 0, regLPTx = 0, regLPRx = 0;
+
+	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;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		(
+			"[BTCoex], High Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+			regHPTxRx,
+			regHPTx,
+			regHPTx,
+			regHPRx,
+			regHPRx
+		)
+	);
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_BT_MONITOR,
+		(
+			"[BTCoex], Low Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
+			regLPTxRx,
+			regLPTx,
+			regLPTx,
+			regLPRx,
+			regLPRx
+		)
+	);
+
+	/*  reset counter */
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x76e, 0xc);
+}
+
+static void halbtc8723b2ant_QueryBtInfo(PBTC_COEXIST pBtCoexist)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	pCoexSta->bC2hBtInfoReqSent = true;
+
+	H2C_Parameter[0] |= BIT0;	/*  trigger */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x61, 1, H2C_Parameter);
+}
+
+static bool halbtc8723b2ant_IsWifiStatusChanged(PBTC_COEXIST pBtCoexist)
+{
+	static bool	bPreWifiBusy = false, bPreUnder4way = false, bPreBtHsOn = false;
+	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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No BT link exists!!!\n"));
+		return algorithm;
+	}
+
+	if (pBtLinkInfo->bScoExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bHidExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bPanExist)
+		numOfDiffProfile++;
+
+	if (pBtLinkInfo->bA2dpExist)
+		numOfDiffProfile++;
+
+	if (numOfDiffProfile == 1) {
+		if (pBtLinkInfo->bScoExist) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
+			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+		} else {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 2) {
+		if (pBtLinkInfo->bScoExist) {
+			if (pBtLinkInfo->bHidExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bA2dpExist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (pBtLinkInfo->bPanExist) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
+				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
+				}
+			}
+		}
+	} else if (numOfDiffProfile == 3) {
+		if (pBtLinkInfo->bScoExist) {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				BTC_PRINT(
+					BTC_MSG_ALGORITHM,
+					ALGO_TRACE,
+					("[BTCoex], SCO + HID + A2DP ==> HID\n")
+				);
+				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+			} else if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], SCO + HID + PAN(HS)\n")
+					);
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(
+						BTC_MSG_ALGORITHM,
+						ALGO_TRACE,
+						("[BTCoex], SCO + HID + PAN(EDR)\n")
+					);
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			} else if (
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		} else {
+			if (
+				pBtLinkInfo->bHidExist &&
+				pBtLinkInfo->bPanExist &&
+				pBtLinkInfo->bA2dpExist
+			) {
+				if (bBtHsOn) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
+					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) {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
+
+				} else {
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n"));
+					algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
+				}
+			}
+		}
+	}
+
+	return algorithm;
+}
+
+static void halbtc8723b2ant_SetFwDacSwingLevel(
+	PBTC_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;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], Set Dac Swing Level = 0x%x\n", dacSwingLvl)
+	);
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		("[BTCoex], FW write 0x64 = 0x%x\n", H2C_Parameter[0])
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x64, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SetFwDecBtPwr(
+	PBTC_COEXIST pBtCoexist, u8 decBtPwrLvl
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	H2C_Parameter[0] = decBtPwrLvl;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], decrease Bt Power level = %d, FW write 0x62 = 0x%x\n",
+			decBtPwrLvl,
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x62, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_DecBtPwr(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 decBtPwrLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s Dec BT power level = %d\n",
+			(bForceExec ? "force to" : ""),
+			decBtPwrLvl
+		)
+	);
+	pCoexDm->curBtDecPwrLvl = decBtPwrLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], preBtDecPwrLvl =%d, curBtDecPwrLvl =%d\n",
+				pCoexDm->preBtDecPwrLvl,
+				pCoexDm->curBtDecPwrLvl
+			)
+		);
+
+		if (pCoexDm->preBtDecPwrLvl == pCoexDm->curBtDecPwrLvl)
+			return;
+	}
+	halbtc8723b2ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->curBtDecPwrLvl);
+
+	pCoexDm->preBtDecPwrLvl = pCoexDm->curBtDecPwrLvl;
+}
+
+static void halbtc8723b2ant_FwDacSwingLvl(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, u8 fwDacSwingLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s set FW Dac Swing level = %d\n",
+			(bForceExec ? "force to" : ""),
+			fwDacSwingLvl
+		)
+	);
+	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
+				pCoexDm->preFwDacSwingLvl,
+				pCoexDm->curFwDacSwingLvl
+			)
+		);
+
+		if (pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl)
+			return;
+	}
+
+	halbtc8723b2ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
+
+	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
+}
+
+static void halbtc8723b2ant_SetSwRfRxLpfCorner(
+	PBTC_COEXIST pBtCoexist,
+	bool bRxRfShrinkOn
+)
+{
+	if (bRxRfShrinkOn) {
+		/* Shrink RF Rx LPF corner */
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_EXEC,
+			("[BTCoex], Shrink RF Rx LPF corner!!\n")
+		);
+		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) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
+			pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
+		}
+	}
+}
+
+static void halbtc8723b2ant_RfShrink(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bRxRfShrinkOn
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn Rx RF Shrink = %s\n",
+			(bForceExec ? "force to" : ""),
+			(bRxRfShrinkOn ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
+				pCoexDm->bPreRfRxLpfShrink,
+				pCoexDm->bCurRfRxLpfShrink
+			)
+		);
+
+		if (pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink)
+			return;
+	}
+	halbtc8723b2ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
+
+	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
+}
+
+static void halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(
+	PBTC_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 */
+	}
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set WiFi Low-Penalty Retry: %s",
+			(bLowPenaltyRa ? "ON!!" : "OFF!!")
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x69, 6, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_LowPenaltyRa(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bLowPenaltyRa
+)
+{
+	/* return; */
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn LowPenaltyRA = %s\n",
+			(bForceExec ? "force to" : ""),
+			(bLowPenaltyRa ? "ON" : "OFF")
+		)
+	);
+	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreLowPenaltyRa =%d, bCurLowPenaltyRa =%d\n",
+				pCoexDm->bPreLowPenaltyRa,
+				pCoexDm->bCurLowPenaltyRa
+			)
+		);
+
+		if (pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa)
+			return;
+	}
+	halbtc8723b2ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
+
+	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
+}
+
+static void halbtc8723b2ant_SetDacSwingReg(PBTC_COEXIST pBtCoexist, u32 level)
+{
+	u8 val = (u8)level;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], Write SwDacSwing = 0x%x\n", level)
+	);
+	pBtCoexist->fBtcWrite1ByteBitMask(pBtCoexist, 0x883, 0x3e, val);
+}
+
+static void halbtc8723b2ant_SetSwFullTimeDacSwing(
+	PBTC_COEXIST pBtCoexist, bool bSwDacSwingOn, u32 swDacSwingLvl
+)
+{
+	if (bSwDacSwingOn)
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, swDacSwingLvl);
+	else
+		halbtc8723b2ant_SetDacSwingReg(pBtCoexist, 0x18);
+}
+
+
+static void halbtc8723b2ant_DacSwing(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	bool bDacSwingOn,
+	u32 dacSwingLvl
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s turn DacSwing =%s, dacSwingLvl = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			(bDacSwingOn ? "ON" : "OFF"),
+			dacSwingLvl
+		)
+	);
+	pCoexDm->bCurDacSwingOn = bDacSwingOn;
+	pCoexDm->curDacSwingLvl = dacSwingLvl;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
+				pCoexDm->bPreDacSwingOn,
+				pCoexDm->preDacSwingLvl,
+				pCoexDm->bCurDacSwingOn,
+				pCoexDm->curDacSwingLvl
+			)
+		);
+
+		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(
+	PBTC_COEXIST pBtCoexist, bool bAgcTableEn
+)
+{
+	u8 rssiAdjustVal = 0;
+
+	/* BB AGC Gain Table */
+	if (bAgcTableEn) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB Agc Table On!\n"));
+		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 {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB Agc Table Off!\n"));
+		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) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		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) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38fff);
+		pBtCoexist->fBtcSetRfReg(pBtCoexist, BTC_RF_A, 0x40, 0xfffff, 0x38ffe);
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
+		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(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bAgcTableEn
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s %s Agc Table\n",
+			(bForceExec ? "force to" : ""),
+			(bAgcTableEn ? "Enable" : "Disable")
+		)
+	);
+	pCoexDm->bCurAgcTableEn = bAgcTableEn;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
+				pCoexDm->bPreAgcTableEn,
+				pCoexDm->bCurAgcTableEn
+			)
+		);
+
+		if (pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn)
+			return;
+	}
+	halbtc8723b2ant_SetAgcTable(pBtCoexist, bAgcTableEn);
+
+	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
+}
+
+static void halbtc8723b2ant_SetCoexTable(
+	PBTC_COEXIST pBtCoexist,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c0, val0x6c0);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c4, val0x6c4);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8)
+	);
+	pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x6c8, val0x6c8);
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW_EXEC,
+		("[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc)
+	);
+	pBtCoexist->fBtcWrite1Byte(pBtCoexist, 0x6cc, val0x6cc);
+}
+
+static void halbtc8723b2ant_CoexTable(
+	PBTC_COEXIST pBtCoexist,
+	bool bForceExec,
+	u32 val0x6c0,
+	u32 val0x6c4,
+	u32 val0x6c8,
+	u8 val0x6cc
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_SW,
+		(
+			"[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
+			(bForceExec ? "force to" : ""),
+			val0x6c0,
+			val0x6c4,
+			val0x6c8,
+			val0x6cc
+		)
+	);
+	pCoexDm->curVal0x6c0 = val0x6c0;
+	pCoexDm->curVal0x6c4 = val0x6c4;
+	pCoexDm->curVal0x6c8 = val0x6c8;
+	pCoexDm->curVal0x6cc = val0x6cc;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x !!\n",
+				pCoexDm->preVal0x6c0,
+				pCoexDm->preVal0x6c4,
+				pCoexDm->preVal0x6c8,
+				pCoexDm->preVal0x6cc
+			)
+		);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_SW_DETAIL,
+			(
+				"[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x, curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
+				pCoexDm->curVal0x6c0,
+				pCoexDm->curVal0x6c4,
+				pCoexDm->curVal0x6c8,
+				pCoexDm->curVal0x6cc
+			)
+		);
+
+		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(
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, bool bEnable
+)
+{
+	u8 	H2C_Parameter[1] = {0};
+
+	if (bEnable)
+		H2C_Parameter[0] |= BIT0;		/*  function enable */
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
+			H2C_Parameter[0]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x63, 1, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_IgnoreWlanAct(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bEnable
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn Ignore WlanAct %s\n",
+			(bForceExec ? "force to" : ""),
+			(bEnable ? "ON" : "OFF")
+		)
+	);
+
+	pCoexDm->bCurIgnoreWlanAct = bEnable;
+
+	if (!bForceExec) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
+			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
+
+		if (pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
+			return;
+	}
+	halbtc8723b2ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
+
+	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
+}
+
+static void halbtc8723b2ant_SetFwPstdma(
+	PBTC_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;
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
+			H2C_Parameter[0],
+			H2C_Parameter[1]<<24|
+			H2C_Parameter[2]<<16|
+			H2C_Parameter[3]<<8|
+			H2C_Parameter[4]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x60, 5, H2C_Parameter);
+}
+
+static void halbtc8723b2ant_SwMechanism1(
+	PBTC_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(
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, u8 antPosType, bool bInitHwCfg, bool bWifiOff
+)
+{
+	PBTC_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(
+	PBTC_COEXIST pBtCoexist, bool bForceExec, bool bTurnOn, u8 type
+)
+{
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW,
+		(
+			"[BTCoex], %s turn %s PS TDMA, type =%d\n",
+			(bForceExec ? "force to" : ""),
+			(bTurnOn ? "ON" : "OFF"),
+			type
+		)
+	);
+	pCoexDm->bCurPsTdmaOn = bTurnOn;
+	pCoexDm->curPsTdma = type;
+
+	if (!bForceExec) {
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
+				pCoexDm->bPrePsTdmaOn,
+				pCoexDm->bCurPsTdmaOn
+			)
+		);
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
+				pCoexDm->prePsTdma, pCoexDm->curPsTdma
+			)
+		);
+
+		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(PBTC_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(PBTC_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(PBTC_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(PBTC_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);
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-connected idle!!\n"));
+
+		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);
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
+
+			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;
+
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
+			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) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi Connected-Busy + BT Busy!!\n"));
+				bCommon = false;
+			} else {
+				if (bBtHsOn)
+					return false;
+
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi Connected-Idle + BT Busy!!\n"));
+				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(
+	PBTC_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;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjust()\n"));
+
+	if (!pCoexDm->bAutoTdmaAdjust) {
+		pCoexDm->bAutoTdmaAdjust = true;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
+		{
+			if (bScoHid) {
+				if (bTxPause) {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				} else {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					}
+				}
+			} else {
+				if (bTxPause) {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					}
+				} else {
+					if (maxInterval == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (maxInterval == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (maxInterval == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					}
+				}
+			}
+		}
+		/*  */
+		up = 0;
+		dn = 0;
+		m = 1;
+		n = 3;
+		result = 0;
+		WaitCount = 0;
+	} else {
+		/* accquire the BT TRx retry count from BT_Info byte2 */
+		retryCount = pCoexSta->btRetryCnt;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], up =%d, dn =%d, m =%d, n =%d, WaitCount =%d\n",
+				up, dn, m, n, WaitCount
+			)
+		);
+		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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
+			}
+		} 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;
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
+			}
+		} 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;
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
+		}
+
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], max Interval = %d\n", maxInterval));
+		if (maxInterval == 1) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+
+				if (pCoexDm->curPsTdma == 71) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					pCoexDm->psTdmaDuAdjType = 5;
+				} else if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+					pCoexDm->psTdmaDuAdjType = 5;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+					pCoexDm->psTdmaDuAdjType = 13;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
+						pCoexDm->psTdmaDuAdjType = 5;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
+						pCoexDm->psTdmaDuAdjType = 13;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 71);
+					pCoexDm->psTdmaDuAdjType = 71;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+					pCoexDm->psTdmaDuAdjType = 9;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 71) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
+						pCoexDm->psTdmaDuAdjType = 1;
+					} else if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 71);
+						pCoexDm->psTdmaDuAdjType = 71;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
+						pCoexDm->psTdmaDuAdjType = 9;
+					}
+				}
+			}
+		} else if (maxInterval == 2) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+					pCoexDm->psTdmaDuAdjType = 6;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+					pCoexDm->psTdmaDuAdjType = 14;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
+						pCoexDm->psTdmaDuAdjType = 6;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
+						pCoexDm->psTdmaDuAdjType = 14;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+					pCoexDm->psTdmaDuAdjType = 2;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+					pCoexDm->psTdmaDuAdjType = 10;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
+						pCoexDm->psTdmaDuAdjType = 2;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
+						pCoexDm->psTdmaDuAdjType = 10;
+					}
+				}
+			}
+		} else if (maxInterval == 3) {
+			if (bTxPause) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
+				if (pCoexDm->curPsTdma == 1) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 2) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 3) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+					pCoexDm->psTdmaDuAdjType = 7;
+				} else if (pCoexDm->curPsTdma == 4) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+					pCoexDm->psTdmaDuAdjType = 8;
+				}
+
+				if (pCoexDm->curPsTdma == 9) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 10) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 11) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+					pCoexDm->psTdmaDuAdjType = 15;
+				} else if (pCoexDm->curPsTdma == 12) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+					pCoexDm->psTdmaDuAdjType = 16;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 5) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
+						pCoexDm->psTdmaDuAdjType = 8;
+					} else if (pCoexDm->curPsTdma == 13) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
+						pCoexDm->psTdmaDuAdjType = 16;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 8) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 7) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 6) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
+						pCoexDm->psTdmaDuAdjType = 7;
+					} else if (pCoexDm->curPsTdma == 16) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 15) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					} else if (pCoexDm->curPsTdma == 14) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
+						pCoexDm->psTdmaDuAdjType = 15;
+					}
+				}
+			} else {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
+				if (pCoexDm->curPsTdma == 5) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 6) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 7) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+					pCoexDm->psTdmaDuAdjType = 3;
+				} else if (pCoexDm->curPsTdma == 8) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+					pCoexDm->psTdmaDuAdjType = 4;
+				}
+
+				if (pCoexDm->curPsTdma == 13) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 14) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 15) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+					pCoexDm->psTdmaDuAdjType = 11;
+				} else if (pCoexDm->curPsTdma == 16) {
+					halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+					pCoexDm->psTdmaDuAdjType = 12;
+				}
+
+				if (result == -1) {
+					if (pCoexDm->curPsTdma == 1) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
+						pCoexDm->psTdmaDuAdjType = 4;
+					} else if (pCoexDm->curPsTdma == 9) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
+						pCoexDm->psTdmaDuAdjType = 12;
+					}
+				} else if (result == 1) {
+					if (pCoexDm->curPsTdma == 4) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 3) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 2) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
+						pCoexDm->psTdmaDuAdjType = 3;
+					} else if (pCoexDm->curPsTdma == 12) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 11) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 11;
+					} else if (pCoexDm->curPsTdma == 10) {
+						halbtc8723b2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
+						pCoexDm->psTdmaDuAdjType = 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;
+		BTC_PRINT(
+			BTC_MSG_ALGORITHM,
+			ALGO_TRACE_FW_DETAIL,
+			(
+				"[BTCoex], PsTdma type dismatch!!!, curPsTdma =%d, recordPsTdma =%d\n",
+				pCoexDm->curPsTdma,
+				pCoexDm->psTdmaDuAdjType
+			)
+		);
+
+		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);
+		else {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
+		}
+	}
+}
+
+/*  SCO only or SCO+PAN(HS) */
+static void halbtc8723b2ant_ActionSco(PBTC_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(PBTC_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(PBTC_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)) {
+		/* DbgPrint(" AP#>10(%d)\n", apNum); */
+		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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	u8 algorithm = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism() ===>\n"));
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
+		return;
+	}
+
+	if (pCoexSta->bUnderIps) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
+		return;
+	}
+
+	algorithm = halbtc8723b2ant_ActionAlgorithm(pBtCoexist);
+	if (pCoexSta->bC2hBtInquiryPage && (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT is under inquiry/page scan !!\n"));
+		halbtc8723b2ant_ActionBtInquiry(pBtCoexist);
+		return;
+	} else {
+		if (pCoexDm->bNeedRecover0x948) {
+			pCoexDm->bNeedRecover0x948 = false;
+			pBtCoexist->fBtcWrite4Byte(pBtCoexist, 0x948, pCoexDm->backup0x948);
+		}
+	}
+
+	pCoexDm->curAlgorithm = algorithm;
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d\n", pCoexDm->curAlgorithm));
+
+	if (halbtc8723b2ant_IsCommonAction(pBtCoexist)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant common.\n"));
+		pCoexDm->bAutoTdmaAdjust = false;
+	} else {
+		if (pCoexDm->curAlgorithm != pCoexDm->preAlgorithm) {
+			BTC_PRINT(
+				BTC_MSG_ALGORITHM,
+				ALGO_TRACE,
+				(
+					"[BTCoex], preAlgorithm =%d, curAlgorithm =%d\n",
+					pCoexDm->preAlgorithm,
+					pCoexDm->curAlgorithm
+				)
+			);
+			pCoexDm->bAutoTdmaAdjust = false;
+		}
+
+
+		switch (pCoexDm->curAlgorithm) {
+		case BT_8723B_2ANT_COEX_ALGO_SCO:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = SCO.\n"));
+			halbtc8723b2ant_ActionSco(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID.\n"));
+			halbtc8723b2ant_ActionHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP.\n"));
+			halbtc8723b2ant_ActionA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n"));
+			halbtc8723b2ant_ActionA2dpPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n"));
+			halbtc8723b2ant_ActionPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANHS:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HS mode.\n"));
+			halbtc8723b2ant_ActionPanHs(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n"));
+			halbtc8723b2ant_ActionPanEdrA2dp(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n"));
+			halbtc8723b2ant_ActionPanEdrHid(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n"));
+			halbtc8723b2ant_ActionHidA2dpPanEdr(pBtCoexist);
+			break;
+		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n"));
+			halbtc8723b2ant_ActionHidA2dp(pBtCoexist);
+			break;
+		default:
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n"));
+			halbtc8723b2ant_CoexAllOff(pBtCoexist);
+			break;
+		}
+		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
+	}
+}
+
+static void halbtc8723b2ant_WifiOffHwCfg(PBTC_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(PBTC_COEXIST pBtCoexist, bool bBackUp)
+{
+	u8 u1Tmp = 0;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 2Ant Init HW Config!!\n"));
+
+	/*  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(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_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(PBTC_COEXIST pBtCoexist, bool bWifiOnly)
+{
+	halbtc8723b2ant_InitHwConfig(pBtCoexist, true);
+}
+
+void EXhalbtc8723b2ant_InitCoexDm(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
+
+	halbtc8723b2ant_InitCoexDm(pBtCoexist);
+}
+
+void EXhalbtc8723b2ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	PBTC_BOARD_INFO pBoardInfo = &pBtCoexist->boardInfo;
+	PBTC_STACK_INFO pStackInfo = &pBtCoexist->stackInfo;
+	PBTC_BT_LINK_INFO pBtLinkInfo = &pBtCoexist->btLinkInfo;
+	u8 *cliBuf = pBtCoexist->cliBuf;
+	u8 u1Tmp[4], i, btInfoExt, psTdmaCase = 0;
+	u32 u4Tmp[4];
+	bool bRoam = false, bScan = false, bLink = false, bWifiUnder5G = false;
+	bool bBtHsOn = false, bWifiBusy = false;
+	s32 wifiRssi = 0, btHsRssi = 0;
+	u32 wifiBw, wifiTrafficDir, faOfdm, faCck;
+	u8 wifiDot11Chnl, wifiHsChnl;
+	u32 fwVer = 0, btPatchVer = 0;
+	u8 apNum = 0;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
+	CL_PRINTF(cliBuf);
+
+	if (pBtCoexist->bManualControl) {
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
+		CL_PRINTF(cliBuf);
+		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
+		CL_PRINTF(cliBuf);
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
+		pBoardInfo->pgAntNum,
+		pBoardInfo->btdmAntNum
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
+		(pStackInfo->bProfileNotified ? "Yes" : "No"),
+		pStackInfo->hciVersion
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
+		GLCoexVerDate8723b2Ant, GLCoexVer8723b2Ant, fwVer, btPatchVer, btPatchVer);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
+		wifiDot11Chnl,
+		wifiHsChnl,
+		bBtHsOn
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
+		pCoexDm->wifiChnlInfo[0],
+		pCoexDm->wifiChnlInfo[1],
+		pCoexDm->wifiChnlInfo[2]
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U1_AP_NUM, &apNum);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d", "Wifi rssi/ HS rssi/ AP#", \
+		wifiRssi,
+		btHsRssi,
+		apNum
+	);
+	CL_PRINTF(cliBuf);
+
+	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);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
+		bLink,
+		bRoam,
+		bScan
+	);
+	CL_PRINTF(cliBuf);
+
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
+	pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s / %s/ %s ", "Wifi status", \
+		(bWifiUnder5G ? "5G" : "2.4G"),
+		((BTC_WIFI_BW_LEGACY == wifiBw) ? "Legacy" : (((BTC_WIFI_BW_HT40 == wifiBw) ? "HT40" : "HT20"))),
+		((!bWifiBusy) ? "idle" : ((BTC_WIFI_TRAFFIC_TX == wifiTrafficDir) ? "uplink" : "downlink"))
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
+		((pBtCoexist->btInfo.bBtDisabled) ? ("disabled") : ((pCoexSta->bC2hBtInquiryPage) ? ("inquiry/page scan") : ((BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) ? "non-connected idle" :
+		((BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) ? "connected-idle" : "busy")))),
+		pCoexSta->btRssi,
+		pCoexSta->btRetryCnt
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
+		pBtLinkInfo->bScoExist,
+		pBtLinkInfo->bHidExist,
+		pBtLinkInfo->bPanExist,
+		pBtLinkInfo->bA2dpExist
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
+
+	btInfoExt = pCoexSta->btInfoExt;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s", "BT Info A2DP rate", \
+		(btInfoExt&BIT0) ? "Basic rate" : "EDR rate"
+	);
+	CL_PRINTF(cliBuf);
+
+	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
+		if (pCoexSta->btInfoC2hCnt[i]) {
+			CL_SPRINTF(
+				cliBuf,
+				BT_TMP_BUF_SIZE,
+				"\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723b2Ant[i], \
+				pCoexSta->btInfoC2h[i][0],
+				pCoexSta->btInfoC2h[i][1],
+				pCoexSta->btInfoC2h[i][2],
+				pCoexSta->btInfoC2h[i][3],
+				pCoexSta->btInfoC2h[i][4],
+				pCoexSta->btInfoC2h[i][5],
+				pCoexSta->btInfoC2h[i][6],
+				pCoexSta->btInfoC2hCnt[i]
+			);
+			CL_PRINTF(cliBuf);
+		}
+	}
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %s/%s", "PS state, IPS/LPS", \
+		((pCoexSta->bUnderIps ? "IPS ON" : "IPS OFF")),
+		((pCoexSta->bUnderLps ? "LPS ON" : "LPS OFF"))
+	);
+	CL_PRINTF(cliBuf);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
+
+	/*  Sw mechanism */
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s", "============[Sw mechanism]============"
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig]", \
+		pCoexDm->bCurRfRxLpfShrink,
+		pCoexDm->bCurLowPenaltyRa,
+		pCoexDm->bLimitedDig
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d/ %d(0x%x) ",
+		"SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
+		pCoexDm->bCurAgcTableEn,
+		pCoexDm->bCurAdcBackOff,
+		pCoexDm->bCurDacSwingOn,
+		pCoexDm->curDacSwingLvl
+	);
+	CL_PRINTF(cliBuf);
+
+	/*  Fw mechanism */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
+	CL_PRINTF(cliBuf);
+
+	psTdmaCase = pCoexDm->curPsTdma;
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
+		pCoexDm->psTdmaPara[0],
+		pCoexDm->psTdmaPara[1],
+		pCoexDm->psTdmaPara[2],
+		pCoexDm->psTdmaPara[3],
+		pCoexDm->psTdmaPara[4],
+		psTdmaCase, pCoexDm->bAutoTdmaAdjust
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
+		pCoexDm->curBtDecPwrLvl,
+		pCoexDm->bCurIgnoreWlanAct
+	);
+	CL_PRINTF(cliBuf);
+
+	/*  Hw setting */
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
+		pCoexDm->btRf0x1eBackup
+	);
+	CL_PRINTF(cliBuf);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x778);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x880);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x778/0x880[29:25]", \
+		u1Tmp[0],
+		(u4Tmp[0]&0x3e000000) >> 25
+	);
+	CL_PRINTF(cliBuf);
+
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x948);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x67);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x765);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x948/ 0x67[5] / 0x765", \
+		u4Tmp[0],
+		((u1Tmp[0]&0x20)>>5),
+		u1Tmp[1]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x92c);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x930);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x944);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]", \
+		u4Tmp[0]&0x3,
+		u4Tmp[1]&0xff,
+		u4Tmp[2]&0x3
+	);
+	CL_PRINTF(cliBuf);
+
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x39);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x40);
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x4c);
+	u1Tmp[2] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x64);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x38[11]/0x40/0x4c[24:23]/0x64[0]", \
+		((u1Tmp[0] & 0x8)>>3),
+		u1Tmp[1],
+		((u4Tmp[0]&0x01800000)>>23),
+		u1Tmp[2]&0x1
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x550);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x522);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
+		u4Tmp[0],
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xc50);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x49c);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x", "0xc50(dig)/0x49c(null-drop)", \
+		u4Tmp[0]&0xff,
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xda8);
+	u4Tmp[3] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0xcf0);
+
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5b);
+	u1Tmp[1] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0xa5c);
+
+	faOfdm =
+		((u4Tmp[0]&0xffff0000) >> 16) +
+		((u4Tmp[1]&0xffff0000) >> 16) +
+		(u4Tmp[1] & 0xffff) +  (u4Tmp[2] & 0xffff) + \
+		((u4Tmp[3]&0xffff0000) >> 16) +
+		(u4Tmp[3] & 0xffff);
+
+	faCck = (u1Tmp[0] << 8) + u1Tmp[1];
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "OFDM-CCA/OFDM-FA/CCK-FA", \
+		u4Tmp[0]&0xffff,
+		faOfdm,
+		faCck
+	);
+	CL_PRINTF(cliBuf);
+
+	u4Tmp[0] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c0);
+	u4Tmp[1] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c4);
+	u4Tmp[2] = pBtCoexist->fBtcRead4Byte(pBtCoexist, 0x6c8);
+	u1Tmp[0] = pBtCoexist->fBtcRead1Byte(pBtCoexist, 0x6cc);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
+		u4Tmp[0],
+		u4Tmp[1],
+		u4Tmp[2],
+		u1Tmp[0]
+	);
+	CL_PRINTF(cliBuf);
+
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x770(high-pri rx/tx)", \
+		pCoexSta->highPriorityRx,
+		pCoexSta->highPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+	CL_SPRINTF(
+		cliBuf,
+		BT_TMP_BUF_SIZE,
+		"\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)", \
+		pCoexSta->lowPriorityRx,
+		pCoexSta->lowPriorityTx
+	);
+	CL_PRINTF(cliBuf);
+
+	halbtc8723b2ant_MonitorBtCtr(pBtCoexist);
+	pBtCoexist->fBtcDispDbgMsg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
+}
+
+
+void EXhalbtc8723b2ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_IPS_ENTER == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
+		pCoexSta->bUnderIps = true;
+		halbtc8723b2ant_WifiOffHwCfg(pBtCoexist);
+		halbtc8723b2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
+		halbtc8723b2ant_CoexAllOff(pBtCoexist);
+	} else if (BTC_IPS_LEAVE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
+		pCoexSta->bUnderIps = false;
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_LPS_ENABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
+		pCoexSta->bUnderLps = true;
+	} else if (BTC_LPS_DISABLE == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
+		pCoexSta->bUnderLps = false;
+	}
+}
+
+void EXhalbtc8723b2ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_SCAN_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
+	} else if (BTC_SCAN_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (BTC_ASSOCIATE_START == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
+	} else if (BTC_ASSOCIATE_FINISH == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	u8 H2C_Parameter[3] = {0};
+	u32 wifiBw;
+	u8 wifiCentralChnl;
+	u8 apNum = 0;
+
+	if (BTC_MEDIA_CONNECT == type) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
+	} else {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
+	}
+
+	/*  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];
+
+	BTC_PRINT(
+		BTC_MSG_ALGORITHM,
+		ALGO_TRACE_FW_EXEC,
+		(
+			"[BTCoex], FW write 0x66 = 0x%x\n",
+			H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]
+		)
+	);
+
+	pBtCoexist->fBtcFillH2c(pBtCoexist, 0x66, 3, H2C_Parameter);
+}
+
+void EXhalbtc8723b2ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type)
+{
+	if (type == BTC_PACKET_DHCP) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
+	}
+}
+
+void EXhalbtc8723b2ant_BtInfoNotify(
+	PBTC_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]++;
+
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length =%d, hex data =[", rspSource, length));
+	for (i = 0; i < length; i++) {
+		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
+		if (i == 1)
+			btInfo = tmpBuf[i];
+
+		if (i == length-1) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
+		} else {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
+		}
+	}
+
+	if (pBtCoexist->bManualControl) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
+		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 */
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Switch BT TRx Mask since BT RF REG 0x3C != 0x01\n"));
+			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)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n"));
+			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)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n"));
+			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;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n"));
+	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION)	{ /*  connection exists but no busy */
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n"));
+	} 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;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT SCO busy!!!\n"));
+	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT ACL busy!!!\n"));
+	} else {
+		pCoexDm->btStatus = BT_8723B_2ANT_BT_STATUS_MAX;
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n"));
+	}
+
+	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(PBTC_COEXIST pBtCoexist)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
+
+	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(PBTC_COEXIST pBtCoexist, u8 pnpState)
+{
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
+
+	if (BTC_WIFI_PNP_SLEEP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to SLEEP\n"));
+	} else if (BTC_WIFI_PNP_WAKE_UP == pnpState) {
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify to WAKE UP\n"));
+		halbtc8723b2ant_InitHwConfig(pBtCoexist, false);
+		halbtc8723b2ant_InitCoexDm(pBtCoexist);
+		halbtc8723b2ant_QueryBtInfo(pBtCoexist);
+	}
+}
+
+void EXhalbtc8723b2ant_Periodical(PBTC_COEXIST pBtCoexist)
+{
+	static u8 disVerInfoCnt = 0;
+	u32 fwVer = 0, btPatchVer = 0;
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical ===========================\n"));
+
+	if (disVerInfoCnt <= 5) {
+		disVerInfoCnt += 1;
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
+		pBtCoexist->fBtcGet(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
+			GLCoexVerDate8723b2Ant, GLCoexVer8723b2Ant, fwVer, btPatchVer, btPatchVer));
+		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
+	}
+
+	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 000000000000..5a0fed69eda7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtc8723b2Ant.h
@@ -0,0 +1,155 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*  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
+
+typedef enum _BT_INFO_SRC_8723B_2ANT {
+	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
+} BT_INFO_SRC_8723B_2ANT, *PBT_INFO_SRC_8723B_2ANT;
+
+typedef enum _BT_8723B_2ANT_BT_STATUS {
+	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
+} BT_8723B_2ANT_BT_STATUS, *PBT_8723B_2ANT_BT_STATUS;
+
+typedef enum _BT_8723B_2ANT_COEX_ALGO {
+	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,
+} BT_8723B_2ANT_COEX_ALGO, *PBT_8723B_2ANT_COEX_ALGO;
+
+typedef 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;
+} COEX_DM_8723B_2ANT, *PCOEX_DM_8723B_2ANT;
+
+typedef 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;
+} COEX_STA_8723B_2ANT, *PCOEX_STA_8723B_2ANT;
+
+/*  */
+/*  The following is interface which will notify coex module. */
+/*  */
+void EXhalbtc8723b2ant_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_InitHwConfig(PBTC_COEXIST pBtCoexist, bool bWifiOnly);
+void EXhalbtc8723b2ant_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_MediaStatusNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtc8723b2ant_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtc8723b2ant_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtc8723b2ant_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtc8723b2ant_DisplayCoexInfo(PBTC_COEXIST pBtCoexist);
diff --git a/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h b/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h
new file mode 100644
index 000000000000..38414dd8adbd
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalBtcOutSrc.h
@@ -0,0 +1,566 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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
+
+typedef enum _BTC_POWERSAVE_TYPE{
+	BTC_PS_WIFI_NATIVE	= 0,	/*  wifi original power save behavior */
+	BTC_PS_LPS_ON		= 1,
+	BTC_PS_LPS_OFF		= 2,
+	BTC_PS_MAX
+} BTC_POWERSAVE_TYPE, *PBTC_POWERSAVE_TYPE;
+
+typedef enum _BTC_BT_REG_TYPE{
+	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
+} BTC_BT_REG_TYPE, *PBTC_BT_REG_TYPE;
+
+typedef enum _BTC_CHIP_INTERFACE{
+	BTC_INTF_UNKNOWN	= 0,
+	BTC_INTF_PCI		= 1,
+	BTC_INTF_USB		= 2,
+	BTC_INTF_SDIO		= 3,
+	BTC_INTF_MAX
+} BTC_CHIP_INTERFACE, *PBTC_CHIP_INTERFACE;
+
+typedef enum _BTC_CHIP_TYPE {
+	BTC_CHIP_UNDEF		= 0,
+	BTC_CHIP_CSR_BC4	= 1,
+	BTC_CHIP_CSR_BC8	= 2,
+	BTC_CHIP_RTL8723A	= 3,
+	BTC_CHIP_RTL8821	= 4,
+	BTC_CHIP_RTL8723B	= 5,
+	BTC_CHIP_MAX
+} BTC_CHIP_TYPE, *PBTC_CHIP_TYPE;
+
+typedef enum _BTC_MSG_TYPE {
+	BTC_MSG_INTERFACE	= 0x0,
+	BTC_MSG_ALGORITHM	= 0x1,
+	BTC_MSG_MAX
+} BTC_MSG_TYPE;
+extern u32 		GLBtcDbgType[];
+
+/*  following is for BTC_MSG_INTERFACE */
+#define INTF_INIT	BIT0
+#define INTF_NOTIFY	BIT2
+
+/*  following is for BTC_ALGORITHM */
+#define ALGO_BT_RSSI_STATE				BIT0
+#define ALGO_WIFI_RSSI_STATE			BIT1
+#define ALGO_BT_MONITOR					BIT2
+#define ALGO_TRACE						BIT3
+#define ALGO_TRACE_FW					BIT4
+#define ALGO_TRACE_FW_DETAIL			BIT5
+#define ALGO_TRACE_FW_EXEC				BIT6
+#define ALGO_TRACE_SW					BIT7
+#define ALGO_TRACE_SW_DETAIL			BIT8
+#define ALGO_TRACE_SW_EXEC				BIT9
+
+/*  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
+
+/*  following is for command line utility */
+#define CL_SPRINTF	snprintf
+#define CL_PRINTF	DCMD_Printf
+
+/*  The following is for dbgview print */
+#if DBG
+#define BTC_PRINT(dbgtype, dbgflag, printstr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag)\
+		DbgPrint printstr;\
+}
+
+#define BTC_PRINT_F(dbgtype, dbgflag, printstr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		DbgPrint("%s(): ", __func__);\
+		DbgPrint printstr;\
+	} \
+}
+
+#define BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		int __i;\
+		u8 *ptr = (u8 *)_Ptr;\
+		DbgPrint printstr;\
+		DbgPrint(" ");\
+		for (__i = 0; __i < 6; __i++)\
+			DbgPrint("%02X%s", ptr[__i], (__i == 5) ? "" : "-");\
+		DbgPrint("\n");\
+	} \
+}
+
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen)\
+{\
+	if (GLBtcDbgType[dbgtype] & dbgflag) {\
+		int __i;\
+		u8 *ptr = (u8 *)_HexData;\
+		DbgPrint(_TitleString);\
+		for (__i = 0; __i < (int)_HexDataLen; __i++) {\
+			DbgPrint("%02X%s", ptr[__i], (((__i + 1) % 4) == 0) ? "  " : " ");\
+			if (((__i + 1) % 16) == 0)\
+				DbgPrint("\n");\
+		} \
+		DbgPrint("\n");\
+	} \
+}
+
+#else
+#define BTC_PRINT(dbgtype, dbgflag, printstr)		 no_printk printstr
+#define BTC_PRINT_F(dbgtype, dbgflag, printstr)		 no_printk printstr
+#define BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _Ptr) no_printk printstr
+#define BTC_PRINT_DATA(dbgtype, dbgflag, _TitleString, _HexData, _HexDataLen) \
+			no_printk("%s %p %zu", _TitleString, _HexData, _HexDataLen)
+#endif
+
+typedef struct _BTC_BOARD_INFO {
+	/*  The following is some board information */
+	u8 btChipType;
+	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; */
+} BTC_BOARD_INFO, *PBTC_BOARD_INFO;
+
+typedef enum _BTC_DBG_OPCODE {
+	BTC_DBG_SET_COEX_NORMAL			    = 0x0,
+	BTC_DBG_SET_COEX_WIFI_ONLY		    = 0x1,
+	BTC_DBG_SET_COEX_BT_ONLY		    = 0x2,
+	BTC_DBG_SET_COEX_DEC_BT_PWR		    = 0x3,
+	BTC_DBG_SET_COEX_BT_AFH_MAP		    = 0x4,
+	BTC_DBG_SET_COEX_BT_IGNORE_WLAN_ACT	= 0x5,
+	BTC_DBG_MAX
+} BTC_DBG_OPCODE, *PBTC_DBG_OPCODE;
+
+typedef enum _BTC_RSSI_STATE {
+	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
+} BTC_RSSI_STATE, *PBTC_RSSI_STATE;
+#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)
+
+typedef enum _BTC_WIFI_ROLE {
+	BTC_ROLE_STATION			= 0x0,
+	BTC_ROLE_AP					= 0x1,
+	BTC_ROLE_IBSS				= 0x2,
+	BTC_ROLE_HS_MODE			= 0x3,
+	BTC_ROLE_MAX
+} BTC_WIFI_ROLE, *PBTC_WIFI_ROLE;
+
+typedef enum _BTC_WIFI_BW_MODE {
+	BTC_WIFI_BW_LEGACY			= 0x0,
+	BTC_WIFI_BW_HT20			= 0x1,
+	BTC_WIFI_BW_HT40			= 0x2,
+	BTC_WIFI_BW_MAX
+} BTC_WIFI_BW_MODE, *PBTC_WIFI_BW_MODE;
+
+typedef enum _BTC_WIFI_TRAFFIC_DIR {
+	BTC_WIFI_TRAFFIC_TX			= 0x0,
+	BTC_WIFI_TRAFFIC_RX			= 0x1,
+	BTC_WIFI_TRAFFIC_MAX
+} BTC_WIFI_TRAFFIC_DIR, *PBTC_WIFI_TRAFFIC_DIR;
+
+typedef enum _BTC_WIFI_PNP {
+	BTC_WIFI_PNP_WAKE_UP		= 0x0,
+	BTC_WIFI_PNP_SLEEP			= 0x1,
+	BTC_WIFI_PNP_MAX
+} BTC_WIFI_PNP, *PBTC_WIFI_PNP;
+
+/* for 8723b-d cut large current issue */
+typedef enum _BT_WIFI_COEX_STATE {
+	BTC_WIFI_STAT_INIT,
+	BTC_WIFI_STAT_IQK,
+	BTC_WIFI_STAT_NORMAL_OFF,
+	BTC_WIFI_STAT_MP_OFF,
+	BTC_WIFI_STAT_NORMAL,
+	BTC_WIFI_STAT_ANT_DIV,
+	BTC_WIFI_STAT_MAX
+} BT_WIFI_COEX_STATE, *PBT_WIFI_COEX_STATE;
+
+/*  defined for BFP_BTC_GET */
+typedef enum _BTC_GET_TYPE {
+	/*  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_UNDER_5G,
+	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
+} BTC_GET_TYPE, *PBTC_GET_TYPE;
+
+/*  defined for BFP_BTC_SET */
+typedef enum _BTC_SET_TYPE {
+	/*  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
+} BTC_SET_TYPE, *PBTC_SET_TYPE;
+
+typedef enum _BTC_DBG_DISP_TYPE {
+	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
+} BTC_DBG_DISP_TYPE, *PBTC_DBG_DISP_TYPE;
+
+typedef enum _BTC_NOTIFY_TYPE_IPS {
+	BTC_IPS_LEAVE						= 0x0,
+	BTC_IPS_ENTER						= 0x1,
+	BTC_IPS_MAX
+} BTC_NOTIFY_TYPE_IPS, *PBTC_NOTIFY_TYPE_IPS;
+
+typedef enum _BTC_NOTIFY_TYPE_LPS {
+	BTC_LPS_DISABLE						= 0x0,
+	BTC_LPS_ENABLE						= 0x1,
+	BTC_LPS_MAX
+} BTC_NOTIFY_TYPE_LPS, *PBTC_NOTIFY_TYPE_LPS;
+
+typedef enum _BTC_NOTIFY_TYPE_SCAN {
+	BTC_SCAN_FINISH						= 0x0,
+	BTC_SCAN_START						= 0x1,
+	BTC_SCAN_MAX
+} BTC_NOTIFY_TYPE_SCAN, *PBTC_NOTIFY_TYPE_SCAN;
+
+typedef enum _BTC_NOTIFY_TYPE_ASSOCIATE {
+	BTC_ASSOCIATE_FINISH				= 0x0,
+	BTC_ASSOCIATE_START					= 0x1,
+	BTC_ASSOCIATE_MAX
+} BTC_NOTIFY_TYPE_ASSOCIATE, *PBTC_NOTIFY_TYPE_ASSOCIATE;
+
+typedef enum _BTC_NOTIFY_TYPE_MEDIA_STATUS {
+	BTC_MEDIA_DISCONNECT				= 0x0,
+	BTC_MEDIA_CONNECT					= 0x1,
+	BTC_MEDIA_MAX
+} BTC_NOTIFY_TYPE_MEDIA_STATUS, *PBTC_NOTIFY_TYPE_MEDIA_STATUS;
+
+typedef enum _BTC_NOTIFY_TYPE_SPECIAL_PACKET {
+	BTC_PACKET_UNKNOWN					= 0x0,
+	BTC_PACKET_DHCP						= 0x1,
+	BTC_PACKET_ARP						= 0x2,
+	BTC_PACKET_EAPOL					= 0x3,
+	BTC_PACKET_MAX
+} BTC_NOTIFY_TYPE_SPECIAL_PACKET, *PBTC_NOTIFY_TYPE_SPECIAL_PACKET;
+
+typedef enum _BTC_NOTIFY_TYPE_STACK_OPERATION {
+	BTC_STACK_OP_NONE					= 0x0,
+	BTC_STACK_OP_INQ_PAGE_PAIR_START	= 0x1,
+	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH	= 0x2,
+	BTC_STACK_OP_MAX
+} BTC_NOTIFY_TYPE_STACK_OPERATION, *PBTC_NOTIFY_TYPE_STACK_OPERATION;
+
+/* Bryant Add */
+typedef enum _BTC_ANTENNA_POS {
+	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
+	BTC_ANTENNA_AT_AUX_PORT  = 0x2,
+} BTC_ANTENNA_POS, *PBTC_ANTENNA_POS;
+
+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);
+
+typedef 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;
+} BTC_BT_INFO, *PBTC_BT_INFO;
+
+typedef 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;
+} BTC_STACK_INFO, *PBTC_STACK_INFO;
+
+typedef 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;
+} BTC_BT_LINK_INFO, *PBTC_BT_LINK_INFO;
+
+typedef 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;
+} BTC_STATISTICS, *PBTC_STATISTICS;
+
+typedef struct _BTC_COEXIST {
+	bool bBinded;		/*  make sure only one adapter can bind the data context */
+	void *Adapter;		/*  default adapter */
+	BTC_BOARD_INFO boardInfo;
+	BTC_BT_INFO btInfo;		/*  some bt info referenced by non-bt module */
+	BTC_STACK_INFO stackInfo;
+	BTC_BT_LINK_INFO btLinkInfo;
+	BTC_CHIP_INTERFACE chipInterface;
+
+	bool bInitilized;
+	bool bStopCoexDm;
+	bool bManualControl;
+	u8 *cliBuf;
+	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;
+	/*  other */
+	BFP_BTC_DISP_DBG_MSG fBtcDispDbgMsg;
+	/*  normal get/set related */
+	BFP_BTC_GET fBtcGet;
+	BFP_BTC_SET fBtcSet;
+
+	BFP_BTC_GET_BT_REG fBtcGetBtReg;
+	BFP_BTC_SET_BT_REG fBtcSetBtReg;
+} BTC_COEXIST, *PBTC_COEXIST;
+
+extern BTC_COEXIST GLBtCoexist;
+
+u8 EXhalbtcoutsrc_InitlizeVariables(void *Adapter);
+void EXhalbtcoutsrc_PowerOnSetting(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_InitHwConfig(PBTC_COEXIST pBtCoexist, u8 bWifiOnly);
+void EXhalbtcoutsrc_InitCoexDm(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_IpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_LpsNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ScanNotify(PBTC_COEXIST pBtCoexist, u8 type);
+void EXhalbtcoutsrc_ConnectNotify(PBTC_COEXIST pBtCoexist, u8 action);
+void EXhalbtcoutsrc_MediaStatusNotify(
+	PBTC_COEXIST pBtCoexist, RT_MEDIA_STATUS mediaStatus
+);
+void EXhalbtcoutsrc_SpecialPacketNotify(PBTC_COEXIST pBtCoexist, u8 pktType);
+void EXhalbtcoutsrc_BtInfoNotify(
+	PBTC_COEXIST pBtCoexist, u8 *tmpBuf, u8 length
+);
+void EXhalbtcoutsrc_HaltNotify(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_PnpNotify(PBTC_COEXIST pBtCoexist, u8 pnpState);
+void EXhalbtcoutsrc_Periodical(PBTC_COEXIST pBtCoexist);
+void EXhalbtcoutsrc_SetChipType(u8 chipType);
+void EXhalbtcoutsrc_SetAntNum(u8 type, u8 antNum);
+void EXhalbtcoutsrc_SetSingleAntPath(u8 singleAntPath);
+void EXhalbtcoutsrc_DisplayBtCoexInfo(PBTC_COEXIST pBtCoexist);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c
new file mode 100644
index 000000000000..51d4219177d3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.c
@@ -0,0 +1,643 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		("	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+
+	/*  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(
+	PDM_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(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_AGC_TAB)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_AGC_TAB;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_AGC_TAB\n")
+	);
+
+	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 == false) {
+				/*  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(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_PHY_REG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_PHY_REG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_PHY_REG\n")
+	);
+
+	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 == false) {
+				/*  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, 0, 0, 0x00000e08, 0x0000ff00, 0x00003800,
+	0, 0, 0, 0x0000086c, 0xffffff00, 0x32343600,
+	0, 0, 0, 0x00000e00, 0xffffffff, 0x40424444,
+	0, 0, 0, 0x00000e04, 0xffffffff, 0x28323638,
+	0, 0, 0, 0x00000e10, 0xffffffff, 0x38404244,
+	0, 0, 0, 0x00000e14, 0xffffffff, 0x26303436
+};
+
+void ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_PHY_REG_PG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_PHY_REG_PG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_PHY_REG_PG\n")
+	);
+
+	pDM_Odm->PhyRegPgVersion = 1;
+	pDM_Odm->PhyRegPgValueType = PHY_REG_PG_EXACT_VALUE;
+
+	for (i = 0; i < ArrayLen; i += 6) {
+		u32 v1 = Array[i];
+		u32 v2 = Array[i+1];
+		u32 v3 = Array[i+2];
+		u32 v4 = Array[i+3];
+		u32 v5 = Array[i+4];
+		u32 v6 = Array[i+5];
+
+		odm_ConfigBB_PHY_REG_PG_8723B(pDM_Odm, v1, v2, v3, v4, v5, v6);
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h
new file mode 100644
index 000000000000..41fe0ba16914
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_BB.h
@@ -0,0 +1,48 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 __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 */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG(/*  TC: Test Chip, MP: MP Chip */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           PHY_REG_PG.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_PHY_REG_PG(/*  TC: Test Chip, MP: MP Chip */
+	PDM_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 000000000000..b868e26f20ac
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.c
@@ -0,0 +1,302 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+
+	/*  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(
+	PDM_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(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_MAC_REG)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_MAC_REG;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_MAC_REG\n")
+	);
+
+	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 == false) {
+				/*  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 000000000000..ae5dd3ccf939
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_MAC.h
@@ -0,0 +1,28 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 __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 */
+	PDM_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 000000000000..84a0be7ba697
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.c
@@ -0,0 +1,799 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+static bool CheckPositive(
+	PDM_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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (cond1, cond2) = (0x%X 0x%X)\n",
+			cond1,
+			cond2
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> [8812A] CheckPositive (driver1, driver2) = (0x%X 0x%X)\n",
+			driver1,
+			driver2
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Platform, Interface) = (0x%X, 0x%X)\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"	(Board, Package) = (0x%X, 0x%X)\n",
+			pDM_Odm->BoardType,
+			pDM_Odm->PackageType
+		)
+	);
+
+	/*  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(
+	PDM_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(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_RadioA)/sizeof(u32);
+	u32 *Array = Array_MP_8723B_RadioA;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_RadioA\n")
+	);
+
+	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 == false) {
+				/*  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_5GB_N_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 1, 2, 2, 3, 4, 5, 5, 6,  6,  7,  7,  8,  8,  9,
+		9, 10, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GB_P_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 16, 17, 17, 18, 19, 20, 20, 20
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 19, 20, 20, 20
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GA_N_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 1, 2, 3, 3, 4, 4, 5, 5, 6,  7,  8,  8,  9,  9, 10,
+		10, 11, 11, 12, 12, 13, 13, 14, 14, 14, 14, 14, 14, 14
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 6,  7,  7,  8,  8,  9, 10,
+		11, 11, 12, 13, 13, 14, 15, 16, 16, 16, 16, 16, 16, 16
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 10, 11,
+		11, 12, 13, 14, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16
+	},
+};
+static u8 gDeltaSwingTableIdx_MP_5GA_P_TxPowerTrack_SDIO_8723B[][DELTA_SWINGIDX_SIZE] = {
+	{
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+		0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+	{
+		0, 1, 2, 3, 3, 4, 5, 6, 6, 7,  8,  9,  9, 10, 11, 12,
+		12, 13, 14, 15, 15, 16, 17, 18, 18, 19, 20, 21, 21, 21
+	},
+};
+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(PDM_ODM_T pDM_Odm)
+{
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_MP_8723B\n")
+	);
+
+
+	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
+	);
+
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P,
+		gDeltaSwingTableIdx_MP_5GA_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N,
+		gDeltaSwingTableIdx_MP_5GA_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P,
+		gDeltaSwingTableIdx_MP_5GB_P_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+	memcpy(
+		pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N,
+		gDeltaSwingTableIdx_MP_5GB_N_TxPowerTrack_SDIO_8723B,
+		DELTA_SWINGIDX_SIZE*3
+	);
+}
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+static u8 *Array_MP_8723B_TXPWR_LMT[] = {
+	"FCC", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "28",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "1T", "01", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "02", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "2T", "01", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "02", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "03", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "04", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "08", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "09", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "14", "63"
+};
+
+void ODM_ReadAndConfig_MP_8723B_TXPWR_LMT(PDM_ODM_T pDM_Odm)
+{
+	u32 i = 0;
+	u32 ArrayLen = sizeof(Array_MP_8723B_TXPWR_LMT)/sizeof(u8 *);
+	u8 **Array = Array_MP_8723B_TXPWR_LMT;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		("===> ODM_ReadAndConfig_MP_8723B_TXPWR_LMT\n")
+	);
+
+	for (i = 0; i < ArrayLen; i += 7) {
+		u8 *regulation = Array[i];
+		u8 *band = Array[i+1];
+		u8 *bandwidth = Array[i+2];
+		u8 *rate = Array[i+3];
+		u8 *rfPath = Array[i+4];
+		u8 *chnl = Array[i+5];
+		u8 *val = Array[i+6];
+
+		odm_ConfigBB_TXPWR_LMT_8723B(
+			pDM_Odm,
+			regulation,
+			band,
+			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 000000000000..98aa2ba97fa9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalHWImg8723B_RF.h
@@ -0,0 +1,49 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License 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 __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 */
+	PDM_ODM_T  pDM_Odm
+);
+
+/******************************************************************************
+*                           TxPowerTrack_SDIO.TXT
+******************************************************************************/
+
+void
+ODM_ReadAndConfig_MP_8723B_TxPowerTrack_SDIO(/*  TC: Test Chip, MP: MP Chip */
+	PDM_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 */
+	PDM_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 000000000000..9adcc3098463
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf.c
@@ -0,0 +1,662 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "Mp_Precomp.h" */
+#include "odm_precomp.h"
+
+
+#define CALCULATE_SWINGTALBE_OFFSET(_offset, _direction, _size, _deltaThermal) \
+	do {\
+		for (_offset = 0; _offset < _size; _offset++) {\
+			if (_deltaThermal < thermalThreshold[_direction][_offset]) {\
+				if (_offset != 0)\
+					_offset--;\
+				break;\
+			} \
+		} \
+		if (_offset >= _size)\
+			_offset = _size-1;\
+	} while (0)
+
+
+void ConfigureTxpowerTrack(PDM_ODM_T pDM_Odm, PTXPWRTRACK_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(PDM_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 = ODM_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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	u8 ThermalValue = 0, delta, delta_LCK, delta_IQK, 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) */
+
+	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. Initilization (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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"===>ODM_TXPowerTrackingCallback_ThermalMeter,\npDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]: %d, pDM_Odm->DefaultOfdmIndex: %d\n",
+			pDM_Odm->BbSwingIdxCckBase,
+			pDM_Odm->BbSwingIdxOfdmBase[ODM_RF_PATH_A],
+			pDM_Odm->DefaultOfdmIndex
+		)
+	);
+
+	ThermalValue = (u8)PHY_QueryRFReg(pDM_Odm->Adapter, ODM_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 */
+
+	if (pDM_Odm->RFCalibrateInfo.bReloadtxpowerindex)
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			("reload ofdm index for band switch\n")
+		);
+
+	/* 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);
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+				ThermalValue,
+				pHalData->EEPROMThermalMeter
+			)
+		);
+	}
+
+	/* 4 5. Calculate delta, delta_LCK, delta_IQK. */
+	/* 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);
+	delta_IQK =
+		(ThermalValue > pDM_Odm->RFCalibrateInfo.ThermalValue_IQK) ?
+		(ThermalValue - pDM_Odm->RFCalibrateInfo.ThermalValue_IQK) :
+		(pDM_Odm->RFCalibrateInfo.ThermalValue_IQK - ThermalValue);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+			delta,
+			delta_LCK,
+			delta_IQK
+		)
+	);
+
+	/* 4 6. If necessary, do LCK. */
+	/*  Delta temperature is equal to or larger than 20 centigrade. */
+	if (delta_LCK >= c.Threshold_IQK) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"delta_LCK(%d) >= Threshold_IQK(%d)\n",
+				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 differrence. */
+		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) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"deltaSwingTableIdx_TUP_A[%d] = %d\n",
+					delta,
+					deltaSwingTableIdx_TUP_A[delta]
+				)
+			);
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] =
+				deltaSwingTableIdx_TUP_A[delta];
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"******Temp is higher and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+					pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A]
+				)
+			);
+
+			if (c.RfPathCount > 1) {
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"deltaSwingTableIdx_TUP_B[%d] = %d\n",
+						delta,
+						deltaSwingTableIdx_TUP_B[delta]
+					)
+				);
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+
+				/*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] =
+					deltaSwingTableIdx_TUP_B[delta];
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"******Temp is higher and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+						pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B]
+					)
+				);
+			}
+
+		} else {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"deltaSwingTableIdx_TDOWN_A[%d] = %d\n",
+					delta,
+					deltaSwingTableIdx_TDOWN_A[delta]
+				)
+			);
+
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_A] =
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A];
+			pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			/*  Record delta swing for mix mode power tracking */
+			pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] =
+				-1 * deltaSwingTableIdx_TDOWN_A[delta];
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"******Temp is lower and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+					pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_A]
+				)
+			);
+
+			if (c.RfPathCount > 1) {
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"deltaSwingTableIdx_TDOWN_B[%d] = %d\n",
+						delta,
+						deltaSwingTableIdx_TDOWN_B[delta]
+					)
+				);
+
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[ODM_RF_PATH_B] =
+					pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B];
+				pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[ODM_RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+
+				 /*  Record delta swing for mix mode power tracking */
+				pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] =
+					-1 * deltaSwingTableIdx_TDOWN_B[delta];
+
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"******Temp is lower and pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+						pDM_Odm->Absolute_OFDMSwingIdx[ODM_RF_PATH_B]
+					)
+				);
+			}
+		}
+
+		for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"\n\n ================================ [Path-%c] Calculating PowerIndexOffset ================================\n",
+					(p == ODM_RF_PATH_A ? 'A' : 'B')
+				)
+			);
+
+			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 */
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
+					(
+						p == ODM_RF_PATH_A ? 'A' : 'B'),
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p],
+						pDM_Odm->RFCalibrateInfo.DeltaPowerIndex[p],
+						pDM_Odm->RFCalibrateInfo.DeltaPowerIndexLast[p]
+					)
+				);
+
+			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];
+
+			/*  *************Print BB Swing Base and Index Offset************* */
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+					pDM_Odm->BbSwingIdxCck,
+					pDM_Odm->BbSwingIdxCckBase,
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p]
+				)
+			);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+					pDM_Odm->BbSwingIdxOfdm[p],
+					(p == ODM_RF_PATH_A ? 'A' : 'B'),
+					pDM_Odm->BbSwingIdxOfdmBase[p],
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[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;
+		}
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			("\n\n ========================================================================================================\n")
+		);
+		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 {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"The thermal meter is unchanged or TxPowerTracking OFF(%d): ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+				pDM_Odm->RFCalibrateInfo.TxPowerTrackControl,
+				ThermalValue,
+				pDM_Odm->RFCalibrateInfo.ThermalValue
+			)
+		);
+
+			for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+				pDM_Odm->RFCalibrateInfo.PowerIndexOffset[p] = 0;
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		(
+			"TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
+			pDM_Odm->RFCalibrateInfo.CCK_index,
+			pDM_Odm->BbSwingIdxCckBase
+		)
+	);
+
+	/* Print Swing base & current */
+	for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK,
+			ODM_DBG_LOUD,
+			(
+				"TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
+				pDM_Odm->RFCalibrateInfo.OFDM_index[p],
+				(p == ODM_RF_PATH_A ? 'A' : 'B'),
+				pDM_Odm->BbSwingIdxOfdmBase[p]
+			)
+		);
+	}
+
+	if (
+		(pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A] != 0 ||
+		 pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_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 > pDM_Odm->RFCalibrateInfo.ThermalValue) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A],
+					delta,
+					ThermalValue,
+					pHalData->EEPROMThermalMeter,
+					pDM_Odm->RFCalibrateInfo.ThermalValue
+				)
+			);
+
+			if (c.RfPathCount > 1)
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"Temperature Increasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_B],
+						delta,
+						ThermalValue,
+						pHalData->EEPROMThermalMeter,
+						pDM_Odm->RFCalibrateInfo.ThermalValue
+					)
+				);
+
+		} else if (ThermalValue < pDM_Odm->RFCalibrateInfo.ThermalValue) { /*  Low temperature */
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+					pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_A],
+					delta,
+					ThermalValue,
+					pHalData->EEPROMThermalMeter,
+					pDM_Odm->RFCalibrateInfo.ThermalValue
+				)
+			);
+
+			if (c.RfPathCount > 1)
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_TX_PWR_TRACK,
+					ODM_DBG_LOUD,
+					(
+						"Temperature Decreasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+						pDM_Odm->RFCalibrateInfo.PowerIndexOffset[ODM_RF_PATH_B],
+						delta,
+						ThermalValue,
+						pHalData->EEPROMThermalMeter,
+						pDM_Odm->RFCalibrateInfo.ThermalValue
+					)
+				);
+
+		}
+
+		if (ThermalValue > pHalData->EEPROMThermalMeter) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature(%d) higher than PG value(%d)\n",
+					ThermalValue,
+					pHalData->EEPROMThermalMeter
+				)
+			);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n")
+			);
+			for (p = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+					(*c.ODM_TxPwrTrackSetPwr)(pDM_Odm, MIX_MODE, p, 0);
+		} else {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				(
+					"Temperature(%d) lower than PG value(%d)\n",
+					ThermalValue,
+					pHalData->EEPROMThermalMeter
+				)
+			);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_TX_PWR_TRACK,
+				ODM_DBG_LOUD,
+				("**********Enter POWER Tracking MIX_MODE**********\n")
+			);
+			for (p = ODM_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 = ODM_RF_PATH_A; p < c.RfPathCount; p++)
+			pDM_Odm->BbSwingIdxOfdmBase[p] = pDM_Odm->BbSwingIdxOfdm[p];
+
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			(
+				"pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue = %d\n",
+				pDM_Odm->RFCalibrateInfo.ThermalValue,
+				ThermalValue
+			)
+		);
+
+		/* Record last Power Tracking Thermal Value */
+		pDM_Odm->RFCalibrateInfo.ThermalValue = ThermalValue;
+	}
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_TX_PWR_TRACK,
+		ODM_DBG_LOUD,
+		("<===ODM_TXPowerTrackingCallback_ThermalMeter\n")
+	);
+
+	pDM_Odm->RFCalibrateInfo.TXPowercount = 0;
+}
+
+
+
+
+/* 3 ============================================================ */
+/* 3 IQ Calibration */
+/* 3 ============================================================ */
+
+u8 ODM_GetRightChnlPlaceforIQK(u8 chnl)
+{
+	u8 channel_all[ODM_TARGET_CHNL_NUM_2G_5G] = {
+		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58,
+		60, 62, 64, 100, 102, 104, 106, 108, 110, 112,
+		114, 116, 118, 120, 122, 124, 126, 128, 130, 132,
+		134, 136, 138, 140, 149, 151, 153, 155, 157, 159,
+		161, 163, 165
+	};
+	u8 place = chnl;
+
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++) {
+			if (channel_all[place] == chnl)
+				return place-13;
+		}
+	}
+	return 0;
+
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf.h b/drivers/staging/rtl8723bs/hal/HalPhyRf.h
new file mode 100644
index 000000000000..bd7462d4ed91
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf.h
@@ -0,0 +1,63 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HAL_PHY_RF_H__
+ #define __HAL_PHY_RF_H__
+
+typedef enum _SPUR_CAL_METHOD {
+	PLL_RESET,
+	AFE_PHASE_SEL
+} SPUR_CAL_METHOD;
+
+typedef enum _PWRTRACK_CONTROL_METHOD {
+	BBSWING,
+	TXAGC,
+	MIX_MODE
+} PWRTRACK_METHOD;
+
+typedef void (*FuncSetPwr)(PDM_ODM_T, PWRTRACK_METHOD, u8, u8);
+typedef void (*FuncIQK)(PDM_ODM_T, u8, u8, u8);
+typedef void (*FuncLCK)(PDM_ODM_T);
+typedef void (*FuncSwing)(PDM_ODM_T, u8 **, u8 **, u8 **, u8 **);
+
+typedef 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;
+} TXPWRTRACK_CFG, *PTXPWRTRACK_CFG;
+
+void ConfigureTxpowerTrack(PDM_ODM_T pDM_Odm, PTXPWRTRACK_CFG pConfig);
+
+
+void ODM_ClearTxPowerTrackingState(PDM_ODM_T pDM_Odm);
+
+void ODM_TXPowerTrackingCallback_ThermalMeter(struct adapter *Adapter);
+
+
+
+#define ODM_TARGET_CHNL_NUM_2G_5G 59
+
+
+u8 ODM_GetRightChnlPlaceforIQK(u8 chnl);
+
+
+#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 000000000000..c16e147d8adc
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.c
@@ -0,0 +1,2091 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 <drv_types.h>
+#include <rtw_debug.h>
+#include "odm_precomp.h"
+
+
+
+/*---------------------------Define Local Constant---------------------------*/
+/*  2010/04/25 MH Define the max tx power tracking tx agc power. */
+#define		ODM_TXPWRTRACK_MAX_IDX8723B	6
+
+/*  MACRO definition for pRFCalibrateInfo->TxIQC_8723B[0] */
+#define		PATH_S0							1 /*  RF_PATH_B */
+#define		IDX_0xC94						0
+#define		IDX_0xC80						1
+#define		IDX_0xC4C						2
+#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_0xC9C						0
+#define		IDX_0xC88						1
+#define		IDX_0xC4C						2
+#define		IDX_0xC1C						0
+#define		IDX_0xC78						1
+
+
+/*---------------------------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(
+	PDM_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) && (*(pDM_Odm->pBandType) == ODM_BAND_2_4G)) {
+		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 == ODM_RF_PATH_A) */
+		switch (RFPath) {
+		case ODM_RF_PATH_A:
+			/* wirte 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 ODM_RF_PATH_B:
+			/* wirte 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 ODM_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 ODM_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;
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("TxPwrTracking path B: X = 0x%x, Y = 0x%x ele_A = 0x%x ele_C = 0x%x ele_D = 0x%x 0xeb4 = 0x%x 0xebc = 0x%x\n",
+	(u32)IqkResult_X, (u32)IqkResult_Y, (u32)ele_A, (u32)ele_C, (u32)ele_D, (u32)IqkResult_X, (u32)IqkResult_Y));
+}
+
+
+static void setCCKFilterCoefficient(PDM_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(
+	PDM_ODM_T pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+)
+{
+}
+
+/*-----------------------------------------------------------------------------
+ * 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.
+ *
+ *---------------------------------------------------------------------------*/
+void ODM_TxPwrTrackSetPwr_8723B(
+	PDM_ODM_T pDM_Odm,
+	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;
+
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("===>ODM_TxPwrTrackSetPwr8723B\n"));
+
+	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 */
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("TxRate = 0x%x, PwrTrackingLimit =%d\n", TxRate, PwrTrackingLimit_OFDM));
+
+	if (Method == TXAGC) {
+		struct adapter *Adapter = pDM_Odm->Adapter;
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("odm_TxPwrTrackSetPwr8723B CH =%d\n", *(pDM_Odm->pChannel)));
+
+		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) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			("pDM_Odm->DefaultOfdmIndex =%d,  pDM_Odm->DefaultCCKIndex =%d, pDM_Odm->Absolute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			pDM_Odm->DefaultOfdmIndex, pDM_Odm->DefaultCckIndex, pDM_Odm->Absolute_OFDMSwingIdx[RFPath], RFPath));
+
+		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);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d\n",
+				PwrTrackingLimit_OFDM, pDM_Odm->Remnant_OFDMSwingIdx[RFPath]));
+		} 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);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d\n",
+				pDM_Odm->Remnant_OFDMSwingIdx[RFPath]));
+		} 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]);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d\n", Final_OFDM_Swing_Index));
+
+			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;
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+					("******Path_A pDM_Odm->Modify_TxAGC_Flag = false\n"));
+			}
+		}
+
+		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);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Over Limit , PwrTrackingLimit_CCK = %d , pDM_Odm->Remnant_CCKSwingIdx  = %d\n", PwrTrackingLimit_CCK, pDM_Odm->Remnant_CCKSwingIdx));
+		} 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);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Under Limit , PwrTrackingLimit_CCK = %d , pDM_Odm->Remnant_CCKSwingIdx  = %d\n", 0, pDM_Odm->Remnant_CCKSwingIdx));
+		} else {
+			setCCKFilterCoefficient(pDM_Odm, Final_CCK_Swing_Index);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+				("******Path_A CCK Compensate with BBSwing , Final_CCK_Swing_Index = %d\n", 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;
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+					("******Path_A pDM_Odm->Modify_TxAGC_Flag_CCK = false\n"));
+			}
+		}
+	} else
+		return; /*  This method is not supported. */
+}
+
+static void GetDeltaSwingTable_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 **TemperatureUP_A,
+	u8 **TemperatureDOWN_A,
+	u8 **TemperatureUP_B,
+	u8 **TemperatureDOWN_B
+)
+{
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	PODM_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 if (36 <= channel && channel <= 64) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[0];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[0];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[0];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[0];
+	} else if (100 <= channel && channel <= 140) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[1];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[1];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[1];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[1];
+	} else if (149 <= channel && channel <= 173) {
+		*TemperatureUP_A   = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[2];
+		*TemperatureDOWN_A = pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[2];
+		*TemperatureUP_B   = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[2];
+		*TemperatureDOWN_B = pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[2];
+	}*/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;
+	}
+
+	return;
+}
+
+
+void ConfigureTxpowerTrack_8723B(PTXPWRTRACK_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
+#define IQK_DELAY_TIME		1		/* ms */
+
+/* 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);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	/*  Save RF Path */
+	Path_SEL_BB = PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK!\n"));
+
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+
+	/* 	enable path A PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87);
+	/* 	disable path B PA in TXIQK mode */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, bRFRegOffsetMask, 0x00020); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A IQK setting!\n")); */
+	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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_8723B)); */
+	/* 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, 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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK!\n")); */
+
+	/*  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);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A RX IQK:Get TXIMR setting\n"));
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table!\n")); */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LNA2 off, PA on for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_8723B)); */
+	/* 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe40 = 0x%x u4tmp = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord), u4tmp));
+
+
+	/* 1 RX IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A RX IQK\n"));
+
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table 2!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	/* LAN2 on, PA off for Dcut */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* PA, PAD setting */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea4 = 0x%x, 0xeac = 0x%x\n", regEA4, regEAC));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea0(before IQK) = 0x%x, 0xea8(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xea0, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xea8, bMaskDWord)));
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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;
+	else							/* if Tx not OK, ignore Rx */
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK fail!!\n"));
+	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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B IQK!\n"));
+
+	/*  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, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x20000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0003f); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xc7f87); */
+	/* 	enable path B PA in TXIQK mode */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-B IQK setting!\n")); */
+	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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path B LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* 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);
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0x948 = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord))); */
+
+
+	/*  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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+	PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, 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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Rx IQK!\n")); */
+
+	/*  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);
+
+	/* 1 Get TXIMR setting */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B RX IQK:Get TXIMR setting!\n"));
+	/* modify RXIQK mode table */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table!\n")); */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	/* open PA S1 & SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B TXIQK @ RXIQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x, 0xe9c = 0x%x\n", regE94, regE9C));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe90(before IQK) = 0x%x, 0xe98(afer IQK) = 0x%x\n",
+		PHY_QueryBBReg(pDM_Odm->Adapter, 0xe90, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xe98, bMaskDWord)));
+
+	/* Allen 20131125 */
+	tmp = (regE9C & 0x03FF0000)>>16;
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("tmp1 = 0x%x\n", tmp)); */
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("tmp2 = 0x%x\n", 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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe40 = 0x%x u4tmp = 0x%x\n", PHY_QueryBBReg(pDM_Odm->Adapter, rTx_IQK, bMaskDWord), u4tmp));
+
+	/* 1 RX IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B RX IQK\n"));
+
+	/* modify RXIQK mode table */
+	/* 20121009, Kordan> RF Mode = 3 */
+	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, bMaskH3Bytes, 0x000000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7d77);
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x0); */
+
+	/* open PA S1 & close SMIXER */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x43, bRFRegOffsetMask, 0x30ebd);
+
+	/* PA, PAD setting */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xdf, bRFRegOffsetMask, 0xf80); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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 */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n")); */
+	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, ODM_RF_PATH_A, 0xb0, bRFRegOffsetMask, 0xeffe0); */
+
+	/* GNT_BT = 0 */
+	PHY_SetBBReg(pDM_Odm->Adapter, 0x764, bMaskDWord, 0x00000800);
+
+	/* One shot, path B LOK & IQK */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path B LOK & IQK!\n")); */
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	PHY_SetBBReg(pDM_Odm->Adapter, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/*  delay x ms */
+/* 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E)); */
+	/* 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);;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regEAC));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea4 = 0x%x, 0xeac = 0x%x\n", regEA4, regEAC));
+	/* monitor image power before & after IQK */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea0(before IQK) = 0x%x, 0xea8(afer IQK) = 0x%x\n",
+		PHY_QueryBBReg(pDM_Odm->Adapter, 0xea0, bMaskDWord), PHY_QueryBBReg(pDM_Odm->Adapter, 0xea8, bMaskDWord)));
+
+	/* 	PA/PAD controlled by 0x0 */
+	/* leave IQK mode */
+/* 	PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_IQK, 0xffffff00, 0x00000000); */
+/* 	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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;
+	else
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B Rx IQK fail!!\n"));
+
+	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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A IQ Calibration %s !\n", (bIQKOK)?"Success":"Failed"));
+
+	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;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("X = 0x%x, TX0_A = 0x%x, Oldval_0 0x%x\n", X, TX0_A, Oldval_0));
+		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;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX = 0x%x\n", Y, TX0_C));
+		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) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("_PHY_PathAFillIQKMatrix8723B only Tx OK\n"));
+
+			/*  <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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQ Calibration %s !\n", (bIQKOK)?"Success":"Failed"));
+
+	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;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("X = 0x%x, TX1_A = 0x%x\n", X, TX1_A));
+
+		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;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX1_C = 0x%x\n", Y, TX1_C));
+
+		/* 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) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("_PHY_PathBFillIQKMatrix8723B only Tx OK\n"));
+
+			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(PDM_ODM_T pDM_Odm, u32 RFpath)
+{
+
+	PODM_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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	if (ODM_CheckPowerStatus(padapter) == false)
+		return;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save ADDA parameters.\n"));
+	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);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save MAC parameters.\n"));
+	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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Reload ADDA power saving parameters !\n"));
+	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 isPathAOn,
+	bool is2T
+)
+{
+	u32 pathOn;
+	u32 i;
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("ADDA ON.\n"));
+
+	pathOn = isPathAOn ? 0x01c00014 : 0x01c00014;
+	if (false == 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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("MAC settings for Calibration.\n"));
+
+	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;
+	bool is2T = true;
+	s32 tmp1 = 0, tmp2 = 0;
+
+	if (is2T)
+		bound = 8;
+	else
+		bound = 4;
+
+	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);
+	PDM_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) {
+/* 		 bbvalue = PHY_QueryBBReg(pDM_Odm->Adapter, rFPGA0_RFMOD, bMaskDWord); */
+/* 			RT_DISP(FINIT, INIT_IQK, ("phy_IQCalibrate_8188E() ==>0x%08x\n", bbvalue)); */
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2T ? "2T2R" : "1T1R"), t));
+
+		/*  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);
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2T ? "2T2R" : "1T1R"), t));
+
+	_PHY_PathADDAOn8723B(padapter, ADDA_REG, true, 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, ODM_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, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7fb7);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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[ODM_RF_PATH_A] = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0x8, bRFRegOffsetMask);
+
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Tx IQK Success!!\n"));
+				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) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK Success!!\n"));
+/* 				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;
+		} else {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK Fail!!\n"));
+		}
+	}
+
+	if (0x00 == PathAOK) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK failed!!\n"));
+	}
+
+/* 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[ODM_RF_PATH_B] = PHY_QueryRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, 0x8, bRFRegOffsetMask);
+
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Tx IQK Success!!\n"));
+				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) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B Rx IQK Success!!\n"));
+/* 				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;
+			} else {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Rx IQK Fail!!\n"));
+			}
+		}
+
+/* Allen end */
+		if (0x00 == PathBOK) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQK failed!!\n"));
+		}
+	}
+
+	/* Back to BB mode, load original value */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK:Back to BB mode, load original value!\n"));
+	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, ODM_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);
+
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("phy_IQCalibrate_8723B() <==\n"));
+
+}
+
+
+static void phy_LCCalibrate_8723B(PDM_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, ODM_RF_PATH_A, RF_AC, bMask12Bits);
+
+		/* Path-B */
+		if (is2T)
+			RF_Bmode = PHY_QueryRFReg(padapter, ODM_RF_PATH_B, RF_AC, bMask12Bits);
+
+		/* 2. Set RF mode = standby mode */
+		/* Path-A */
+		PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_AC, bMask12Bits, (RF_Amode&0x8FFFF)|0x10000);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_AC, bMask12Bits, (RF_Bmode&0x8FFFF)|0x10000);
+	}
+
+	/* 3. Read RF reg18 */
+	LC_Cal = PHY_QueryRFReg(padapter, ODM_RF_PATH_A, RF_CHNLBW, bMask12Bits);
+
+	/* 4. Set LC calibration begin	bit15 */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xB0, bRFRegOffsetMask, 0xDFBE0); /*  LDO ON */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal|0x08000);
+
+	mdelay(100);
+
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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, ODM_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, ODM_RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
+
+		/* Path-B */
+		if (is2T)
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
+	} else /*  Deal with Packet TX case */
+		rtw_write8(pDM_Odm->Adapter, REG_TXPAUSE, 0x00);
+}
+
+/* Analog Pre-distortion calibration */
+#define		APK_BB_REG_NUM	8
+#define		APK_CURVE_REG_NUM 4
+#define		PATH_NUM		2
+
+#define		DP_BB_REG_NUM		7
+#define		DP_RF_REG_NUM		1
+#define		DP_RETRY_LIMIT		10
+#define		DP_PATH_NUM	2
+#define		DP_DPK_NUM			3
+#define		DP_DPK_VALUE_NUM	2
+
+
+
+/* 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);
+
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+
+	s32 result[4][8];	/* last is final result */
+	u8 i, final_candidate, Indexforchannel;
+	bool bPathAOK, bPathBOK;
+	s32 RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC, 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;
+	u32 		StartTime;
+	s32			ProgressingTime;
+
+	if (ODM_CheckPowerStatus(padapter) == false)
+		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 DISABLE_BB_RF
+	return;
+#endif
+	if (pDM_Odm->RFCalibrateInfo.bIQKInProgress)
+		return;
+
+
+	pDM_Odm->RFCalibrateInfo.bIQKInProgress = true;
+
+	if (bRestore) {
+		u32 offset, data;
+		u8 path, bResult = SUCCESS;
+		PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+
+		path = (PHY_QueryBBReg(pDM_Odm->Adapter, rS0S1_PathSwitch, bMaskByte0) == 0x00) ? ODM_RF_PATH_A : ODM_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)) {
+				DBG_871X(
+					"%s =>path:%s Restore TX IQK result failed\n",
+					__func__,
+					(path == ODM_RF_PATH_A)?"A":"B"
+				);
+				bResult = FAIL;
+				break;
+			}
+			/* RT_TRACE(_module_mp_, _drv_notice_, ("Switch to S1 TxIQC(offset, data) = (0x%X, 0x%X)\n", offset, data)); */
+			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)) {
+				DBG_871X(
+					"%s =>path:%s  Restore RX IQK result failed\n",
+					__func__,
+					(path == ODM_RF_PATH_A)?"A":"B"
+				);
+				bResult = FAIL;
+				break;
+			}
+			/* RT_TRACE(_module_mp_, _drv_notice_, ("Switch to S1 RxIQC (offset, data) = (0x%X, 0x%X)\n", offset, data)); */
+			PHY_SetBBReg(pDM_Odm->Adapter, offset, bMaskDWord, data);
+		}
+
+		if (pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_A] == 0) {
+			DBG_871X("%s => Restore Path-A TxLOK result failed\n", __func__);
+			bResult = FAIL;
+		} else {
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_A]);
+			PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_B, RF_TXM_IDAC, bRFRegOffsetMask, pDM_Odm->RFCalibrateInfo.TxLOK[ODM_RF_PATH_B]);
+		}
+
+		if (bResult == SUCCESS)
+			return;
+	}
+
+	if (bReCovery) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD, ("PHY_IQCalibrate_8723B: Return due to bReCovery!\n"));
+		_PHY_ReloadADDARegisters8723B(padapter, IQK_BB_REG_92C, pDM_Odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+		return;
+	}
+	StartTime = jiffies;
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK:Start!!!\n"));
+
+	/* 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, ODM_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;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is12simular final_candidate is %x\n", final_candidate));
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = phy_SimularityCompare_8723B(padapter, result, 0, 2);
+			if (is13simular) {
+				final_candidate = 0;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is13simular final_candidate is %x\n", final_candidate));
+
+				break;
+			}
+
+			is23simular = phy_SimularityCompare_8723B(padapter, result, 1, 2);
+			if (is23simular) {
+				final_candidate = 1;
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is23simular final_candidate is %x\n", final_candidate));
+			} else {
+				for (i = 0; i < 8; i++)
+					RegTmp += result[3][i];
+
+				if (RegTmp != 0)
+					final_candidate = 3;
+				else
+					final_candidate = 0xFF;
+			}
+		}
+	}
+/* 	RT_TRACE(COMP_INIT, DBG_LOUD, ("Release Mutex in IQCalibrate\n")); */
+
+	for (i = 0; i < 4; i++) {
+		RegE94 = result[i][0];
+		RegE9C = result[i][1];
+		RegEA4 = result[i][2];
+		RegEAC = result[i][3];
+		RegEB4 = result[i][4];
+		RegEBC = result[i][5];
+		RegEC4 = result[i][6];
+		RegECC = result[i][7];
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: RegE94 =%x RegE9C =%x RegEA4 =%x RegEAC =%x RegEB4 =%x RegEBC =%x RegEC4 =%x RegECC =%x\n ", RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
+	}
+
+	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];
+		RegEAC = result[final_candidate][3];
+		pDM_Odm->RFCalibrateInfo.RegEB4 = RegEB4 = result[final_candidate][4];
+		pDM_Odm->RFCalibrateInfo.RegEBC = RegEBC = result[final_candidate][5];
+		RegEC4 = result[final_candidate][6];
+		RegECC = result[final_candidate][7];
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: final_candidate is %x\n", final_candidate));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: RegE94 =%x RegE9C =%x RegEA4 =%x RegEAC =%x RegEB4 =%x RegEBC =%x RegEC4 =%x RegECC =%x\n ", RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
+		bPathAOK = bPathBOK = true;
+	} else {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: FAIL use default value\n"));
+
+		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));
+	}
+
+	Indexforchannel = ODM_GetRightChnlPlaceforIQK(pHalData->CurrentChannel);
+
+/* 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[Indexforchannel].Value[0][i] = result[final_candidate][i];
+		pDM_Odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].bIQKDone = true;
+	}
+	/* RT_DISP(FINIT, INIT_IQK, ("\nIQK OK Indexforchannel %d.\n", Indexforchannel)); */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("\nIQK OK Indexforchannel %d.\n", Indexforchannel));
+
+	_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, ODM_RF_PATH_A, 0xb0, 0xfffff, Path_SEL_RF); */
+
+	/* Resotr RX mode table parameter */
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x18000);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0001f);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xe6177);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_RF_PATH_A, 0xed, 0x20, 0x1);
+	PHY_SetRFReg(pDM_Odm->Adapter, ODM_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;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK finished\n"));
+	ProgressingTime = jiffies_to_msecs(jiffies - StartTime);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK ProgressingTime = %d\n", ProgressingTime));
+
+
+}
+
+
+void PHY_LCCalibrate_8723B(PDM_ODM_T pDM_Odm)
+{
+	bool		bSingleTone = false, bCarrierSuppression = false;
+	u32 		timeout = 2000, timecount = 0;
+	u32 		StartTime;
+	s32			ProgressingTime;
+
+#if DISABLE_BB_RF
+	return;
+#endif
+
+	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;
+
+	StartTime = jiffies;
+	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;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LCK:Finish!!!interface %d\n", pDM_Odm->InterfaceIndex));
+	ProgressingTime = jiffies_to_msecs(jiffies - StartTime);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("LCK ProgressingTime = %d\n", ProgressingTime));
+}
diff --git a/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h
new file mode 100644
index 000000000000..ae4eca144606
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPhyRf_8723B.h
@@ -0,0 +1,83 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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(PTXPWRTRACK_CFG	pConfig);
+
+void DoIQK_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 DeltaThermalIndex,
+	u8 ThermalValue,
+	u8 Threshold
+);
+
+void ODM_TxPwrTrackSetPwr_8723B(
+	PDM_ODM_T pDM_Odm,
+	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(PDM_ODM_T pDM_Odm, u32 RFpath);
+
+/*  */
+/*  LC calibrate */
+/*  */
+void PHY_LCCalibrate_8723B(PDM_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 000000000000..f4619768a99f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/HalPwrSeqCmd.c
@@ -0,0 +1,205 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*++
+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,
+	WLAN_PWR_CFG PwrSeqCmd[]
+)
+{
+	WLAN_PWR_CFG PwrCfgCmd = {0};
+	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];
+
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_info_,
+			(
+				"HalPwrSeqCmdParsing: offset(%#x) cut_msk(%#x) fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x) msk(%#x) value(%#x)\n",
+				GET_PWR_CFG_OFFSET(PwrCfgCmd),
+				GET_PWR_CFG_CUT_MASK(PwrCfgCmd),
+				GET_PWR_CFG_FAB_MASK(PwrCfgCmd),
+				GET_PWR_CFG_INTF_MASK(PwrCfgCmd),
+				GET_PWR_CFG_BASE(PwrCfgCmd),
+				GET_PWR_CFG_CMD(PwrCfgCmd),
+				GET_PWR_CFG_MASK(PwrCfgCmd),
+				GET_PWR_CFG_VALUE(PwrCfgCmd)
+			)
+		);
+
+		/* 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:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_READ\n")
+				);
+				break;
+
+			case PWR_CMD_WRITE:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_WRITE\n")
+				);
+				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 sytem register */
+					rtw_write8(padapter, offset, value);
+				}
+				break;
+
+			case PWR_CMD_POLLING:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_POLLING\n")
+				);
+
+				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) {
+						DBG_871X(
+							"Fail to polling Offset[%#x]=%02x\n",
+							offset,
+							value
+						);
+						return false;
+					}
+				} while (!bPollingBit);
+
+				break;
+
+			case PWR_CMD_DELAY:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_DELAY\n")
+				);
+				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 */
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_info_,
+					("HalPwrSeqCmdParsing: PWR_CMD_END\n")
+				);
+				return true;
+
+			default:
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_err_,
+					("HalPwrSeqCmdParsing: Unknown CMD!!\n")
+				);
+				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 000000000000..248b9fb3886a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/Mp_Precomp.h
@@ -0,0 +1,33 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __MP_PRECOMP_H__
+#define __MP_PRECOMP_H__
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#define BT_TMP_BUF_SIZE	100
+
+#define DCMD_Printf			DBG_BT_INFO
+
+#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 000000000000..cc7e0903ee9d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
@@ -0,0 +1,1720 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define __HAL_BTCOEX_C__
+
+#include <hal_data.h>
+#include <rtw_debug.h>
+#include <hal_btcoex.h>
+#include <Mp_Precomp.h>
+
+/* 		Global variables */
+static const char *const BtProfileString[] = {
+	"NONE",
+	"A2DP",
+	"PAN",
+	"HID",
+	"SCO",
+};
+
+static const char *const BtSpecString[] = {
+	"1.0b",
+	"1.1",
+	"1.2",
+	"2.0+EDR",
+	"2.1+EDR",
+	"3.0+HS",
+	"4.0",
+};
+
+static const char *const BtLinkRoleString[] = {
+	"Master",
+	"Slave",
+};
+
+static const char *const h2cStaString[] = {
+	"successful",
+	"h2c busy",
+	"rf off",
+	"fw not read",
+};
+
+static const char *const ioStaString[] = {
+	"success",
+	"can not IO",
+	"rf off",
+	"fw not read",
+	"wait io timeout",
+	"invalid len",
+	"idle Q empty",
+	"insert waitQ fail",
+	"unknown fail",
+	"wrong level",
+	"h2c stopped",
+};
+
+BTC_COEXIST GLBtCoexist;
+static u8 GLBtcWiFiInScanState;
+static u8 GLBtcWiFiInIQKState;
+
+u32 GLBtcDbgType[BTC_MSG_MAX];
+static u8 GLBtcDbgBuf[BT_TMP_BUF_SIZE];
+
+typedef struct _btcoexdbginfo {
+	u8 *info;
+	u32 size; /*  buffer total size */
+	u32 len; /*  now used length */
+} BTCDBGINFO, *PBTCDBGINFO;
+
+static BTCDBGINFO GLBtcDbgInfo;
+
+#define	BT_Operation(Adapter)						false
+
+static void DBG_BT_INFO_INIT(PBTCDBGINFO pinfo, u8 *pbuf, u32 size)
+{
+	if (NULL == pinfo)
+		return;
+
+	memset(pinfo, 0, sizeof(BTCDBGINFO));
+
+	if (pbuf && size) {
+		pinfo->info = pbuf;
+		pinfo->size = size;
+	}
+}
+
+void DBG_BT_INFO(u8 *dbgmsg)
+{
+	PBTCDBGINFO pinfo;
+	u32 msglen;
+	u8 *pbuf;
+
+
+	pinfo = &GLBtcDbgInfo;
+
+	if (NULL == pinfo->info)
+		return;
+
+	msglen = strlen(dbgmsg);
+	if (pinfo->len + msglen > pinfo->size)
+		return;
+
+	pbuf = pinfo->info + pinfo->len;
+	memcpy(pbuf, dbgmsg, msglen);
+	pinfo->len += msglen;
+}
+
+/*  */
+/* 		Debug related function */
+/*  */
+static u8 halbtcoutsrc_IsBtCoexistAvailable(PBTC_COEXIST pBtCoexist)
+{
+	if (!pBtCoexist->bBinded ||
+		NULL == pBtCoexist->Adapter){
+		return false;
+	}
+	return true;
+}
+
+static void halbtcoutsrc_DbgInit(void)
+{
+	u8 i;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		GLBtcDbgType[i] = 0;
+
+	GLBtcDbgType[BTC_MSG_INTERFACE]			=	\
+/* 			INTF_INIT								| */
+/* 			INTF_NOTIFY							| */
+			0;
+
+	GLBtcDbgType[BTC_MSG_ALGORITHM]			=	\
+/* 			ALGO_BT_RSSI_STATE					| */
+/* 			ALGO_WIFI_RSSI_STATE					| */
+/* 			ALGO_BT_MONITOR						| */
+/* 			ALGO_TRACE							| */
+/* 			ALGO_TRACE_FW						| */
+/* 			ALGO_TRACE_FW_DETAIL				| */
+/* 			ALGO_TRACE_FW_EXEC					| */
+/* 			ALGO_TRACE_SW						| */
+/* 			ALGO_TRACE_SW_DETAIL				| */
+/* 			ALGO_TRACE_SW_EXEC					| */
+			0;
+}
+
+static void halbtcoutsrc_LeaveLps(PBTC_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(PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Normal LPS behavior!!!\n"));
+
+	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(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	s32 ready;
+	unsigned long stime;
+	unsigned long utime;
+	u32 timeout; /*  unit: ms */
+
+
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	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(PBTC_COEXIST pBtCoexist)
+{
+	struct adapter *padapter;
+
+
+	padapter = pBtCoexist->Adapter;
+	rtw_unregister_task_alive(padapter, BTCOEX_ALIVE);
+}
+
+static void halbtcoutsrc_DisableLowPower(PBTC_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(PBTC_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 (true == 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 (true == bp2p)
+				portConnectedStatus |= WIFI_P2P_GO_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_AP_CONNECTED;
+		} else {
+			if (true == bp2p)
+				portConnectedStatus |= WIFI_P2P_GC_CONNECTED;
+			else
+				portConnectedStatus |= WIFI_STA_CONNECTED;
+		}
+	}
+
+	return portConnectedStatus;
+}
+
+static u32 halbtcoutsrc_GetWifiLinkStatus(PBTC_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(PBTC_COEXIST pBtCoexist)
+{
+	return pBtCoexist->btInfo.btRealFwVer;
+}
+
+static s32 halbtcoutsrc_GetWifiRssi(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	s32 UndecoratedSmoothedPWDB = 0;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	UndecoratedSmoothedPWDB = pHalData->dmpriv.EntryMinUndecoratedSmoothedPWDB;
+
+	return UndecoratedSmoothedPWDB;
+}
+
+static u8 halbtcoutsrc_GetWifiScanAPNum(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext;
+	static u8 scan_AP_num = 0;
+
+	pmlmeext = &padapter->mlmeextpriv;
+
+	if (GLBtcWiFiInScanState == false) {
+		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)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	struct mlme_ext_priv *mlmeext;
+	u8 *pu8;
+	s32 *pS4Tmp;
+	u32 *pU4Tmp;
+	u8 *pU1Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return false;
+
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	mlmeext = &padapter->mlmeextpriv;
+	pu8 = (u8 *)pOutBuf;
+	pS4Tmp = (s32 *)pOutBuf;
+	pU4Tmp = (u32 *)pOutBuf;
+	pU1Tmp = (u8 *)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_UNDER_5G:
+		*pu8 = (pHalData->CurrentBandType == 1) ? true : 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 ? false : true;
+		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 (IsLegacyOnly(mlmeext->cur_wireless_mode))
+			*pU4Tmp = BTC_WIFI_BW_LEGACY;
+		else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_20)
+			*pU4Tmp = BTC_WIFI_BW_HT20;
+		else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40)
+			*pU4Tmp = BTC_WIFI_BW_HT40;
+		else
+			*pU4Tmp = BTC_WIFI_BW_HT40; /* todo */
+		break;
+
+	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
+		{
+			PRT_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:
+		*pU1Tmp = padapter->mlmeextpriv.cur_channel;
+		break;
+
+	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
+		*pU1Tmp = pHalData->CurrentChannel;
+		break;
+
+	case BTC_GET_U1_WIFI_HS_CHNL:
+		*pU1Tmp = 0;
+		ret = false;
+		break;
+
+	case BTC_GET_U1_MAC_PHY_MODE:
+		*pU1Tmp = BTC_SMSP;
+/* 			*pU1Tmp = BTC_DMSP; */
+/* 			*pU1Tmp = BTC_DMDP; */
+/* 			*pU1Tmp = BTC_MP_UNKNOWN; */
+		break;
+
+	case BTC_GET_U1_AP_NUM:
+		*pU1Tmp = halbtcoutsrc_GetWifiScanAPNum(padapter);
+		break;
+
+	/* 1Ant =========== */
+	case BTC_GET_U1_LPS_MODE:
+		*pU1Tmp = padapter->dvobj->pwrctl_priv.pwr_mode;
+		break;
+
+	default:
+		ret = false;
+		break;
+	}
+
+	return ret;
+}
+
+static u8 halbtcoutsrc_Set(void *pBtcContext, u8 setType, void *pInBuf)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	u8 *pu8;
+	u8 *pU1Tmp;
+	u32 *pU4Tmp;
+	u8 ret;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+	pHalData = GET_HAL_DATA(padapter);
+	pu8 = (u8 *)pInBuf;
+	pU1Tmp = (u8 *)pInBuf;
+	pU4Tmp = (u32 *)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 = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_AGG_BUF_SIZE:
+		pBtCoexist->btInfo.aggBufSize = *pU1Tmp;
+		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 = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_LPS_VAL:
+		pBtCoexist->btInfo.lpsVal = *pU1Tmp;
+		break;
+
+	case BTC_SET_U1_RPWM_VAL:
+		pBtCoexist->btInfo.rpwmVal = *pU1Tmp;
+		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->MacAddress);
+			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;
+}
+
+static void halbtcoutsrc_DisplayFwPwrModeCmd(PBTC_COEXIST pBtCoexist)
+{
+	u8 *cliBuf = pBtCoexist->cliBuf;
+
+	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x ", "Power mode cmd ", \
+		pBtCoexist->pwrModeVal[0], pBtCoexist->pwrModeVal[1],
+		pBtCoexist->pwrModeVal[2], pBtCoexist->pwrModeVal[3],
+		pBtCoexist->pwrModeVal[4], pBtCoexist->pwrModeVal[5]);
+	CL_PRINTF(cliBuf);
+}
+
+/*  */
+/* 		IO related function */
+/*  */
+static u8 halbtcoutsrc_Read1Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return rtw_read8(padapter, RegAddr);
+}
+
+static u16 halbtcoutsrc_Read2Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read16(padapter, RegAddr);
+}
+
+static u32 halbtcoutsrc_Read4Byte(void *pBtcContext, u32 RegAddr)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return	rtw_read32(padapter, RegAddr);
+}
+
+static void halbtcoutsrc_Write1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write8(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_BitMaskWrite1Byte(void *pBtcContext, u32 regAddr, u8 bitMask, u8 data1b)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+	u8 originalValue, bitShift;
+	u8 i;
+
+
+	pBtCoexist = (PBTC_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)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write16(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_Write4Byte(void *pBtcContext, u32 RegAddr, u32 Data)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_write32(padapter, RegAddr, Data);
+}
+
+static void halbtcoutsrc_WriteLocalReg1Byte(void *pBtcContext, u32 RegAddr, u8 Data)
+{
+	PBTC_COEXIST		pBtCoexist = (PBTC_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)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetBBReg(padapter, RegAddr, BitMask, Data);
+}
+
+
+static u32 halbtcoutsrc_GetBbReg(void *pBtcContext, u32 RegAddr, u32 BitMask)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_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)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	PHY_SetRFReg(padapter, eRFPath, RegAddr, BitMask, Data);
+}
+
+static u32 halbtcoutsrc_GetRfReg(void *pBtcContext, u8 eRFPath, u32 RegAddr, u32 BitMask)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	return PHY_QueryRFReg(padapter, eRFPath, RegAddr, BitMask);
+}
+
+static void halbtcoutsrc_SetBtReg(void *pBtcContext, u8 RegType, u32 RegAddr, u32 Data)
+{
+	PBTC_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 = (PBTC_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)
+{
+	PBTC_COEXIST pBtCoexist;
+	struct adapter *padapter;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	padapter = pBtCoexist->Adapter;
+
+	rtw_hal_fill_h2c_cmd(padapter, elementId, cmdLen, pCmdBuffer);
+}
+
+static void halbtcoutsrc_DisplayDbgMsg(void *pBtcContext, u8 dispType)
+{
+	PBTC_COEXIST pBtCoexist;
+
+
+	pBtCoexist = (PBTC_COEXIST)pBtcContext;
+	switch (dispType) {
+	case BTC_DBG_DISP_COEX_STATISTICS:
+		break;
+	case BTC_DBG_DISP_BT_LINK_INFO:
+		break;
+	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
+		halbtcoutsrc_DisplayFwPwrModeCmd(pBtCoexist);
+		break;
+	default:
+		break;
+	}
+}
+
+/*  */
+/* 		Extern functions called by other module */
+/*  */
+static u8 EXhalbtcoutsrc_BindBtCoexWithAdapter(void *padapter)
+{
+	PBTC_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;
+}
+
+u8 EXhalbtcoutsrc_InitlizeVariables(void *padapter)
+{
+	PBTC_COEXIST pBtCoexist = &GLBtCoexist;
+
+	/* pBtCoexist->statistics.cntBind++; */
+
+	halbtcoutsrc_DbgInit();
+
+	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->fBtcDispDbgMsg = halbtcoutsrc_DisplayDbgMsg;
+
+	pBtCoexist->fBtcGet = halbtcoutsrc_Get;
+	pBtCoexist->fBtcSet = halbtcoutsrc_Set;
+	pBtCoexist->fBtcGetBtReg = halbtcoutsrc_GetBtReg;
+	pBtCoexist->fBtcSetBtReg = halbtcoutsrc_SetBtReg;
+
+	pBtCoexist->cliBuf = &GLBtcDbgBuf[0];
+
+	pBtCoexist->boardInfo.singleAntPath = 0;
+
+	GLBtcWiFiInScanState = false;
+
+	GLBtcWiFiInIQKState = false;
+
+	return true;
+}
+
+void EXhalbtcoutsrc_PowerOnSetting(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_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(PBTC_COEXIST pBtCoexist, 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(PBTC_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{
+		packetType = BTC_PACKET_UNKNOWN;
+		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(PBTC_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(PBTC_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(PBTC_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(PBTC_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_SetChipType(u8 chipType)
+{
+	GLBtCoexist.boardInfo.btChipType = BTC_CHIP_RTL8723B;
+}
+
+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;
+}
+
+void EXhalbtcoutsrc_DisplayBtCoexInfo(PBTC_COEXIST pBtCoexist)
+{
+	if (!halbtcoutsrc_IsBtCoexistAvailable(pBtCoexist))
+		return;
+
+	halbtcoutsrc_LeaveLowPower(pBtCoexist);
+
+	if (pBtCoexist->boardInfo.btdmAntNum == 2)
+		EXhalbtc8723b2ant_DisplayCoexInfo(pBtCoexist);
+	else if (pBtCoexist->boardInfo.btdmAntNum == 1)
+		EXhalbtc8723b1ant_DisplayCoexInfo(pBtCoexist);
+
+	halbtcoutsrc_NormalLowPower(pBtCoexist);
+}
+
+/*
+ * Description:
+ *Run BT-Coexist mechansim 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
+ */
+u8 hal_btcoex_IsBtExist(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	return pHalData->bt_coexist.bBtExist;
+}
+
+u8 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);
+	pHalData->bt_coexist.btChipType = chipType;
+
+	EXhalbtcoutsrc_SetChipType(chipType);
+}
+
+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);
+}
+
+u8 hal_btcoex_Initialize(struct adapter *padapter)
+{
+	u8 ret1;
+	u8 ret2;
+
+
+	memset(&GLBtCoexist, 0, sizeof(GLBtCoexist));
+	ret1 = EXhalbtcoutsrc_InitlizeVariables((void *)padapter);
+	ret2 = (ret1 == true) ? true : false;
+
+	return ret2;
+}
+
+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 == true)
+		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_Hanlder(struct adapter *padapter)
+{
+	EXhalbtcoutsrc_Periodical(&GLBtCoexist);
+}
+
+s32 hal_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter)
+{
+	return (s32)GLBtCoexist.btInfo.bBtCtrlAggBufSize;
+}
+
+void hal_btcoex_SetManualControl(struct adapter *padapter, u8 bmanual)
+{
+	GLBtCoexist.bManualControl = bmanual;
+}
+
+u8 hal_btcoex_IsBtControlLps(struct adapter *padapter)
+{
+	if (hal_btcoex_IsBtExist(padapter) == false)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtDisabled)
+		return false;
+
+	if (GLBtCoexist.btInfo.bBtCtrlLps)
+		return true;
+
+	return false;
+}
+
+u8 hal_btcoex_IsLpsOn(struct adapter *padapter)
+{
+	if (hal_btcoex_IsBtExist(padapter) == false)
+		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)
+{
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write pwrModeCmd = 0x%04x%08x\n",
+		pCmdBuf[0]<<8|pCmdBuf[1],
+		pCmdBuf[2]<<24|pCmdBuf[3]<<16|pCmdBuf[4]<<8|pCmdBuf[5]));
+
+	memcpy(GLBtCoexist.pwrModeVal, pCmdBuf, cmdLen);
+}
+
+void hal_btcoex_DisplayBtCoexInfo(struct adapter *padapter, u8 *pbuf, u32 bufsize)
+{
+	PBTCDBGINFO pinfo;
+
+
+	pinfo = &GLBtcDbgInfo;
+	DBG_BT_INFO_INIT(pinfo, pbuf, bufsize);
+	EXhalbtcoutsrc_DisplayBtCoexInfo(&GLBtCoexist);
+	DBG_BT_INFO_INIT(pinfo, NULL, 0);
+}
+
+void hal_btcoex_SetDBG(struct adapter *padapter, u32 *pDbgModule)
+{
+	u32 i;
+
+
+	if (NULL == pDbgModule)
+		return;
+
+	for (i = 0; i < BTC_MSG_MAX; i++)
+		GLBtcDbgType[i] = pDbgModule[i];
+}
+
+u32 hal_btcoex_GetDBG(struct adapter *padapter, u8 *pStrBuf, u32 bufSize)
+{
+	s32 count;
+	u8 *pstr;
+	u32 leftSize;
+
+
+	if ((NULL == pStrBuf) || (0 == bufSize))
+		return 0;
+
+	pstr = pStrBuf;
+	leftSize = bufSize;
+/* 	DBG_871X(FUNC_ADPT_FMT ": bufsize =%d\n", FUNC_ADPT_ARG(padapter), bufSize); */
+
+	count = rtw_sprintf(pstr, leftSize, "#define DBG\t%d\n", DBG);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "BTCOEX Debug Setting:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize,
+		"INTERFACE / ALGORITHM: 0x%08X / 0x%08X\n\n",
+		GLBtcDbgType[BTC_MSG_INTERFACE],
+		GLBtcDbgType[BTC_MSG_ALGORITHM]);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "INTERFACE Debug Setting Definition:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[0]=%d for INTF_INIT\n",
+		(GLBtcDbgType[BTC_MSG_INTERFACE]&INTF_INIT)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[2]=%d for INTF_NOTIFY\n\n",
+		(GLBtcDbgType[BTC_MSG_INTERFACE]&INTF_NOTIFY)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+	count = rtw_sprintf(pstr, leftSize, "ALGORITHM Debug Setting Definition:\n");
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[0]=%d for BT_RSSI_STATE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_BT_RSSI_STATE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[1]=%d for WIFI_RSSI_STATE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_WIFI_RSSI_STATE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[2]=%d for BT_MONITOR\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_BT_MONITOR)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[3]=%d for TRACE\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[4]=%d for TRACE_FW\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[5]=%d for TRACE_FW_DETAIL\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW_DETAIL)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[6]=%d for TRACE_FW_EXEC\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_FW_EXEC)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[7]=%d for TRACE_SW\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[8]=%d for TRACE_SW_DETAIL\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW_DETAIL)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+	count = rtw_sprintf(pstr, leftSize, "\tbit[9]=%d for TRACE_SW_EXEC\n",
+		(GLBtcDbgType[BTC_MSG_ALGORITHM]&ALGO_TRACE_SW_EXEC)?1:0);
+	if ((count < 0) || (count >= leftSize))
+		goto exit;
+	pstr += count;
+	leftSize -= count;
+
+exit:
+	count = pstr - pStrBuf;
+/* 	DBG_871X(FUNC_ADPT_FMT ": usedsize =%d\n", FUNC_ADPT_ARG(padapter), count); */
+
+	return count;
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_com.c b/drivers/staging/rtl8723bs/hal/hal_com.c
new file mode 100644
index 000000000000..1880d4140bee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_com.c
@@ -0,0 +1,1751 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HAL_COM_C_
+
+#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 == NULL) {
+			DBG_8192C("cant not alloc memory for HAL DATA\n");
+			return _FAIL;
+		}
+	}
+	return _SUCCESS;
+}
+
+void rtw_hal_data_deinit(struct adapter *padapter)
+{
+	if (is_primary_adapter(padapter)) {	/* if (padapter->isprimary) */
+		if (padapter->HalData) {
+			phy_free_filebuf(padapter);
+			vfree(padapter->HalData);
+			padapter->HalData = NULL;
+			padapter->hal_data_sz = 0;
+		}
+	}
+}
+
+
+void dump_chip_info(HAL_VERSION	ChipVersion)
+{
+	int cnt = 0;
+	u8 buf[128];
+
+	cnt += sprintf((buf+cnt), "Chip Version Info: CHIP_8723B_");
+	cnt += sprintf((buf+cnt), "%s_", IS_NORMAL_CHIP(ChipVersion) ? "Normal_Chip" : "Test_Chip");
+	if (IS_CHIP_VENDOR_TSMC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "TSMC");
+	else if (IS_CHIP_VENDOR_UMC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "UMC");
+	else if (IS_CHIP_VENDOR_SMIC(ChipVersion))
+		cnt += sprintf((buf+cnt), "%s_", "SMIC");
+
+	if (IS_A_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "A_CUT_");
+	else if (IS_B_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "B_CUT_");
+	else if (IS_C_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "C_CUT_");
+	else if (IS_D_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "D_CUT_");
+	else if (IS_E_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "E_CUT_");
+	else if (IS_I_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "I_CUT_");
+	else if (IS_J_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "J_CUT_");
+	else if (IS_K_CUT(ChipVersion))
+		cnt += sprintf((buf+cnt), "K_CUT_");
+	else
+		cnt += sprintf((buf+cnt), "UNKNOWN_CUT(%d)_", ChipVersion.CUTVersion);
+
+	if (IS_1T1R(ChipVersion))
+		cnt += sprintf((buf+cnt), "1T1R_");
+	else if (IS_1T2R(ChipVersion))
+		cnt += sprintf((buf+cnt), "1T2R_");
+	else if (IS_2T2R(ChipVersion))
+		cnt += sprintf((buf+cnt), "2T2R_");
+	else
+		cnt += sprintf((buf+cnt), "UNKNOWN_RFTYPE(%d)_", ChipVersion.RFType);
+
+	cnt += sprintf((buf+cnt), "RomVer(%d)\n", ChipVersion.ROMVer);
+
+	DBG_871X("%s", buf);
+}
+
+
+#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 (false == AutoLoadFail) {
+		u8 hw_chnlPlan;
+
+		hw_chnlPlan = hw_channel_plan & (~EEPROM_CHANNEL_PLAN_BY_HW_MASK);
+		if (rtw_is_channel_plan_valid(hw_chnlPlan)) {
+#ifndef CONFIG_SW_CHANNEL_PLAN
+			if (hw_channel_plan & EEPROM_CHANNEL_PLAN_BY_HW_MASK)
+				pHalData->bDisableSWChannelPlan = true;
+#endif /*  !CONFIG_SW_CHANNEL_PLAN */
+
+			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) {
+		bLegalChannel = false;
+		DBG_871X("Channel > 14 but wireless_mode do not support 5G\n");
+	} else if ((Channel <= 14) && (Channel >= 1)) {
+		if (IsSupported24G(Adapter->registrypriv.wireless_mode) == false) {
+			bLegalChannel = false;
+			DBG_871X("(Channel <= 14) && (Channel >= 1) but wireless_mode do not support 2.4G\n");
+		}
+	} else {
+		bLegalChannel = false;
+		DBG_871X("Channel is Invalid !!!\n");
+	}
+
+	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;
+	case MGN_MCS8:
+		ret = DESC_RATEMCS8;
+		break;
+	case MGN_MCS9:
+		ret = DESC_RATEMCS9;
+		break;
+	case MGN_MCS10:
+		ret = DESC_RATEMCS10;
+		break;
+	case MGN_MCS11:
+		ret = DESC_RATEMCS11;
+		break;
+	case MGN_MCS12:
+		ret = DESC_RATEMCS12;
+		break;
+	case MGN_MCS13:
+		ret = DESC_RATEMCS13;
+		break;
+	case MGN_MCS14:
+		ret = DESC_RATEMCS14;
+		break;
+	case MGN_MCS15:
+		ret = DESC_RATEMCS15;
+		break;
+	case MGN_MCS16:
+		ret = DESC_RATEMCS16;
+		break;
+	case MGN_MCS17:
+		ret = DESC_RATEMCS17;
+		break;
+	case MGN_MCS18:
+		ret = DESC_RATEMCS18;
+		break;
+	case MGN_MCS19:
+		ret = DESC_RATEMCS19;
+		break;
+	case MGN_MCS20:
+		ret = DESC_RATEMCS20;
+		break;
+	case MGN_MCS21:
+		ret = DESC_RATEMCS21;
+		break;
+	case MGN_MCS22:
+		ret = DESC_RATEMCS22;
+		break;
+	case MGN_MCS23:
+		ret = DESC_RATEMCS23;
+		break;
+	case MGN_MCS24:
+		ret = DESC_RATEMCS24;
+		break;
+	case MGN_MCS25:
+		ret = DESC_RATEMCS25;
+		break;
+	case MGN_MCS26:
+		ret = DESC_RATEMCS26;
+		break;
+	case MGN_MCS27:
+		ret = DESC_RATEMCS27;
+		break;
+	case MGN_MCS28:
+		ret = DESC_RATEMCS28;
+		break;
+	case MGN_MCS29:
+		ret = DESC_RATEMCS29;
+		break;
+	case MGN_MCS30:
+		ret = DESC_RATEMCS30;
+		break;
+	case MGN_MCS31:
+		ret = DESC_RATEMCS31;
+		break;
+	case MGN_VHT1SS_MCS0:
+		ret = DESC_RATEVHTSS1MCS0;
+		break;
+	case MGN_VHT1SS_MCS1:
+		ret = DESC_RATEVHTSS1MCS1;
+		break;
+	case MGN_VHT1SS_MCS2:
+		ret = DESC_RATEVHTSS1MCS2;
+		break;
+	case MGN_VHT1SS_MCS3:
+		ret = DESC_RATEVHTSS1MCS3;
+		break;
+	case MGN_VHT1SS_MCS4:
+		ret = DESC_RATEVHTSS1MCS4;
+		break;
+	case MGN_VHT1SS_MCS5:
+		ret = DESC_RATEVHTSS1MCS5;
+		break;
+	case MGN_VHT1SS_MCS6:
+		ret = DESC_RATEVHTSS1MCS6;
+		break;
+	case MGN_VHT1SS_MCS7:
+		ret = DESC_RATEVHTSS1MCS7;
+		break;
+	case MGN_VHT1SS_MCS8:
+		ret = DESC_RATEVHTSS1MCS8;
+		break;
+	case MGN_VHT1SS_MCS9:
+		ret = DESC_RATEVHTSS1MCS9;
+		break;
+	case MGN_VHT2SS_MCS0:
+		ret = DESC_RATEVHTSS2MCS0;
+		break;
+	case MGN_VHT2SS_MCS1:
+		ret = DESC_RATEVHTSS2MCS1;
+		break;
+	case MGN_VHT2SS_MCS2:
+		ret = DESC_RATEVHTSS2MCS2;
+		break;
+	case MGN_VHT2SS_MCS3:
+		ret = DESC_RATEVHTSS2MCS3;
+		break;
+	case MGN_VHT2SS_MCS4:
+		ret = DESC_RATEVHTSS2MCS4;
+		break;
+	case MGN_VHT2SS_MCS5:
+		ret = DESC_RATEVHTSS2MCS5;
+		break;
+	case MGN_VHT2SS_MCS6:
+		ret = DESC_RATEVHTSS2MCS6;
+		break;
+	case MGN_VHT2SS_MCS7:
+		ret = DESC_RATEVHTSS2MCS7;
+		break;
+	case MGN_VHT2SS_MCS8:
+		ret = DESC_RATEVHTSS2MCS8;
+		break;
+	case MGN_VHT2SS_MCS9:
+		ret = DESC_RATEVHTSS2MCS9;
+		break;
+	case MGN_VHT3SS_MCS0:
+		ret = DESC_RATEVHTSS3MCS0;
+		break;
+	case MGN_VHT3SS_MCS1:
+		ret = DESC_RATEVHTSS3MCS1;
+		break;
+	case MGN_VHT3SS_MCS2:
+		ret = DESC_RATEVHTSS3MCS2;
+		break;
+	case MGN_VHT3SS_MCS3:
+		ret = DESC_RATEVHTSS3MCS3;
+		break;
+	case MGN_VHT3SS_MCS4:
+		ret = DESC_RATEVHTSS3MCS4;
+		break;
+	case MGN_VHT3SS_MCS5:
+		ret = DESC_RATEVHTSS3MCS5;
+		break;
+	case MGN_VHT3SS_MCS6:
+		ret = DESC_RATEVHTSS3MCS6;
+		break;
+	case MGN_VHT3SS_MCS7:
+		ret = DESC_RATEVHTSS3MCS7;
+		break;
+	case MGN_VHT3SS_MCS8:
+		ret = DESC_RATEVHTSS3MCS8;
+		break;
+	case MGN_VHT3SS_MCS9:
+		ret = DESC_RATEVHTSS3MCS9;
+		break;
+	case MGN_VHT4SS_MCS0:
+		ret = DESC_RATEVHTSS4MCS0;
+		break;
+	case MGN_VHT4SS_MCS1:
+		ret = DESC_RATEVHTSS4MCS1;
+		break;
+	case MGN_VHT4SS_MCS2:
+		ret = DESC_RATEVHTSS4MCS2;
+		break;
+	case MGN_VHT4SS_MCS3:
+		ret = DESC_RATEVHTSS4MCS3;
+		break;
+	case MGN_VHT4SS_MCS4:
+		ret = DESC_RATEVHTSS4MCS4;
+		break;
+	case MGN_VHT4SS_MCS5:
+		ret = DESC_RATEVHTSS4MCS5;
+		break;
+	case MGN_VHT4SS_MCS6:
+		ret = DESC_RATEVHTSS4MCS6;
+		break;
+	case MGN_VHT4SS_MCS7:
+		ret = DESC_RATEVHTSS4MCS7;
+		break;
+	case MGN_VHT4SS_MCS8:
+		ret = DESC_RATEVHTSS4MCS8;
+		break;
+	case MGN_VHT4SS_MCS9:
+		ret = DESC_RATEVHTSS4MCS9;
+		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;
+	case DESC_RATEMCS8:
+		ret_rate = MGN_MCS8;
+		break;
+	case DESC_RATEMCS9:
+		ret_rate = MGN_MCS9;
+		break;
+	case DESC_RATEMCS10:
+		ret_rate = MGN_MCS10;
+		break;
+	case DESC_RATEMCS11:
+		ret_rate = MGN_MCS11;
+		break;
+	case DESC_RATEMCS12:
+		ret_rate = MGN_MCS12;
+		break;
+	case DESC_RATEMCS13:
+		ret_rate = MGN_MCS13;
+		break;
+	case DESC_RATEMCS14:
+		ret_rate = MGN_MCS14;
+		break;
+	case DESC_RATEMCS15:
+		ret_rate = MGN_MCS15;
+		break;
+	case DESC_RATEMCS16:
+		ret_rate = MGN_MCS16;
+		break;
+	case DESC_RATEMCS17:
+		ret_rate = MGN_MCS17;
+		break;
+	case DESC_RATEMCS18:
+		ret_rate = MGN_MCS18;
+		break;
+	case DESC_RATEMCS19:
+		ret_rate = MGN_MCS19;
+		break;
+	case DESC_RATEMCS20:
+		ret_rate = MGN_MCS20;
+		break;
+	case DESC_RATEMCS21:
+		ret_rate = MGN_MCS21;
+		break;
+	case DESC_RATEMCS22:
+		ret_rate = MGN_MCS22;
+		break;
+	case DESC_RATEMCS23:
+		ret_rate = MGN_MCS23;
+		break;
+	case DESC_RATEMCS24:
+		ret_rate = MGN_MCS24;
+		break;
+	case DESC_RATEMCS25:
+		ret_rate = MGN_MCS25;
+		break;
+	case DESC_RATEMCS26:
+		ret_rate = MGN_MCS26;
+		break;
+	case DESC_RATEMCS27:
+		ret_rate = MGN_MCS27;
+		break;
+	case DESC_RATEMCS28:
+		ret_rate = MGN_MCS28;
+		break;
+	case DESC_RATEMCS29:
+		ret_rate = MGN_MCS29;
+		break;
+	case DESC_RATEMCS30:
+		ret_rate = MGN_MCS30;
+		break;
+	case DESC_RATEMCS31:
+		ret_rate = MGN_MCS31;
+		break;
+	case DESC_RATEVHTSS1MCS0:
+		ret_rate = MGN_VHT1SS_MCS0;
+		break;
+	case DESC_RATEVHTSS1MCS1:
+		ret_rate = MGN_VHT1SS_MCS1;
+		break;
+	case DESC_RATEVHTSS1MCS2:
+		ret_rate = MGN_VHT1SS_MCS2;
+		break;
+	case DESC_RATEVHTSS1MCS3:
+		ret_rate = MGN_VHT1SS_MCS3;
+		break;
+	case DESC_RATEVHTSS1MCS4:
+		ret_rate = MGN_VHT1SS_MCS4;
+		break;
+	case DESC_RATEVHTSS1MCS5:
+		ret_rate = MGN_VHT1SS_MCS5;
+		break;
+	case DESC_RATEVHTSS1MCS6:
+		ret_rate = MGN_VHT1SS_MCS6;
+		break;
+	case DESC_RATEVHTSS1MCS7:
+		ret_rate = MGN_VHT1SS_MCS7;
+		break;
+	case DESC_RATEVHTSS1MCS8:
+		ret_rate = MGN_VHT1SS_MCS8;
+		break;
+	case DESC_RATEVHTSS1MCS9:
+		ret_rate = MGN_VHT1SS_MCS9;
+		break;
+	case DESC_RATEVHTSS2MCS0:
+		ret_rate = MGN_VHT2SS_MCS0;
+		break;
+	case DESC_RATEVHTSS2MCS1:
+		ret_rate = MGN_VHT2SS_MCS1;
+		break;
+	case DESC_RATEVHTSS2MCS2:
+		ret_rate = MGN_VHT2SS_MCS2;
+		break;
+	case DESC_RATEVHTSS2MCS3:
+		ret_rate = MGN_VHT2SS_MCS3;
+		break;
+	case DESC_RATEVHTSS2MCS4:
+		ret_rate = MGN_VHT2SS_MCS4;
+		break;
+	case DESC_RATEVHTSS2MCS5:
+		ret_rate = MGN_VHT2SS_MCS5;
+		break;
+	case DESC_RATEVHTSS2MCS6:
+		ret_rate = MGN_VHT2SS_MCS6;
+		break;
+	case DESC_RATEVHTSS2MCS7:
+		ret_rate = MGN_VHT2SS_MCS7;
+		break;
+	case DESC_RATEVHTSS2MCS8:
+		ret_rate = MGN_VHT2SS_MCS8;
+		break;
+	case DESC_RATEVHTSS2MCS9:
+		ret_rate = MGN_VHT2SS_MCS9;
+		break;
+	case DESC_RATEVHTSS3MCS0:
+		ret_rate = MGN_VHT3SS_MCS0;
+		break;
+	case DESC_RATEVHTSS3MCS1:
+		ret_rate = MGN_VHT3SS_MCS1;
+		break;
+	case DESC_RATEVHTSS3MCS2:
+		ret_rate = MGN_VHT3SS_MCS2;
+		break;
+	case DESC_RATEVHTSS3MCS3:
+		ret_rate = MGN_VHT3SS_MCS3;
+		break;
+	case DESC_RATEVHTSS3MCS4:
+		ret_rate = MGN_VHT3SS_MCS4;
+		break;
+	case DESC_RATEVHTSS3MCS5:
+		ret_rate = MGN_VHT3SS_MCS5;
+		break;
+	case DESC_RATEVHTSS3MCS6:
+		ret_rate = MGN_VHT3SS_MCS6;
+		break;
+	case DESC_RATEVHTSS3MCS7:
+		ret_rate = MGN_VHT3SS_MCS7;
+		break;
+	case DESC_RATEVHTSS3MCS8:
+		ret_rate = MGN_VHT3SS_MCS8;
+		break;
+	case DESC_RATEVHTSS3MCS9:
+		ret_rate = MGN_VHT3SS_MCS9;
+		break;
+	case DESC_RATEVHTSS4MCS0:
+		ret_rate = MGN_VHT4SS_MCS0;
+		break;
+	case DESC_RATEVHTSS4MCS1:
+		ret_rate = MGN_VHT4SS_MCS1;
+		break;
+	case DESC_RATEVHTSS4MCS2:
+		ret_rate = MGN_VHT4SS_MCS2;
+		break;
+	case DESC_RATEVHTSS4MCS3:
+		ret_rate = MGN_VHT4SS_MCS3;
+		break;
+	case DESC_RATEVHTSS4MCS4:
+		ret_rate = MGN_VHT4SS_MCS4;
+		break;
+	case DESC_RATEVHTSS4MCS5:
+		ret_rate = MGN_VHT4SS_MCS5;
+		break;
+	case DESC_RATEVHTSS4MCS6:
+		ret_rate = MGN_VHT4SS_MCS6;
+		break;
+	case DESC_RATEVHTSS4MCS7:
+		ret_rate = MGN_VHT4SS_MCS7;
+		break;
+	case DESC_RATEVHTSS4MCS8:
+		ret_rate = MGN_VHT4SS_MCS8;
+		break;
+	case DESC_RATEVHTSS4MCS9:
+		ret_rate = MGN_VHT4SS_MCS9;
+		break;
+
+	default:
+		DBG_871X("HwRateToMRate(): Non supported Rate [%x]!!!\n", rate);
+		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 == NULL)
+		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);
+
+	RT_PRINT_DATA(
+		_module_hal_init_c_,
+		_drv_info_,
+		"c2h_evt_read(): ",
+		&c2h_evt,
+		sizeof(c2h_evt)
+	);
+
+	DBG_871X(
+		"%s id:%u, len:%u, seq:%u, trigger:0x%02x\n",
+		__func__,
+		c2h_evt->id,
+		c2h_evt->plen,
+		c2h_evt->seq,
+		trigger
+	);
+
+	/* Read the content */
+	for (i = 0; i < c2h_evt->plen; i++)
+		c2h_evt->payload[i] = rtw_read8(adapter, REG_C2HEVT_MSG_NORMAL + 2 + i);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_info_, "c2h_evt_read(): Command Content:\n",
+		c2h_evt->payload, c2h_evt->plen);
+
+	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_hal_networktype_to_raid(struct adapter *adapter, struct sta_info *psta)
+{
+	return networktype_to_raid_ex(adapter, psta);
+}
+
+u8 rtw_get_mgntframe_raid(struct adapter *adapter, unsigned char network_type)
+{
+
+	u8 raid;
+	raid = (network_type & WIRELESS_11B) ? RATEID_IDX_B : RATEID_IDX_G;
+	return raid;
+}
+
+void rtw_hal_update_sta_rate_mask(struct adapter *padapter, struct sta_info *psta)
+{
+	u8 i, rf_type, limit;
+	u32 tx_ra_bitmap;
+
+	if (psta == NULL)
+		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) {
+		rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+		if (rf_type == RF_2T2R)
+			limit = 16; /*  2R */
+		else
+			limit = 8; /*   1R */
+
+		for (i = 0; i < limit; i++) {
+			if (psta->htpriv.ht_cap.supp_mcs_set[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);
+	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:
+		DBG_871X_LEVEL(
+			_drv_always_,
+			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);
+	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;
+	case HW_VAR_RF_TYPE:
+		*((u8 *)val) = hal_data->rf_type;
+		break;
+	default:
+		DBG_871X_LEVEL(
+			_drv_always_,
+			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);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+	u8 bResult = _SUCCESS;
+
+	switch (variable) {
+	case HW_DEF_FA_CNT_DUMP:
+		/* ODM_COMP_COMMON */
+		if (*((u8 *)value))
+			odm->DebugComponents |= (ODM_COMP_DIG | ODM_COMP_FA_CNT);
+		else
+			odm->DebugComponents &= ~(ODM_COMP_DIG | ODM_COMP_FA_CNT);
+		break;
+	case HAL_DEF_DBG_RX_INFO_DUMP:
+		DBG_871X("============ Rx Info dump ===================\n");
+		DBG_871X("bLinked = %d, RSSI_Min = %d(%%)\n",
+			odm->bLinked, odm->RSSI_Min);
+
+		if (odm->bLinked) {
+			DBG_871X("RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+				HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B);
+
+			#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;
+			DBG_8192C("==> Disable all dynamic function...\n");
+		} else if (dm_func == 1) {/* disable DIG */
+			odm->SupportAbility  &= (~DYNAMIC_BB_DIG);
+			DBG_8192C("==> Disable DIG...\n");
+		} 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);
+			DBG_8192C("==> Disable tx power tracking...\n");
+		} 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)) {
+				DIG_T	*pDigTable = &odm->DM_DigTable;
+				pDigTable->CurIGValue = rtw_read8(adapter, 0xc50);
+			}
+			dm->DMFlag |= DYNAMIC_FUNC_BT;
+			odm->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
+			DBG_8192C("==> Turn on all dynamic function...\n");
+		}
+	}
+		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:
+		DBG_871X_LEVEL(_drv_always_, "%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);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+	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.MacAddress);
+			if (psta)
+				*((int *)value) = psta->rssi_stat.UndecoratedSmoothedPWDB;
+		}
+		break;
+	case HW_DEF_ODM_DBG_FLAG:
+		*((u64 *)value) = odm->DebugComponents;
+		break;
+	case HW_DEF_ODM_DBG_LEVEL:
+		*((u32 *)value) = odm->DebugLevel;
+		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:
+		DBG_871X_LEVEL(_drv_always_, "%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) {
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+	case HAL_ODM_NOISE_MONITOR:
+		{
+			struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+			u8 chan = *(u8 *)pValue1;
+			*(s16 *)pValue2 = pHalData->noise[chan];
+			#ifdef DBG_NOISE_MONITOR
+			DBG_8192C("### Noise monitor chan(%d)-noise:%d (dBm) ###\n",
+				chan, pHalData->noise[chan]);
+			#endif
+
+		}
+		break;
+#endif/* ifdef CONFIG_BACKGROUND_NOISE_MONITOR */
+	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);
+	PDM_ODM_T podmpriv = &pHalData->odmpriv;
+	/* _irqL irqL; */
+	switch (eVariable) {
+	case HAL_ODM_STA_INFO:
+		{
+			struct sta_info *psta = (struct sta_info *)pValue1;
+			if (bSet) {
+				DBG_8192C("### Set STA_(%d) info ###\n", psta->mac_id);
+				ODM_CmnInfoPtrArrayHook(podmpriv, ODM_CMNINFO_STA_STATUS, psta->mac_id, psta);
+			} else {
+				DBG_8192C("### Clean STA_(%d) info ###\n", psta->mac_id);
+				/* 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;
+	#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+	case HAL_ODM_NOISE_MONITOR:
+		{
+			struct noise_info *pinfo = (struct noise_info *)pValue1;
+
+			#ifdef DBG_NOISE_MONITOR
+			DBG_8192C("### Noise monitor chan(%d)-bPauseDIG:%d, IGIValue:0x%02x, max_time:%d (ms) ###\n",
+				pinfo->chan, pinfo->bPauseDIG, pinfo->IGIValue, pinfo->max_time);
+			#endif
+
+			pHalData->noise[pinfo->chan] = ODM_InbandNoise_Monitor(podmpriv, pinfo->bPauseDIG, pinfo->IGIValue, pinfo->max_time);
+			DBG_871X("chan_%d, noise = %d (dBm)\n", pinfo->chan, pHalData->noise[pinfo->chan]);
+			#ifdef DBG_NOISE_MONITOR
+			DBG_871X("noise_a = %d, noise_b = %d  noise_all:%d\n",
+				podmpriv->noise_level.noise[ODM_RF_PATH_A],
+				podmpriv->noise_level.noise[ODM_RF_PATH_B],
+				podmpriv->noise_level.noise_all);
+			#endif
+		}
+		break;
+	#endif/* ifdef CONFIG_BACKGROUND_NOISE_MONITOR */
+
+	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: */
+/* 		Return true if chTmp is represent for hex digit and */
+/* 		false otherwise. */
+/*  */
+/*  */
+bool IsHexDigit(char chTmp)
+{
+	if (
+		(chTmp >= '0' && chTmp <= '9') ||
+		(chTmp >= 'a' && chTmp <= 'f') ||
+		(chTmp >= 'A' && chTmp <= 'F')
+	)
+		return true;
+	else
+		return false;
+}
+
+
+/*  */
+/* 	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;
+}
+
+
+
+/* 	Description: */
+/* 		Parse hex number from the string pucStr. */
+bool GetHexValueFromString(char *szStr, u32 *pu4bVal, u32 *pu4bMove)
+{
+	char *szScan = szStr;
+
+	/*  Check input parameter. */
+	if (szStr == NULL || pu4bVal == NULL || pu4bMove == NULL) {
+		DBG_871X("GetHexValueFromString(): Invalid inpur argumetns! szStr: %p, pu4bVal: %p, pu4bMove: %p\n", szStr, pu4bVal, pu4bMove);
+		return false;
+	}
+
+	/*  Initialize output. */
+	*pu4bMove = 0;
+	*pu4bVal = 0;
+
+	/*  Skip leading space. */
+	while (*szScan != '\0' && (*szScan == ' ' || *szScan == '\t')) {
+		szScan++;
+		(*pu4bMove)++;
+	}
+
+	/*  Skip leading '0x' or '0X'. */
+	if (*szScan == '0' && (*(szScan+1) == 'x' || *(szScan+1) == 'X')) {
+		szScan += 2;
+		(*pu4bMove) += 2;
+	}
+
+	/*  Check if szScan is now pointer to a character for hex digit, */
+	/*  if not, it means this is not a valid hex number. */
+	if (!IsHexDigit(*szScan))
+		return false;
+
+	/*  Parse each digit. */
+	do {
+		(*pu4bVal) <<= 4;
+		*pu4bVal += MapCharToHexDigit(*szScan);
+
+		szScan++;
+		(*pu4bMove)++;
+	} while (IsHexDigit(*szScan));
+
+	return true;
+}
+
+bool GetFractionValueFromString(
+	char *szStr, u8 *pInteger, u8 *pFraction, u32 *pu4bMove
+)
+{
+	char *szScan = szStr;
+
+	/*  Initialize output. */
+	*pu4bMove = 0;
+	*pInteger = 0;
+	*pFraction = 0;
+
+	/*  Skip leading space. */
+	while (*szScan != '\0' &&	(*szScan == ' ' || *szScan == '\t')) {
+		++szScan;
+		++(*pu4bMove);
+	}
+
+	/*  Parse each digit. */
+	do {
+		(*pInteger) *= 10;
+		*pInteger += (*szScan - '0');
+
+		++szScan;
+		++(*pu4bMove);
+
+		if (*szScan == '.') {
+			++szScan;
+			++(*pu4bMove);
+
+			if (*szScan < '0' || *szScan > '9')
+				return false;
+			else {
+				*pFraction = *szScan - '0';
+				++szScan;
+				++(*pu4bMove);
+				return true;
+			}
+		}
+	} while (*szScan >= '0' && *szScan <= '9');
+
+	return true;
+}
+
+/*  */
+/* 	Description: */
+/* 		Return true if szStr is comment out with leading "//". */
+/*  */
+bool IsCommentString(char *szStr)
+{
+	if (*szStr == '/' && *(szStr+1) == '/')
+		return true;
+	else
+		return false;
+}
+
+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;
+
+	DBG_871X("%s %s\n", __func__, (benable) ? "enable" : "disable");
+
+	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;
+
+	DBG_871X_SEL_NL(
+		sel,
+		"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) ? true : false;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_singal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		DBG_871X_SEL_NL(
+			sel,
+			"RF_PATH_%d =>singal_strength:%d(%%), singal_quality:%d(%%)\n",
+			rf_path, psample_pkt_rssi->mimo_singal_strength[rf_path],
+			psample_pkt_rssi->mimo_singal_quality[rf_path]
+		);
+
+		if (!isCCKrate) {
+			DBG_871X_SEL_NL(
+				sel,
+				"\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;
+	DBG_871X("============ RAW Rx Info dump ===================\n");
+	DBG_871X("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) ? true : false;
+
+	if (isCCKrate)
+		psample_pkt_rssi->mimo_singal_strength[0] = psample_pkt_rssi->pwdball;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		DBG_871X("RF_PATH_%d =>singal_strength:%d(%%), singal_quality:%d(%%)"
+			, rf_path, psample_pkt_rssi->mimo_singal_strength[rf_path], psample_pkt_rssi->mimo_singal_quality[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;
+
+	PODM_PHY_INFO_T 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) ? true : false;
+
+	psample_pkt_rssi->pwdball = pPhyInfo->RxPWDBAll;
+	psample_pkt_rssi->pwr_all = pPhyInfo->RecvSignalPower;
+
+	for (rf_path = 0; rf_path < pHalData->NumTotalRFPath; rf_path++) {
+		psample_pkt_rssi->mimo_singal_strength[rf_path] = pPhyInfo->RxMIMOSignalStrength[rf_path];
+		psample_pkt_rssi->mimo_singal_quality[rf_path] = pPhyInfo->RxMIMOSignalQuality[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 ArrayLen = sizeof(Array_kfreemap)/sizeof(u32);
+	u32 *Array = Array_kfreemap;
+	u32 v1 = 0, v2 = 0, target = 0;
+	/* DBG_871X("+%s value: 0x%02x+\n", __func__, value); */
+
+	if (value & BIT4) {
+		DBG_871X("Offset RF Gain.\n");
+		DBG_871X("Offset RF Gain.  padapter->eeprompriv.EEPROMRFGainVal = 0x%x\n", padapter->eeprompriv.EEPROMRFGainVal);
+		if (padapter->eeprompriv.EEPROMRFGainVal != 0xff) {
+			res = rtw_hal_read_rfreg(padapter, RF_PATH_A, 0x7f, 0xffffffff);
+			res &= 0xfff87fff;
+			DBG_871X("Offset RF Gain. before reg 0x7f = 0x%08x\n", res);
+			/* res &= 0xfff87fff; */
+			for (i = 0; i < ArrayLen; i += 2) {
+				v1 = Array[i];
+				v2 = Array[i+1];
+				if (v1 == padapter->eeprompriv.EEPROMRFGainVal) {
+					DBG_871X("Offset RF Gain. got v1 = 0x%x , v2 = 0x%x\n", v1, v2);
+					target = v2;
+					break;
+				}
+			}
+			DBG_871X("padapter->eeprompriv.EEPROMRFGainVal = 0x%x , Gain offset Target Value = 0x%x\n", padapter->eeprompriv.EEPROMRFGainVal, target);
+			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);
+			DBG_871X("Offset RF Gain. After reg 0x7f = 0x%08x\n", res);
+		} else
+			DBG_871X("Offset RF Gain.  padapter->eeprompriv.EEPROMRFGainVal = 0x%x	!= 0xff, didn't run Kfree\n", padapter->eeprompriv.EEPROMRFGainVal);
+	} else
+		DBG_871X("Using the default RF gain.\n");
+}
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 000000000000..566b6f0997da
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
@@ -0,0 +1,3286 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HAL_COM_PHYCFG_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <hal_data.h>
+
+u8 PHY_GetTxPowerByRateBase(struct adapter *Adapter, u8 Band, u8 RfPath,
+			    u8 TxNum, enum RATE_SECTION RateSection)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8	value = 0;
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n", RfPath);
+		return 0;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		switch (RateSection) {
+		case CCK:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][0];
+			break;
+		case OFDM:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][3];
+			break;
+		case HT_MCS16_MCS23:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][4];
+			break;
+		case HT_MCS24_MCS31:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][5];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][6];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][7];
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][8];
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			value = pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][9];
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+
+		};
+	} else if (Band == BAND_ON_5G) {
+		switch (RateSection) {
+		case OFDM:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][0];
+			break;
+		case HT_MCS0_MCS7:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][1];
+			break;
+		case HT_MCS8_MCS15:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][2];
+			break;
+		case HT_MCS16_MCS23:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][3];
+			break;
+		case HT_MCS24_MCS31:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][4];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][5];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][6];
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][7];
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			value = pHalData->TxPwrByRateBase5G[RfPath][TxNum][8];
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else
+		DBG_871X("Invalid Band %d in PHY_GetTxPowerByRateBase()\n", Band);
+
+	return value;
+}
+
+static void
+phy_SetTxPowerByRateBase(
+	struct adapter *Adapter,
+	u8 Band,
+	u8 RfPath,
+	enum RATE_SECTION	RateSection,
+	u8 TxNum,
+	u8 Value
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n", RfPath);
+		return;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		switch (RateSection) {
+		case CCK:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][0] = Value;
+			break;
+		case OFDM:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][1] = Value;
+			break;
+		case HT_MCS0_MCS7:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][2] = Value;
+			break;
+		case HT_MCS8_MCS15:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][3] = Value;
+			break;
+		case HT_MCS16_MCS23:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][4] = Value;
+			break;
+		case HT_MCS24_MCS31:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][5] = Value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][6] = Value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][7] = Value;
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][8] = Value;
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			pHalData->TxPwrByRateBase2_4G[RfPath][TxNum][9] = Value;
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in phy_SetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else if (Band == BAND_ON_5G) {
+		switch (RateSection) {
+		case OFDM:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][0] = Value;
+			break;
+		case HT_MCS0_MCS7:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][1] = Value;
+			break;
+		case HT_MCS8_MCS15:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][2] = Value;
+			break;
+		case HT_MCS16_MCS23:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][3] = Value;
+			break;
+		case HT_MCS24_MCS31:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][4] = Value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][5] = Value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][6] = Value;
+			break;
+		case VHT_3SSMCS0_3SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][7] = Value;
+			break;
+		case VHT_4SSMCS0_4SSMCS9:
+			pHalData->TxPwrByRateBase5G[RfPath][TxNum][8] = Value;
+			break;
+		default:
+			DBG_871X("Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in phy_SetTxPowerByRateBase()\n",
+					 RateSection, RfPath, TxNum);
+			break;
+		};
+	} else
+		DBG_871X("Invalid Band %d in phy_SetTxPowerByRateBase()\n", Band);
+}
+
+static void
+phy_StoreTxPowerByRateBase(
+struct adapter *padapter
+	)
+{
+	u8 path, base;
+
+	/* DBG_871X("===>%s\n", __func__); */
+
+	for (path = ODM_RF_PATH_A; path <= ODM_RF_PATH_B; ++path) {
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_11M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, CCK, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx CCK = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_54M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, OFDM, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx OFDM = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS0_MCS7, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx MCS0-7 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_2TX, MGN_MCS15);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS8_MCS15, RF_2TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 2Tx MCS8-15 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_3TX, MGN_MCS23);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, HT_MCS16_MCS23, RF_3TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 3Tx MCS16-23 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_1TX, MGN_VHT1SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 1Tx VHT1SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_2TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 2Tx VHT2SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, path, RF_3TX, MGN_VHT3SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_2_4G, path, VHT_3SSMCS0_3SSMCS9, RF_3TX, base);
+		/* DBG_871X("Power index base of 2.4G path %d 3Tx VHT3SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_54M);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, OFDM, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx OFDM = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS0_MCS7, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx MCS0~7 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_2TX, MGN_MCS15);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS8_MCS15, RF_2TX, base);
+		/* DBG_871X("Power index base of 5G path %d 2Tx MCS8~15 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_3TX, MGN_MCS23);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, HT_MCS16_MCS23, RF_3TX, base);
+		/* DBG_871X("Power index base of 5G path %d 3Tx MCS16~23 = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_1TX, MGN_VHT1SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+		/* DBG_871X("Power index base of 5G path %d 1Tx VHT1SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_2TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+		/* DBG_871X("Power index base of 5G path %d 2Tx VHT2SS = > 0x%x\n", path, base); */
+
+		base = PHY_GetTxPowerByRate(padapter, BAND_ON_5G, path, RF_3TX, MGN_VHT2SS_MCS7);
+		phy_SetTxPowerByRateBase(padapter, BAND_ON_5G, path, VHT_3SSMCS0_3SSMCS9, RF_3TX, base);
+		/* DBG_871X("Power index base of 5G path %d 3Tx VHT3SS = > 0x%x\n", path, base); */
+	}
+
+	/* DBG_871X("<===%s\n", __func__); */
+}
+
+u8 PHY_GetRateSectionIndexOfTxPowerByRate(
+	struct adapter *padapter, u32 RegAddr, u32 BitMask
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_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_A_Mcs11_Mcs08:
+			index = 4;
+			break;
+		case rTxAGC_A_Mcs15_Mcs12:
+			index = 5;
+			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;
+		case rTxAGC_B_Mcs11_Mcs08:
+			index = 12;
+			break;
+		case rTxAGC_B_Mcs15_Mcs12:
+			index = 13;
+			break;
+		default:
+			DBG_871X("Invalid RegAddr 0x3%x in PHY_GetRateSectionIndexOfTxPowerByRate()", RegAddr);
+			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_A_Mcs11_Mcs08:
+	case rTxAGC_B_Mcs11_Mcs08:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS10);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS11);
+		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_Mcs15_Mcs12:
+	case rTxAGC_B_Mcs15_Mcs12:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS12);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS13);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS14);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS15);
+		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;
+
+	case 0xC34:
+	case 0xE34:
+	case 0x1834:
+	case 0x1a34:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS10);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS11);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC38:
+	case 0xE38:
+	case 0x1838:
+	case 0x1a38:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS12);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS13);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS14);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS15);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC3C:
+	case 0xE3C:
+	case 0x183C:
+	case 0x1a3C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC40:
+	case 0xE40:
+	case 0x1840:
+	case 0x1a40:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC44:
+	case 0xE44:
+	case 0x1844:
+	case 0x1a44:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT1SS_MCS9);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS0);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS1);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC48:
+	case 0xE48:
+	case 0x1848:
+	case 0x1a48:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS2);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS3);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS4);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS5);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xC4C:
+	case 0xE4C:
+	case 0x184C:
+	case 0x1a4C:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS6);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS7);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS8);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS9);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCD8:
+	case 0xED8:
+	case 0x18D8:
+	case 0x1aD8:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS16);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS17);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS18);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS19);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCDC:
+	case 0xEDC:
+	case 0x18DC:
+	case 0x1aDC:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS20);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS21);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS22);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_MCS23);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE0:
+	case 0xEE0:
+	case 0x18E0:
+	case 0x1aE0:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS0);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS1);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS2);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS3);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE4:
+	case 0xEE4:
+	case 0x18E4:
+	case 0x1aE4:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS4);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS5);
+		RateIndex[2] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS6);
+		RateIndex[3] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS7);
+		for (i = 0; i < 4; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	case 0xCE8:
+	case 0xEE8:
+	case 0x18E8:
+	case 0x1aE8:
+		RateIndex[0] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS8);
+		RateIndex[1] = PHY_GetRateIndexOfTxPowerByRate(MGN_VHT3SS_MCS9);
+		for (i = 0; i < 2; ++i) {
+			PwrByRateVal[i] = (s8) ((((Value >> (i * 8 + 4)) & 0xF)) * 10 +
+											((Value >> (i * 8)) & 0xF));
+		}
+		*RateNum = 4;
+		break;
+
+	default:
+		DBG_871X("Invalid RegAddr 0x%x in %s()\n", RegAddr, __func__);
+		break;
+	};
+}
+
+static void PHY_StoreTxPowerByRateNew(
+	struct adapter *padapter,
+	u32	Band,
+	u32	RfPath,
+	u32	TxNum,
+	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 (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid Band %d\n", Band);
+		return;
+	}
+
+	if (RfPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d\n", RfPath);
+		return;
+	}
+
+	if (TxNum > ODM_RF_PATH_D) {
+		DBG_871X("Invalid TxNum %d\n", TxNum);
+		return;
+	}
+
+	for (i = 0; i < rateNum; ++i) {
+		if (rateIndex[i] == PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS0) ||
+			 rateIndex[i] == PHY_GetRateIndexOfTxPowerByRate(MGN_VHT2SS_MCS1))
+			TxNum = RF_2TX;
+
+		pHalData->TxPwrByRateOffset[Band][RfPath][TxNum][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;
+	/* DBG_871X("MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n", pHalData->pwrGroupCnt, */
+	/*	pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][0]); */
+}
+
+void PHY_InitTxPowerByRate(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 band, rfPath, TxNum, rate;
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
+			for (rfPath = 0; rfPath < TX_PWR_BY_RATE_NUM_RF; ++rfPath)
+				for (TxNum = 0; TxNum < TX_PWR_BY_RATE_NUM_RF; ++TxNum)
+					for (rate = 0; rate < TX_PWR_BY_RATE_NUM_RATE; ++rate)
+						pHalData->TxPwrByRateOffset[band][rfPath][TxNum][rate] = 0;
+}
+
+void PHY_StoreTxPowerByRate(
+	struct adapter *padapter,
+	u32	Band,
+	u32	RfPath,
+	u32	TxNum,
+	u32	RegAddr,
+	u32	BitMask,
+	u32	Data
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	PDM_ODM_T		pDM_Odm = &pHalData->odmpriv;
+
+	if (pDM_Odm->PhyRegPgVersion > 0)
+		PHY_StoreTxPowerByRateNew(padapter, Band, RfPath, TxNum, RegAddr, BitMask, Data);
+	else if (pDM_Odm->PhyRegPgVersion == 0) {
+		PHY_StoreTxPowerByRateOld(padapter, RegAddr, BitMask, Data);
+
+		if (RegAddr == rTxAGC_A_Mcs15_Mcs12 && pHalData->rf_type == RF_1T1R)
+			pHalData->pwrGroupCnt++;
+		else if (RegAddr == rTxAGC_B_Mcs15_Mcs12 && pHalData->rf_type != RF_1T1R)
+			pHalData->pwrGroupCnt++;
+	} else
+		DBG_871X("Invalid PHY_REG_PG.txt version %d\n",  pDM_Odm->PhyRegPgVersion);
+
+}
+
+static void
+phy_ConvertTxPowerByRateInDbmToRelativeValues(
+struct adapter *padapter
+	)
+{
+	u8	base = 0, i = 0, value = 0, band = 0, path = 0, txNum = 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
+	};
+	u8 mcs8_15Rates[8] = {
+		MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15
+	};
+	u8 mcs16_23Rates[8] = {
+		MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23
+	};
+	u8 vht1ssRates[10] = {
+		MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4,
+		MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9
+	};
+	u8 vht2ssRates[10] = {
+		MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4,
+		MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9
+	};
+	u8 vht3ssRates[10] = {
+		MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4,
+		MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9
+	};
+
+	/* DBG_871X("===>PHY_ConvertTxPowerByRateInDbmToRelativeValues()\n"); */
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band) {
+		for (path = ODM_RF_PATH_A; path <= ODM_RF_PATH_D; ++path) {
+			for (txNum = RF_1TX; txNum < RF_MAX_TX_NUM; ++txNum) {
+				/*  CCK */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_11M);
+				for (i = 0; i < sizeof(cckRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, cckRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, cckRates[i], value - base);
+				}
+
+				/*  OFDM */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_54M);
+				for (i = 0; i < sizeof(ofdmRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, ofdmRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, ofdmRates[i], value - base);
+				}
+
+				/*  HT MCS0~7 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS7);
+				for (i = 0; i < sizeof(mcs0_7Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs0_7Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs0_7Rates[i], value - base);
+				}
+
+				/*  HT MCS8~15 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS15);
+				for (i = 0; i < sizeof(mcs8_15Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs8_15Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs8_15Rates[i], value - base);
+				}
+
+				/*  HT MCS16~23 */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_MCS23);
+				for (i = 0; i < sizeof(mcs16_23Rates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, mcs16_23Rates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, mcs16_23Rates[i], value - base);
+				}
+
+				/*  VHT 1SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT1SS_MCS7);
+				for (i = 0; i < sizeof(vht1ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht1ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht1ssRates[i], value - base);
+				}
+
+				/*  VHT 2SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT2SS_MCS7);
+				for (i = 0; i < sizeof(vht2ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht2ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht2ssRates[i], value - base);
+				}
+
+				/*  VHT 3SS */
+				base = PHY_GetTxPowerByRate(padapter, band, path, txNum, MGN_VHT3SS_MCS7);
+				for (i = 0; i < sizeof(vht3ssRates); ++i) {
+					value = PHY_GetTxPowerByRate(padapter, band, path, txNum, vht3ssRates[i]);
+					PHY_SetTxPowerByRate(padapter, band, path, txNum, vht3ssRates[i], value - base);
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("<===PHY_ConvertTxPowerByRateInDbmToRelativeValues()\n"); */
+}
+
+/*
+  * 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};
+		if (pHalData->CurrentBandType == BAND_ON_2_4G)
+			PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									  cckRates, sizeof(cckRates)/sizeof(u8));
+
+	} 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, sizeof(ofdmRates)/sizeof(u8));
+
+	} 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, sizeof(htRates1T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS8_MCS15) {
+		u8 htRates2T[]  = {MGN_MCS8, MGN_MCS9, MGN_MCS10, MGN_MCS11, MGN_MCS12, MGN_MCS13, MGN_MCS14, MGN_MCS15};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates2T, sizeof(htRates2T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS16_MCS23) {
+		u8 htRates3T[]  = {MGN_MCS16, MGN_MCS17, MGN_MCS18, MGN_MCS19, MGN_MCS20, MGN_MCS21, MGN_MCS22, MGN_MCS23};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates3T, sizeof(htRates3T)/sizeof(u8));
+
+	} else if (RateSection == HT_MCS24_MCS31) {
+		u8 htRates4T[]  = {MGN_MCS24, MGN_MCS25, MGN_MCS26, MGN_MCS27, MGN_MCS28, MGN_MCS29, MGN_MCS30, MGN_MCS31};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									 htRates4T, sizeof(htRates4T)/sizeof(u8));
+
+	} else if (RateSection == VHT_1SSMCS0_1SSMCS9) {
+		u8 vhtRates1T[] = {MGN_VHT1SS_MCS0, MGN_VHT1SS_MCS1, MGN_VHT1SS_MCS2, MGN_VHT1SS_MCS3, MGN_VHT1SS_MCS4,
+				MGN_VHT1SS_MCS5, MGN_VHT1SS_MCS6, MGN_VHT1SS_MCS7, MGN_VHT1SS_MCS8, MGN_VHT1SS_MCS9};
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+									vhtRates1T, sizeof(vhtRates1T)/sizeof(u8));
+
+	} else if (RateSection == VHT_2SSMCS0_2SSMCS9) {
+		u8 vhtRates2T[] = {MGN_VHT2SS_MCS0, MGN_VHT2SS_MCS1, MGN_VHT2SS_MCS2, MGN_VHT2SS_MCS3, MGN_VHT2SS_MCS4,
+				MGN_VHT2SS_MCS5, MGN_VHT2SS_MCS6, MGN_VHT2SS_MCS7, MGN_VHT2SS_MCS8, MGN_VHT2SS_MCS9};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates2T, sizeof(vhtRates2T)/sizeof(u8));
+	} else if (RateSection == VHT_3SSMCS0_3SSMCS9) {
+		u8 vhtRates3T[] = {MGN_VHT3SS_MCS0, MGN_VHT3SS_MCS1, MGN_VHT3SS_MCS2, MGN_VHT3SS_MCS3, MGN_VHT3SS_MCS4,
+				MGN_VHT3SS_MCS5, MGN_VHT3SS_MCS6, MGN_VHT3SS_MCS7, MGN_VHT3SS_MCS8, MGN_VHT3SS_MCS9};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates3T, sizeof(vhtRates3T)/sizeof(u8));
+	} else if (RateSection == VHT_4SSMCS0_4SSMCS9) {
+		u8 vhtRates4T[] = {MGN_VHT4SS_MCS0, MGN_VHT4SS_MCS1, MGN_VHT4SS_MCS2, MGN_VHT4SS_MCS3, MGN_VHT4SS_MCS4,
+				MGN_VHT4SS_MCS5, MGN_VHT4SS_MCS6, MGN_VHT4SS_MCS7, MGN_VHT4SS_MCS8, MGN_VHT4SS_MCS9};
+
+		PHY_SetTxPowerIndexByRateArray(padapter, RFPath, pHalData->CurrentChannelBW, Channel,
+								  vhtRates4T, sizeof(vhtRates4T)/sizeof(u8));
+	} else
+		DBG_871X("Invalid RateSection %d in %s", RateSection, __func__);
+}
+
+static bool phy_GetChnlIndex(u8 Channel, u8 *ChannelIdx)
+{
+	u8 channel5G[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 100, 102,
+		104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128, 130,
+		132, 134, 136, 138, 140, 142, 144, 149, 151, 153, 155, 157, 159, 161,
+		163, 165, 167, 168, 169, 171, 173, 175, 177
+	};
+	u8  i = 0;
+	bool bIn24G = true;
+
+	if (Channel <= 14) {
+		bIn24G = true;
+		*ChannelIdx = Channel-1;
+	} else {
+		bIn24G = false;
+
+		for (i = 0; i < sizeof(channel5G)/sizeof(u8); ++i) {
+			if (channel5G[i] == Channel) {
+				*ChannelIdx = i;
+				return bIn24G;
+			}
+		}
+	}
+
+	return bIn24G;
+}
+
+u8 PHY_GetTxPowerIndexBase(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum CHANNEL_WIDTH	BandWidth,
+	u8 Channel,
+	bool *bIn24G
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	u8 i = 0;	/* default set to 1S */
+	u8 txPower = 0;
+	u8 chnlIdx = (Channel-1);
+
+	if (HAL_IsLegalChannel(padapter, Channel) == false) {
+		chnlIdx = 0;
+		DBG_871X("Illegal channel!!\n");
+	}
+
+	*bIn24G = phy_GetChnlIndex(Channel, &chnlIdx);
+
+	/* DBG_871X("[%s] Channel Index: %d\n", (*bIn24G?"2.4G":"5G"), chnlIdx); */
+
+	if (*bIn24G) { /* 3 ============================== 2.4 G ============================== */
+		if (IS_CCK_RATE(Rate))
+			txPower = pHalData->Index24G_CCK_Base[RFPath][chnlIdx];
+		else if (MGN_6M <= Rate)
+			txPower = pHalData->Index24G_BW40_Base[RFPath][chnlIdx];
+		else
+			DBG_871X("PHY_GetTxPowerIndexBase: INVALID Rate.\n");
+
+		/* DBG_871X("Base Tx power(RF-%c, Rate #%d, Channel Index %d) = 0x%X\n", */
+		/*		((RFPath == 0)?'A':'B'), Rate, chnlIdx, txPower); */
+
+		/*  OFDM-1T */
+		if ((MGN_6M <= Rate && Rate <= MGN_54M) && !IS_CCK_RATE(Rate)) {
+			txPower += pHalData->OFDM_24G_Diff[RFPath][TX_1S];
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (OFDM-1T) = (%d)\n", ((RFPath == 0)?'A':'B'), pHalData->OFDM_24G_Diff[RFPath][TX_1S]); */
+		}
+		if (BandWidth == CHANNEL_WIDTH_20) { /*  BW20-1S, BW20-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW20-1S, BW20-2S, BW20-3S, BW20-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW20_24G_Diff[RFPath][TX_1S], pHalData->BW20_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW20_24G_Diff[RFPath][TX_3S], pHalData->BW20_24G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_40) { /*  BW40-1S, BW40-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW40-1S, BW40-2S, BW40-3S, BW40-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_1S], pHalData->BW40_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_3S], pHalData->BW40_24G_Diff[RFPath][TX_4S]); */
+		}
+		/*  Willis suggest adopt BW 40M power index while in BW 80 mode */
+		else if (BandWidth == CHANNEL_WIDTH_80) {
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_24G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 2.4G (RF-%c): (BW40-1S, BW40-2S, BW40-3S, BW40-4T) = (%d, %d, %d, %d) P.S. Current is in BW 80MHz\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_1S], pHalData->BW40_24G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_24G_Diff[RFPath][TX_3S], pHalData->BW40_24G_Diff[RFPath][TX_4S]); */
+		}
+	} else {/* 3 ============================== 5 G ============================== */
+		if (MGN_6M <= Rate)
+			txPower = pHalData->Index5G_BW40_Base[RFPath][chnlIdx];
+		else
+			DBG_871X("===> mpt_ProQueryCalTxPower_Jaguar: INVALID Rate.\n");
+
+		/* DBG_871X("Base Tx power(RF-%c, Rate #%d, Channel Index %d) = 0x%X\n", */
+		/*	((RFPath == 0)?'A':'B'), Rate, chnlIdx, txPower); */
+
+		/*  OFDM-1T */
+		if ((MGN_6M <= Rate && Rate <= MGN_54M) && !IS_CCK_RATE(Rate)) {
+			txPower += pHalData->OFDM_5G_Diff[RFPath][TX_1S];
+			/* DBG_871X("+PowerDiff 5G (RF-%c): (OFDM-1T) = (%d)\n", ((RFPath == 0)?'A':'B'), pHalData->OFDM_5G_Diff[RFPath][TX_1S]); */
+		}
+
+		/*  BW20-1S, BW20-2S */
+		if (BandWidth == CHANNEL_WIDTH_20) {
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW20_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G (RF-%c): (BW20-1S, BW20-2S, BW20-3S, BW20-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW20_5G_Diff[RFPath][TX_1S], pHalData->BW20_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW20_5G_Diff[RFPath][TX_3S], pHalData->BW20_5G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_40) { /*  BW40-1S, BW40-2S */
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS24 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW40_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G(RF-%c): (BW40-1S, BW40-2S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW40_5G_Diff[RFPath][TX_1S], pHalData->BW40_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW40_5G_Diff[RFPath][TX_3S], pHalData->BW40_5G_Diff[RFPath][TX_4S]); */
+		} else if (BandWidth == CHANNEL_WIDTH_80) { /*  BW80-1S, BW80-2S */
+			/*  <20121220, Kordan> Get the index of array "Index5G_BW80_Base". */
+			u8 channel5G_80M[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+			for (i = 0; i < sizeof(channel5G_80M)/sizeof(u8); ++i)
+				if (channel5G_80M[i] == Channel)
+					chnlIdx = i;
+
+			txPower = pHalData->Index5G_BW80_Base[RFPath][chnlIdx];
+
+			if ((MGN_MCS0 <= Rate && Rate <= MGN_MCS31)  || (MGN_VHT1SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += + pHalData->BW80_5G_Diff[RFPath][TX_1S];
+			if ((MGN_MCS8 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT2SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_2S];
+			if ((MGN_MCS16 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT3SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_3S];
+			if ((MGN_MCS23 <= Rate && Rate <= MGN_MCS31) || (MGN_VHT4SS_MCS0 <= Rate && Rate <= MGN_VHT4SS_MCS9))
+				txPower += pHalData->BW80_5G_Diff[RFPath][TX_4S];
+
+			/* DBG_871X("+PowerDiff 5G(RF-%c): (BW80-1S, BW80-2S, BW80-3S, BW80-4S) = (%d, %d, %d, %d)\n", ((RFPath == 0)?'A':(RFPath == 1)?'B':(RFPath ==2)?'C':'D'), */
+			/*	pHalData->BW80_5G_Diff[RFPath][TX_1S], pHalData->BW80_5G_Diff[RFPath][TX_2S], */
+			/*	pHalData->BW80_5G_Diff[RFPath][TX_3S], pHalData->BW80_5G_Diff[RFPath][TX_4S]); */
+		}
+	}
+
+	return txPower;
+}
+
+s8 PHY_GetTxPowerTrackingOffset(struct adapter *padapter, u8 RFPath, u8 Rate)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PDM_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;
+		/* DBG_871X("+Remnant_CCKSwingIdx = 0x%x\n", RFPath, Rate, pDM_Odm->Remnant_CCKSwingIdx); */
+	} else {
+		offset = pDM_Odm->Remnant_OFDMSwingIdx[RFPath];
+		/* DBG_871X("+Remanant_OFDMSwingIdx[RFPath %u][Rate 0x%x] = 0x%x\n", RFPath, Rate, 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;
+	case MGN_MCS8:
+		index = 20;
+		break;
+	case MGN_MCS9:
+		index = 21;
+		break;
+	case MGN_MCS10:
+		index = 22;
+		break;
+	case MGN_MCS11:
+		index = 23;
+		break;
+	case MGN_MCS12:
+		index = 24;
+		break;
+	case MGN_MCS13:
+		index = 25;
+		break;
+	case MGN_MCS14:
+		index = 26;
+		break;
+	case MGN_MCS15:
+		index = 27;
+		break;
+	case MGN_MCS16:
+		index = 28;
+		break;
+	case MGN_MCS17:
+		index = 29;
+		break;
+	case MGN_MCS18:
+		index = 30;
+		break;
+	case MGN_MCS19:
+		index = 31;
+		break;
+	case MGN_MCS20:
+		index = 32;
+		break;
+	case MGN_MCS21:
+		index = 33;
+		break;
+	case MGN_MCS22:
+		index = 34;
+		break;
+	case MGN_MCS23:
+		index = 35;
+		break;
+	case MGN_MCS24:
+		index = 36;
+		break;
+	case MGN_MCS25:
+		index = 37;
+		break;
+	case MGN_MCS26:
+		index = 38;
+		break;
+	case MGN_MCS27:
+		index = 39;
+		break;
+	case MGN_MCS28:
+		index = 40;
+		break;
+	case MGN_MCS29:
+		index = 41;
+		break;
+	case MGN_MCS30:
+		index = 42;
+		break;
+	case MGN_MCS31:
+		index = 43;
+		break;
+	case MGN_VHT1SS_MCS0:
+		index = 44;
+		break;
+	case MGN_VHT1SS_MCS1:
+		index = 45;
+		break;
+	case MGN_VHT1SS_MCS2:
+		index = 46;
+		break;
+	case MGN_VHT1SS_MCS3:
+		index = 47;
+		break;
+	case MGN_VHT1SS_MCS4:
+		index = 48;
+		break;
+	case MGN_VHT1SS_MCS5:
+		index = 49;
+		break;
+	case MGN_VHT1SS_MCS6:
+		index = 50;
+		break;
+	case MGN_VHT1SS_MCS7:
+		index = 51;
+		break;
+	case MGN_VHT1SS_MCS8:
+		index = 52;
+		break;
+	case MGN_VHT1SS_MCS9:
+		index = 53;
+		break;
+	case MGN_VHT2SS_MCS0:
+		index = 54;
+		break;
+	case MGN_VHT2SS_MCS1:
+		index = 55;
+		break;
+	case MGN_VHT2SS_MCS2:
+		index = 56;
+		break;
+	case MGN_VHT2SS_MCS3:
+		index = 57;
+		break;
+	case MGN_VHT2SS_MCS4:
+		index = 58;
+		break;
+	case MGN_VHT2SS_MCS5:
+		index = 59;
+		break;
+	case MGN_VHT2SS_MCS6:
+		index = 60;
+		break;
+	case MGN_VHT2SS_MCS7:
+		index = 61;
+		break;
+	case MGN_VHT2SS_MCS8:
+		index = 62;
+		break;
+	case MGN_VHT2SS_MCS9:
+		index = 63;
+		break;
+	case MGN_VHT3SS_MCS0:
+		index = 64;
+		break;
+	case MGN_VHT3SS_MCS1:
+		index = 65;
+		break;
+	case MGN_VHT3SS_MCS2:
+		index = 66;
+		break;
+	case MGN_VHT3SS_MCS3:
+		index = 67;
+		break;
+	case MGN_VHT3SS_MCS4:
+		index = 68;
+		break;
+	case MGN_VHT3SS_MCS5:
+		index = 69;
+		break;
+	case MGN_VHT3SS_MCS6:
+		index = 70;
+		break;
+	case MGN_VHT3SS_MCS7:
+		index = 71;
+		break;
+	case MGN_VHT3SS_MCS8:
+		index = 72;
+		break;
+	case MGN_VHT3SS_MCS9:
+		index = 73;
+		break;
+	case MGN_VHT4SS_MCS0:
+		index = 74;
+		break;
+	case MGN_VHT4SS_MCS1:
+		index = 75;
+		break;
+	case MGN_VHT4SS_MCS2:
+		index = 76;
+		break;
+	case MGN_VHT4SS_MCS3:
+		index = 77;
+		break;
+	case MGN_VHT4SS_MCS4:
+		index = 78;
+		break;
+	case MGN_VHT4SS_MCS5:
+		index = 79;
+		break;
+	case MGN_VHT4SS_MCS6:
+		index = 80;
+		break;
+	case MGN_VHT4SS_MCS7:
+		index = 81;
+		break;
+	case MGN_VHT4SS_MCS8:
+		index = 82;
+		break;
+	case MGN_VHT4SS_MCS9:
+		index = 83;
+		break;
+	default:
+		DBG_871X("Invalid rate 0x%x in %s\n", Rate, __func__);
+		break;
+	};
+
+	return index;
+}
+
+s8 PHY_GetTxPowerByRate(
+	struct adapter *padapter, u8 Band, u8 RFPath, u8 TxNum, 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 (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid band %d in %s\n", Band, __func__);
+		return value;
+	}
+	if (RFPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d in %s\n", RFPath, __func__);
+		return value;
+	}
+	if (TxNum >= RF_MAX_TX_NUM) {
+		DBG_871X("Invalid TxNum %d in %s\n", TxNum, __func__);
+		return value;
+	}
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE) {
+		DBG_871X("Invalid RateIndex %d in %s\n", rateIndex, __func__);
+		return value;
+	}
+
+	value = pHalData->TxPwrByRateOffset[Band][RFPath][TxNum][rateIndex];
+
+	return value;
+
+}
+
+void PHY_SetTxPowerByRate(
+	struct adapter *padapter,
+	u8 Band,
+	u8 RFPath,
+	u8 TxNum,
+	u8 Rate,
+	s8 Value
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 rateIndex = PHY_GetRateIndexOfTxPowerByRate(Rate);
+
+	if (Band != BAND_ON_2_4G && Band != BAND_ON_5G) {
+		DBG_871X("Invalid band %d in %s\n", Band, __func__);
+		return;
+	}
+	if (RFPath > ODM_RF_PATH_D) {
+		DBG_871X("Invalid RfPath %d in %s\n", RFPath, __func__);
+		return;
+	}
+	if (TxNum >= RF_MAX_TX_NUM) {
+		DBG_871X("Invalid TxNum %d in %s\n", TxNum, __func__);
+		return;
+	}
+	if (rateIndex >= TX_PWR_BY_RATE_NUM_RATE) {
+		DBG_871X("Invalid RateIndex %d in %s\n", rateIndex, __func__);
+		return;
+	}
+
+	pHalData->TxPwrByRateOffset[Band][RFPath][TxNum][rateIndex] = Value;
+}
+
+void PHY_SetTxPowerLevelByPath(struct adapter *Adapter, u8 channel, u8 path)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	bool bIsIn24G = (pHalData->CurrentBandType == BAND_ON_2_4G);
+
+	/* if (pMgntInfo->RegNByteAccess == 0) */
+	{
+		if (bIsIn24G)
+			PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, CCK);
+
+		PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, OFDM);
+		PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, HT_MCS0_MCS7);
+
+		if (pHalData->NumTotalRFPath >= 2)
+			PHY_SetTxPowerIndexByRateSection(Adapter, path, channel, HT_MCS8_MCS15);
+
+	}
+}
+
+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 Band, u8 Channel)
+{
+	s8	channelIndex = -1;
+	u8 channel5G[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 100, 102,
+		104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128, 130,
+		132, 134, 136, 138, 140, 142, 144, 149, 151, 153, 155, 157, 159, 161,
+		163, 165, 167, 168, 169, 171, 173, 175, 177
+	};
+	u8 i = 0;
+	if (Band == BAND_ON_2_4G)
+		channelIndex = Channel - 1;
+	else if (Band == BAND_ON_5G) {
+		for (i = 0; i < sizeof(channel5G)/sizeof(u8); ++i) {
+			if (channel5G[i] == Channel)
+				channelIndex = i;
+		}
+	} else
+		DBG_871X("Invalid Band %d in %s", Band, __func__);
+
+	if (channelIndex == -1)
+		DBG_871X("Invalid Channel %d of Band %d in %s", Channel, Band, __func__);
+
+	return channelIndex;
+}
+
+s8 PHY_GetTxPowerLimit(
+	struct adapter *Adapter,
+	u32 RegPwrTblSel,
+	enum BAND_TYPE Band,
+	enum CHANNEL_WIDTH Bandwidth,
+	u8 RfPath,
+	u8 DataRate,
+	u8 Channel
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	s16	band = -1, regulation = -1, bandwidth = -1, rateSection = -1, channel = -1;
+	s8 powerLimit = MAX_POWER_INDEX;
+
+	if ((Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory != 1) ||
+		   Adapter->registrypriv.RegEnableTxPowerLimit == 0)
+		return MAX_POWER_INDEX;
+
+	switch (Adapter->registrypriv.RegPwrTblSel) {
+	case 1:
+			regulation = TXPWR_LMT_ETSI;
+			break;
+	case 2:
+			regulation = TXPWR_LMT_MKK;
+			break;
+	case 3:
+			regulation = TXPWR_LMT_FCC;
+			break;
+
+	case 4:
+			regulation = TXPWR_LMT_WW;
+			break;
+
+	default:
+			regulation = (Band == BAND_ON_2_4G) ? pHalData->Regulation2_4G : pHalData->Regulation5G;
+			break;
+	}
+
+	/* DBG_871X("pMgntInfo->RegPwrTblSel %d, final regulation %d\n", Adapter->registrypriv.RegPwrTblSel, regulation); */
+
+
+	if (Band == BAND_ON_2_4G)
+		band = 0;
+	else if (Band == BAND_ON_5G)
+		band = 1;
+
+	if (Bandwidth == CHANNEL_WIDTH_20)
+		bandwidth = 0;
+	else if (Bandwidth == CHANNEL_WIDTH_40)
+		bandwidth = 1;
+	else if (Bandwidth == CHANNEL_WIDTH_80)
+		bandwidth = 2;
+	else if (Bandwidth == CHANNEL_WIDTH_160)
+		bandwidth = 3;
+
+	switch (DataRate) {
+	case MGN_1M: case MGN_2M: case MGN_5_5M: case MGN_11M:
+		rateSection = 0;
+		break;
+
+	case MGN_6M: case MGN_9M: case MGN_12M: case MGN_18M:
+	case MGN_24M: case MGN_36M: case MGN_48M: case MGN_54M:
+		rateSection = 1;
+		break;
+
+	case MGN_MCS0: case MGN_MCS1: case MGN_MCS2: case MGN_MCS3:
+	case MGN_MCS4: case MGN_MCS5: case MGN_MCS6: case MGN_MCS7:
+		rateSection = 2;
+		break;
+
+	case MGN_MCS8: case MGN_MCS9: case MGN_MCS10: case MGN_MCS11:
+	case MGN_MCS12: case MGN_MCS13: case MGN_MCS14: case MGN_MCS15:
+		rateSection = 3;
+		break;
+
+	case MGN_MCS16: case MGN_MCS17: case MGN_MCS18: case MGN_MCS19:
+	case MGN_MCS20: case MGN_MCS21: case MGN_MCS22: case MGN_MCS23:
+		rateSection = 4;
+		break;
+
+	case MGN_MCS24: case MGN_MCS25: case MGN_MCS26: case MGN_MCS27:
+	case MGN_MCS28: case MGN_MCS29: case MGN_MCS30: case MGN_MCS31:
+		rateSection = 5;
+		break;
+
+	case MGN_VHT1SS_MCS0: case MGN_VHT1SS_MCS1: case MGN_VHT1SS_MCS2:
+	case MGN_VHT1SS_MCS3: case MGN_VHT1SS_MCS4: case MGN_VHT1SS_MCS5:
+	case MGN_VHT1SS_MCS6: case MGN_VHT1SS_MCS7: case MGN_VHT1SS_MCS8:
+	case MGN_VHT1SS_MCS9:
+		rateSection = 6;
+		break;
+
+	case MGN_VHT2SS_MCS0: case MGN_VHT2SS_MCS1: case MGN_VHT2SS_MCS2:
+	case MGN_VHT2SS_MCS3: case MGN_VHT2SS_MCS4: case MGN_VHT2SS_MCS5:
+	case MGN_VHT2SS_MCS6: case MGN_VHT2SS_MCS7: case MGN_VHT2SS_MCS8:
+	case MGN_VHT2SS_MCS9:
+		rateSection = 7;
+		break;
+
+	case MGN_VHT3SS_MCS0: case MGN_VHT3SS_MCS1: case MGN_VHT3SS_MCS2:
+	case MGN_VHT3SS_MCS3: case MGN_VHT3SS_MCS4: case MGN_VHT3SS_MCS5:
+	case MGN_VHT3SS_MCS6: case MGN_VHT3SS_MCS7: case MGN_VHT3SS_MCS8:
+	case MGN_VHT3SS_MCS9:
+		rateSection = 8;
+		break;
+
+	case MGN_VHT4SS_MCS0: case MGN_VHT4SS_MCS1: case MGN_VHT4SS_MCS2:
+	case MGN_VHT4SS_MCS3: case MGN_VHT4SS_MCS4: case MGN_VHT4SS_MCS5:
+	case MGN_VHT4SS_MCS6: case MGN_VHT4SS_MCS7: case MGN_VHT4SS_MCS8:
+	case MGN_VHT4SS_MCS9:
+		rateSection = 9;
+		break;
+
+	default:
+		DBG_871X("Wrong rate 0x%x\n", DataRate);
+		break;
+	}
+
+	if (Band == BAND_ON_5G  && rateSection == 0)
+			DBG_871X("Wrong rate 0x%x: No CCK in 5G Band\n", DataRate);
+
+	/*  workaround for wrong index combination to obtain tx power limit, */
+	/*  OFDM only exists in BW 20M */
+	if (rateSection == 1)
+		bandwidth = 0;
+
+	/*  workaround for wrong index combination to obtain tx power limit, */
+	/*  CCK table will only be given in BW 20M */
+	if (rateSection == 0)
+		bandwidth = 0;
+
+	/*  workaround for wrong indxe combination to obtain tx power limit, */
+	/*  HT on 80M will reference to HT on 40M */
+	if ((rateSection == 2 || rateSection == 3) && Band == BAND_ON_5G && bandwidth == 2) {
+		bandwidth = 1;
+	}
+
+	if (Band == BAND_ON_2_4G)
+		channel = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_2_4G, Channel);
+	else if (Band == BAND_ON_5G)
+		channel = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_5G, Channel);
+	else if (Band == BAND_ON_BOTH) {
+		/*  BAND_ON_BOTH don't care temporarily */
+	}
+
+	if (band == -1 || regulation == -1 || bandwidth == -1 ||
+	     rateSection == -1 || channel == -1) {
+		/* DBG_871X("Wrong index value to access power limit table [band %d][regulation %d][bandwidth %d][rf_path %d][rate_section %d][chnlGroup %d]\n", */
+		/*	  band, regulation, bandwidth, RfPath, rateSection, channelGroup); */
+
+		return MAX_POWER_INDEX;
+	}
+
+	if (Band == BAND_ON_2_4G) {
+		s8 limits[10] = {0}; u8 i = 0;
+		for (i = 0; i < MAX_REGULATION_NUM; i++)
+			limits[i] = pHalData->TxPwrLimit_2_4G[i][bandwidth][rateSection][channel][RfPath];
+
+		powerLimit = (regulation == TXPWR_LMT_WW) ? phy_GetWorldWideLimit(limits) :
+			pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channel][RfPath];
+
+	} else if (Band == BAND_ON_5G) {
+		s8 limits[10] = {0}; u8 i = 0;
+		for (i = 0; i < MAX_REGULATION_NUM; ++i)
+			limits[i] = pHalData->TxPwrLimit_5G[i][bandwidth][rateSection][channel][RfPath];
+
+		powerLimit = (regulation == TXPWR_LMT_WW) ? phy_GetWorldWideLimit(limits) :
+			pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channel][RfPath];
+	} else
+		DBG_871X("No power limit table of the specified band\n");
+
+	/*  combine 5G VHT & HT rate */
+	/*  5G 20M and 40M HT and VHT can cross reference */
+	/*
+	if (Band == BAND_ON_5G && powerLimit == MAX_POWER_INDEX) {
+		if (bandwidth == 0 || bandwidth == 1) {
+			RT_TRACE(COMP_INIT, DBG_LOUD, ("No power limit table of the specified band %d, bandwidth %d, ratesection %d, rf path %d\n",
+					  band, bandwidth, rateSection, RfPath));
+			if (rateSection == 2)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][4][channelGroup][RfPath];
+			else if (rateSection == 4)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][2][channelGroup][RfPath];
+			else if (rateSection == 3)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][5][channelGroup][RfPath];
+			else if (rateSection == 5)
+				powerLimit = pHalData->TxPwrLimit_5G[regulation]
+										[bandwidth][3][channelGroup][RfPath];
+		}
+	}
+	*/
+	/* DBG_871X("TxPwrLmt[Regulation %d][Band %d][BW %d][RFPath %d][Rate 0x%x][Chnl %d] = %d\n", */
+	/*		regulation, pHalData->CurrentBandType, Bandwidth, RfPath, DataRate, Channel, powerLimit); */
+	return powerLimit;
+}
+
+static void phy_CrossReferenceHTAndVHTTxPowerLimit(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	u8 regulation, bw, channel, rateSection;
+	s8 tempPwrLmt = 0;
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_5G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_5G; ++channel) {
+				for (rateSection = 0; rateSection < MAX_RATE_SECTION_NUM; ++rateSection) {
+					tempPwrLmt = pHalData->TxPwrLimit_5G[regulation][bw][rateSection][channel][ODM_RF_PATH_A];
+					if (tempPwrLmt == MAX_POWER_INDEX) {
+						u8 baseSection = 2, refSection = 6;
+						if (bw == 0 || bw == 1) { /*  5G 20M 40M VHT and HT can cross reference */
+							/* DBG_871X("No power limit table of the specified band %d, bandwidth %d, ratesection %d, channel %d, rf path %d\n", */
+							/*			1, bw, rateSection, channel, ODM_RF_PATH_A); */
+							if (rateSection >= 2 && rateSection <= 9) {
+								if (rateSection == 2) {
+									baseSection = 2;
+									refSection = 6;
+								} else if (rateSection == 3) {
+									baseSection = 3;
+									refSection = 7;
+								} else if (rateSection == 4) {
+									baseSection = 4;
+									refSection = 8;
+								} else if (rateSection == 5) {
+									baseSection = 5;
+									refSection = 9;
+								} else if (rateSection == 6) {
+									baseSection = 6;
+									refSection = 2;
+								} else if (rateSection == 7) {
+									baseSection = 7;
+									refSection = 3;
+								} else if (rateSection == 8) {
+									baseSection = 8;
+									refSection = 4;
+								} else if (rateSection == 9) {
+									baseSection = 9;
+									refSection = 5;
+								}
+								pHalData->TxPwrLimit_5G[regulation][bw][baseSection][channel][ODM_RF_PATH_A] =
+									pHalData->TxPwrLimit_5G[regulation][bw][refSection][channel][ODM_RF_PATH_A];
+							}
+
+							/* DBG_871X("use other value %d", tempPwrLmt); */
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+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;
+
+	/* DBG_871X("=====> PHY_ConvertTxPowerLimitToPowerIndex()\n"); */
+
+	phy_CrossReferenceHTAndVHTTxPowerLimit(Adapter);
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_2_4G_BANDWITH_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][ODM_RF_PATH_A];
+
+					for (rfPath = ODM_RF_PATH_A; rfPath < MAX_RF_PATH_NUM; ++rfPath) {
+						if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+							if (rateSection == 5) /*  HT 4T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_4TX, HT_MCS24_MCS31);
+							else if (rateSection == 4) /*  HT 3T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_3TX, HT_MCS16_MCS23);
+							else if (rateSection == 3) /*  HT 2T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_2TX, HT_MCS8_MCS15);
+							else if (rateSection == 2) /*  HT 1T */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, HT_MCS0_MCS7);
+							else if (rateSection == 1) /*  OFDM */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, OFDM);
+							else if (rateSection == 0) /*  CCK */
+								BW40PwrBasedBm2_4G = PHY_GetTxPowerByRateBase(Adapter, BAND_ON_2_4G, rfPath, RF_1TX, 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;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	/* DBG_871X("<===== PHY_ConvertTxPowerLimitToPowerIndex()\n"); */
+}
+
+void PHY_InitTxPowerLimit(struct adapter *Adapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	u8 i, j, k, l, m;
+
+	/* DBG_871X("=====> PHY_InitTxPowerLimit()!\n"); */
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_2_4G_BANDWITH_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;
+	}
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_5G_BANDWITH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_5G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						pHalData->TxPwrLimit_5G[i][j][k][m][l] = MAX_POWER_INDEX;
+	}
+
+	/* DBG_871X("<===== PHY_InitTxPowerLimit()!\n"); */
+}
+
+void PHY_SetTxPowerLimit(
+	struct adapter *Adapter,
+	u8 *Regulation,
+	u8 *Band,
+	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;
+
+	/* DBG_871X("Index of power limit table [band %s][regulation %s][bw %s][rate section %s][rf path %s][chnl %s][val %s]\n", */
+	/*	  Band, Regulation, Bandwidth, RateSection, RfPath, Channel, PowerLimit); */
+
+	if (!GetU1ByteIntegerFromStringInDecimal((s8 *)Channel, &channel) ||
+		 !GetU1ByteIntegerFromStringInDecimal((s8 *)PowerLimit, &powerLimit))
+		DBG_871X("Illegal index of power limit table [chnl %s][val %s]\n", Channel, 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 if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("2T"), 2))
+		rateSection = 3;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("3T"), 2))
+		rateSection = 4;
+	else if (eqNByte(RateSection, (u8 *)("HT"), 2) && eqNByte(RfPath, (u8 *)("4T"), 2))
+		rateSection = 5;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("1T"), 2))
+		rateSection = 6;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("2T"), 2))
+		rateSection = 7;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("3T"), 2))
+		rateSection = 8;
+	else if (eqNByte(RateSection, (u8 *)("VHT"), 3) && eqNByte(RfPath, (u8 *)("4T"), 2))
+		rateSection = 9;
+	else {
+		DBG_871X("Wrong rate section!\n");
+		return;
+	}
+
+
+	if (eqNByte(Bandwidth, (u8 *)("20M"), 3))
+		bandwidth = 0;
+	else if (eqNByte(Bandwidth, (u8 *)("40M"), 3))
+		bandwidth = 1;
+	else if (eqNByte(Bandwidth, (u8 *)("80M"), 3))
+		bandwidth = 2;
+	else if (eqNByte(Bandwidth, (u8 *)("160M"), 4))
+		bandwidth = 3;
+
+	if (eqNByte(Band, (u8 *)("2.4G"), 4)) {
+		channelIndex = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_2_4G, channel);
+
+		if (channelIndex == -1)
+			return;
+
+		prevPowerLimit = pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A];
+
+		if (powerLimit < prevPowerLimit)
+			pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A] = powerLimit;
+
+		/* DBG_871X("2.4G Band value : [regulation %d][bw %d][rate_section %d][chnl %d][val %d]\n", */
+		/*	  regulation, bandwidth, rateSection, channelIndex, pHalData->TxPwrLimit_2_4G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A]); */
+	} else if (eqNByte(Band, (u8 *)("5G"), 2)) {
+		channelIndex = phy_GetChannelIndexOfTxPowerLimit(BAND_ON_5G, channel);
+
+		if (channelIndex == -1)
+			return;
+
+		prevPowerLimit = pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A];
+
+		if (powerLimit < prevPowerLimit)
+			pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A] = powerLimit;
+
+		/* DBG_871X("5G Band value : [regulation %d][bw %d][rate_section %d][chnl %d][val %d]\n", */
+		/*	  regulation, bandwidth, rateSection, channel, pHalData->TxPwrLimit_5G[regulation][bandwidth][rateSection][channelIndex][ODM_RF_PATH_A]); */
+	} else {
+		DBG_871X("Cannot recognize the band info in %s\n", Band);
+		return;
+	}
+}
+
+u8 PHY_GetTxPowerIndex(
+	struct adapter *padapter,
+	u8 RFPath,
+	u8 Rate,
+	enum CHANNEL_WIDTH BandWidth,
+	u8 Channel
+)
+{
+	return PHY_GetTxPowerIndex_8723B(padapter, RFPath, Rate, BandWidth, Channel);
+}
+
+void PHY_SetTxPowerIndex(
+	struct adapter *padapter, u32 PowerIndex, u8 RFPath, u8 Rate
+)
+{
+	PHY_SetTxPowerIndex_8723B(padapter, PowerIndex, RFPath, Rate);
+}
+
+void Hal_ChannelPlanToRegulation(struct adapter *Adapter, u16 ChannelPlan)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	pHalData->Regulation2_4G = TXPWR_LMT_WW;
+	pHalData->Regulation5G = 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;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK1:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_KCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_MKK;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC5:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_FCC6:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC7:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK2:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_MKK1_MKK3:
+		pHalData->Regulation2_4G = TXPWR_LMT_MKK;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC1:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_GLOBAL_NULL:
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		pHalData->Regulation5G = TXPWR_LMT_WW;
+		break;
+	case RT_CHANNEL_DOMAIN_ETSI1_ETSI4:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC2:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC3:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI5:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC8:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI6:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI7:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI8:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI9:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI10:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI11:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_NCC4:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI12:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC9:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_WORLD_ETSI13:
+		pHalData->Regulation2_4G = TXPWR_LMT_ETSI;
+		pHalData->Regulation5G = TXPWR_LMT_ETSI;
+		break;
+	case RT_CHANNEL_DOMAIN_FCC1_FCC10:
+		pHalData->Regulation2_4G = TXPWR_LMT_FCC;
+		pHalData->Regulation5G = TXPWR_LMT_FCC;
+		break;
+	case RT_CHANNEL_DOMAIN_REALTEK_DEFINE: /* Realtek Reserve */
+		pHalData->Regulation2_4G = TXPWR_LMT_WW;
+		pHalData->Regulation5G = TXPWR_LMT_WW;
+		break;
+	default:
+		break;
+	}
+}
+
+
+static char file_path_bs[PATH_MAX];
+
+#define GetLineFromBuffer(buffer)	 strsep(&buffer, "\n")
+
+int phy_ConfigMACWithParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_MAC_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->mac_reg_len == 0) && (pHalData->mac_reg == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->mac_reg = vzalloc(rlen);
+				if (pHalData->mac_reg) {
+					memcpy(pHalData->mac_reg, pHalData->para_file_buf, rlen);
+					pHalData->mac_reg_len = rlen;
+				} else
+					DBG_871X("%s mac_reg alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->mac_reg_len != 0) && (pHalData->mac_reg != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->mac_reg, pHalData->mac_reg_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove))
+						rtw_write8(Adapter, u4bRegOffset, (u8)u4bRegValue);
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+int phy_ConfigBBWithParaFile(
+	struct adapter *Adapter, char *pFileName, u32 ConfigType
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+	char *pBuf = NULL;
+	u32 *pBufLen = NULL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_BB_PARA_FILE))
+		return rtStatus;
+
+	switch (ConfigType) {
+	case CONFIG_BB_PHY_REG:
+		pBuf = pHalData->bb_phy_reg;
+		pBufLen = &pHalData->bb_phy_reg_len;
+		break;
+	case CONFIG_BB_AGC_TAB:
+		pBuf = pHalData->bb_agc_tab;
+		pBufLen = &pHalData->bb_agc_tab_len;
+		break;
+	default:
+		DBG_871X("Unknown ConfigType!! %d\r\n", ConfigType);
+		break;
+	}
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pBuf = vzalloc(rlen);
+				if (pBuf) {
+					memcpy(pBuf, pHalData->para_file_buf, rlen);
+					*pBufLen = rlen;
+
+					switch (ConfigType) {
+					case CONFIG_BB_PHY_REG:
+						pHalData->bb_phy_reg = pBuf;
+						break;
+					case CONFIG_BB_AGC_TAB:
+						pHalData->bb_agc_tab = pBuf;
+						break;
+					}
+				} else
+					DBG_871X("%s(): ConfigType %d  alloc fail !\n", __func__, ConfigType);
+			}
+		}
+	} else {
+		if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+			memcpy(pHalData->para_file_buf, pBuf, *pBufLen);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+					else if (u4bRegOffset == 0xfe || u4bRegOffset == 0xffe)
+						msleep(50);
+					else if (u4bRegOffset == 0xfd)
+						mdelay(5);
+					else if (u4bRegOffset == 0xfc)
+						mdelay(1);
+					else if (u4bRegOffset == 0xfb)
+						udelay(50);
+					else if (u4bRegOffset == 0xfa)
+						udelay(5);
+					else if (u4bRegOffset == 0xf9)
+						udelay(1);
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+						/* DBG_871X("[BB-ADDR]%03lX =%08lX\n", u4bRegOffset, u4bRegValue); */
+						PHY_SetBBReg(Adapter, u4bRegOffset, bMaskDWord, u4bRegValue);
+
+						if (u4bRegOffset == 0xa24)
+							pHalData->odmpriv.RFCalibrateInfo.RegA24 = u4bRegValue;
+
+						/*  Add 1us delay between BB/RF register setting. */
+						udelay(1);
+					}
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static void phy_DecryptBBPgParaFile(struct adapter *Adapter, char *buffer)
+{
+	u32 i = 0, j = 0;
+	u8 map[95] = {0};
+	u8 currentChar;
+	char *BufOfLines, *ptmp;
+
+	/* DBG_871X("=====>phy_DecryptBBPgParaFile()\n"); */
+	/*  32 the ascii code of the first visable char, 126 the last one */
+	for (i = 0; i < 95; ++i)
+		map[i] = (u8) (94 - i);
+
+	ptmp = buffer;
+	i = 0;
+	for (BufOfLines = GetLineFromBuffer(ptmp); BufOfLines != NULL; BufOfLines = GetLineFromBuffer(ptmp)) {
+		/* DBG_871X("Encrypted Line: %s\n", BufOfLines); */
+
+		for (j = 0; j < strlen(BufOfLines); ++j) {
+			currentChar = BufOfLines[j];
+
+			if (currentChar == '\0')
+				break;
+
+			currentChar -=  (u8) ((((i + j) * 3) % 128));
+
+			BufOfLines[j] = map[currentChar - 32] + 32;
+		}
+		/* DBG_871X("Decrypted Line: %s\n", BufOfLines); */
+		if (strlen(BufOfLines) != 0)
+			i++;
+		BufOfLines[strlen(BufOfLines)] = '\n';
+	}
+}
+
+static int phy_ParseBBPgParaFile(struct adapter *Adapter, char *buffer)
+{
+	int	rtStatus = _SUCCESS;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegMask, u4bRegValue;
+	u32 u4bMove;
+	bool firstLine = true;
+	u8 tx_num = 0;
+	u8 band = 0, rf_path = 0;
+
+	/* DBG_871X("=====>phy_ParseBBPgParaFile()\n"); */
+
+	if (Adapter->registrypriv.RegDecryptCustomFile == 1)
+		phy_DecryptBBPgParaFile(Adapter, buffer);
+
+	ptmp = buffer;
+	for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+		if (!IsCommentString(szLine)) {
+			if (isAllSpaceOrTab(szLine, sizeof(*szLine)))
+				continue;
+
+			/*  Get header info (relative value or exact value) */
+			if (firstLine) {
+				if (eqNByte(szLine, (u8 *)("#[v1]"), 5)) {
+
+					pHalData->odmpriv.PhyRegPgVersion = szLine[3] - '0';
+					/* DBG_871X("This is a new format PHY_REG_PG.txt\n"); */
+				} else if (eqNByte(szLine, (u8 *)("#[v0]"), 5)) {
+					pHalData->odmpriv.PhyRegPgVersion = szLine[3] - '0';
+					/* DBG_871X("This is a old format PHY_REG_PG.txt ok\n"); */
+				} else {
+					DBG_871X("The format in PHY_REG_PG are invalid %s\n", szLine);
+					return _FAIL;
+				}
+
+				if (eqNByte(szLine + 5, (u8 *)("[Exact]#"), 8)) {
+					pHalData->odmpriv.PhyRegPgValueType = PHY_REG_PG_EXACT_VALUE;
+					/* DBG_871X("The values in PHY_REG_PG are exact values ok\n"); */
+					firstLine = false;
+					continue;
+				} else if (eqNByte(szLine + 5, (u8 *)("[Relative]#"), 11)) {
+					pHalData->odmpriv.PhyRegPgValueType = PHY_REG_PG_RELATIVE_VALUE;
+					/* DBG_871X("The values in PHY_REG_PG are relative values ok\n"); */
+					firstLine = false;
+					continue;
+				} else {
+					DBG_871X("The values in PHY_REG_PG are invalid %s\n", szLine);
+					return _FAIL;
+				}
+			}
+
+			if (pHalData->odmpriv.PhyRegPgVersion == 0) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					szLine += u4bMove;
+					if (u4bRegOffset == 0xffff) /*  Ending. */
+						break;
+
+					/*  Get 2nd hex value as register mask. */
+					if (GetHexValueFromString(szLine, &u4bRegMask, &u4bMove))
+						szLine += u4bMove;
+					else
+						return _FAIL;
+
+					if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_RELATIVE_VALUE) {
+						/*  Get 3rd hex value as register value. */
+						if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+							PHY_StoreTxPowerByRate(Adapter, 0, 0, 1, u4bRegOffset, u4bRegMask, u4bRegValue);
+							/* DBG_871X("[ADDR] %03X =%08X Mask =%08x\n", u4bRegOffset, u4bRegValue, u4bRegMask); */
+						} else
+							return _FAIL;
+					} else if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+						u32 combineValue = 0;
+						u8 integer = 0, fraction = 0;
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+
+						if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						integer *= 2;
+						if (fraction == 5)
+							integer += 1;
+						combineValue <<= 8;
+						combineValue |= (((integer / 10) << 4) + (integer % 10));
+						/* DBG_871X(" %d", integer); */
+						PHY_StoreTxPowerByRate(Adapter, 0, 0, 1, u4bRegOffset, u4bRegMask, combineValue);
+
+						/* DBG_871X("[ADDR] 0x%3x = 0x%4x\n", u4bRegOffset, combineValue); */
+					}
+				}
+			} else if (pHalData->odmpriv.PhyRegPgVersion > 0) {
+				u32 index = 0;
+
+				if (eqNByte(szLine, "0xffff", 6))
+					break;
+
+				if (!eqNByte("#[END]#", szLine, 7)) {
+					/*  load the table label info */
+					if (szLine[0] == '#') {
+						index = 0;
+						if (eqNByte(szLine, "#[2.4G]", 7)) {
+							band = BAND_ON_2_4G;
+							index += 8;
+						} else if (eqNByte(szLine, "#[5G]", 5)) {
+							band = BAND_ON_5G;
+							index += 6;
+						} else {
+							DBG_871X("Invalid band %s in PHY_REG_PG.txt\n", szLine);
+							return _FAIL;
+						}
+
+						rf_path = szLine[index] - 'A';
+						/* DBG_871X(" Table label Band %d, RfPath %d\n", band, rf_path); */
+					} else { /*  load rows of tables */
+						if (szLine[1] == '1')
+							tx_num = RF_1TX;
+						else if (szLine[1] == '2')
+							tx_num = RF_2TX;
+						else if (szLine[1] == '3')
+							tx_num = RF_3TX;
+						else if (szLine[1] == '4')
+							tx_num = RF_4TX;
+						else {
+							DBG_871X("Invalid row in PHY_REG_PG.txt %c\n", szLine[1]);
+							return _FAIL;
+						}
+
+						while (szLine[index] != ']')
+							++index;
+						++index;/*  skip ] */
+
+						/*  Get 2nd hex value as register offset. */
+						szLine += index;
+						if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						/*  Get 2nd hex value as register mask. */
+						if (GetHexValueFromString(szLine, &u4bRegMask, &u4bMove))
+							szLine += u4bMove;
+						else
+							return _FAIL;
+
+						if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_RELATIVE_VALUE) {
+							/*  Get 3rd hex value as register value. */
+							if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+								PHY_StoreTxPowerByRate(Adapter, band, rf_path, tx_num, u4bRegOffset, u4bRegMask, u4bRegValue);
+								/* DBG_871X("[ADDR] %03X (tx_num %d) =%08X Mask =%08x\n", u4bRegOffset, tx_num, u4bRegValue, u4bRegMask); */
+							} else
+								return _FAIL;
+						} else if (pHalData->odmpriv.PhyRegPgValueType == PHY_REG_PG_EXACT_VALUE) {
+							u32 combineValue = 0;
+							u8 integer = 0, fraction = 0;
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+
+							if (GetFractionValueFromString(szLine, &integer, &fraction, &u4bMove))
+								szLine += u4bMove;
+							else
+								return _FAIL;
+
+							integer *= 2;
+							if (fraction == 5)
+								integer += 1;
+							combineValue <<= 8;
+							combineValue |= (((integer / 10) << 4) + (integer % 10));
+							/* DBG_871X(" %d", integer); */
+							PHY_StoreTxPowerByRate(Adapter, band, rf_path, tx_num, u4bRegOffset, u4bRegMask, combineValue);
+
+							/* DBG_871X("[ADDR] 0x%3x (tx_num %d) = 0x%4x\n", u4bRegOffset, tx_num, combineValue); */
+						}
+					}
+				}
+			}
+		}
+	}
+	/* DBG_871X("<=====phy_ParseBBPgParaFile()\n"); */
+	return rtStatus;
+}
+
+int phy_ConfigBBWithPgParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_BB_PG_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->bb_phy_reg_pg_len == 0) && (pHalData->bb_phy_reg_pg == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->bb_phy_reg_pg = vzalloc(rlen);
+				if (pHalData->bb_phy_reg_pg) {
+					memcpy(pHalData->bb_phy_reg_pg, pHalData->para_file_buf, rlen);
+					pHalData->bb_phy_reg_pg_len = rlen;
+				} else
+					DBG_871X("%s bb_phy_reg_pg alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->bb_phy_reg_pg_len != 0) && (pHalData->bb_phy_reg_pg != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->bb_phy_reg_pg, pHalData->bb_phy_reg_pg_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("phy_ConfigBBWithPgParaFile(): read %s ok\n", pFileName); */
+		phy_ParseBBPgParaFile(Adapter, pHalData->para_file_buf);
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+int PHY_ConfigRFWithParaFile(
+	struct adapter *Adapter, char *pFileName, u8 eRFPath
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 u4bRegOffset, u4bRegValue, u4bMove;
+	u16 i;
+	char *pBuf = NULL;
+	u32 *pBufLen = NULL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_PARA_FILE))
+		return rtStatus;
+
+	switch (eRFPath) {
+	case ODM_RF_PATH_A:
+		pBuf = pHalData->rf_radio_a;
+		pBufLen = &pHalData->rf_radio_a_len;
+		break;
+	case ODM_RF_PATH_B:
+		pBuf = pHalData->rf_radio_b;
+		pBufLen = &pHalData->rf_radio_b_len;
+		break;
+	default:
+		DBG_871X("Unknown RF path!! %d\r\n", eRFPath);
+		break;
+	}
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pBuf = vzalloc(rlen);
+				if (pBuf) {
+					memcpy(pBuf, pHalData->para_file_buf, rlen);
+					*pBufLen = rlen;
+
+					switch (eRFPath) {
+					case ODM_RF_PATH_A:
+						pHalData->rf_radio_a = pBuf;
+						break;
+					case ODM_RF_PATH_B:
+						pHalData->rf_radio_b = pBuf;
+						break;
+					}
+				} else
+					DBG_871X("%s(): eRFPath =%d  alloc fail !\n", __func__, eRFPath);
+			}
+		}
+	} else {
+		if ((pBufLen != NULL) && (*pBufLen == 0) && (pBuf == NULL)) {
+			memcpy(pHalData->para_file_buf, pBuf, *pBufLen);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s successfully\n", __func__, pFileName); */
+
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				/*  Get 1st hex value as register offset. */
+				if (GetHexValueFromString(szLine, &u4bRegOffset, &u4bMove)) {
+					if (u4bRegOffset == 0xfe || u4bRegOffset == 0xffe) /*  Deay specific ms. Only RF configuration require delay. */
+						msleep(50);
+					else if (u4bRegOffset == 0xfd) {
+						/* mdelay(5); */
+						for (i = 0; i < 100; i++)
+							udelay(MAX_STALL_TIME);
+					} else if (u4bRegOffset == 0xfc) {
+						/* mdelay(1); */
+						for (i = 0; i < 20; i++)
+							udelay(MAX_STALL_TIME);
+					} else if (u4bRegOffset == 0xfb)
+						udelay(50);
+					else if (u4bRegOffset == 0xfa)
+						udelay(5);
+					else if (u4bRegOffset == 0xf9)
+						udelay(1);
+					else if (u4bRegOffset == 0xffff)
+						break;
+
+					/*  Get 2nd hex value as register value. */
+					szLine += u4bMove;
+					if (GetHexValueFromString(szLine, &u4bRegValue, &u4bMove)) {
+						PHY_SetRFReg(Adapter, eRFPath, u4bRegOffset, bRFRegOffsetMask, u4bRegValue);
+
+						/*  Temp add, for frequency lock, if no delay, that may cause */
+						/*  frequency shift, ex: 2412MHz => 2417MHz */
+						/*  If frequency shift, the following action may works. */
+						/*  Fractional-N table in radio_a.txt */
+						/* 0x2a 0x00001		channel 1 */
+						/* 0x2b 0x00808		frequency divider. */
+						/* 0x2b 0x53333 */
+						/* 0x2c 0x0000c */
+						udelay(1);
+					}
+				}
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static void initDeltaSwingIndexTables(
+	struct adapter *Adapter,
+	char *Band,
+	char *Path,
+	char *Sign,
+	char *Channel,
+	char *Rate,
+	char *Data
+)
+{
+	#define STR_EQUAL_5G(_band, _path, _sign, _rate, _chnl) \
+		((strcmp(Band, _band) == 0) && (strcmp(Path, _path) == 0) && (strcmp(Sign, _sign) == 0) &&\
+		(strcmp(Rate, _rate) == 0) && (strcmp(Channel, _chnl) == 0)\
+	)
+	#define STR_EQUAL_2G(_band, _path, _sign, _rate) \
+		((strcmp(Band, _band) == 0) && (strcmp(Path, _path) == 0) && (strcmp(Sign, _sign) == 0) &&\
+		(strcmp(Rate, _rate) == 0)\
+	)
+
+	#define STORE_SWING_TABLE(_array, _iteratedIdx) \
+		for (token = strsep(&Data, delim); token != NULL; token = strsep(&Data, delim)) {\
+			sscanf(token, "%d", &idx);\
+			_array[_iteratedIdx++] = (u8)idx;\
+		} \
+
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	PODM_RF_CAL_T pRFCalibrateInfo = &(pDM_Odm->RFCalibrateInfo);
+	u32 j = 0;
+	char *token;
+	char delim[] = ",";
+	u32 idx = 0;
+
+	/* DBG_871X("===>initDeltaSwingIndexTables(): Band: %s;\nPath: %s;\nSign: %s;\nChannel: %s;\nRate: %s;\n, Data: %s;\n", */
+	/*	Band, Path, Sign, Channel, Rate, Data); */
+
+	if (STR_EQUAL_2G("2G", "A", "+", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_P, j);
+	} else if (STR_EQUAL_2G("2G", "A", "-", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKA_N, j);
+	} else if (STR_EQUAL_2G("2G", "B", "+", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_P, j);
+	} else if (STR_EQUAL_2G("2G", "B", "-", "CCK")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GCCKB_N, j);
+	} else if (STR_EQUAL_2G("2G", "A", "+", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GA_P, j);
+	} else if (STR_EQUAL_2G("2G", "A", "-", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GA_N, j);
+	} else if (STR_EQUAL_2G("2G", "B", "+", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GB_P, j);
+	} else if (STR_EQUAL_2G("2G", "B", "-", "ALL")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_2GB_N, j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[0], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[0], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[0], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "0")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[0], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[1], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[1], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[1], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "1")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[1], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[2], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[2], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[2], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "2")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[2], j);
+	} else if (STR_EQUAL_5G("5G", "A", "+", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_P[3], j);
+	} else if (STR_EQUAL_5G("5G", "A", "-", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GA_N[3], j);
+	} else if (STR_EQUAL_5G("5G", "B", "+", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_P[3], j);
+	} else if (STR_EQUAL_5G("5G", "B", "-", "ALL", "3")) {
+		STORE_SWING_TABLE(pRFCalibrateInfo->DeltaSwingTableIdx_5GB_N[3], j);
+	} else
+		DBG_871X("===>initDeltaSwingIndexTables(): The input is invalid!!\n");
+}
+
+int PHY_ConfigRFWithTxPwrTrackParaFile(struct adapter *Adapter, char *pFileName)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+	char *szLine, *ptmp;
+	u32 i = 0;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_TXPWR_TRACK_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->rf_tx_pwr_track_len == 0) && (pHalData->rf_tx_pwr_track == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->rf_tx_pwr_track = vzalloc(rlen);
+				if (pHalData->rf_tx_pwr_track) {
+					memcpy(pHalData->rf_tx_pwr_track, pHalData->para_file_buf, rlen);
+					pHalData->rf_tx_pwr_track_len = rlen;
+				} else
+					DBG_871X("%s rf_tx_pwr_track alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->rf_tx_pwr_track_len != 0) && (pHalData->rf_tx_pwr_track != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->rf_tx_pwr_track, pHalData->rf_tx_pwr_track_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s successfully\n", __func__, pFileName); */
+
+		ptmp = pHalData->para_file_buf;
+		for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+			if (!IsCommentString(szLine)) {
+				char band[5] = "", path[5] = "", sign[5] = "";
+				char chnl[5] = "", rate[10] = "";
+				char data[300] = ""; /*  100 is too small */
+
+				if (strlen(szLine) < 10 || szLine[0] != '[')
+					continue;
+
+				strncpy(band, szLine+1, 2);
+				strncpy(path, szLine+5, 1);
+				strncpy(sign, szLine+8, 1);
+
+				i = 10; /*  szLine+10 */
+				if (!ParseQualifiedString(szLine, &i, rate, '[', ']')) {
+					/* DBG_871X("Fail to parse rate!\n"); */
+				}
+				if (!ParseQualifiedString(szLine, &i, chnl, '[', ']')) {
+					/* DBG_871X("Fail to parse channel group!\n"); */
+				}
+				while (szLine[i] != '{' && i < strlen(szLine))
+					i++;
+				if (!ParseQualifiedString(szLine, &i, data, '{', '}')) {
+					/* DBG_871X("Fail to parse data!\n"); */
+				}
+
+				initDeltaSwingIndexTables(Adapter, band, path, sign, chnl, rate, data);
+			}
+		}
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+static int phy_ParsePowerLimitTableFile(struct adapter *Adapter, char *buffer)
+{
+	u32 i = 0, forCnt = 0;
+	u8 loadingStage = 0, limitValue = 0, fraction = 0;
+	char *szLine, *ptmp;
+	int	rtStatus = _SUCCESS;
+	char band[10], bandwidth[10], rateSection[10],
+		regulation[TXPWR_LMT_MAX_REGULATION_NUM][10], rfPath[10], colNumBuf[10];
+	u8 colNum = 0;
+
+	DBG_871X("===>phy_ParsePowerLimitTableFile()\n");
+
+	if (Adapter->registrypriv.RegDecryptCustomFile == 1)
+		phy_DecryptBBPgParaFile(Adapter, buffer);
+
+	ptmp = buffer;
+	for (szLine = GetLineFromBuffer(ptmp); szLine != NULL; szLine = GetLineFromBuffer(ptmp)) {
+		/*  skip comment */
+		if (IsCommentString(szLine)) {
+			continue;
+		}
+
+		if (loadingStage == 0) {
+			for (forCnt = 0; forCnt < TXPWR_LMT_MAX_REGULATION_NUM; ++forCnt)
+				memset((void *) regulation[forCnt], 0, 10);
+
+			memset((void *) band, 0, 10);
+			memset((void *) bandwidth, 0, 10);
+			memset((void *) rateSection, 0, 10);
+			memset((void *) rfPath, 0, 10);
+			memset((void *) colNumBuf, 0, 10);
+
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			szLine[--i] = ' '; /*  return the space in front of the regulation info */
+
+			/*  Parse the label of the table */
+			if (!ParseQualifiedString(szLine, &i, band, ' ', ',')) {
+				DBG_871X("Fail to parse band!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, bandwidth, ' ', ',')) {
+				DBG_871X("Fail to parse bandwidth!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, rfPath, ' ', ',')) {
+				DBG_871X("Fail to parse rf path!\n");
+				return _FAIL;
+			}
+			if (!ParseQualifiedString(szLine, &i, rateSection, ' ', ',')) {
+				DBG_871X("Fail to parse rate!\n");
+				return _FAIL;
+			}
+
+			loadingStage = 1;
+		} else if (loadingStage == 1) {
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			if (!eqNByte((u8 *)(szLine + i), (u8 *)("START"), 5)) {
+				DBG_871X("Lost \"##   START\" label\n");
+				return _FAIL;
+			}
+
+			loadingStage = 2;
+		} else if (loadingStage == 2) {
+			if (szLine[0] != '#' || szLine[1] != '#')
+				continue;
+
+			/*  skip the space */
+			i = 2;
+			while (szLine[i] == ' ' || szLine[i] == '\t')
+				++i;
+
+			if (!ParseQualifiedString(szLine, &i, colNumBuf, '#', '#')) {
+				DBG_871X("Fail to parse column number!\n");
+				return _FAIL;
+			}
+
+			if (!GetU1ByteIntegerFromStringInDecimal(colNumBuf, &colNum))
+				return _FAIL;
+
+			if (colNum > TXPWR_LMT_MAX_REGULATION_NUM) {
+				DBG_871X(
+					"unvalid col number %d (greater than max %d)\n",
+					colNum, TXPWR_LMT_MAX_REGULATION_NUM
+				);
+				return _FAIL;
+			}
+
+			for (forCnt = 0; forCnt < colNum; ++forCnt) {
+				u8 regulation_name_cnt = 0;
+
+				/*  skip the space */
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				while (szLine[i] != ' ' && szLine[i] != '\t' && szLine[i] != '\0')
+					regulation[forCnt][regulation_name_cnt++] = szLine[i++];
+				/* DBG_871X("regulation %s!\n", regulation[forCnt]); */
+
+				if (regulation_name_cnt == 0) {
+					DBG_871X("unvalid number of regulation!\n");
+					return _FAIL;
+				}
+			}
+
+			loadingStage = 3;
+		} else if (loadingStage == 3) {
+			char channel[10] = {0}, powerLimit[10] = {0};
+			u8 cnt = 0;
+
+			/*  the table ends */
+			if (szLine[0] == '#' && szLine[1] == '#') {
+				i = 2;
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				if (eqNByte((u8 *)(szLine + i), (u8 *)("END"), 3)) {
+					loadingStage = 0;
+					continue;
+				} else {
+					DBG_871X("Wrong format\n");
+					DBG_871X("<===== phy_ParsePowerLimitTableFile()\n");
+					return _FAIL;
+				}
+			}
+
+			if ((szLine[0] != 'c' && szLine[0] != 'C') ||
+				 (szLine[1] != 'h' && szLine[1] != 'H')) {
+				DBG_871X("Meet wrong channel => power limt pair\n");
+				continue;
+			}
+			i = 2;/*  move to the  location behind 'h' */
+
+			/*  load the channel number */
+			cnt = 0;
+			while (szLine[i] >= '0' && szLine[i] <= '9') {
+				channel[cnt] = szLine[i];
+				++cnt;
+				++i;
+			}
+			/* DBG_871X("chnl %s!\n", channel); */
+
+			for (forCnt = 0; forCnt < colNum; ++forCnt) {
+				/*  skip the space between channel number and the power limit value */
+				while (szLine[i] == ' ' || szLine[i] == '\t')
+					++i;
+
+				/*  load the power limit value */
+				cnt = 0;
+				fraction = 0;
+				memset((void *) powerLimit, 0, 10);
+				while ((szLine[i] >= '0' && szLine[i] <= '9') || szLine[i] == '.') {
+					if (szLine[i] == '.') {
+						if ((szLine[i+1] >= '0' && szLine[i+1] <= '9')) {
+							fraction = szLine[i+1];
+							i += 2;
+						} else {
+							DBG_871X("Wrong fraction in TXPWR_LMT.txt\n");
+							return _FAIL;
+						}
+
+						break;
+					}
+
+					powerLimit[cnt] = szLine[i];
+					++cnt;
+					++i;
+				}
+
+				if (powerLimit[0] == '\0') {
+					powerLimit[0] = '6';
+					powerLimit[1] = '3';
+					i += 2;
+				} else {
+					if (!GetU1ByteIntegerFromStringInDecimal(powerLimit, &limitValue))
+						return _FAIL;
+
+					limitValue *= 2;
+					cnt = 0;
+					if (fraction == '5')
+						++limitValue;
+
+					/*  the value is greater or equal to 100 */
+					if (limitValue >= 100) {
+						powerLimit[cnt++] = limitValue/100 + '0';
+						limitValue %= 100;
+
+						if (limitValue >= 10) {
+							powerLimit[cnt++] = limitValue/10 + '0';
+							limitValue %= 10;
+						} else
+							powerLimit[cnt++] = '0';
+
+						powerLimit[cnt++] = limitValue + '0';
+					} else if (limitValue >= 10) { /*  the value is greater or equal to 10 */
+						powerLimit[cnt++] = limitValue/10 + '0';
+						limitValue %= 10;
+						powerLimit[cnt++] = limitValue + '0';
+					}
+					/*  the value is less than 10 */
+					else
+						powerLimit[cnt++] = limitValue + '0';
+
+					powerLimit[cnt] = '\0';
+				}
+
+				/* DBG_871X("ch%s => %s\n", channel, powerLimit); */
+
+				/*  store the power limit value */
+				PHY_SetTxPowerLimit(Adapter, (u8 *)regulation[forCnt], (u8 *)band,
+					(u8 *)bandwidth, (u8 *)rateSection, (u8 *)rfPath, (u8 *)channel, (u8 *)powerLimit);
+
+			}
+		} else {
+			DBG_871X("Abnormal loading stage in phy_ParsePowerLimitTableFile()!\n");
+			rtStatus = _FAIL;
+			break;
+		}
+	}
+
+	DBG_871X("<===phy_ParsePowerLimitTableFile()\n");
+	return rtStatus;
+}
+
+int PHY_ConfigRFWithPowerLimitTableParaFile(
+	struct adapter *Adapter, char *pFileName
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	int	rlen = 0, rtStatus = _FAIL;
+
+	if (!(Adapter->registrypriv.load_phy_file & LOAD_RF_TXPWR_LMT_PARA_FILE))
+		return rtStatus;
+
+	memset(pHalData->para_file_buf, 0, MAX_PARA_FILE_BUF_LEN);
+
+	if ((pHalData->rf_tx_pwr_lmt_len == 0) && (pHalData->rf_tx_pwr_lmt == NULL)) {
+		rtw_merge_string(file_path_bs, PATH_MAX, rtw_phy_file_path, pFileName);
+
+		if (rtw_is_file_readable(file_path_bs) == true) {
+			rlen = rtw_retrive_from_file(file_path_bs, pHalData->para_file_buf, MAX_PARA_FILE_BUF_LEN);
+			if (rlen > 0) {
+				rtStatus = _SUCCESS;
+				pHalData->rf_tx_pwr_lmt = vzalloc(rlen);
+				if (pHalData->rf_tx_pwr_lmt) {
+					memcpy(pHalData->rf_tx_pwr_lmt, pHalData->para_file_buf, rlen);
+					pHalData->rf_tx_pwr_lmt_len = rlen;
+				} else
+					DBG_871X("%s rf_tx_pwr_lmt alloc fail !\n", __func__);
+			}
+		}
+	} else {
+		if ((pHalData->rf_tx_pwr_lmt_len != 0) && (pHalData->rf_tx_pwr_lmt != NULL)) {
+			memcpy(pHalData->para_file_buf, pHalData->rf_tx_pwr_lmt, pHalData->rf_tx_pwr_lmt_len);
+			rtStatus = _SUCCESS;
+		} else
+			DBG_871X("%s(): Critical Error !!!\n", __func__);
+	}
+
+	if (rtStatus == _SUCCESS) {
+		/* DBG_871X("%s(): read %s ok\n", __func__, pFileName); */
+		rtStatus = phy_ParsePowerLimitTableFile(Adapter, pHalData->para_file_buf);
+	} else
+		DBG_871X("%s(): No File %s, Load from HWImg Array!\n", __func__, pFileName);
+
+	return rtStatus;
+}
+
+void phy_free_filebuf(struct adapter *padapter)
+{
+	struct hal_com_data		*pHalData = GET_HAL_DATA(padapter);
+
+	if (pHalData->mac_reg)
+		vfree(pHalData->mac_reg);
+	if (pHalData->bb_phy_reg)
+		vfree(pHalData->bb_phy_reg);
+	if (pHalData->bb_agc_tab)
+		vfree(pHalData->bb_agc_tab);
+	if (pHalData->bb_phy_reg_pg)
+		vfree(pHalData->bb_phy_reg_pg);
+	if (pHalData->bb_phy_reg_mp)
+		vfree(pHalData->bb_phy_reg_mp);
+	if (pHalData->rf_radio_a)
+		vfree(pHalData->rf_radio_a);
+	if (pHalData->rf_radio_b)
+		vfree(pHalData->rf_radio_b);
+	if (pHalData->rf_tx_pwr_track)
+		vfree(pHalData->rf_tx_pwr_track);
+	if (pHalData->rf_tx_pwr_lmt)
+		vfree(pHalData->rf_tx_pwr_lmt);
+
+}
+
diff --git a/drivers/staging/rtl8723bs/hal/hal_intf.c b/drivers/staging/rtl8723bs/hal/hal_intf.c
new file mode 100644
index 000000000000..3463f40d6a31
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_intf.c
@@ -0,0 +1,474 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+
+#define _HAL_INTF_C_
+
+#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);
+	sint 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;
+		DBG_871X("rtw_hal_init: hal__init fail\n");
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_err_, ("-rtl871x_hal_init:status = 0x%x\n", status));
+
+	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;
+	} else {
+		DBG_871X("\n rtw_hal_deinit: hal_init fail\n");
+	}
+	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);
+	else
+		DBG_871X("%s: HalFunc.enable_interrupt is NULL!\n", __func__);
+
+}
+
+void rtw_hal_disable_interrupt(struct adapter *padapter)
+{
+	if (padapter->HalFunc.disable_interrupt)
+		padapter->HalFunc.disable_interrupt(padapter);
+	else
+		DBG_871X("%s: HalFunc.disable_interrupt is NULL!\n", __func__);
+
+}
+
+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);
+	else
+		DBG_871X("%s: HalFunc.check_ips_status is NULL!\n", __func__);
+
+	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 != NULL)
+		return padapter->HalFunc.init_xmit_priv(padapter);
+	return _FAIL;
+}
+
+void rtw_hal_free_xmit_priv(struct adapter *padapter)
+{
+	if (padapter->HalFunc.free_xmit_priv != NULL)
+		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)->bFwCurrentInPSMode == true) {
+		if (padapter->HalFunc.hal_dm_watchdog_in_lps)
+			padapter->HalFunc.hal_dm_watchdog_in_lps(padapter); /* this fuction caller is in interrupt context */
+	}
+}
+
+void rtw_hal_bcn_related_reg_setting(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_evt_read(struct adapter *adapter, u8 *buf)
+{
+	return c2h_evt_read_88xx(adapter, 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_is_disable_sw_channel_plan(struct adapter *padapter)
+{
+	return GET_HAL_DATA(padapter)->bDisableSWChannelPlan;
+}
+
+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);
+	else
+		DBG_871X("%s:  func[fill_h2c_cmd] not defined!\n", __func__);
+
+	return ret;
+}
diff --git a/drivers/staging/rtl8723bs/hal/hal_phy.c b/drivers/staging/rtl8723bs/hal/hal_phy.c
new file mode 100644
index 000000000000..c0a899df7aaf
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_phy.c
@@ -0,0 +1,224 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HAL_PHY_C_
+
+#include <drv_types.h>
+
+/**
+* Function:	PHY_CalculateBitShift
+*
+* OverView:	Get shifted position of the BitMask
+*
+* Input:
+*		u32 	BitMask,
+*
+* Output:	none
+* Return:		u32 	Return the shift bit bit position of the mask
+*/
+u32 PHY_CalculateBitShift(u32 BitMask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((BitMask>>i) &  0x1) == 1)
+			break;
+	}
+
+	return i;
+}
+
+
+/*  */
+/*  ==> RF shadow Operation API Code Section!!! */
+/*  */
+/*-----------------------------------------------------------------------------
+ * Function:	PHY_RFShadowRead
+ *			PHY_RFShadowWrite
+ *			PHY_RFShadowCompare
+ *			PHY_RFShadowRecorver
+ *			PHY_RFShadowCompareAll
+ *			PHY_RFShadowRecorverAll
+ *			PHY_RFShadowCompareFlagSet
+ *			PHY_RFShadowRecorverFlagSet
+ *
+ * Overview:	When we set RF register, we must write shadow at first.
+ *		When we are running, we must compare shadow abd locate error addr.
+ *		Decide to recorver or not.
+ *
+ * Input:       NONE
+ *
+ * Output:      NONE
+ *
+ * Return:      NONE
+ *
+ * Revised History:
+ * When			Who		Remark
+ * 11/20/2008	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------*/
+u32 PHY_RFShadowRead(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	return	RF_Shadow[eRFPath][Offset].Value;
+
+}	/* PHY_RFShadowRead */
+
+
+void PHY_RFShadowWrite(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u32 Data
+)
+{
+	RF_Shadow[eRFPath][Offset].Value = (Data & bRFRegOffsetMask);
+	RF_Shadow[eRFPath][Offset].Driver_Write = true;
+
+}	/* PHY_RFShadowWrite */
+
+
+bool PHY_RFShadowCompare(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	u32 reg;
+	/*  Check if we need to check the register */
+	if (RF_Shadow[eRFPath][Offset].Compare == true) {
+		reg = rtw_hal_read_rfreg(Adapter, eRFPath, Offset, bRFRegOffsetMask);
+		/*  Compare shadow and real rf register for 20bits!! */
+		if (RF_Shadow[eRFPath][Offset].Value != reg) {
+			/*  Locate error position. */
+			RF_Shadow[eRFPath][Offset].ErrorOrNot = true;
+			/* RT_TRACE(COMP_INIT, DBG_LOUD, */
+			/* PHY_RFShadowCompare RF-%d Addr%02lx Err = %05lx\n", */
+			/* eRFPath, Offset, reg)); */
+		}
+		return RF_Shadow[eRFPath][Offset].ErrorOrNot;
+	}
+	return false;
+}	/* PHY_RFShadowCompare */
+
+
+void PHY_RFShadowRecorver(IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset)
+{
+	/*  Check if the address is error */
+	if (RF_Shadow[eRFPath][Offset].ErrorOrNot == true) {
+		/*  Check if we need to recorver the register. */
+		if (RF_Shadow[eRFPath][Offset].Recorver == true) {
+			rtw_hal_write_rfreg(Adapter, eRFPath, Offset, bRFRegOffsetMask,
+							RF_Shadow[eRFPath][Offset].Value);
+			/* RT_TRACE(COMP_INIT, DBG_LOUD, */
+			/* PHY_RFShadowRecorver RF-%d Addr%02lx=%05lx", */
+			/* eRFPath, Offset, RF_Shadow[eRFPath][Offset].Value)); */
+		}
+	}
+
+}	/* PHY_RFShadowRecorver */
+
+
+void PHY_RFShadowCompareAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			PHY_RFShadowCompare(Adapter, eRFPath, Offset);
+		}
+	}
+
+}	/* PHY_RFShadowCompareAll */
+
+
+void PHY_RFShadowRecorverAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			PHY_RFShadowRecorver(Adapter, eRFPath, Offset);
+		}
+	}
+
+}	/* PHY_RFShadowRecorverAll */
+
+
+void
+PHY_RFShadowCompareFlagSet(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u8 Type
+)
+{
+	/*  Set True or False!!! */
+	RF_Shadow[eRFPath][Offset].Compare = Type;
+
+}	/* PHY_RFShadowCompareFlagSet */
+
+
+void PHY_RFShadowRecorverFlagSet(
+	IN PADAPTER Adapter, IN u8 eRFPath, IN u32 Offset, IN u8 Type
+)
+{
+	/*  Set True or False!!! */
+	RF_Shadow[eRFPath][Offset].Recorver = Type;
+
+}	/* PHY_RFShadowRecorverFlagSet */
+
+
+void PHY_RFShadowCompareFlagSetAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			/*  2008/11/20 MH For S3S4 test, we only check reg 26/27 now!!!! */
+			if (Offset != 0x26 && Offset != 0x27)
+				PHY_RFShadowCompareFlagSet(Adapter, eRFPath, Offset, false);
+			else
+				PHY_RFShadowCompareFlagSet(Adapter, eRFPath, Offset, true);
+		}
+	}
+
+}	/* PHY_RFShadowCompareFlagSetAll */
+
+
+void PHY_RFShadowRecorverFlagSetAll(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			/*  2008/11/20 MH For S3S4 test, we only check reg 26/27 now!!!! */
+			if (Offset != 0x26 && Offset != 0x27)
+				PHY_RFShadowRecorverFlagSet(Adapter, eRFPath, Offset, false);
+			else
+				PHY_RFShadowRecorverFlagSet(Adapter, eRFPath, Offset, true);
+		}
+	}
+
+}	/* PHY_RFShadowCompareFlagSetAll */
+
+void PHY_RFShadowRefresh(IN PADAPTER Adapter)
+{
+	u8 eRFPath = 0;
+	u32 Offset = 0, maxReg = GET_RF6052_REAL_MAX_REG(Adapter);
+
+	for (eRFPath = 0; eRFPath < RF6052_MAX_PATH; eRFPath++) {
+		for (Offset = 0; Offset < maxReg; Offset++) {
+			RF_Shadow[eRFPath][Offset].Value = 0;
+			RF_Shadow[eRFPath][Offset].Compare = false;
+			RF_Shadow[eRFPath][Offset].Recorver  = false;
+			RF_Shadow[eRFPath][Offset].ErrorOrNot = false;
+			RF_Shadow[eRFPath][Offset].Driver_Write = false;
+		}
+	}
+
+}	/* PHY_RFShadowRead */
diff --git a/drivers/staging/rtl8723bs/hal/hal_sdio.c b/drivers/staging/rtl8723bs/hal/hal_sdio.c
new file mode 100644
index 000000000000..e147c69e7222
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/hal_sdio.c
@@ -0,0 +1,115 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HAL_SDIO_C_
+
+#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 000000000000..2dbf19971e04
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm.c
@@ -0,0 +1,1446 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+static const u16 dB_Invert_Table[8][12] = {
+	{1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4},
+	{4, 5, 6, 6, 7, 8, 9, 10, 11, 13, 14, 16},
+	{18, 20, 22, 25, 28, 32, 35, 40, 45, 50, 56, 63},
+	{71, 79, 89, 100, 112, 126, 141, 158, 178, 200, 224, 251},
+	{282, 316, 355, 398, 447, 501, 562, 631, 708, 794, 891, 1000},
+	{1122, 1259, 1413, 1585, 1778, 1995, 2239, 2512, 2818, 3162, 3548, 3981},
+	{4467, 5012, 5623, 6310, 7079, 7943, 8913, 10000, 11220, 12589, 14125,
+	 15849},
+	{17783, 19953, 22387, 25119, 28184, 31623, 35481, 39811, 44668, 50119,
+	 56234, 65535}
+ };
+
+/*  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 */
+};
+
+/*  Local Function predefine. */
+
+/* START------------COMMON INFO RELATED--------------- */
+void odm_CommonInfoSelfInit(PDM_ODM_T pDM_Odm);
+
+void odm_CommonInfoSelfUpdate(PDM_ODM_T pDM_Odm);
+
+void odm_CmnInfoInit_Debug(PDM_ODM_T pDM_Odm);
+
+void odm_BasicDbgMessage(PDM_ODM_T pDM_Odm);
+
+/* END------------COMMON INFO RELATED--------------- */
+
+/* START---------------DIG--------------------------- */
+
+/* Remove by Yuchen */
+
+/* END---------------DIG--------------------------- */
+
+/* START-------BB POWER SAVE----------------------- */
+/* Remove BB power Saving by YuChen */
+/* END---------BB POWER SAVE----------------------- */
+
+void odm_RefreshRateAdaptiveMaskCE(PDM_ODM_T pDM_Odm);
+
+/* Remove by YuChen */
+
+void odm_RSSIMonitorInit(PDM_ODM_T pDM_Odm);
+
+void odm_RSSIMonitorCheckCE(PDM_ODM_T pDM_Odm);
+
+void odm_RSSIMonitorCheck(PDM_ODM_T pDM_Odm);
+
+void odm_SwAntDetectInit(PDM_ODM_T pDM_Odm);
+
+void odm_SwAntDivChkAntSwitchCallback(void *FunctionContext);
+
+
+
+void odm_GlobalAdapterCheck(void);
+
+void odm_RefreshRateAdaptiveMask(PDM_ODM_T pDM_Odm);
+
+void ODM_TXPowerTrackingCheck(PDM_ODM_T pDM_Odm);
+
+void odm_RateAdaptiveMaskInit(PDM_ODM_T pDM_Odm);
+
+void odm_TXPowerTrackingThermalMeterInit(PDM_ODM_T pDM_Odm);
+
+
+void odm_TXPowerTrackingInit(PDM_ODM_T pDM_Odm);
+
+void odm_TXPowerTrackingCheckCE(PDM_ODM_T pDM_Odm);
+
+/* Remove Edca by Yu Chen */
+
+
+#define RxDefaultAnt1		0x65a9
+#define RxDefaultAnt2		0x569a
+
+void odm_InitHybridAntDiv(PDM_ODM_T pDM_Odm);
+
+bool odm_StaDefAntSel(
+	PDM_ODM_T pDM_Odm,
+	u32 OFDM_Ant1_Cnt,
+	u32 OFDM_Ant2_Cnt,
+	u32 CCK_Ant1_Cnt,
+	u32 CCK_Ant2_Cnt,
+	u8 *pDefAnt
+);
+
+void odm_SetRxIdleAnt(PDM_ODM_T pDM_Odm, u8 Ant, bool bDualPath);
+
+
+
+void odm_HwAntDiv(PDM_ODM_T pDM_Odm);
+
+
+/*  */
+/* 3 Export Interface */
+/*  */
+
+/*  */
+/*  2011/09/21 MH Add to describe different team necessary resource allocate?? */
+/*  */
+void ODM_DMInit(PDM_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);
+
+	if (*(pDM_Odm->mp_mode) != 1)
+		odm_PathDiversityInit(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(PDM_ODM_T pDM_Odm)
+{
+	odm_CommonInfoSelfUpdate(pDM_Odm);
+	odm_BasicDbgMessage(pDM_Odm);
+	odm_FalseAlarmCounterStatistics(pDM_Odm);
+	odm_NHMCounterStatistics(pDM_Odm);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): RSSI = 0x%x\n", pDM_Odm->RSSI_Min));
+
+	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_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("----Step1: odm_DIG is in LPS mode\n"));
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("---Step2: 8723AS is in LPS mode\n"));
+			odm_DIGbyRSSI_LPS(pDM_Odm);
+	} else
+		odm_DIG(pDM_Odm);
+
+	{
+		pDIG_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_PathDiversity(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(PDM_ODM_T pDM_Odm, 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_RF_TYPE:
+		pDM_Odm->RFType = (u8)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_MP_TEST_CHIP:
+		pDM_Odm->bIsMPChip = (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_BOARD_TYPE:
+		pDM_Odm->BoardType = (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_5G_EXT_LNA:
+		pDM_Odm->ExtLNA5G = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_EXT_PA:
+		pDM_Odm->ExtPA = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_5G_EXT_PA:
+		pDM_Odm->ExtPA5G = (u8)Value;
+		break;
+
+	case ODM_CMNINFO_GPA:
+		pDM_Odm->TypeGPA = (ODM_TYPE_GPA_E)Value;
+		break;
+	case ODM_CMNINFO_APA:
+		pDM_Odm->TypeAPA = (ODM_TYPE_APA_E)Value;
+		break;
+	case ODM_CMNINFO_GLNA:
+		pDM_Odm->TypeGLNA = (ODM_TYPE_GLNA_E)Value;
+		break;
+	case ODM_CMNINFO_ALNA:
+		pDM_Odm->TypeALNA = (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(PDM_ODM_T pDM_Odm, 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 = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_TX_UNI:
+		pDM_Odm->pNumTxBytesUnicast = (u64 *)pValue;
+		break;
+
+	case ODM_CMNINFO_RX_UNI:
+		pDM_Odm->pNumRxBytesUnicast = (u64 *)pValue;
+		break;
+
+	case ODM_CMNINFO_WM_MODE:
+		pDM_Odm->pwirelessmode = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_BAND:
+		pDM_Odm->pBandType = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_CHNL_OFFSET:
+		pDM_Odm->pSecChOffset = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_SEC_MODE:
+		pDM_Odm->pSecurity = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_BW:
+		pDM_Odm->pBandWidth = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_CHNL:
+		pDM_Odm->pChannel = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_GET_VALUE:
+		pDM_Odm->pbGetValueFromOtherMac = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_BUDDY_ADAPTOR:
+		pDM_Odm->pBuddyAdapter = (struct adapter **)pValue;
+		break;
+
+	case ODM_CMNINFO_DMSP_IS_MASTER:
+		pDM_Odm->pbMasterOfDMSP = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_SCAN:
+		pDM_Odm->pbScanInProcess = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_POWER_SAVING:
+		pDM_Odm->pbPowerSaving = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_ONE_PATH_CCA:
+		pDM_Odm->pOnePathCCA = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_DRV_STOP:
+		pDM_Odm->pbDriverStopped =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_PNP_IN:
+		pDM_Odm->pbDriverIsGoingToPnpSetPowerSleep =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_INIT_ON:
+		pDM_Odm->pinit_adpt_in_progress =  (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_ANT_TEST:
+		pDM_Odm->pAntennaTest =  (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_NET_CLOSED:
+		pDM_Odm->pbNet_closed = (bool *)pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_RATE:
+		pDM_Odm->pForcedDataRate = (u16 *)pValue;
+		break;
+
+	case ODM_CMNINFO_FORCED_IGI_LB:
+		pDM_Odm->pu1ForcedIgiLb = (u8 *)pValue;
+		break;
+
+	case ODM_CMNINFO_MP_MODE:
+		pDM_Odm->mp_mode = (u8 *)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 = (ODM_MAC_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(
+	PDM_ODM_T pDM_Odm,
+	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(PDM_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_RF_TYPE:
+		pDM_Odm->RFType = (u8)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_DBG_COMP:
+		pDM_Odm->DebugComponents = Value;
+		break;
+
+	case ODM_CMNINFO_DBG_LEVEL:
+		pDM_Odm->DebugLevel = (u32)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_BAND:
+		pDM_Odm->BandType = (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;
+	}
+
+
+}
+
+void odm_CommonInfoSelfInit(PDM_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));
+
+	ODM_InitDebugSetting(pDM_Odm);
+
+	pDM_Odm->TxRate = 0xFF;
+}
+
+void odm_CommonInfoSelfUpdate(PDM_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;
+}
+
+void odm_CmnInfoInit_Debug(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_CmnInfoInit_Debug ==>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportPlatform =%d\n", pDM_Odm->SupportPlatform));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportAbility = 0x%x\n", pDM_Odm->SupportAbility));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportInterface =%d\n", pDM_Odm->SupportInterface));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("SupportICType = 0x%x\n", pDM_Odm->SupportICType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("CutVersion =%d\n", pDM_Odm->CutVersion));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("FabVersion =%d\n", pDM_Odm->FabVersion));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RFType =%d\n", pDM_Odm->RFType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("BoardType =%d\n", pDM_Odm->BoardType));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtLNA =%d\n", pDM_Odm->ExtLNA));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtPA =%d\n", pDM_Odm->ExtPA));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("ExtTRSW =%d\n", pDM_Odm->ExtTRSW));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("PatchID =%d\n", pDM_Odm->PatchID));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bInHctTest =%d\n", pDM_Odm->bInHctTest));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bWIFITest =%d\n", pDM_Odm->bWIFITest));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bDualMacSmartConcurrent =%d\n", pDM_Odm->bDualMacSmartConcurrent));
+
+}
+
+void odm_BasicDbgMessage(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_BasicDbgMsg ==>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("bLinked = %d, RSSI_Min = %d,\n",
+		pDM_Odm->bLinked, pDM_Odm->RSSI_Min));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RxRate = 0x%x, RSSI_A = %d, RSSI_B = %d\n",
+		pDM_Odm->RxRate, pDM_Odm->RSSI_A, pDM_Odm->RSSI_B));
+}
+
+/* 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 RATR MASK */
+/* 3 ============================================================ */
+/* 3 ============================================================ */
+/* 3 Rate Adaptive */
+/* 3 ============================================================ */
+
+void odm_RateAdaptiveMaskInit(PDM_ODM_T pDM_Odm)
+{
+	PODM_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(
+	PDM_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):
+	case (ODM_WM_A):
+		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):
+	case (ODM_WM_A|ODM_WM_N5G):
+		if (pDM_Odm->RFType == ODM_1T2R || pDM_Odm->RFType == ODM_1T1R) {
+			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;
+			}
+		} else {
+			if (rssi_level == DM_RATR_STA_HIGH)
+				rate_bitmap = 0x0f8f0000;
+			else if (rssi_level == DM_RATR_STA_MIDDLE)
+				rate_bitmap = 0x0f8ff000;
+			else {
+				if (*(pDM_Odm->pBandWidth) == ODM_BW40M)
+					rate_bitmap = 0x0f8ff015;
+				else
+					rate_bitmap = 0x0f8ff005;
+			}
+		}
+		break;
+
+	case (ODM_WM_AC|ODM_WM_G):
+		if (rssi_level == 1)
+			rate_bitmap = 0xfc3f0000;
+		else if (rssi_level == 2)
+			rate_bitmap = 0xfffff000;
+		else
+			rate_bitmap = 0xffffffff;
+		break;
+
+	case (ODM_WM_AC|ODM_WM_A):
+
+		if (pDM_Odm->RFType == RF_1T1R) {
+			if (rssi_level == 1)				/*  add by Gary for ac-series */
+				rate_bitmap = 0x003f8000;
+			else if (rssi_level == 2)
+				rate_bitmap = 0x003ff000;
+			else
+				rate_bitmap = 0x003ff010;
+		} else {
+			if (rssi_level == 1)				/*  add by Gary for ac-series */
+				rate_bitmap = 0xfe3f8000;       /*  VHT 2SS MCS3~9 */
+			else if (rssi_level == 2)
+				rate_bitmap = 0xfffff000;       /*  VHT 2SS MCS0~9 */
+			else
+				rate_bitmap = 0xfffff010;       /*  All */
+		}
+		break;
+
+	default:
+		if (pDM_Odm->RFType == RF_1T2R)
+			rate_bitmap = 0x000fffff;
+		else
+			rate_bitmap = 0x0fffffff;
+		break;
+	}
+
+	/* printk("%s ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x\n", __func__, rssi_level, WirelessMode, rate_bitmap); */
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, (" ==> rssi_level:0x%02x, WirelessMode:0x%02x, rate_bitmap:0x%08x\n", rssi_level, WirelessMode, rate_bitmap));
+
+	return (ra_mask&rate_bitmap);
+
+}
+
+/*-----------------------------------------------------------------------------
+* 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.
+*
+* --------------------------------------------------------------------------
+*/
+void odm_RefreshRateAdaptiveMask(PDM_ODM_T pDM_Odm)
+{
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("odm_RefreshRateAdaptiveMask()---------->\n"));
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RA_MASK)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("odm_RefreshRateAdaptiveMask(): Return cos not supported\n"));
+		return;
+	}
+	odm_RefreshRateAdaptiveMaskCE(pDM_Odm);
+}
+
+void odm_RefreshRateAdaptiveMaskCE(PDM_ODM_T pDM_Odm)
+{
+	u8 i;
+	struct adapter *padapter =  pDM_Odm->Adapter;
+
+	if (padapter->bDriverStopped) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_TRACE, ("<---- odm_RefreshRateAdaptiveMask(): driver is going to unload\n"));
+		return;
+	}
+
+	if (!pDM_Odm->bUseRAMask) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("<---- odm_RefreshRateAdaptiveMask(): driver does not control rate adaptive mask\n"));
+		return;
+	}
+
+	/* printk("==> %s\n", __func__); */
+
+	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 (IS_MCAST(pstat->hwaddr))
+				continue;
+
+			if (true == ODM_RAStateCheck(pDM_Odm, pstat->rssi_stat.UndecoratedSmoothedPWDB, false, &pstat->rssi_level)) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("RSSI:%d, RSSI_LEVEL:%d\n", pstat->rssi_stat.UndecoratedSmoothedPWDB, 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);
+			}
+
+		}
+	}
+}
+
+/*  Return Value: bool */
+/*  - true: RATRState is changed. */
+bool ODM_RAStateCheck(
+	PDM_ODM_T pDM_Odm,
+	s32 RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+)
+{
+	PODM_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:
+		ODM_RT_ASSERT(pDM_Odm, false, ("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) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_RA_MASK, ODM_DBG_LOUD, ("RSSI Level %d -> %d\n", *pRATRState, RATRState));
+		*pRATRState = RATRState;
+		return true;
+	}
+
+	return false;
+}
+
+
+/*  */
+
+/* 3 ============================================================ */
+/* 3 Dynamic Tx Power */
+/* 3 ============================================================ */
+
+/* Remove BY YuChen */
+
+/* 3 ============================================================ */
+/* 3 RSSI Monitor */
+/* 3 ============================================================ */
+
+void odm_RSSIMonitorInit(PDM_ODM_T pDM_Odm)
+{
+	pRA_T pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	pRA_Table->firstconnect = false;
+
+}
+
+void odm_RSSIMonitorCheck(PDM_ODM_T pDM_Odm)
+{
+	if (!(pDM_Odm->SupportAbility & ODM_BB_RSSI_MONITOR))
+		return;
+
+	odm_RSSIMonitorCheckCE(pDM_Odm);
+
+}	/*  odm_RSSIMonitorCheck */
+
+static void FindMinimumRSSI(struct adapter *padapter)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+	struct dm_priv *pdmpriv = &pHalData->dmpriv;
+	PDM_ODM_T pDM_Odm = &(pHalData->odmpriv);
+
+	/* 1 1.Determine the minimum RSSI */
+
+	if (
+		(pDM_Odm->bLinked != true) &&
+		(pdmpriv->EntryMinUndecoratedSmoothedPWDB == 0)
+	) {
+		pdmpriv->MinUndecoratedPWDBForDM = 0;
+		/* ODM_RT_TRACE(pDM_Odm, COMP_BB_POWERSAVING, DBG_LOUD, ("Not connected to any\n")); */
+	} else
+		pdmpriv->MinUndecoratedPWDBForDM = pdmpriv->EntryMinUndecoratedSmoothedPWDB;
+
+	/* DBG_8192C("%s =>MinUndecoratedPWDBForDM(%d)\n", __func__, pdmpriv->MinUndecoratedPWDBForDM); */
+	/* ODM_RT_TRACE(pDM_Odm, COMP_DIG, DBG_LOUD, ("MinUndecoratedPWDBForDM =%d\n", pHalData->MinUndecoratedPWDBForDM)); */
+}
+
+void odm_RSSIMonitorCheckCE(PDM_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 */
+	bool FirstConnect = false;
+	pRA_T pRA_Table = &pDM_Odm->DM_RA_Table;
+
+	if (pDM_Odm->bLinked != true)
+		return;
+
+	FirstConnect = (pDM_Odm->bLinked) && (pRA_Table->firstconnect == false);
+	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); */
+}
+
+/* 3 ============================================================ */
+/* 3 Tx Power Tracking */
+/* 3 ============================================================ */
+
+void odm_TXPowerTrackingInit(PDM_ODM_T pDM_Odm)
+{
+	odm_TXPowerTrackingThermalMeterInit(pDM_Odm);
+}
+
+static u8 getSwingIndex(PDM_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_TXPowerTrackingThermalMeterInit(PDM_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;
+
+
+	/* MSG_8192C("pdmpriv->TxPowerTrackControl = %d\n", pdmpriv->TxPowerTrackControl); */
+
+	/* 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 = ODM_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(PDM_ODM_T pDM_Odm)
+{
+	odm_TXPowerTrackingCheckCE(pDM_Odm);
+}
+
+void odm_TXPowerTrackingCheckCE(PDM_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, ODM_RF_PATH_A, RF_T_METER_NEW, (BIT17 | BIT16), 0x03);
+
+		/* DBG_871X("Trigger Thermal Meter!!\n"); */
+
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 1;
+		return;
+	} else {
+		/* DBG_871X("Schedule TxPowerTracking direct call!!\n"); */
+		ODM_TXPowerTrackingCallback_ThermalMeter(Adapter);
+		pDM_Odm->RFCalibrateInfo.TM_Trigger = 0;
+	}
+}
+
+/* 3 ============================================================ */
+/* 3 SW Antenna Diversity */
+/* 3 ============================================================ */
+void odm_SwAntDetectInit(PDM_ODM_T pDM_Odm)
+{
+	pSWAT_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;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm.h b/drivers/staging/rtl8723bs/hal/odm.h
new file mode 100644
index 000000000000..0b3541a91548
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm.h
@@ -0,0 +1,1465 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__HALDMOUTSRC_H__
+#define __HALDMOUTSRC_H__
+
+
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_PathDiv.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+#include "odm_NoiseMonitor.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 */
+
+typedef 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;
+} Pri_CCA_T, *pPri_CCA_T;
+
+typedef struct _Rate_Adaptive_Table_ {
+	u8 firstconnect;
+} RA_T, *pRA_T;
+
+typedef struct _RX_High_Power_ {
+	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;
+	RT_TIMER PSDTimer;
+} RXHP_T, *pRXHP_T;
+
+#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
+
+typedef struct _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 Ant5G;
+	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;
+	RT_TIMER SwAntennaSwitchTimer;
+	RT_TIMER SwAntennaSwitchTimer_8723B;
+	u32 PktCnt_SWAntDivByCtrlFrame;
+	bool bSWAntDivByCtrlFrame;
+} SWAT_T, *pSWAT_T;
+
+/* Remove Edca by YuChen */
+
+
+typedef 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 */
+
+} ODM_RATE_ADAPTIVE, *PODM_RATE_ADAPTIVE;
+
+
+#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+24+21 /*  Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
+
+#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
+
+typedef struct _ODM_Phy_Status_Info_ {
+	/*  */
+	/*  Be care, if you want to add any element please insert between */
+	/*  RxPWDBAll & SignalStrength. */
+	/*  */
+	u8 RxPWDBAll;
+
+	u8 SignalQuality;			/*  in 0-100 index. */
+	s8 RxMIMOSignalQuality[4];	/* per-path's EVM */
+	u8 RxMIMOEVMdbm[4];		/* per-path's EVM dbm */
+
+	u8 RxMIMOSignalStrength[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 BTRxRSSIPercentage;
+	u8 SignalStrength;			/*  in 0-100 index. */
+
+	s8 RxPwr[4];				/* per-path's pwdb */
+
+	u8 RxSNR[4];				/* per-path's SNR */
+	u8 BandWidth;
+	u8 btCoexPwrAdjust;
+} ODM_PHY_INFO_T, *PODM_PHY_INFO_T;
+
+
+typedef struct _ODM_Per_Pkt_Info_ {
+	/* u8 Rate; */
+	u8 DataRate;
+	u8 StationID;
+	bool bPacketMatchBSSID;
+	bool bPacketToSelf;
+	bool bPacketBeacon;
+	bool bToSelf;
+} ODM_PACKET_INFO_T, *PODM_PACKET_INFO_T;
+
+
+typedef struct _ODM_Phy_Dbg_Info_ {
+	/* ODM Write, debug info */
+	s8 RxSNRdB[4];
+	u32 NumQryPhyStatus;
+	u32 NumQryPhyStatusCCK;
+	u32 NumQryPhyStatusOFDM;
+	u8 NumQryBeaconPkt;
+	/* Others */
+	s32 RxEVM[4];
+
+} ODM_PHY_DBG_INFO_T;
+
+
+typedef struct _ODM_Mac_Status_Info_ {
+	u8 test;
+} ODM_MAC_INFO;
+
+
+typedef enum tag_Dynamic_ODM_Support_Ability_Type {
+	/*  BB Team */
+	ODM_DIG				= 0x00000001,
+	ODM_HIGH_POWER		= 0x00000002,
+	ODM_CCK_CCA_TH		= 0x00000004,
+	ODM_FA_STATISTICS	= 0x00000008,
+	ODM_RAMASK			= 0x00000010,
+	ODM_RSSI_MONITOR	= 0x00000020,
+	ODM_SW_ANTDIV		= 0x00000040,
+	ODM_HW_ANTDIV		= 0x00000080,
+	ODM_BB_PWRSV		= 0x00000100,
+	ODM_2TPATHDIV		= 0x00000200,
+	ODM_1TPATHDIV		= 0x00000400,
+	ODM_PSD2AFH			= 0x00000800
+} ODM_Ability_E;
+
+/*  */
+/*  2011/20/20 MH For MP driver RT_WLAN_STA =  STA_INFO_T */
+/*  Please declare below ODM relative info in your STA info structure. */
+/*  */
+typedef struct _ODM_STA_INFO {
+	/*  Driver Write */
+	bool bUsed;				/*  record the sta status link or not? */
+	/* u8 WirelessMode;		 */
+	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];
+
+	/*  ODM Write */
+	/* 1 TX_INFO (may changed by IC) */
+	/* TX_INFO_T		pTxInfo;		Define in IC folder. Move lower layer. */
+
+	/*  */
+	/* 	Please use compile flag to disabe the strcutrue for other IC except 88E. */
+	/* 	Move To lower layer. */
+	/*  */
+	/*  ODM Write Wilson will handle this part(said by Luke.Lee) */
+	/* TX_RPT_T		pTxRpt;			Define in IC folder. Move lower layer. */
+} ODM_STA_INFO_T, *PODM_STA_INFO_T;
+
+/*  */
+/*  2011/10/20 MH Define Common info enum for all team. */
+/*  */
+typedef enum _ODM_Common_Info_Definition {
+	/*  Fixed value: */
+
+	/* HOOK BEFORE REG INIT----------- */
+	ODM_CMNINFO_PLATFORM = 0,
+	ODM_CMNINFO_ABILITY,					/*  ODM_ABILITY_E */
+	ODM_CMNINFO_INTERFACE,				/*  ODM_INTERFACE_E */
+	ODM_CMNINFO_MP_TEST_CHIP,
+	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_RF_TYPE,					/*  ODM_RF_PATH_E or ODM_RF_TYPE_E? */
+	ODM_CMNINFO_RFE_TYPE,
+	ODM_CMNINFO_BOARD_TYPE,				/*  ODM_BOARD_TYPE_E */
+	ODM_CMNINFO_PACKAGE_TYPE,
+	ODM_CMNINFO_EXT_LNA,					/*  true */
+	ODM_CMNINFO_5G_EXT_LNA,
+	ODM_CMNINFO_EXT_PA,
+	ODM_CMNINFO_5G_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_BAND,		/*  ODM_BAND_TYPE_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,
+
+
+} ODM_CMNINFO_E;
+
+/*  2011/10/20 MH Define ODM support ability.  ODM_CMNINFO_ABILITY */
+typedef 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_ABILITY_E;
+
+/* 	ODM_CMNINFO_INTERFACE */
+typedef enum tag_ODM_Support_Interface_Definition {
+	ODM_ITRF_SDIO	=	0x4,
+	ODM_ITRF_ALL	=	0x7,
+} ODM_INTERFACE_E;
+
+/*  ODM_CMNINFO_IC_TYPE */
+typedef enum tag_ODM_Support_IC_Type_Definition {
+	ODM_RTL8723B	=	BIT8,
+} ODM_IC_TYPE_E;
+
+/* ODM_CMNINFO_CUT_VER */
+typedef 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_CUT_VERSION_E;
+
+/*  ODM_CMNINFO_FAB_VER */
+typedef enum tag_ODM_Fab_Version_Definition {
+	ODM_TSMC	=	0,
+	ODM_UMC		=	1,
+} ODM_FAB_E;
+
+/*  ODM_CMNINFO_RF_TYPE */
+/*  */
+/*  For example 1T2R (A+AB = BIT0|BIT4|BIT5) */
+/*  */
+typedef enum tag_ODM_RF_Path_Bit_Definition {
+	ODM_RF_TX_A	=	BIT0,
+	ODM_RF_TX_B	=	BIT1,
+	ODM_RF_TX_C	=	BIT2,
+	ODM_RF_TX_D	=	BIT3,
+	ODM_RF_RX_A	=	BIT4,
+	ODM_RF_RX_B	=	BIT5,
+	ODM_RF_RX_C	=	BIT6,
+	ODM_RF_RX_D	=	BIT7,
+} ODM_RF_PATH_E;
+
+
+typedef 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_RF_TYPE_E;
+
+
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+/* typedef enum _MACPHY_MODE_8192D{ */
+/* 	SINGLEMAC_SINGLEPHY, */
+/* 	DUALMAC_DUALPHY, */
+/* 	DUALMAC_SINGLEPHY, */
+/* MACPHY_MODE_8192D,*PMACPHY_MODE_8192D; */
+/*  Above is the original define in MP driver. Please use the same define. THX. */
+typedef enum tag_ODM_MAC_PHY_Mode_Definition {
+	ODM_SMSP	= 0,
+	ODM_DMSP	= 1,
+	ODM_DMDP	= 2,
+} ODM_MAC_PHY_MODE_E;
+
+
+typedef enum tag_BT_Coexist_Definition {
+	ODM_BT_BUSY		= 1,
+	ODM_BT_ON		= 2,
+	ODM_BT_OFF		= 3,
+	ODM_BT_NONE		= 4,
+} ODM_BT_COEXIST_E;
+
+/*  ODM_CMNINFO_OP_MODE */
+typedef enum tag_Operation_Mode_Definition {
+	ODM_NO_LINK      = BIT0,
+	ODM_LINK         = BIT1,
+	ODM_SCAN         = BIT2,
+	ODM_POWERSAVE    = BIT3,
+	ODM_AP_MODE      = BIT4,
+	ODM_CLIENT_MODE  = BIT5,
+	ODM_AD_HOC       = BIT6,
+	ODM_WIFI_DIRECT  = BIT7,
+	ODM_WIFI_DISPLAY = BIT8,
+} ODM_OPERATION_MODE_E;
+
+/*  ODM_CMNINFO_WM_MODE */
+typedef enum tag_Wireless_Mode_Definition {
+	ODM_WM_UNKNOW     = 0x0,
+	ODM_WM_B          = BIT0,
+	ODM_WM_G          = BIT1,
+	ODM_WM_A          = BIT2,
+	ODM_WM_N24G       = BIT3,
+	ODM_WM_N5G        = BIT4,
+	ODM_WM_AUTO       = BIT5,
+	ODM_WM_AC         = BIT6,
+} ODM_WIRELESS_MODE_E;
+
+/*  ODM_CMNINFO_BAND */
+typedef enum tag_Band_Type_Definition {
+	ODM_BAND_2_4G = 0,
+	ODM_BAND_5G,
+	ODM_BAND_ON_BOTH,
+	ODM_BANDMAX
+} ODM_BAND_TYPE_E;
+
+/*  ODM_CMNINFO_SEC_CHNL_OFFSET */
+typedef enum tag_Secondary_Channel_Offset_Definition {
+	ODM_DONT_CARE	= 0,
+	ODM_BELOW		= 1,
+	ODM_ABOVE		= 2
+} ODM_SEC_CHNL_OFFSET_E;
+
+/*  ODM_CMNINFO_SEC_MODE */
+typedef enum tag_Security_Definition {
+	ODM_SEC_OPEN		= 0,
+	ODM_SEC_WEP40		= 1,
+	ODM_SEC_TKIP		= 2,
+	ODM_SEC_RESERVE		= 3,
+	ODM_SEC_AESCCMP		= 4,
+	ODM_SEC_WEP104		= 5,
+	ODM_WEP_WPA_MIXED	= 6, /*  WEP + WPA */
+	ODM_SEC_SMS4		= 7,
+} ODM_SECURITY_E;
+
+/*  ODM_CMNINFO_BW */
+typedef enum tag_Bandwidth_Definition {
+	ODM_BW20M		= 0,
+	ODM_BW40M		= 1,
+	ODM_BW80M		= 2,
+	ODM_BW160M		= 3,
+	ODM_BW10M		= 4,
+} ODM_BW_E;
+
+
+/*  ODM_CMNINFO_BOARD_TYPE */
+/*  For non-AC-series IC , ODM_BOARD_5G_EXT_PA and ODM_BOARD_5G_EXT_LNA are ignored */
+/*  For AC-series IC, external PA & LNA can be indivisuallly added on 2.4G and/or 5G */
+typedef enum tag_Board_Definition {
+	ODM_BOARD_DEFAULT    = 0,      /*  The DEFAULT case. */
+	ODM_BOARD_MINICARD   = BIT(0), /*  0 = non-mini card, 1 = mini card. */
+	ODM_BOARD_SLIM       = BIT(1), /*  0 = non-slim card, 1 = slim card */
+	ODM_BOARD_BT         = BIT(2), /*  0 = without BT card, 1 = with BT */
+	ODM_BOARD_EXT_PA     = BIT(3), /*  0 = no 2G ext-PA, 1 = existing 2G ext-PA */
+	ODM_BOARD_EXT_LNA    = BIT(4), /*  0 = no 2G ext-LNA, 1 = existing 2G ext-LNA */
+	ODM_BOARD_EXT_TRSW   = BIT(5), /*  0 = no ext-TRSW, 1 = existing ext-TRSW */
+	ODM_BOARD_EXT_PA_5G  = BIT(6), /*  0 = no 5G ext-PA, 1 = existing 5G ext-PA */
+	ODM_BOARD_EXT_LNA_5G = BIT(7), /*  0 = no 5G ext-LNA, 1 = existing 5G ext-LNA */
+} ODM_BOARD_TYPE_E;
+
+typedef enum tag_ODM_Package_Definition {
+	ODM_PACKAGE_DEFAULT      = 0,
+	ODM_PACKAGE_QFN68        = BIT(0),
+	ODM_PACKAGE_TFBGA90      = BIT(1),
+	ODM_PACKAGE_TFBGA79      = BIT(2),
+} ODM_Package_TYPE_E;
+
+typedef enum tag_ODM_TYPE_GPA_Definition {
+	TYPE_GPA0 = 0,
+	TYPE_GPA1 = BIT(1)|BIT(0)
+} ODM_TYPE_GPA_E;
+
+typedef enum tag_ODM_TYPE_APA_Definition {
+	TYPE_APA0 = 0,
+	TYPE_APA1 = BIT(1)|BIT(0)
+} ODM_TYPE_APA_E;
+
+typedef enum 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)
+} ODM_TYPE_GLNA_E;
+
+typedef enum 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)
+} ODM_TYPE_ALNA_E;
+
+/*  ODM_CMNINFO_ONE_PATH_CCA */
+typedef enum tag_CCA_Path {
+	ODM_CCA_2R			= 0,
+	ODM_CCA_1R_A		= 1,
+	ODM_CCA_1R_B		= 2,
+} ODM_CCA_PATH_E;
+
+
+typedef 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;
+} ODM_RA_INFO_T, *PODM_RA_INFO_T;
+
+typedef struct _IQK_MATRIX_REGS_SETTING {
+	bool bIQKDone;
+	s32 Value[3][IQK_Matrix_REG_NUM];
+	bool bBWIqkResultSaved[3];
+} IQK_MATRIX_REGS_SETTING, *PIQK_MATRIX_REGS_SETTING;
+
+
+/* Remove PATHDIV_PARA struct to odm_PathDiv.h */
+
+typedef struct 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 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 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;
+	IQK_MATRIX_REGS_SETTING IQKMatrixRegSetting[IQK_Matrix_Settings_NUM];
+	bool bNeedIQK;
+	bool bIQKInProgress;
+	u8 Delta_IQK;
+	u8 Delta_LCK;
+	s8 BBSwingDiff2G, BBSwingDiff5G; /*  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_5GA_P[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GA_N[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GB_P[BAND_NUM][DELTA_SWINGIDX_SIZE];
+	u8 DeltaSwingTableIdx_5GB_N[BAND_NUM][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_RF_CAL_T, *PODM_RF_CAL_T;
+/*  */
+/*  ODM Dynamic common info value definition */
+/*  */
+
+typedef struct _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;
+	u8 idx_AntDiv_counter_5G;
+	u32 AntDiv_2G_5G;
+	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;
+
+} FAT_T, *pFAT_T;
+
+typedef enum _FAT_STATE {
+	FAT_NORMAL_STATE			= 0,
+	FAT_TRAINING_STATE		= 1,
+} FAT_STATE_E, *PFAT_STATE_E;
+
+typedef 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,
+	CGCS_RX_SW_ANTDIV	= 0x05,
+	S0S1_SW_ANTDIV          = 0x06 /* 8723B intrnal switch S0 S1 */
+} ANT_DIV_TYPE_E, *PANT_DIV_TYPE_E;
+
+typedef struct _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];
+} PATHDIV_T, *pPATHDIV_T;
+
+
+typedef enum _BASEBAND_CONFIG_PHY_REG_PG_VALUE_TYPE{
+	PHY_REG_PG_RELATIVE_VALUE = 0,
+	PHY_REG_PG_EXACT_VALUE = 1
+} PHY_REG_PG_TYPE;
+
+
+/*  */
+/*  Antenna detection information from single tone mechanism, added by Roger, 2012.11.27. */
+/*  */
+typedef struct _ANT_DETECTED_INFO {
+	bool bAntDetected;
+	u32 dBForAntA;
+	u32 dBForAntB;
+	u32 dBForAntO;
+} ANT_DETECTED_INFO, *PANT_DETECTED_INFO;
+
+/*  */
+/*  2011/09/22 MH Copy from SD4 defined structure. We use to support PHY DM integration. */
+/*  */
+typedef  struct DM_Out_Source_Dynamic_Mechanism_Structure {
+	/* RT_TIMER	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;
+
+	PHY_REG_PG_TYPE PhyRegPgValueType;
+	u8 PhyRegPgVersion;
+
+	u64	DebugComponents;
+	u32 DebugLevel;
+
+	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/ ?K?K = 1/2/3/?K */
+	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 RFType;
+	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;
+	u8 ExtLNA5G;
+	/*  with external PA  NO/Yes = 0/1 */
+	u8 ExtPA;
+	u8 ExtPA5G;
+	/*  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 */
+	/*  Frequence band 2.4G/5G = 0/1 */
+	u8 *pBandType;
+	/*  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 bIsMPChip;
+	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;
+
+
+	ODM_NOISE_MONITOR noise_level;/* ODM_MAX_CHANNEL_NUM]; */
+	/*  */
+	/* 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) */
+	ODM_PHY_DBG_INFO_T PhyDbgInfo;
+	/* PHY_INFO_88E		PhyInfo; */
+
+	/*  Latest packet phy info (ODM write) */
+	ODM_MAC_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 */
+	/*  */
+	FAT_T DM_FatTable;
+	DIG_T DM_DigTable;
+	PS_T DM_PSTable;
+	Pri_CCA_T DM_PriCCA;
+	RXHP_T DM_RXHP_Table;
+	RA_T DM_RA_Table;
+	false_ALARM_STATISTICS FalseAlmCnt;
+	false_ALARM_STATISTICS FlaseAlmCntBuddyAdapter;
+	SWAT_T DM_SWAT_Table;
+	bool RSSI_test;
+	CFO_TRACKING DM_CfoTrack;
+
+	EDCA_T DM_EDCA_Table;
+	u32 WMMEDCA_BE;
+	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;
+	RT_TIMER PSDTimer;
+	u8 RSSI_BT;			/* come from BT */
+	bool bPSDinProcess;
+	bool bPSDactive;
+	bool bDMInitialGainEnable;
+
+	/* MPT DIG */
+	RT_TIMER MPT_DIGTimer;
+
+	/* for rate adaptive, in fact,  88c/92c fw will handle this */
+	u8 bUseRAMask;
+
+	ODM_RATE_ADAPTIVE RateAdaptive;
+
+	ANT_DETECTED_INFO AntDetectedInfo; /*  Antenna detected information for RSSI tool */
+
+	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. */
+	RT_TIMER PathDivSwitchTimer;
+	/* 2011.09.27 add for Path Diversity */
+	RT_TIMER CCKPathDiversityTimer;
+	RT_TIMER FastAntTrainingTimer;
+
+	/*  ODM relative workitem. */
+
+	#if (BEAMFORMING_SUPPORT == 1)
+	RT_BEAMFORMING_INFO BeamformingInfo;
+	#endif
+} DM_ODM_T, *PDM_ODM_T; /*  DM_Dynamic_Mechanism_Structure */
+
+#define ODM_RF_PATH_MAX 2
+
+typedef enum _ODM_RF_RADIO_PATH {
+	ODM_RF_PATH_A = 0,   /* Radio Path A */
+	ODM_RF_PATH_B = 1,   /* Radio Path B */
+	ODM_RF_PATH_C = 2,   /* Radio Path C */
+	ODM_RF_PATH_D = 3,   /* Radio Path D */
+	ODM_RF_PATH_AB,
+	ODM_RF_PATH_AC,
+	ODM_RF_PATH_AD,
+	ODM_RF_PATH_BC,
+	ODM_RF_PATH_BD,
+	ODM_RF_PATH_CD,
+	ODM_RF_PATH_ABC,
+	ODM_RF_PATH_ACD,
+	ODM_RF_PATH_BCD,
+	ODM_RF_PATH_ABCD,
+	/*   ODM_RF_PATH_MAX,    Max RF number 90 support */
+} ODM_RF_RADIO_PATH_E, *PODM_RF_RADIO_PATH_E;
+
+ typedef enum _ODM_RF_CONTENT {
+	odm_radioa_txt = 0x1000,
+	odm_radiob_txt = 0x1001,
+	odm_radioc_txt = 0x1002,
+	odm_radiod_txt = 0x1003
+} ODM_RF_CONTENT;
+
+typedef enum _ODM_BB_Config_Type {
+	CONFIG_BB_PHY_REG,
+	CONFIG_BB_AGC_TAB,
+	CONFIG_BB_AGC_TAB_2G,
+	CONFIG_BB_AGC_TAB_5G,
+	CONFIG_BB_PHY_REG_PG,
+	CONFIG_BB_PHY_REG_MP,
+	CONFIG_BB_AGC_TAB_DIFF,
+} ODM_BB_Config_Type, *PODM_BB_Config_Type;
+
+typedef enum _ODM_RF_Config_Type {
+	CONFIG_RF_RADIO,
+	CONFIG_RF_TXPWR_LMT,
+} ODM_RF_Config_Type, *PODM_RF_Config_Type;
+
+typedef 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,
+} ODM_FW_Config_Type;
+
+/*  Status code */
+typedef enum _RT_STATUS {
+	RT_STATUS_SUCCESS,
+	RT_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,
+} RT_STATUS, *PRT_STATUS;
+
+#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 =========================================================== */
+
+typedef enum tag_1R_CCA_Type_Definition {
+	CCA_1R = 0,
+	CCA_2R = 1,
+	CCA_MAX = 2,
+} DM_1R_CCA_E;
+
+typedef enum tag_RF_Type_Definition {
+	RF_Save = 0,
+	RF_Normal = 1,
+	RF_MAX = 2,
+} DM_RF_E;
+
+/* 3 =========================================================== */
+/* 3 Antenna Diversity */
+/* 3 =========================================================== */
+typedef enum tag_SW_Antenna_Switch_Definition {
+	Antenna_A = 1,
+	Antenna_B = 2,
+	Antenna_MAX = 3,
+} DM_SWAS_E;
+
+
+/*  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 DIG by yuchen */
+
+void ODM_SetAntenna(PDM_ODM_T pDM_Odm, u8 Antenna);
+
+
+/* Remove BB power saving by Yuchen */
+
+#define dm_CheckTXPowerTracking ODM_TXPowerTrackingCheck
+void ODM_TXPowerTrackingCheck(PDM_ODM_T pDM_Odm);
+
+bool ODM_RAStateCheck(
+	PDM_ODM_T pDM_Odm,
+	s32	RSSI,
+	bool bForceUpdate,
+	u8 *pRATRState
+);
+
+#define dm_SWAW_RSSI_Check	ODM_SwAntDivChkPerPktRssi
+void ODM_SwAntDivChkPerPktRssi(
+	PDM_ODM_T pDM_Odm,
+	u8 StationID,
+	PODM_PHY_INFO_T pPhyInfo
+);
+
+u32 ODM_Get_Rate_Bitmap(
+	PDM_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(PDM_ODM_T pDM_Odm);
+
+void ODM_DMInit(PDM_ODM_T pDM_Odm);
+
+void ODM_DMWatchdog(PDM_ODM_T pDM_Odm); /*  For common use in the future */
+
+void ODM_CmnInfoInit(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, u32 Value);
+
+void ODM_CmnInfoHook(PDM_ODM_T pDM_Odm, ODM_CMNINFO_E CmnInfo, void *pValue);
+
+void ODM_CmnInfoPtrArrayHook(
+	PDM_ODM_T pDM_Odm,
+	ODM_CMNINFO_E CmnInfo,
+	u16 Index,
+	void *pValue
+);
+
+void ODM_CmnInfoUpdate(PDM_ODM_T pDM_Odm, u32 CmnInfo, u64 Value);
+
+void ODM_InitAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_CancelAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_ReleaseAllTimers(PDM_ODM_T pDM_Odm);
+
+void ODM_AntselStatistics_88C(
+	PDM_ODM_T pDM_Odm,
+	u8 MacId,
+	u32 PWDBAll,
+	bool isCCKrate
+);
+
+void ODM_DynamicARFBSelect(PDM_ODM_T pDM_Odm, u8 rate, bool Collision_State);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_AntDiv.c b/drivers/staging/rtl8723bs/hal/odm_AntDiv.c
new file mode 100644
index 000000000000..e0b20562ccd6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_AntDiv.c
@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 files
+//============================================================
+
+#include "odm_precomp.h"
+
+//======================================================
+// when antenna test utility is on or some testing
+// need to disable antenna diversity
+// call this function to disable all ODM related mechanisms
+// which will switch antenna.
+//======================================================
+void ODM_StopAntennaSwitchDm(PDM_ODM_T pDM_Odm)
+{
+	// disable ODM antenna diversity
+	pDM_Odm->SupportAbility &= ~ODM_BB_ANT_DIV;
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_ANT_DIV,
+		ODM_DBG_LOUD,
+		("STOP Antenna Diversity\n")
+	);
+}
+
+void ODM_SetAntConfig(PDM_ODM_T pDM_Odm, u8 antSetting)// 0=A, 1=B, 2=C, ....
+{
+	if (antSetting == 0) // ant A
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000000);
+	else if (antSetting == 1)
+		PHY_SetBBReg(pDM_Odm->Adapter, 0x948, bMaskDWord, 0x00000280);
+}
+
+//======================================================
+
+
+void ODM_SwAntDivRestAfterLink(PDM_ODM_T pDM_Odm)
+{
+	pSWAT_T pDM_SWAT_Table = &pDM_Odm->DM_SWAT_Table;
+	pFAT_T pDM_FatTable = &pDM_Odm->DM_FatTable;
+	u32 i;
+
+	pDM_Odm->RSSI_test = false;
+	pDM_SWAT_Table->try_flag = 0xff;
+	pDM_SWAT_Table->RSSI_Trying = 0;
+	pDM_SWAT_Table->Double_chk_flag = 0;
+
+	pDM_FatTable->RxIdleAnt = MAIN_ANT;
+
+	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
+		pDM_FatTable->MainAnt_Sum[i] = 0;
+		pDM_FatTable->AuxAnt_Sum[i] = 0;
+		pDM_FatTable->MainAnt_Cnt[i] = 0;
+		pDM_FatTable->AuxAnt_Cnt[i] = 0;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_AntDiv.h b/drivers/staging/rtl8723bs/hal/odm_AntDiv.h
new file mode 100644
index 000000000000..92cdad55b7a4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_AntDiv.h
@@ -0,0 +1,38 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODMANTDIV_H__
+#define    __ODMANTDIV_H__
+
+
+
+#define ANT1_2G 0 /*  = ANT2_5G */
+#define ANT2_2G 1 /*  = ANT1_5G */
+
+/* Antenna Diversty Control Type */
+#define	ODM_AUTO_ANT	0
+#define	ODM_FIX_MAIN_ANT	1
+#define	ODM_FIX_AUX_ANT	2
+
+#define	TX_BY_REG	0
+
+#define ANTDIV_ON 1
+#define ANTDIV_OFF 0
+
+#define INIT_ANTDIV_TIMMER 0
+#define CANCEL_ANTDIV_TIMMER 1
+#define RELEASE_ANTDIV_TIMMER 2
+
+#endif /* ifndef	__ODMANTDIV_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c
new file mode 100644
index 000000000000..9cde6c66235b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.c
@@ -0,0 +1,338 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+static void odm_SetCrystalCap(void *pDM_VOID, u8 CrystalCap)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	bool bEEPROMCheck;
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	bEEPROMCheck = (pHalData->EEPROMVersion >= 0x01) ? true : false;
+
+	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))
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		(
+			"odm_SetCrystalCap(): CrystalCap = 0x%x\n",
+			CrystalCap
+		)
+	);
+}
+
+static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	u8 CrystalCap = 0x20;
+
+	struct adapter *Adapter = pDM_Odm->Adapter;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	CrystalCap = pHalData->CrystalCap;
+
+	CrystalCap = CrystalCap & 0x3f;
+
+	return CrystalCap;
+}
+
+static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_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;
+	PDM_ODM_T pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+
+	pCfoTrack->DefXCap =
+		pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
+	pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
+	pCfoTrack->bAdjust = true;
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		("ODM_CfoTracking_init() =========>\n")
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		(
+			"ODM_CfoTracking_init(): bATCStatus = %d, CrystalCap = 0x%x\n",
+			pCfoTrack->bATCStatus,
+			pCfoTrack->DefXCap
+		)
+	);
+}
+
+void ODM_CfoTracking(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	int CFO_kHz_A, CFO_kHz_B, 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)) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			("ODM_CfoTracking(): Return: SupportAbility ODM_BB_CFO_TRACKING is disabled\n")
+		);
+		return;
+	}
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CFO_TRACKING,
+		ODM_DBG_LOUD,
+		("ODM_CfoTracking() =========>\n")
+	);
+
+	if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
+		/* 4 No link or more than one entry */
+		ODM_CfoTrackingReset(pDM_Odm);
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): Reset: bLinked = %d, bOneEntryOnly = %d\n",
+				pDM_Odm->bLinked,
+				pDM_Odm->bOneEntryOnly
+			)
+		);
+	} else {
+		/* 3 1. CFO Tracking */
+		/* 4 1.1 No new packet */
+		if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				(
+					"ODM_CfoTracking(): packet counter doesn't change\n"
+				)
+			);
+			return;
+		}
+		pCfoTrack->packetCount_pre = pCfoTrack->packetCount;
+
+		/* 4 1.2 Calculate CFO */
+		CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;
+		CFO_kHz_B =  (int)(pCfoTrack->CFO_tail[1] * 3125)  / 1280;
+
+		if (pDM_Odm->RFType < ODM_2T2R)
+			CFO_ave = CFO_kHz_A;
+		else
+			CFO_ave = (int)(CFO_kHz_A + CFO_kHz_B) >> 1;
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): CFO_kHz_A = %dkHz, CFO_kHz_B = %dkHz, CFO_ave = %dkHz\n",
+				CFO_kHz_A,
+				CFO_kHz_B,
+				CFO_ave
+			)
+		);
+
+		/* 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
+		) {
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_CFO_TRACKING, ODM_DBG_LOUD, ("ODM_CfoTracking(): first large CFO hit\n"));
+			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);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Disable CFO tracking for BT!!\n")
+			);
+		}
+
+		/* 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);
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				(
+					"ODM_CfoTracking(): Crystal cap offset = %d\n",
+					Adjust_Xtal
+				)
+			);
+		}
+
+		/* 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);
+		}
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CFO_TRACKING,
+			ODM_DBG_LOUD,
+			(
+				"ODM_CfoTracking(): Crystal cap = 0x%x, Default Crystal cap = 0x%x\n",
+				pCfoTrack->CrystalCap,
+				pCfoTrack->DefXCap
+			)
+		);
+
+		/* 3 2. Dynamic ATC switch */
+		if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
+			odm_SetATCStatus(pDM_Odm, false);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Disable ATC!!\n")
+			);
+		} else {
+			odm_SetATCStatus(pDM_Odm, true);
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_CFO_TRACKING,
+				ODM_DBG_LOUD,
+				("ODM_CfoTracking(): Enable ATC!!\n")
+			);
+		}
+	}
+}
+
+void ODM_ParsingCFO(void *pDM_VOID, void *pPktinfo_VOID, s8 *pcfotail)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	PODM_PACKET_INFO_T pPktinfo = (PODM_PACKET_INFO_T)pPktinfo_VOID;
+	PCFO_TRACKING pCfoTrack = &pDM_Odm->DM_CfoTrack;
+	u8 i;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING))
+		return;
+
+	if (pPktinfo->StationID != 0) {
+		/* 3 Update CFO report for path-A & path-B */
+		/*  Only paht-A and path-B have CFO tail and short CFO */
+		for (i = ODM_RF_PATH_A; i <= ODM_RF_PATH_B; i++)
+			pCfoTrack->CFO_tail[i] = (int)pcfotail[i];
+
+		/* 3 Update packet counter */
+		if (pCfoTrack->packetCount == 0xffffffff)
+			pCfoTrack->packetCount = 0;
+		else
+			pCfoTrack->packetCount++;
+	}
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h
new file mode 100644
index 000000000000..0c92899d9967
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_CfoTracking.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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 */
+
+typedef 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;
+} CFO_TRACKING, *PCFO_TRACKING;
+
+void ODM_CfoTrackingReset(void *pDM_VOID
+);
+
+void ODM_CfoTrackingInit(void *pDM_VOID);
+
+void ODM_CfoTracking(void *pDM_VOID);
+
+void ODM_ParsingCFO(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 000000000000..ba8e8eb534ef
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DIG.c
@@ -0,0 +1,1221 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+#define ADAPTIVITY_VERSION "5.0"
+
+void odm_NHMCounterStatisticsInit(void *pDM_VOID)
+{
+	PDM_ODM_T		pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/*  Get NHM report */
+	odm_GetNHMCounterStatistics(pDM_Odm);
+
+	/*  Reset NHM counter */
+	odm_NHMCounterStatisticsReset(pDM_Odm);
+}
+
+void odm_GetNHMCounterStatistics(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	/* u8 test_status; */
+	/* Pfalse_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);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"NHM_cnt_0 =%d, NHMCurTxOkcnt = %llu, NHMCurRxOkcnt = %llu\n",
+			pDM_Odm->NHM_cnt_0,
+			pDM_Odm->NHMCurTxOkcnt,
+			pDM_Odm->NHMCurRxOkcnt
+		)
+	);
+
+
+	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;
+			}
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("adaptivity_flag = %d\n ", pDM_Odm->adaptivity_flag));
+}
+
+void odm_SearchPwdBLowerBound(void *pDM_VOID, u8 IGI_target)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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;
+		}
+		/* DbgPrint("txEdcca1 = %d, txEdcca0 = %d\n", pDM_Odm->txEdcca1, pDM_Odm->txEdcca0); */
+
+		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;
+		}
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("IGI = 0x%x, H2L_lb = 0x%x, L2H_lb = 0x%x\n", IGI, pDM_Odm->H2L_lb, pDM_Odm->L2H_lb));
+}
+
+void odm_AdaptivityInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("Go to odm_DynamicEDCCA()\n"));
+		return;
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_Adaptivity() =====>\n"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("ForceEDCCA =%d, IGI_Base = 0x%x, TH_L2H_ini = %d, TH_EDCCA_HL_diff = %d, AdapEn_RSSI = %d\n",
+		pDM_Odm->ForceEDCCA, pDM_Odm->IGI_Base, pDM_Odm->TH_L2H_ini, pDM_Odm->TH_EDCCA_HL_diff, pDM_Odm->AdapEn_RSSI));
+
+	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 if (*pDM_Odm->pBandWidth == ODM_BW80M)
+		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 = 1;
+		else if (pDM_Odm->RSSI_Min < (pDM_Odm->AdapEn_RSSI - 5))
+			EDCCA_State = 0;
+	} else
+		EDCCA_State = 1;
+
+	if (
+		pDM_Odm->bLinked &&
+		pDM_Odm->Carrier_Sense_enable == false &&
+		pDM_Odm->NHM_disable == false &&
+		pDM_Odm->TxHangFlg == false
+	)
+		odm_NHMBB(pDM_Odm);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"BandWidth =%s, IGI_target = 0x%x, EDCCA_State =%d\n",
+			(*pDM_Odm->pBandWidth == ODM_BW80M) ? "80M" :
+			((*pDM_Odm->pBandWidth == ODM_BW40M) ? "40M" : "20M"),
+			IGI_target,
+			EDCCA_State
+		)
+	);
+
+	if (EDCCA_State == 1) {
+		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;
+	}
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("IGI = 0x%x, TH_L2H_dmc = %d, TH_H2L_dmc = %d\n",
+		IGI, TH_L2H_dmc, TH_H2L_dmc));
+	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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+
+	if (pDM_DigTable->bStopDIG) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("Stop Writing IGI\n"));
+		return;
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("ODM_REG(IGI_A, pDM_Odm) = 0x%x, ODM_BIT(IGI, pDM_Odm) = 0x%x\n",
+		ODM_REG(IGI_A, pDM_Odm), ODM_BIT(IGI, pDM_Odm)));
+
+	if (pDM_DigTable->CurIGValue != CurrentIGI) {
+		/* 1 Check initial gain by upper bound */
+		if (!pDM_DigTable->bPSDInProgress) {
+			if (CurrentIGI > pDM_DigTable->rx_gain_range_max) {
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("CurrentIGI(0x%02x) is larger than upper bound !!\n", 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);
+
+		if (pDM_Odm->RFType > ODM_1T1R)
+			PHY_SetBBReg(pDM_Odm->Adapter, ODM_REG(IGI_B, pDM_Odm), ODM_BIT(IGI, pDM_Odm), CurrentIGI);
+
+		pDM_DigTable->CurIGValue = CurrentIGI;
+	}
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_TRACE, ("CurrentIGI(0x%02x).\n", CurrentIGI));
+
+}
+
+void odm_PauseDIG(
+	void *pDM_VOID,
+	ODM_Pause_DIG_TYPE PauseType,
+	u8 IGIValue
+)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	static bool bPaused = false;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG() =========>\n"));
+
+	if (
+		(pDM_Odm->SupportAbility & ODM_BB_ADAPTIVITY) &&
+		pDM_Odm->TxHangFlg == true
+	) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_PauseDIG(): Dynamic adjust threshold in progress !!\n")
+		);
+		return;
+	}
+
+	if (
+		!bPaused && (!(pDM_Odm->SupportAbility & ODM_BB_DIG) ||
+		!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
+	){
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_PauseDIG(): Return: SupportAbility ODM_BB_DIG or ODM_BB_FA_CNT is disabled\n")
+		);
+		return;
+	}
+
+	switch (PauseType) {
+	/* 1 Pause DIG */
+	case ODM_PAUSE_DIG:
+		/* 2 Disable DIG */
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pDM_Odm->SupportAbility & (~ODM_BB_DIG));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Pause DIG !!\n"));
+
+		/* 2 Backup IGI value */
+		if (!bPaused) {
+			pDM_DigTable->IGIBackup = pDM_DigTable->CurIGValue;
+			bPaused = true;
+		}
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Backup IGI  = 0x%x\n", pDM_DigTable->IGIBackup));
+
+		/* 2 Write new IGI value */
+		ODM_Write_DIG(pDM_Odm, IGIValue);
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Write new IGI = 0x%x\n", IGIValue));
+		break;
+
+	/* 1 Resume DIG */
+	case ODM_RESUME_DIG:
+		if (bPaused) {
+			/* 2 Write backup IGI value */
+			ODM_Write_DIG(pDM_Odm, pDM_DigTable->IGIBackup);
+			bPaused = false;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Write original IGI = 0x%x\n", pDM_DigTable->IGIBackup));
+
+			/* 2 Enable DIG */
+			ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_ABILITY, pDM_Odm->SupportAbility | ODM_BB_DIG);
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Resume DIG !!\n"));
+		}
+		break;
+
+	default:
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_PauseDIG(): Wrong  type !!\n"));
+		break;
+	}
+}
+
+bool odm_DigAbort(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: SupportAbility ODM_BB_FA_CNT is disabled\n"));
+		return	true;
+	}
+
+	/* SupportAbility */
+	if (!(pDM_Odm->SupportAbility & ODM_BB_DIG)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: SupportAbility ODM_BB_DIG is disabled\n"));
+		return	true;
+	}
+
+	/* ScanInProcess */
+	if (*(pDM_Odm->pbScanInProcess)) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: In Scan Progress\n"));
+		return	true;
+	}
+
+	/* add by Neil Chen to avoid PSD is processing */
+	if (pDM_Odm->bDMInitialGainEnable == false) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Return: PSD is Processing\n"));
+		return	true;
+	}
+
+	return	false;
+}
+
+void odm_DIGInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_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;
+
+	if (pDM_Odm->BoardType & (ODM_BOARD_EXT_PA|ODM_BOARD_EXT_LNA)) {
+		pDM_DigTable->rx_gain_range_max = DM_DIG_MAX_NIC;
+		pDM_DigTable->rx_gain_range_min = DM_DIG_MIN_NIC;
+	} else {
+		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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	/*  Common parameters */
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	Pfalse_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) == true)
+		return;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG() ===========================>\n\n"));
+
+	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;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Absolutly upper bound = 0x%x, lower bound = 0x%x\n", dm_dig_max, dm_dig_min));
+
+	/* 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;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Coex. case: Force upper bound to RSSI + %d !!!!!!\n", offset));
+		} 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;
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_ANT_DIV,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Antenna diversity case: Force lower bound to 0x%x !!!!!!\n",
+						DIG_Dynamic_MIN
+					)
+				);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_ANT_DIV,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Antenna diversity case: RSSI_max = 0x%x !!!!!!\n",
+						pDM_DigTable->AntDiv_RSSI_max
+					)
+				);
+			}
+		}
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): Adjust boundary by RSSI Upper bound = 0x%x, Lower bound = 0x%x\n",
+			pDM_DigTable->rx_gain_range_max,
+			DIG_Dynamic_MIN
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): Link status: bLinked = %d, RSSI = %d, bFirstConnect = %d, bFirsrDisConnect = %d\n\n",
+			pDM_Odm->bLinked,
+			pDM_Odm->RSSI_Min,
+			FirstConnect,
+			FirstDisConnect
+		)
+	);
+
+	/* 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;
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				(
+					"odm_DIG(): Abnrormal #beacon (%d) case in STA mode: Force lower bound to 0x%x !!!!!!\n\n",
+					pDM_Odm->PhyDbgInfo.NumQryBeaconPkt,
+					pDM_DigTable->rx_gain_range_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;
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			(
+				"odm_DIG(): Abnrormal lower bound case: Force lower bound to 0x%x !!!!!!\n\n",
+				pDM_DigTable->rx_gain_range_min
+			)
+		);
+	}
+
+
+	/* 1 False alarm threshold decision */
+	odm_FAThresholdCheck(pDM_Odm, bDFSBand, bPerformance, RxTp, TxTp, dm_FA_thres);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): False alarm threshold = %d, %d, %d\n\n", dm_FA_thres[0], dm_FA_thres[1], dm_FA_thres[2]));
+
+	/* 1 Adjust initial gain by false alarm */
+	if (pDM_Odm->bLinked && bPerformance) {
+		/* 2 After link */
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_DIG(): Adjust IGI after link\n")
+		);
+
+		if (bFirstTpTarget || (FirstConnect && bPerformance)) {
+			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;
+			}
+
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				(
+					"odm_DIG(): First connect case: IGI does on-shot to 0x%x\n",
+					CurrentIGI
+				)
+			);
+
+		} 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;
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_DIG,
+					ODM_DBG_LOUD,
+					(
+						"odm_DIG(): Abnormal #beacon (%d) case: IGI does one-shot to 0x%x\n",
+						pDM_Odm->PhyDbgInfo.NumQryBeaconPkt,
+						CurrentIGI
+					)
+				);
+			}
+		}
+	} else {
+		/* 2 Before link */
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_DIG,
+			ODM_DBG_LOUD,
+			("odm_DIG(): Adjust IGI before link\n")
+		);
+
+		if (FirstDisConnect || bFirstCoverage) {
+			CurrentIGI = dm_dig_min;
+			ODM_RT_TRACE(
+				pDM_Odm,
+				ODM_COMP_DIG,
+				ODM_DBG_LOUD,
+				("odm_DIG(): First disconnect case: IGI does on-shot to lower bound\n")
+			);
+		} 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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		(
+			"odm_DIG(): CurIGValue = 0x%x, TotalFA = %d\n\n",
+			CurrentIGI,
+			pFalseAlmCnt->Cnt_all
+		)
+	);
+
+	/* 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;
+		}
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Adaptivity case: Force upper bound to 0x%x !!!!!!\n", Adap_IGI_Upper));
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Adaptivity case: Force lower bound to 0x%x !!!!!!\n\n", 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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS() ==>\n")
+	);
+
+	/*  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_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): pFalseAlmCnt->Cnt_all = %d\n", pFalseAlmCnt->Cnt_all)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): pDM_Odm->RSSI_Min = %d\n", pDM_Odm->RSSI_Min)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_DIG,
+		ODM_DBG_LOUD,
+		("odm_DIGbyRSSI_LPS(): CurrentIGI = 0x%x\n", CurrentIGI)
+	);
+
+	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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_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;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Enter odm_FalseAlarmCounterStatistics\n")
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Fast_Fsync =%d, Cnt_SB_Search_fail =%d\n",
+			FalseAlmCnt->Cnt_Fast_Fsync,
+			FalseAlmCnt->Cnt_SB_Search_fail
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Parity_Fail =%d, Cnt_Rate_Illegal =%d\n",
+			FalseAlmCnt->Cnt_Parity_Fail,
+			FalseAlmCnt->Cnt_Rate_Illegal
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		(
+			"Cnt_Crc8_fail =%d, Cnt_Mcs_fail =%d\n",
+			FalseAlmCnt->Cnt_Crc8_fail,
+			FalseAlmCnt->Cnt_Mcs_fail
+		)
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_OFDM_CCA =%d\n", FalseAlmCnt->Cnt_OFDM_CCA)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_CCK_CCA =%d\n", FalseAlmCnt->Cnt_CCK_CCA)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_CCA_all =%d\n", FalseAlmCnt->Cnt_CCA_all)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Ofdm_fail =%d\n",	FalseAlmCnt->Cnt_Ofdm_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Cck_fail =%d\n",	FalseAlmCnt->Cnt_Cck_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Cnt_Ofdm_fail =%d\n",	FalseAlmCnt->Cnt_Ofdm_fail)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_FA_CNT,
+		ODM_DBG_LOUD,
+		("Total False Alarm =%d\n",	FalseAlmCnt->Cnt_all)
+	);
+}
+
+
+void odm_FAThresholdCheck(
+	void *pDM_VOID,
+	bool bDFSBand,
+	bool bPerformance,
+	u32 RxTp,
+	u32 TxTp,
+	u32 *dm_FA_thres
+)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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;
+	}
+	return;
+}
+
+u8 odm_ForbiddenIGICheck(void *pDM_VOID, u8 DIG_Dynamic_MIN, u8 CurrentIGI)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_T pDM_DigTable = &pDM_Odm->DM_DigTable;
+	Pfalse_ALARM_STATISTICS pFalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	u8 rx_gain_range_min = pDM_DigTable->rx_gain_range_min;
+
+	if (pFalseAlmCnt->Cnt_all > 10000) {
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Abnormally false alarm case.\n"));
+
+		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;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Abnormally false alarm case: Recover_cnt = %d\n", pDM_DigTable->Recover_cnt));
+		}
+	} else {
+		if (pDM_DigTable->Recover_cnt != 0) {
+			pDM_DigTable->Recover_cnt--;
+			ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: Recover_cnt = %d\n", 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; */
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: At Lower Bound\n"));
+				} else {
+					pDM_DigTable->ForbiddenIGI -= 2;
+					rx_gain_range_min = (pDM_DigTable->ForbiddenIGI + 2);
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG(): Normal Case: Approach Lower Bound\n"));
+				}
+			} else
+				pDM_DigTable->LargeFAHit = 0;
+		}
+	}
+
+	return rx_gain_range_min;
+
+}
+
+/* 3 ============================================================ */
+/* 3 CCK Packet Detect Threshold */
+/* 3 ============================================================ */
+
+void odm_CCKPacketDetectionThresh(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	Pfalse_ALARM_STATISTICS FalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	u8 CurCCK_CCAThres;
+
+
+	if (
+		!(pDM_Odm->SupportAbility & ODM_BB_CCK_PD) ||
+		!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT)
+	) {
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_CCK_PD,
+			ODM_DBG_LOUD,
+			("odm_CCKPacketDetectionThresh()  return ==========\n")
+		);
+		return;
+	}
+
+	if (pDM_Odm->ExtLNA)
+		return;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CCK_PD,
+		ODM_DBG_LOUD,
+		("odm_CCKPacketDetectionThresh()  ==========>\n")
+	);
+
+	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);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_CCK_PD,
+		ODM_DBG_LOUD,
+		(
+			"odm_CCKPacketDetectionThresh()  CurCCK_CCAThres = 0x%x\n",
+			CurCCK_CCAThres
+		)
+	);
+}
+
+void ODM_Write_CCK_CCA_Thres(void *pDM_VOID, u8 CurCCK_CCAThres)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pDIG_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 000000000000..e27a69126086
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DIG.h
@@ -0,0 +1,195 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __ODMDIG_H__
+#define __ODMDIG_H__
+
+typedef struct _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;
+} DIG_T, *pDIG_T;
+
+typedef 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 */
+} false_ALARM_STATISTICS, *Pfalse_ALARM_STATISTICS;
+
+typedef enum tag_Dynamic_Init_Gain_Operation_Type_Definition {
+	DIG_TYPE_THRESH_HIGH = 0,
+	DIG_TYPE_THRESH_LOW = 1,
+	DIG_TYPE_BACKOFF = 2,
+	DIG_TYPE_RX_GAIN_MIN = 3,
+	DIG_TYPE_RX_GAIN_MAX = 4,
+	DIG_TYPE_ENABLE = 5,
+	DIG_TYPE_DISABLE = 6,
+	DIG_OP_TYPE_MAX
+} DM_DIG_OP_E;
+
+typedef enum tag_ODM_PauseDIG_Type {
+	ODM_PAUSE_DIG = BIT0,
+	ODM_RESUME_DIG = BIT1
+} ODM_Pause_DIG_TYPE;
+
+typedef enum tag_ODM_PauseCCKPD_Type {
+	ODM_PAUSE_CCKPD = BIT0,
+	ODM_RESUME_CCKPD = BIT1
+} ODM_Pause_CCKPD_TYPE;
+
+#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_PauseDIG(void *pDM_VOID, ODM_Pause_DIG_TYPE PauseType, u8 IGIValue);
+
+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 000000000000..6af017562539
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.c
@@ -0,0 +1,89 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+void odm_DynamicBBPowerSavingInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pPS_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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+	pPS_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 000000000000..b435cae4ab63
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicBBPowerSaving.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODMDYNAMICBBPOWERSAVING_H__
+#define    __ODMDYNAMICBBPOWERSAVING_H__
+
+typedef struct _Dynamic_Power_Saving_ {
+	u8 PreCCAState;
+	u8 CurCCAState;
+
+	u8 PreRFState;
+	u8 CurRFState;
+
+	int Rssi_val_min;
+
+	u8 initialize;
+	u32 Reg874, RegC70, Reg85C, RegA74;
+
+} PS_T, *pPS_T;
+
+#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 000000000000..d24e5f712c20
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.c
@@ -0,0 +1,30 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+void odm_DynamicTxPowerInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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 000000000000..35cdbab6d965
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_DynamicTxPower.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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 000000000000..8d53cb73cea5
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.c
@@ -0,0 +1,186 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "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)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VO PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_VO_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial VI PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_VI_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BE PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_BE_PARAM)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("Orginial BK PARAM: 0x%x\n",
+		      rtw_read32(pDM_Odm->Adapter, ODM_EDCA_BK_PARAM)));
+}	/*  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.
+	 */
+	PDM_ODM_T		pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("odm_EdcaTurboCheck ========================>\n"));
+
+	if (!(pDM_Odm->SupportAbility & ODM_MAC_EDCA_TURBO))
+		return;
+
+	odm_EdcaTurboCheckCE(pDM_Odm);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_EDCA_TURBO, ODM_DBG_LOUD,
+		     ("<========================odm_EdcaTurboCheck\n"));
+}	/*  odm_CheckEdcaTurbo */
+
+void odm_EdcaTurboCheckCE(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_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_A) ||
+			    (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) ||
+			    (wirelessmode == ODM_WM_A))) {
+			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 000000000000..b0590abe87f0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_EdcaTurboCheck.h
@@ -0,0 +1,31 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __ODMEDCATURBOCHECK_H__
+#define __ODMEDCATURBOCHECK_H__
+
+typedef struct _EDCA_TURBO_ {
+	bool bCurrentTurboEDCA;
+	bool bIsCurRDLState;
+
+	u32 prv_traffic_idx; /*  edca turbo */
+} EDCA_T, *pEDCA_T;
+
+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 000000000000..ba2700135b60
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_HWConfig.c
@@ -0,0 +1,535 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "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
+#define GET_VERSION_MP(ic, txt) (ODM_GetVersion_MP_##ic##txt())
+#define GET_VERSION(ic, txt) (pDM_Odm->bIsMPChip?GET_VERSION_MP(ic, txt):GET_VERSION_TC(ic, txt))
+
+static u8 odm_QueryRxPwrPercentage(s8 AntPower)
+{
+	if ((AntPower <= -100) || (AntPower >= 20))
+		return	0;
+	else if (AntPower >= 0)
+		return	100;
+	else
+		return	(100+AntPower);
+
+}
+
+static s32 odm_SignalScaleMapping_92CSeries(PDM_ODM_T pDM_Odm, s32 CurrSig)
+{
+	s32 RetSig = 0;
+
+	if (pDM_Odm->SupportInterface  == ODM_ITRF_SDIO) {
+		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;
+}
+
+s32 odm_SignalScaleMapping(PDM_ODM_T pDM_Odm, s32 CurrSig)
+{
+	return odm_SignalScaleMapping_92CSeries(pDM_Odm, CurrSig);
+}
+
+static u8 odm_EVMdbToPercentage(s8 Value)
+{
+	/*  */
+	/*  -33dB~0dB to 0%~99% */
+	/*  */
+	s8 ret_val;
+
+	ret_val = Value;
+	ret_val /= 2;
+
+	/* DbgPrint("Value =%d\n", Value); */
+	/* ODM_RT_DISP(FRX, RX_PHY_SQ, ("EVMdbToPercentage92C Value =%d / %x\n", ret_val, ret_val)); */
+
+	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 void odm_RxPhyStatus92CSeries_Parsing(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+	u8 i, Max_spatial_stream;
+	s8 rx_pwr[4], rx_pwr_all = 0;
+	u8 EVM, PWDB_ALL = 0, PWDB_ALL_BT;
+	u8 RSSI, total_rssi = 0;
+	bool isCCKrate = false;
+	u8 rf_rx_num = 0;
+	u8 cck_highpwr = 0;
+	u8 LNA_idx, VGA_idx;
+	PPHY_STATUS_RPT_8192CD_T pPhyStaRpt = (PPHY_STATUS_RPT_8192CD_T)pPhyStatus;
+
+	isCCKrate = (pPktinfo->DataRate <= DESC_RATE11M) ? true : false;
+	pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_A] = -1;
+	pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_B] = -1;
+
+
+	if (isCCKrate) {
+		u8 cck_agc_rpt;
+
+		pDM_Odm->PhyDbgInfo.NumQryPhyStatusCCK++;
+		/*  */
+		/*  (1)Hardware does not provide RSSI for CCK */
+		/*  (2)PWDB, Average PWDB cacluated by hardware (for rate adaptive) */
+		/*  */
+
+		/* if (pHalData->eRFPowerState == eRfOn) */
+		cck_highpwr = pDM_Odm->bCckHighPower;
+		/* else */
+		/* cck_highpwr = false; */
+
+		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);
+		rx_pwr_all = odm_CCKRSSI_8723B(LNA_idx, VGA_idx);
+		PWDB_ALL = odm_QueryRxPwrPercentage(rx_pwr_all);
+		if (PWDB_ALL > 100)
+			PWDB_ALL = 100;
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		pPhyInfo->BTRxRSSIPercentage = PWDB_ALL;
+		pPhyInfo->RecvSignalPower = rx_pwr_all;
+		/*  */
+		/*  (3) Get Signal Quality (EVM) */
+		/*  */
+		/* if (pPktinfo->bPacketMatchBSSID) */
+		{
+			u8 SQ, SQ_rpt;
+
+			if (pPhyInfo->RxPWDBAll > 40 && !pDM_Odm->bInHctTest)
+				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;
+
+			}
+
+			/* DbgPrint("cck SQ = %d\n", SQ); */
+			pPhyInfo->SignalQuality = SQ;
+			pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_A] = SQ;
+			pPhyInfo->RxMIMOSignalQuality[ODM_RF_PATH_B] = -1;
+		}
+	} else { /* is OFDM rate */
+		pDM_Odm->PhyDbgInfo.NumQryPhyStatusOFDM++;
+
+		/*  */
+		/*  (1)Get RSSI for HT rate */
+		/*  */
+
+		for (i = ODM_RF_PATH_A; i < ODM_RF_PATH_MAX; i++) {
+			/*  2008/01/30 MH we will judge RF RX path now. */
+			if (pDM_Odm->RFPathRxEnable & BIT(i))
+				rf_rx_num++;
+			/* else */
+				/* continue; */
+
+			rx_pwr[i] = ((pPhyStaRpt->path_agc[i].gain&0x3F)*2) - 110;
+
+
+			pPhyInfo->RxPwr[i] = rx_pwr[i];
+
+			/* Translate DBM to percentage. */
+			RSSI = odm_QueryRxPwrPercentage(rx_pwr[i]);
+			total_rssi += RSSI;
+			/* RT_DISP(FRX, RX_PHY_SS, ("RF-%d RXPWR =%x RSSI =%d\n", i, rx_pwr[i], RSSI)); */
+
+			pPhyInfo->RxMIMOSignalStrength[i] = (u8) RSSI;
+
+			/* Get Rx snr value in DB */
+			pPhyInfo->RxSNR[i] = pDM_Odm->PhyDbgInfo.RxSNRdB[i] = (s32)(pPhyStaRpt->path_rxsnr[i]/2);
+		}
+
+
+		/*  */
+		/*  (2)PWDB, Average PWDB cacluated by hardware (for rate adaptive) */
+		/*  */
+		rx_pwr_all = (((pPhyStaRpt->cck_sig_qual_ofdm_pwdb_all) >> 1)&0x7f)-110;
+
+		PWDB_ALL_BT = PWDB_ALL = odm_QueryRxPwrPercentage(rx_pwr_all);
+		/* RT_DISP(FRX, RX_PHY_SS, ("PWDB_ALL =%d\n", PWDB_ALL)); */
+
+		pPhyInfo->RxPWDBAll = PWDB_ALL;
+		/* ODM_RT_TRACE(pDM_Odm, ODM_COMP_RSSI_MONITOR, ODM_DBG_LOUD, ("ODM OFDM RSSI =%d\n", pPhyInfo->RxPWDBAll)); */
+		pPhyInfo->BTRxRSSIPercentage = PWDB_ALL_BT;
+		pPhyInfo->RxPower = rx_pwr_all;
+		pPhyInfo->RecvSignalPower = rx_pwr_all;
+
+		{/* pMgntInfo->CustomerID != RT_CID_819x_Lenovo */
+			/*  */
+			/*  (3)EVM of HT rate */
+			/*  */
+			if (pPktinfo->DataRate >= DESC_RATEMCS8 && pPktinfo->DataRate <= DESC_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_EVMdbToPercentage((pPhyStaRpt->stream_rxevm[i]));	/* dbm */
+
+				/* RT_DISP(FRX, RX_PHY_SQ, ("RXRATE =%x RXEVM =%x EVM =%s%d\n", */
+				/* GET_RX_STATUS_DESC_RX_MCS(pDesc), pDrvInfo->rxevm[i], "%", EVM)); */
+
+				/* if (pPktinfo->bPacketMatchBSSID) */
+				{
+					if (i == ODM_RF_PATH_A) /*  Fill value in RFD, Get the first spatial stream only */
+						pPhyInfo->SignalQuality = (u8)(EVM & 0xff);
+
+					pPhyInfo->RxMIMOSignalQuality[i] = (u8)(EVM & 0xff);
+				}
+			}
+		}
+
+		ODM_ParsingCFO(pDM_Odm, pPktinfo, pPhyStaRpt->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 (isCCKrate) {
+#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+		pPhyInfo->SignalStrength = (u8)PWDB_ALL;
+#else
+		pPhyInfo->SignalStrength = (u8)(odm_SignalScaleMapping(pDM_Odm, PWDB_ALL));/* PWDB_ALL; */
+#endif
+	} else {
+		if (rf_rx_num != 0) {
+#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+			total_rssi /= rf_rx_num;
+			pPhyInfo->SignalStrength = (u8)total_rssi;
+#else
+			pPhyInfo->SignalStrength = (u8)(odm_SignalScaleMapping(pDM_Odm, total_rssi /= rf_rx_num));
+#endif
+		}
+	}
+
+	/* DbgPrint("isCCKrate = %d, pPhyInfo->RxPWDBAll = %d, pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a = 0x%x\n", */
+		/* isCCKrate, pPhyInfo->RxPWDBAll, pPhyStaRpt->cck_agc_rpt_ofdm_cfosho_a); */
+}
+
+static void odm_Process_RSSIForDM(
+	PDM_ODM_T pDM_Odm, PODM_PHY_INFO_T pPhyInfo, PODM_PACKET_INFO_T 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->StationID == 0xFF)
+		return;
+
+	pEntry = pDM_Odm->pODM_StaInfo[pPktinfo->StationID];
+
+	if (!IS_STA_VALID(pEntry))
+		return;
+
+	if ((!pPktinfo->bPacketMatchBSSID))
+		return;
+
+	if (pPktinfo->bPacketBeacon)
+		pDM_Odm->PhyDbgInfo.NumQryBeaconPkt++;
+
+	isCCKrate = ((pPktinfo->DataRate <= DESC_RATE11M)) ? true : false;
+	pDM_Odm->RxRate = pPktinfo->DataRate;
+
+	/* 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->bPacketToSelf || pPktinfo->bPacketBeacon) {
+
+		if (!isCCKrate) { /* ofdm rate */
+			if (pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B] == 0) {
+				RSSI_Ave = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_A = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_B = 0;
+			} else {
+				/* DbgPrint("pRfd->Status.RxMIMOSignalStrength[0] = %d, pRfd->Status.RxMIMOSignalStrength[1] = %d\n", */
+					/* pRfd->Status.RxMIMOSignalStrength[0], pRfd->Status.RxMIMOSignalStrength[1]); */
+				pDM_Odm->RSSI_A =  pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+				pDM_Odm->RSSI_B = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+
+				if (
+					pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A] >
+					pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B]
+				) {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_A];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+				} else {
+					RSSI_max = pPhyInfo->RxMIMOSignalStrength[ODM_RF_PATH_B];
+					RSSI_min = pPhyInfo->RxMIMOSignalStrength[ODM_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) | BIT0;
+
+		} else {
+			RSSI_Ave = pPhyInfo->RxPWDBAll;
+			pDM_Odm->RSSI_A = (u8) pPhyInfo->RxPWDBAll;
+			pDM_Odm->RSSI_B = 0;
+
+			/* 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;
+		}
+
+		/* 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;
+
+			/* DbgPrint("OFDM_pkt =%d, Weighting =%d\n", OFDM_pkt, Weighting); */
+			/* DbgPrint("UndecoratedSmoothedOFDM =%d, UndecoratedSmoothedPWDB =%d, UndecoratedSmoothedCCK =%d\n", */
+			/* UndecoratedSmoothedOFDM, UndecoratedSmoothedPWDB, UndecoratedSmoothedCCK); */
+
+		}
+
+	}
+}
+
+
+/*  */
+/*  Endianness before calling this API */
+/*  */
+static void ODM_PhyStatusQuery_92CSeries(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+
+	odm_RxPhyStatus92CSeries_Parsing(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
+
+	if (!pDM_Odm->RSSI_test)
+		odm_Process_RSSIForDM(pDM_Odm, pPhyInfo, pPktinfo);
+}
+
+void ODM_PhyStatusQuery(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+)
+{
+
+	ODM_PhyStatusQuery_92CSeries(pDM_Odm, pPhyInfo, pPhyStatus, pPktinfo);
+}
+
+/*  */
+/*  If you want to add a new IC, Please follow below template and generate a new one. */
+/*  */
+/*  */
+
+HAL_STATUS ODM_ConfigRFWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_RF_Config_Type ConfigType,
+	ODM_RF_RADIO_PATH_E eRFPath
+)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("===>ODM_ConfigRFWithHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	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;
+}
+
+HAL_STATUS ODM_ConfigRFWithTxPwrTrackHeaderFile(PDM_ODM_T pDM_Odm)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				 ("===>ODM_ConfigRFWithTxPwrTrackHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				 ("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				 pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	if (pDM_Odm->SupportInterface == ODM_ITRF_SDIO)
+		READ_AND_CONFIG(8723B, _TxPowerTrack_SDIO);
+
+	return HAL_STATUS_SUCCESS;
+}
+
+HAL_STATUS ODM_ConfigBBWithHeaderFile(
+	PDM_ODM_T pDM_Odm, ODM_BB_Config_Type ConfigType
+)
+{
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("===>ODM_ConfigBBWithHeaderFile (%s)\n", (pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+				("pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+				pDM_Odm->SupportPlatform, pDM_Odm->SupportInterface, pDM_Odm->BoardType));
+
+	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;
+}
+
+HAL_STATUS ODM_ConfigMACWithHeaderFile(PDM_ODM_T pDM_Odm)
+{
+	u8 result = HAL_STATUS_SUCCESS;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"===>ODM_ConfigMACWithHeaderFile (%s)\n",
+			(pDM_Odm->bIsMPChip) ? "MPChip" : "TestChip"
+		)
+	);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"pDM_Odm->SupportPlatform: 0x%X, pDM_Odm->SupportInterface: 0x%X, pDM_Odm->BoardType: 0x%X\n",
+			pDM_Odm->SupportPlatform,
+			pDM_Odm->SupportInterface,
+			pDM_Odm->BoardType
+		)
+	);
+
+	READ_AND_CONFIG(8723B, _MAC_REG);
+
+	return result;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_HWConfig.h b/drivers/staging/rtl8723bs/hal/odm_HWConfig.h
new file mode 100644
index 000000000000..f029922d12f0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_HWConfig.h
@@ -0,0 +1,162 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HALHWOUTSRC_H__
+#define __HALHWOUTSRC_H__
+
+
+/*--------------------------Define -------------------------------------------*/
+/* define READ_NEXT_PAIR(v1, v2, i) do { i += 2; v1 = Array[i]; v2 = Array[i+1]; } while (0) */
+#define AGC_DIFF_CONFIG_MP(ic, band) (ODM_ReadAndConfig_MP_##ic##_AGC_TAB_DIFF(pDM_Odm, Array_MP_##ic##_AGC_TAB_DIFF_##band, \
+	sizeof(Array_MP_##ic##_AGC_TAB_DIFF_##band)/sizeof(u32)))
+#define AGC_DIFF_CONFIG_TC(ic, band) (ODM_ReadAndConfig_TC_##ic##_AGC_TAB_DIFF(pDM_Odm, Array_TC_##ic##_AGC_TAB_DIFF_##band, \
+	sizeof(Array_TC_##ic##_AGC_TAB_DIFF_##band)/sizeof(u32)))
+
+#define AGC_DIFF_CONFIG(ic, band)\
+	do {\
+		if (pDM_Odm->bIsMPChip)\
+			AGC_DIFF_CONFIG_MP(ic, band);\
+		else\
+			AGC_DIFF_CONFIG_TC(ic, band);\
+	} while (0)
+
+
+/*  */
+/*  structure and define */
+/*  */
+
+typedef struct _Phy_Rx_AGC_Info {
+	#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+		u8 gain:7, trsw:1;
+	#else
+		u8 trsw:1, gain:7;
+	#endif
+} PHY_RX_AGC_INFO_T, *pPHY_RX_AGC_INFO_T;
+
+typedef struct _Phy_Status_Rpt_8192cd {
+	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
+} PHY_STATUS_RPT_8192CD_T, *PPHY_STATUS_RPT_8192CD_T;
+
+
+typedef struct _Phy_Status_Rpt_8812 {
+	/* 2012.05.24 LukeLee: This structure should take big/little endian in consideration later..... */
+
+	/* DWORD 0 */
+	u8 gain_trsw[2];
+#if (ODM_ENDIAN_TYPE == ODM_ENDIAN_LITTLE)
+	u16 chl_num:10;
+	u16 sub_chnl:4;
+	u16 r_RFMOD:2;
+#else	/*  _BIG_ENDIAN_ */
+	u16 r_RFMOD:2;
+	u16 sub_chnl:4;
+	u16 chl_num:10;
+#endif
+
+	/* DWORD 1 */
+	u8 pwdb_all;
+	u8 cfosho[4];	/*  DW 1 byte 1 DW 2 byte 0 */
+
+	/* DWORD 2 */
+	s8 cfotail[4]; /*  DW 2 byte 1 DW 3 byte 0 */
+
+	/* DWORD 3 */
+	s8 rxevm[2]; /*  DW 3 byte 1 DW 3 byte 2 */
+	s8 rxsnr[2]; /*  DW 3 byte 3 DW 4 byte 0 */
+
+	/* DWORD 4 */
+	u8 PCTS_MSK_RPT[2];
+	u8 pdsnr[2]; /*  DW 4 byte 3 DW 5 Byte 0 */
+
+	/* DWORD 5 */
+	u8 csi_current[2];
+	u8 rx_gain_c;
+
+	/* DWORD 6 */
+	u8 rx_gain_d;
+	s8 sigevm;
+	u8 resvd_0;
+	u8 antidx_anta:3;
+	u8 antidx_antb:3;
+	u8 resvd_1:2;
+} PHY_STATUS_RPT_8812_T, *PPHY_STATUS_RPT_8812_T;
+
+
+void ODM_PhyStatusQuery(
+	PDM_ODM_T pDM_Odm,
+	PODM_PHY_INFO_T pPhyInfo,
+	u8 *pPhyStatus,
+	PODM_PACKET_INFO_T pPktinfo
+);
+
+HAL_STATUS ODM_ConfigRFWithTxPwrTrackHeaderFile(PDM_ODM_T pDM_Odm);
+
+HAL_STATUS ODM_ConfigRFWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_RF_Config_Type ConfigType,
+	ODM_RF_RADIO_PATH_E eRFPath
+);
+
+HAL_STATUS ODM_ConfigBBWithHeaderFile(
+	PDM_ODM_T pDM_Odm, ODM_BB_Config_Type ConfigType
+);
+
+HAL_STATUS ODM_ConfigMACWithHeaderFile(PDM_ODM_T pDM_Odm);
+
+HAL_STATUS ODM_ConfigFWWithHeaderFile(
+	PDM_ODM_T pDM_Odm,
+	ODM_FW_Config_Type ConfigType,
+	u8 *pFirmware,
+	u32 *pSize
+);
+
+s32 odm_SignalScaleMapping(PDM_ODM_T pDM_Odm, s32 CurrSig);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.c b/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.c
new file mode 100644
index 000000000000..af7b44e29092
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.c
@@ -0,0 +1,175 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+/*  This function is for inband noise test utility only */
+/*  To obtain the inband noise level(dbm), do the following. */
+/*  1. disable DIG and Power Saving */
+/*  2. Set initial gain = 0x1a */
+/*  3. Stop updating idle time pwer report (for driver read) */
+/* - 0x80c[25] */
+
+#define Valid_Min				-35
+#define Valid_Max			10
+#define ValidCnt				5
+
+static s16 odm_InbandNoise_Monitor_NSeries(
+	PDM_ODM_T pDM_Odm,
+	u8 bPauseDIG,
+	u8 IGIValue,
+	u32 max_time
+)
+{
+	u32 tmp4b;
+	u8 max_rf_path = 0, rf_path;
+	u8 reg_c50, reg_c58, valid_done = 0;
+	struct noise_level noise_data;
+	u32 start  = 0, func_start = 0, func_end = 0;
+
+	func_start = jiffies;
+	pDM_Odm->noise_level.noise_all = 0;
+
+	if ((pDM_Odm->RFType == ODM_1T2R) || (pDM_Odm->RFType == ODM_2T2R))
+		max_rf_path = 2;
+	else
+		max_rf_path = 1;
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_DebugControlInbandNoise_Nseries() ==>\n"));
+
+	memset(&noise_data, 0, sizeof(struct noise_level));
+
+	/*  */
+	/*  Step 1. Disable DIG && Set initial gain. */
+	/*  */
+
+	if (bPauseDIG)
+		odm_PauseDIG(pDM_Odm, ODM_PAUSE_DIG, IGIValue);
+	/*  */
+	/*  Step 2. Disable all power save for read registers */
+	/*  */
+	/* dcmd_DebugControlPowerSave(padapter, PSDisable); */
+
+	/*  */
+	/*  Step 3. Get noise power level */
+	/*  */
+	start = jiffies;
+	while (1) {
+
+		/* Stop updating idle time pwer report (for driver read) */
+		PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_TxGainStage, BIT25, 1);
+
+		/* Read Noise Floor Report */
+		tmp4b = PHY_QueryBBReg(pDM_Odm->Adapter, 0x8f8, bMaskDWord);
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("Noise Floor Report (0x8f8) = 0x%08x\n", tmp4b));
+
+		/* PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XAAGCCore1, bMaskByte0, TestInitialGain); */
+		/* if (max_rf_path == 2) */
+		/* PHY_SetBBReg(pDM_Odm->Adapter, rOFDM0_XBAGCCore1, bMaskByte0, TestInitialGain); */
+
+		/* update idle time pwer report per 5us */
+		PHY_SetBBReg(pDM_Odm->Adapter, rFPGA0_TxGainStage, BIT25, 0);
+
+		noise_data.value[ODM_RF_PATH_A] = (u8)(tmp4b&0xff);
+		noise_data.value[ODM_RF_PATH_B]  = (u8)((tmp4b&0xff00)>>8);
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("value_a = 0x%x(%d), value_b = 0x%x(%d)\n",
+			noise_data.value[ODM_RF_PATH_A], noise_data.value[ODM_RF_PATH_A], noise_data.value[ODM_RF_PATH_B], noise_data.value[ODM_RF_PATH_B]));
+
+		for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+			noise_data.sval[rf_path] = (s8)noise_data.value[rf_path];
+			noise_data.sval[rf_path] /= 2;
+		}
+
+
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("sval_a = %d, sval_b = %d\n",
+			noise_data.sval[ODM_RF_PATH_A], noise_data.sval[ODM_RF_PATH_B]));
+		/* mdelay(10); */
+		/* msleep(10); */
+
+		for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+			if ((noise_data.valid_cnt[rf_path] < ValidCnt) && (noise_data.sval[rf_path] < Valid_Max && noise_data.sval[rf_path] >= Valid_Min)) {
+				noise_data.valid_cnt[rf_path]++;
+				noise_data.sum[rf_path] += noise_data.sval[rf_path];
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("RF_Path:%d Valid sval = %d\n", rf_path, noise_data.sval[rf_path]));
+				ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("Sum of sval = %d,\n", noise_data.sum[rf_path]));
+				if (noise_data.valid_cnt[rf_path] == ValidCnt) {
+					valid_done++;
+					ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("After divided, RF_Path:%d , sum = %d\n", rf_path, noise_data.sum[rf_path]));
+				}
+
+			}
+
+		}
+
+		/* printk("####### valid_done:%d #############\n", valid_done); */
+		if ((valid_done == max_rf_path) || (jiffies_to_msecs(jiffies - start) > max_time)) {
+			for (rf_path = ODM_RF_PATH_A; rf_path < max_rf_path; rf_path++) {
+				/* printk("%s PATH_%d - sum = %d, valid_cnt = %d\n", __func__, rf_path, noise_data.sum[rf_path], noise_data.valid_cnt[rf_path]); */
+				if (noise_data.valid_cnt[rf_path])
+					noise_data.sum[rf_path] /= noise_data.valid_cnt[rf_path];
+				else
+					noise_data.sum[rf_path]  = 0;
+			}
+			break;
+		}
+	}
+	reg_c50 = (s32)PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XAAGCCore1, bMaskByte0);
+	reg_c50 &= ~BIT7;
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("0x%x = 0x%02x(%d)\n", rOFDM0_XAAGCCore1, reg_c50, reg_c50));
+	pDM_Odm->noise_level.noise[ODM_RF_PATH_A] = -110 + reg_c50 + noise_data.sum[ODM_RF_PATH_A];
+	pDM_Odm->noise_level.noise_all += pDM_Odm->noise_level.noise[ODM_RF_PATH_A];
+
+	if (max_rf_path == 2) {
+		reg_c58 = (s32)PHY_QueryBBReg(pDM_Odm->Adapter, rOFDM0_XBAGCCore1, bMaskByte0);
+		reg_c58 &= ~BIT7;
+		ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("0x%x = 0x%02x(%d)\n", rOFDM0_XBAGCCore1, reg_c58, reg_c58));
+		pDM_Odm->noise_level.noise[ODM_RF_PATH_B] = -110 + reg_c58 + noise_data.sum[ODM_RF_PATH_B];
+		pDM_Odm->noise_level.noise_all += pDM_Odm->noise_level.noise[ODM_RF_PATH_B];
+	}
+	pDM_Odm->noise_level.noise_all /= max_rf_path;
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_COMMON,
+		ODM_DBG_LOUD,
+		(
+			"noise_a = %d, noise_b = %d\n",
+			pDM_Odm->noise_level.noise[ODM_RF_PATH_A],
+			pDM_Odm->noise_level.noise[ODM_RF_PATH_B]
+		)
+	);
+
+	/*  */
+	/*  Step 4. Recover the Dig */
+	/*  */
+	if (bPauseDIG)
+		odm_PauseDIG(pDM_Odm, ODM_RESUME_DIG, IGIValue);
+
+	func_end = jiffies_to_msecs(jiffies - func_start);
+	/* printk("%s noise_a = %d, noise_b = %d noise_all:%d (%d ms)\n", __func__, */
+	/* pDM_Odm->noise_level.noise[ODM_RF_PATH_A], */
+	/* pDM_Odm->noise_level.noise[ODM_RF_PATH_B], */
+	/* pDM_Odm->noise_level.noise_all, func_end); */
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_DebugControlInbandNoise_Nseries() <==\n"));
+	return pDM_Odm->noise_level.noise_all;
+
+}
+
+s16 ODM_InbandNoise_Monitor(void *pDM_VOID, u8 bPauseDIG, u8 IGIValue, u32 max_time)
+{
+	return odm_InbandNoise_Monitor_NSeries(pDM_VOID, bPauseDIG, IGIValue, max_time);
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.h b/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.h
new file mode 100644
index 000000000000..3f650912f4ab
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_NoiseMonitor.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODMNOISEMONITOR_H__
+#define __ODMNOISEMONITOR_H__
+
+#define	ODM_MAX_CHANNEL_NUM					38/* 14+24 */
+struct noise_level {
+	/* u8 value_a, value_b; */
+	u8 value[MAX_RF_PATH];
+	/* s8 sval_a, sval_b; */
+	s8 sval[MAX_RF_PATH];
+
+	/* s32 noise_a = 0, noise_b = 0, sum_a = 0, sum_b = 0; */
+	/* s32 noise[ODM_RF_PATH_MAX]; */
+	s32 sum[MAX_RF_PATH];
+	/* u8 valid_cnt_a = 0, valid_cnt_b = 0, */
+	u8 valid[MAX_RF_PATH];
+	u8 valid_cnt[MAX_RF_PATH];
+
+};
+
+
+typedef struct _ODM_NOISE_MONITOR_ {
+	s8 noise[MAX_RF_PATH];
+	s16 noise_all;
+} ODM_NOISE_MONITOR;
+
+s16 ODM_InbandNoise_Monitor(
+	void *pDM_VOID,
+	u8 bPauseDIG,
+	u8 IGIValue,
+	u32 max_time
+);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_PathDiv.c b/drivers/staging/rtl8723bs/hal/odm_PathDiv.c
new file mode 100644
index 000000000000..2735ebfb6be3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_PathDiv.c
@@ -0,0 +1,42 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+void odm_PathDiversityInit(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_PATH_DIV))
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_PATH_DIV,
+			ODM_DBG_LOUD,
+			("Return: Not Support PathDiv\n")
+		);
+}
+
+void odm_PathDiversity(void *pDM_VOID)
+{
+	PDM_ODM_T pDM_Odm = (PDM_ODM_T)pDM_VOID;
+
+	if (!(pDM_Odm->SupportAbility & ODM_BB_PATH_DIV))
+		ODM_RT_TRACE(
+			pDM_Odm,
+			ODM_COMP_PATH_DIV,
+			ODM_DBG_LOUD,
+			("Return: Not Support PathDiv\n")
+		);
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_PathDiv.h b/drivers/staging/rtl8723bs/hal/odm_PathDiv.h
new file mode 100644
index 000000000000..becde2e2a9ee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_PathDiv.h
@@ -0,0 +1,29 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODMPATHDIV_H__
+#define    __ODMPATHDIV_H__
+
+void
+odm_PathDiversityInit(
+	void *pDM_VOID
+	);
+
+void
+odm_PathDiversity(
+	void *pDM_VOID
+	);
+
+ #endif		 /* ifndef  __ODMPATHDIV_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/odm_RTL8723B.c b/drivers/staging/rtl8723bs/hal/odm_RTL8723B.c
new file mode 100644
index 000000000000..0e4ce2741737
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RTL8723B.c
@@ -0,0 +1,45 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+s8 odm_CCKRSSI_8723B(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)); */
+		/* DbgPrint("wrong LNA index\n"); */
+		break;
+
+	}
+	return rx_pwr_all;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_RTL8723B.h b/drivers/staging/rtl8723bs/hal/odm_RTL8723B.h
new file mode 100644
index 000000000000..0700351a2b74
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RTL8723B.h
@@ -0,0 +1,22 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODM_RTL8723B_H__
+#define __ODM_RTL8723B_H__
+
+#define	DM_DIG_MIN_NIC_8723	0x1C
+
+s8 odm_CCKRSSI_8723B(u8 LNA_idx, u8 VGA_idx);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c
new file mode 100644
index 000000000000..cdc9f38438d1
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.c
@@ -0,0 +1,257 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+void odm_ConfigRFReg_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Data,
+	ODM_RF_RADIO_PATH_E 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);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_INIT,
+					ODM_DBG_TRACE,
+					(
+						"===> ODM_ConfigRFWithHeaderFile: [B6] getvalue 0x%x, Data 0x%x, count %d\n",
+						getvalue,
+						Data,
+						count
+					)
+				);
+				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
+				);
+				ODM_RT_TRACE(
+					pDM_Odm,
+					ODM_COMP_INIT,
+					ODM_DBG_TRACE,
+					(
+						"===> ODM_ConfigRFWithHeaderFile: [B2] getvalue 0x%x, Data 0x%x, count %d\n",
+						getvalue,
+						Data,
+						count
+					)
+				);
+
+				if (count > 5)
+					break;
+			}
+		}
+	}
+}
+
+
+void odm_ConfigRF_RadioA_8723B(PDM_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,
+		ODM_RF_PATH_A,
+		Addr|maskforPhySet
+	);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigRFWithHeaderFile: [RadioA] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigMAC_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u8 Data)
+{
+	rtw_write8(pDM_Odm->Adapter, Addr, Data);
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigMACWithHeaderFile: [MAC_REG] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_AGC_8723B(
+	PDM_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);
+
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_TRACE,
+		(
+			"===> ODM_ConfigBBWithHeaderFile: [AGC_TAB] %08X %08X\n",
+			Addr,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_PHY_REG_PG_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Band,
+	u32 RfPath,
+	u32 TxNum,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+)
+{
+	if (Addr == 0xfe || Addr == 0xffe)
+		msleep(50);
+	else {
+		PHY_StoreTxPowerByRate(pDM_Odm->Adapter, Band, RfPath, TxNum, Addr, Bitmask, Data);
+	}
+	ODM_RT_TRACE(
+		pDM_Odm,
+		ODM_COMP_INIT,
+		ODM_DBG_LOUD,
+		(
+			"===> ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X %08X\n",
+			Addr,
+			Bitmask,
+			Data
+		)
+	);
+}
+
+void odm_ConfigBB_PHY_8723B(
+	PDM_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);
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("===> ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X\n", Addr, Data));
+}
+
+void odm_ConfigBB_TXPWR_LMT_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 *Regulation,
+	u8 *Band,
+	u8 *Bandwidth,
+	u8 *RateSection,
+	u8 *RfPath,
+	u8 *Channel,
+	u8 *PowerLimit
+)
+{
+	PHY_SetTxPowerLimit(
+		pDM_Odm->Adapter,
+		Regulation,
+		Band,
+		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 000000000000..a6b3d21e6463
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegConfig8723B.h
@@ -0,0 +1,65 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __INC_ODM_REGCONFIG_H_8723B
+#define __INC_ODM_REGCONFIG_H_8723B
+
+void odm_ConfigRFReg_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Data,
+	ODM_RF_RADIO_PATH_E RF_PATH,
+	u32 RegAddr
+);
+
+void odm_ConfigRF_RadioA_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u32 Data);
+
+void odm_ConfigMAC_8723B(PDM_ODM_T pDM_Odm, u32 Addr, u8 Data);
+
+void odm_ConfigBB_AGC_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_PHY_REG_PG_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Band,
+	u32 RfPath,
+	u32 TxNum,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_PHY_8723B(
+	PDM_ODM_T pDM_Odm,
+	u32 Addr,
+	u32 Bitmask,
+	u32 Data
+);
+
+void odm_ConfigBB_TXPWR_LMT_8723B(
+	PDM_ODM_T pDM_Odm,
+	u8 *Regulation,
+	u8 *Band,
+	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 000000000000..dc20e6165911
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
@@ -0,0 +1,172 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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_DEFUALT_A_11N		0x858
+#define	ODM_REG_RX_DEFUALT_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_TXAGC_A_MCS8_11_11N	0xE18
+#define	ODM_REG_TXAGC_A_MCS12_15_11N	0xE1C
+#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_debug.c b/drivers/staging/rtl8723bs/hal/odm_debug.c
new file mode 100644
index 000000000000..28cf0a66ff93
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_debug.c
@@ -0,0 +1,52 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 "odm_precomp.h"
+
+void ODM_InitDebugSetting(PDM_ODM_T pDM_Odm)
+{
+	pDM_Odm->DebugLevel = ODM_DBG_LOUD;
+
+	pDM_Odm->DebugComponents =
+/* BB Functions */
+/* 		ODM_COMP_DIG					| */
+/* 		ODM_COMP_RA_MASK				| */
+/* 		ODM_COMP_DYNAMIC_TXPWR		| */
+/* 		ODM_COMP_FA_CNT				| */
+/* 		ODM_COMP_RSSI_MONITOR			| */
+/* 		ODM_COMP_CCK_PD				| */
+/* 		ODM_COMP_ANT_DIV				| */
+/* 		ODM_COMP_PWR_SAVE				| */
+/* 		ODM_COMP_PWR_TRAIN			| */
+/* 		ODM_COMP_RATE_ADAPTIVE		| */
+/* 		ODM_COMP_PATH_DIV				| */
+/* 		ODM_COMP_DYNAMIC_PRICCA		| */
+/* 		ODM_COMP_RXHP					| */
+/* 		ODM_COMP_MP					| */
+/* 		ODM_COMP_CFO_TRACKING		| */
+
+/* MAC Functions */
+/* 		ODM_COMP_EDCA_TURBO			| */
+/* 		ODM_COMP_EARLY_MODE			| */
+/* RF Functions */
+/* 		ODM_COMP_TX_PWR_TRACK		| */
+/* 		ODM_COMP_RX_GAIN_TRACK		| */
+/* 		ODM_COMP_CALIBRATION			| */
+/* Common */
+/* 		ODM_COMP_COMMON				| */
+/* 		ODM_COMP_INIT					| */
+/* 		ODM_COMP_PSD					| */
+0;
+}
diff --git a/drivers/staging/rtl8723bs/hal/odm_debug.h b/drivers/staging/rtl8723bs/hal/odm_debug.h
new file mode 100644
index 000000000000..2ec4baf57464
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_debug.h
@@ -0,0 +1,166 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __ODM_DBG_H__
+#define __ODM_DBG_H__
+
+
+/*  */
+/* Define the debug levels */
+/*  */
+/* 1.	DBG_TRACE and DBG_LOUD are used for normal cases. */
+/* So that, they can help SW engineer to develope or trace states changed */
+/* and also help HW enginner to trace every operation to and from HW, */
+/* e.g IO, Tx, Rx. */
+/*  */
+/* 2.	DBG_WARNNING and DBG_SERIOUS are used for unusual or error cases, */
+/* which help us to debug SW or HW. */
+/*  */
+/*  */
+/*  */
+/* Never used in a call to ODM_RT_TRACE()! */
+/*  */
+#define ODM_DBG_OFF					1
+
+/*  */
+/* Fatal bug. */
+/* For example, Tx/Rx/IO locked up, OS hangs, memory access violation, */
+/* resource allocation failed, unexpected HW behavior, HW BUG and so on. */
+/*  */
+#define ODM_DBG_SERIOUS				2
+
+/*  */
+/* Abnormal, rare, or unexpeted cases. */
+/* For example, */
+/* IRP/Packet/OID canceled, */
+/* device suprisely unremoved and so on. */
+/*  */
+#define ODM_DBG_WARNING				3
+
+/*  */
+/* Normal case with useful information about current SW or HW state. */
+/* For example, Tx/Rx descriptor to fill, Tx/Rx descriptor completed status, */
+/* SW protocol state change, dynamic mechanism state change and so on. */
+/*  */
+#define ODM_DBG_LOUD				4
+
+/*  */
+/* Normal case with detail execution flow or information. */
+/*  */
+#define ODM_DBG_TRACE				5
+
+/*  */
+/*  Define the tracing components */
+/*  */
+/*  */
+/* BB Functions */
+#define ODM_COMP_DIG				BIT0
+#define ODM_COMP_RA_MASK			BIT1
+#define ODM_COMP_DYNAMIC_TXPWR		BIT2
+#define ODM_COMP_FA_CNT				BIT3
+#define ODM_COMP_RSSI_MONITOR		BIT4
+#define ODM_COMP_CCK_PD				BIT5
+#define ODM_COMP_ANT_DIV			BIT6
+#define ODM_COMP_PWR_SAVE			BIT7
+#define ODM_COMP_PWR_TRAIN			BIT8
+#define ODM_COMP_RATE_ADAPTIVE		BIT9
+#define ODM_COMP_PATH_DIV			BIT10
+#define ODM_COMP_PSD				BIT11
+#define ODM_COMP_DYNAMIC_PRICCA		BIT12
+#define ODM_COMP_RXHP				BIT13
+#define ODM_COMP_MP					BIT14
+#define ODM_COMP_CFO_TRACKING		BIT15
+/* MAC Functions */
+#define ODM_COMP_EDCA_TURBO			BIT16
+#define ODM_COMP_EARLY_MODE			BIT17
+/* RF Functions */
+#define ODM_COMP_TX_PWR_TRACK		BIT24
+#define ODM_COMP_RX_GAIN_TRACK		BIT25
+#define ODM_COMP_CALIBRATION		BIT26
+/* Common Functions */
+#define ODM_COMP_COMMON				BIT30
+#define ODM_COMP_INIT				BIT31
+
+/*------------------------Export Marco Definition---------------------------*/
+	#define DbgPrint printk
+	#define RT_PRINTK(fmt, args...)\
+		DbgPrint("%s(): " fmt, __func__, ## args)
+	#define RT_DISP(dbgtype, dbgflag, printstr)
+
+#ifndef ASSERT
+	#define ASSERT(expr)
+#endif
+
+#if DBG
+#define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel || level == ODM_DBG_SERIOUS)\
+	) {\
+		RT_PRINTK fmt;\
+	}
+
+#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel)\
+	) {\
+		RT_PRINTK fmt;\
+	}
+
+#define ODM_RT_ASSERT(pDM_Odm, expr, fmt)\
+	if (!expr) {\
+		DbgPrint("Assertion failed! %s at ......\n", #expr);\
+		DbgPrint(\
+			"      ......%s,%s, line =%d\n",\
+			__FILE__,\
+			__func__,\
+			__LINE__\
+		);\
+		RT_PRINTK fmt;\
+		ASSERT(false);\
+	}
+#define ODM_dbg_enter() { DbgPrint("==> %s\n", __func__); }
+#define ODM_dbg_exit() { DbgPrint("<== %s\n", __func__); }
+#define ODM_dbg_trace(str) { DbgPrint("%s:%s\n", __func__, str); }
+
+#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr)\
+	if (\
+		(comp & pDM_Odm->DebugComponents) &&\
+		(level <= pDM_Odm->DebugLevel)\
+	) {\
+		int __i;\
+		u8 *__ptr = (u8 *)ptr;\
+		DbgPrint("[ODM] ");\
+		DbgPrint(title_str);\
+		DbgPrint(" ");\
+		for (__i = 0; __i < 6; __i++)\
+			DbgPrint("%02X%s", __ptr[__i], (__i == 5) ? "" : "-");\
+		DbgPrint("\n");\
+	}
+#else
+#define ODM_RT_TRACE(pDM_Odm, comp, level, fmt)		no_printk fmt
+#define ODM_RT_TRACE_F(pDM_Odm, comp, level, fmt)	no_printk fmt
+#define ODM_RT_ASSERT(pDM_Odm, expr, fmt)		no_printk fmt
+#define ODM_dbg_enter()					do {} while (0)
+#define ODM_dbg_exit()					do {} while (0)
+#define ODM_dbg_trace(str)				no_printk("%s", str)
+#define ODM_PRINT_ADDR(pDM_Odm, comp, level, title_str, ptr) \
+	no_printk("%s %p", title_str, ptr)
+#endif
+
+void ODM_InitDebugSetting(PDM_ODM_T pDM_Odm);
+
+#endif	/*  __ODM_DBG_H__ */
diff --git a/drivers/staging/rtl8723bs/hal/odm_interface.h b/drivers/staging/rtl8723bs/hal/odm_interface.h
new file mode 100644
index 000000000000..8ad0a0afca57
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_interface.h
@@ -0,0 +1,59 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__ODM_INTERFACE_H__
+#define __ODM_INTERFACE_H__
+
+
+
+/*  =========== Constant/Structure/Enum/... Define */
+
+/*  =========== 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)
+
+typedef enum _ODM_H2C_CMD {
+	ODM_H2C_RSSI_REPORT = 0,
+	ODM_H2C_PSD_RESULT = 1,
+	ODM_H2C_PathDiv = 2,
+	ODM_H2C_WIFI_CALIBRATION = 3,
+	ODM_MAX_H2CCMD
+} ODM_H2C_CMD;
+
+
+#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 000000000000..f543bdb31a89
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_precomp.h
@@ -0,0 +1,60 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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_debug.h"
+#include "odm_RegDefine11N.h"
+#include "odm_AntDiv.h"
+#include "odm_EdcaTurboCheck.h"
+#include "odm_DIG.h"
+#include "odm_PathDiv.h"
+#include "odm_DynamicBBPowerSaving.h"
+#include "odm_DynamicTxPower.h"
+#include "odm_CfoTracking.h"
+#include "odm_NoiseMonitor.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_RTL8723B.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 000000000000..2496dcef7a9f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_reg.h
@@ -0,0 +1,103 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*  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_TXAGC_B_MCS8_MCS11			0x84c
+#define	ODM_ANALOG_REGISTER			0x85c
+#define	ODM_RF_INTERFACE_OUTPUT			0x860
+#define	ODM_TXAGC_B_MCS12_MCS15			0x868
+#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
+#define	ODM_TXAGC_A_MCS8_MCS11			0xe18
+#define	ODM_TXAGC_A_MCS12_MCS15			0xe1c
+
+/* 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 000000000000..9e3d072b03db
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/odm_types.h
@@ -0,0 +1,102 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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)))
+
+typedef 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,*/
+} HAL_STATUS, *PHAL_STATUS;
+
+
+/*  */
+/*  Declare for ODM spin lock defintion temporarily fro compile pass. */
+/*  */
+typedef enum _RT_SPINLOCK_TYPE {
+	RT_TX_SPINLOCK = 1,
+	RT_RX_SPINLOCK = 2,
+	RT_RM_SPINLOCK = 3,
+	RT_CAM_SPINLOCK = 4,
+	RT_SCAN_SPINLOCK = 5,
+	RT_LOG_SPINLOCK = 7,
+	RT_BW_SPINLOCK = 8,
+	RT_CHNLOP_SPINLOCK = 9,
+	RT_RF_OPERATE_SPINLOCK = 10,
+	RT_INITIAL_SPINLOCK = 11,
+	RT_RF_STATE_SPINLOCK = 12, /*  For RF state. Added by Bruce, 2007-10-30. */
+	/* Shall we define Ndis 6.2 SpinLock Here ? */
+	RT_PORT_SPINLOCK = 16,
+	RT_H2C_SPINLOCK = 20, /*  For H2C cmd. Added by tynli. 2009.11.09. */
+
+	RT_BTData_SPINLOCK = 25,
+
+	RT_WAPI_OPTION_SPINLOCK = 26,
+	RT_WAPI_RX_SPINLOCK = 27,
+
+	/*  add for 92D CCK control issue */
+	RT_CCK_PAGEA_SPINLOCK = 28,
+	RT_BUFFER_SPINLOCK = 29,
+	RT_CHANNEL_AND_BANDWIDTH_SPINLOCK = 30,
+	RT_GEN_TEMP_BUF_SPINLOCK = 31,
+	RT_AWB_SPINLOCK = 32,
+	RT_FW_PS_SPINLOCK = 33,
+	RT_HW_TIMER_SPIN_LOCK = 34,
+	RT_MPT_WI_SPINLOCK = 35,
+	RT_P2P_SPIN_LOCK = 36,	/*  Protect P2P context */
+	RT_DBG_SPIN_LOCK = 37,
+	RT_IQK_SPINLOCK = 38,
+	RT_PENDED_OID_SPINLOCK = 39,
+	RT_CHNLLIST_SPINLOCK = 40,
+	RT_INDIC_SPINLOCK = 41,	/* protect indication */
+} RT_SPINLOCK_TYPE;
+
+	#if defined(__LITTLE_ENDIAN)
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_LITTLE
+	#else
+		#define	ODM_ENDIAN_TYPE			ODM_ENDIAN_BIG
+	#endif
+
+	typedef struct timer_list		RT_TIMER, *PRT_TIMER;
+	typedef  void *RT_TIMER_CALL_BACK;
+	#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 000000000000..69eed62b7c94
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
@@ -0,0 +1,2358 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTL8723B_CMD_C_
+
+#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;
+
+	/* DBG_8192C(" _is_fw_read_cmd_down , reg_1cc(%x), msg_box(%d)...\n", rtw_read8(padapter, REG_HMETFR), msgbox_num); */
+
+	do {
+		valid = rtw_read8(padapter, REG_HMETFR) & BIT(msgbox_num);
+		if (0 == valid) {
+			read_down = true;
+		}
+#ifdef CONFIG_WOWLAN
+		else
+			msleep(1);
+#endif
+	} 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 == true)
+		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)) {
+			DBG_8192C(" fw read cmd failed...\n");
+			/* DBG_8192C(" 0x1c0: 0x%8x\n", rtw_read32(padapter, 0x1c0)); */
+			/* DBG_8192C(" 0x1c4: 0x%8x\n", rtw_read32(padapter, 0x1c4)); */
+			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);
+
+		/* DBG_8192C("MSG_BOX:%d, CmdLen(%d), CmdID(0x%x), reg:0x%x =>h2c_cmd:0x%.8x, reg:0x%x =>h2c_cmd_ex:0x%.8x\n" */
+		/* 	, pHalData->LastHMEBoxNum , CmdLen, ElementID, msgbox_addr, h2c_cmd, msgbox_ex_addr, h2c_cmd_ex); */
+
+		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);
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	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) {
+		/* DBG_871X("ie len =%d\n", cur_network->IELength); */
+		pktlen += cur_network->IELength - 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, _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 = cur->Configuration.ATIMWindow; */
+		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) {
+		DBG_871X("beacon frame too large\n");
+		return;
+	}
+
+	*pLength = pktlen;
+
+	/* DBG_871X("%s bcn_sz =%d\n", __func__, 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);
+
+	/* DBG_871X("%s\n", __func__); */
+
+	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);
+
+
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	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(&(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 == true) {
+		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;
+}
+
+
+#ifdef CONFIG_WOWLAN
+/*  */
+/*  Description: */
+/* 	Construct the ARP response packet to support ARP offload. */
+/*  */
+static void ConstructARPResponse(
+	struct adapter *padapter,
+	u8 *pframe,
+	u32 *pLength,
+	u8 *pIPAddress
+)
+{
+	struct ieee80211_hdr	*pwlanhdr;
+	__le16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	static u8 	ARPLLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x08, 0x06};
+	u8 		*pARPRspPkt = pframe;
+	/* for TKIP Cal MIC */
+	u8 		*payload = pframe;
+	u8 	EncryptionHeadOverhead = 0;
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	/*  */
+	/*  MAC Header. */
+	/*  */
+	SetFrameType(fctrl, WIFI_DATA);
+	/* SetFrameSubType(fctrl, 0); */
+	SetToDs(fctrl);
+	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, 0);
+	SetDuration(pwlanhdr, 0);
+	/* SET_80211_HDR_FRAME_CONTROL(pARPRspPkt, 0); */
+	/* SET_80211_HDR_TYPE_AND_SUBTYPE(pARPRspPkt, Type_Data); */
+	/* SET_80211_HDR_TO_DS(pARPRspPkt, 1); */
+	/* SET_80211_HDR_ADDRESS1(pARPRspPkt, pMgntInfo->Bssid); */
+	/* SET_80211_HDR_ADDRESS2(pARPRspPkt, Adapter->CurrentAddress); */
+	/* SET_80211_HDR_ADDRESS3(pARPRspPkt, pMgntInfo->Bssid); */
+
+	/* SET_80211_HDR_DURATION(pARPRspPkt, 0); */
+	/* SET_80211_HDR_FRAGMENT_SEQUENCE(pARPRspPkt, 0); */
+	*pLength = 24;
+
+	/*  */
+	/*  Security Header: leave space for it if necessary. */
+	/*  */
+
+	switch (psecuritypriv->dot11PrivacyAlgrthm) {
+	case _WEP40_:
+	case _WEP104_:
+		EncryptionHeadOverhead = 4;
+		break;
+	case _TKIP_:
+		EncryptionHeadOverhead = 8;
+		break;
+	case _AES_:
+		EncryptionHeadOverhead = 8;
+		break;
+	default:
+		EncryptionHeadOverhead = 0;
+	}
+
+	if (EncryptionHeadOverhead > 0) {
+		memset(&(pframe[*pLength]), 0, EncryptionHeadOverhead);
+		*pLength += EncryptionHeadOverhead;
+		SetPrivacy(fctrl);
+	}
+
+	/*  */
+	/*  Frame Body. */
+	/*  */
+	pARPRspPkt = (u8 *)(pframe + *pLength);
+	payload = pARPRspPkt; /* Get Payload pointer */
+	/*  LLC header */
+	memcpy(pARPRspPkt, ARPLLCHeader, 8);
+	*pLength += 8;
+
+	/*  ARP element */
+	pARPRspPkt += 8;
+	SET_ARP_PKT_HW(pARPRspPkt, 0x0100);
+	SET_ARP_PKT_PROTOCOL(pARPRspPkt, 0x0008);	/*  IP protocol */
+	SET_ARP_PKT_HW_ADDR_LEN(pARPRspPkt, 6);
+	SET_ARP_PKT_PROTOCOL_ADDR_LEN(pARPRspPkt, 4);
+	SET_ARP_PKT_OPERATION(pARPRspPkt, 0x0200); /*  ARP response */
+	SET_ARP_PKT_SENDER_MAC_ADDR(pARPRspPkt, myid(&(padapter->eeprompriv)));
+	SET_ARP_PKT_SENDER_IP_ADDR(pARPRspPkt, pIPAddress);
+	{
+		SET_ARP_PKT_TARGET_MAC_ADDR(pARPRspPkt, get_my_bssid(&(pmlmeinfo->network)));
+		SET_ARP_PKT_TARGET_IP_ADDR(pARPRspPkt, pIPAddress);
+		DBG_871X("%s Target Mac Addr:" MAC_FMT "\n", __func__, MAC_ARG(get_my_bssid(&(pmlmeinfo->network))));
+		DBG_871X("%s Target IP Addr" IP_FMT "\n", __func__, IP_ARG(pIPAddress));
+	}
+
+	*pLength += 28;
+
+	if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_) {
+		u8 mic[8];
+		struct mic_data	micdata;
+		struct sta_info *psta = NULL;
+		u8 priority[4] = {
+			0x0, 0x0, 0x0, 0x0
+		};
+		u8 null_key[16] = {
+			0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+			0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0
+		};
+
+		DBG_871X("%s(): Add MIC\n", __func__);
+
+		psta = rtw_get_stainfo(&padapter->stapriv, get_my_bssid(&(pmlmeinfo->network)));
+		if (psta != NULL) {
+			if (!memcmp(&psta->dot11tkiptxmickey.skey[0], null_key, 16)) {
+				DBG_871X("%s(): STA dot11tkiptxmickey == 0\n", __func__);
+			}
+			/* start to calculate the mic code */
+			rtw_secmicsetkey(&micdata, &psta->dot11tkiptxmickey.skey[0]);
+		}
+
+		rtw_secmicappend(&micdata, pwlanhdr->addr3, 6);  /* DA */
+
+		rtw_secmicappend(&micdata, pwlanhdr->addr2, 6); /* SA */
+
+		priority[0] = 0;
+		rtw_secmicappend(&micdata, &priority[0], 4);
+
+		rtw_secmicappend(&micdata, payload, 36); /* payload length = 8 + 28 */
+
+		rtw_secgetmic(&micdata, &(mic[0]));
+
+		pARPRspPkt += 28;
+		memcpy(pARPRspPkt, &(mic[0]), 8);
+
+		*pLength += 8;
+	}
+}
+
+#ifdef CONFIG_PNO_SUPPORT
+static void ConstructPnoInfo(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	u8 *pPnoInfoPkt = pframe;
+	pPnoInfoPkt = (u8 *)(pframe + *pLength);
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_num, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->fast_scan_period, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->fast_scan_iterations, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->slow_scan_period, 4);
+
+	*pLength += 4;
+	pPnoInfoPkt += 4;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_length,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_cipher_info,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+	memcpy(pPnoInfoPkt, &pwrctl->pnlo_info->ssid_channel_info,
+			MAX_PNO_LIST_COUNT);
+
+	*pLength += MAX_PNO_LIST_COUNT;
+	pPnoInfoPkt += MAX_PNO_LIST_COUNT;
+}
+
+static void ConstructSSIDList(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	int i = 0;
+	u8 *pSSIDListPkt = pframe;
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	pSSIDListPkt = (u8 *)(pframe + *pLength);
+
+	for (i = 0; i < pwrctl->pnlo_info->ssid_num ; i++) {
+		memcpy(pSSIDListPkt, &pwrctl->pno_ssid_list->node[i].SSID,
+			pwrctl->pnlo_info->ssid_length[i]);
+
+		*pLength += WLAN_SSID_MAXLEN;
+		pSSIDListPkt += WLAN_SSID_MAXLEN;
+	}
+}
+
+static void ConstructScanInfo(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	int i = 0;
+	u8 *pScanInfoPkt = pframe;
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	pScanInfoPkt = (u8 *)(pframe + *pLength);
+
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->channel_num, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_ch, 1);
+
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_bw, 1);
+
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_40_offset, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->orig_80_offset, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->periodScan, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->period_scan_time, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->enableRFE, 1);
+
+	*pLength += 1;
+	pScanInfoPkt += 1;
+	memcpy(pScanInfoPkt, &pwrctl->pscan_info->rfe_type, 8);
+
+	*pLength += 8;
+	pScanInfoPkt += 8;
+
+	for (i = 0; i < MAX_SCAN_LIST_COUNT; i++) {
+		memcpy(pScanInfoPkt, &pwrctl->pscan_info->ssid_channel_info[i], 4);
+		*pLength += 4;
+		pScanInfoPkt += 4;
+	}
+}
+#endif
+
+#ifdef CONFIG_GTK_OL
+static void ConstructGTKResponse(
+	struct adapter *padapter, u8 *pframe, u32 *pLength
+)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	static u8 LLCHeader[8] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8E};
+	static u8 GTKbody_a[11] = {0x01, 0x03, 0x00, 0x5F, 0x02, 0x03, 0x12, 0x00, 0x10, 0x42, 0x0B};
+	u8 *pGTKRspPkt = pframe;
+	u8 EncryptionHeadOverhead = 0;
+	/* DBG_871X("%s:%d\n", __func__, bForcePowerSave); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	fctrl = &pwlanhdr->frame_control;
+	*(fctrl) = 0;
+
+	/*  */
+	/*  MAC Header. */
+	/*  */
+	SetFrameType(fctrl, WIFI_DATA);
+	/* SetFrameSubType(fctrl, 0); */
+	SetToDs(fctrl);
+	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, 0);
+	SetDuration(pwlanhdr, 0);
+
+	*pLength = 24;
+
+	/*  */
+	/*  Security Header: leave space for it if necessary. */
+	/*  */
+
+	switch (psecuritypriv->dot11PrivacyAlgrthm) {
+	case _WEP40_:
+	case _WEP104_:
+		EncryptionHeadOverhead = 4;
+		break;
+	case _TKIP_:
+		EncryptionHeadOverhead = 8;
+		break;
+	case _AES_:
+		EncryptionHeadOverhead = 8;
+		break;
+	default:
+		EncryptionHeadOverhead = 0;
+	}
+
+	if (EncryptionHeadOverhead > 0) {
+		memset(&(pframe[*pLength]), 0, EncryptionHeadOverhead);
+		*pLength += EncryptionHeadOverhead;
+		/* GTK's privacy bit is done by FW */
+		/* SetPrivacy(fctrl); */
+	}
+
+	/*  */
+	/*  Frame Body. */
+	/*  */
+	pGTKRspPkt =  (u8 *)(pframe + *pLength);
+	/*  LLC header */
+	memcpy(pGTKRspPkt, LLCHeader, 8);
+	*pLength += 8;
+
+	/*  GTK element */
+	pGTKRspPkt += 8;
+
+	/* GTK frame body after LLC, part 1 */
+	memcpy(pGTKRspPkt, GTKbody_a, 11);
+	*pLength += 11;
+	pGTKRspPkt += 11;
+	/* GTK frame body after LLC, part 2 */
+	memset(&(pframe[*pLength]), 0, 88);
+	*pLength += 88;
+	pGTKRspPkt += 88;
+
+}
+#endif /* CONFIG_GTK_OL */
+
+#ifdef CONFIG_PNO_SUPPORT
+static void ConstructProbeReq(struct adapter *padapter, u8 *pframe, u32 *pLength)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	u32 pktlen;
+	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);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	int bssrate_len = 0;
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	mac = myid(&(padapter->eeprompriv));
+
+	fctrl = &(pwlanhdr->frame_control);
+	*(fctrl) = 0;
+
+	/* broadcast probe request frame */
+	memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, bc_addr, ETH_ALEN);
+
+	memcpy(pwlanhdr->addr2, mac, ETH_ALEN);
+
+	SetSeqNum(pwlanhdr, 0);
+	SetFrameSubType(pframe, WIFI_PROBEREQ);
+
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+	pframe += pktlen;
+
+	pframe = rtw_set_ie(pframe, _SSID_IE_, 0, NULL, &pktlen);
+
+	get_rate_set(padapter, bssrate, &bssrate_len);
+
+	if (bssrate_len > 8) {
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, 8, bssrate, &pktlen);
+		pframe = rtw_set_ie(pframe, _EXT_SUPPORTEDRATES_IE_, (bssrate_len - 8), (bssrate + 8), &pktlen);
+	} else
+		pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, bssrate_len, bssrate, &pktlen);
+
+	*pLength = pktlen;
+}
+#endif /* CONFIG_PNO_SUPPORT */
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+static void ConstructProbeRsp(struct adapter *padapter, u8 *pframe, u32 *pLength, u8 *StaAddr, bool bHideSSID)
+{
+	struct ieee80211_hdr *pwlanhdr;
+	u16 *fctrl;
+	u8 *mac, *bssid;
+	u32 pktlen;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct wlan_bssid_ex *cur_network = &(pmlmeinfo->network);
+	u8 *pwps_ie;
+	uint wps_ielen;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	/* DBG_871X("%s\n", __func__); */
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+
+	mac = myid(&(padapter->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);
+
+	DBG_871X("%s FW Mac Addr:" MAC_FMT "\n", __func__, MAC_ARG(mac));
+	DBG_871X("%s FW IP Addr" IP_FMT "\n", __func__, IP_ARG(StaAddr));
+
+	SetSeqNum(pwlanhdr, 0);
+	SetFrameSubType(fctrl, WIFI_PROBERSP);
+
+	pktlen = sizeof(struct ieee80211_hdr_3addr);
+	pframe += pktlen;
+
+	if (cur_network->IELength > MAX_IE_SZ)
+		return;
+
+	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 != NULL) && 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;
+		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;
+			pktlen += wps_ielen+2;
+		}
+
+		if ((wps_offset+wps_ielen+2+remainder_ielen) <= MAX_IE_SZ) {
+			memcpy(pframe, premainder_ie, remainder_ielen);
+			pframe += remainder_ielen;
+			pktlen += remainder_ielen;
+		}
+	} else {
+		memcpy(pframe, cur_network->IEs, cur_network->IELength);
+		pframe += cur_network->IELength;
+		pktlen += cur_network->IELength;
+	}
+
+	/* retrieve SSID IE from cur_network->Ssid */
+	{
+		u8 *ssid_ie;
+		sint ssid_ielen;
+		sint ssid_ielen_diff;
+		u8 buf[MAX_IE_SZ];
+		u8 *ies = pframe + sizeof(struct ieee80211_hdr_3addr);
+
+		ssid_ie = rtw_get_ie(ies+_FIXED_IE_LENGTH_, _SSID_IE_, &ssid_ielen,
+					(pframe-ies)-_FIXED_IE_LENGTH_);
+
+		ssid_ielen_diff = cur_network->Ssid.SsidLength - ssid_ielen;
+
+		if (ssid_ie &&	cur_network->Ssid.SsidLength) {
+			uint remainder_ielen;
+			u8 *remainder_ie;
+			remainder_ie = ssid_ie+2;
+			remainder_ielen = (pframe-remainder_ie);
+
+			if (remainder_ielen > MAX_IE_SZ) {
+				DBG_871X_LEVEL(_drv_warning_, 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.SsidLength;
+			memcpy(ssid_ie+2, cur_network->Ssid.Ssid, cur_network->Ssid.SsidLength);
+			pframe += ssid_ielen_diff;
+			pktlen += ssid_ielen_diff;
+		}
+	}
+
+	*pLength = pktlen;
+
+}
+#endif /*  CONFIG_AP_WOWLAN */
+
+/*  To check if reserved page content is destroyed by beacon beacuse beacon is too large. */
+/*  2010.06.23. Added by tynli. */
+void CheckFwRsvdPageContent(struct adapter *Adapter)
+{
+}
+
+static void rtl8723b_set_FwRsvdPage_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc)
+{
+	u8 u1H2CRsvdPageParm[H2C_RSVDPAGE_LOC_LEN] = {0};
+
+	DBG_871X("8723BRsvdPageLoc: ProbeRsp =%d PsPoll =%d Null =%d QoSNull =%d BTNull =%d\n",
+		rsvdpageloc->LocProbeRsp, rsvdpageloc->LocPsPoll,
+		rsvdpageloc->LocNullData, rsvdpageloc->LocQosNull,
+		rsvdpageloc->LocBTQosNull);
+
+	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);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CRsvdPageParm:", u1H2CRsvdPageParm, H2C_RSVDPAGE_LOC_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_RSVD_PAGE, H2C_RSVDPAGE_LOC_LEN, u1H2CRsvdPageParm);
+}
+
+static void rtl8723b_set_FwAoacRsvdPage_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc)
+{
+#ifdef CONFIG_WOWLAN
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	u8 u1H2CAoacRsvdPageParm[H2C_AOAC_RSVDPAGE_LOC_LEN] = {0};
+
+	DBG_871X("8723BAOACRsvdPageLoc: RWC =%d ArpRsp =%d NbrAdv =%d GtkRsp =%d GtkInfo =%d ProbeReq =%d NetworkList =%d\n",
+			rsvdpageloc->LocRemoteCtrlInfo, rsvdpageloc->LocArpRsp,
+			rsvdpageloc->LocNbrAdv, rsvdpageloc->LocGTKRsp,
+			rsvdpageloc->LocGTKInfo, rsvdpageloc->LocProbeReq,
+			rsvdpageloc->LocNetList);
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocRemoteCtrlInfo);
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(u1H2CAoacRsvdPageParm, rsvdpageloc->LocArpRsp);
+		/* SET_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(u1H2CAoacRsvdPageParm, rsvdpageloc->LocNbrAdv); */
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKRsp);
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKInfo);
+#ifdef CONFIG_GTK_OL
+		SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(u1H2CAoacRsvdPageParm, rsvdpageloc->LocGTKEXTMEM);
+#endif /*  CONFIG_GTK_OL */
+		RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CAoacRsvdPageParm:", u1H2CAoacRsvdPageParm, H2C_AOAC_RSVDPAGE_LOC_LEN);
+		FillH2CCmd8723B(padapter, H2C_8723B_AOAC_RSVD_PAGE, H2C_AOAC_RSVDPAGE_LOC_LEN, u1H2CAoacRsvdPageParm);
+	} else {
+#ifdef CONFIG_PNO_SUPPORT
+		if (!pwrpriv->pno_in_resume) {
+			DBG_871X("NLO_INFO =%d\n", rsvdpageloc->LocPNOInfo);
+			memset(&u1H2CAoacRsvdPageParm, 0, sizeof(u1H2CAoacRsvdPageParm));
+			SET_H2CCMD_AOAC_RSVDPAGE_LOC_NLO_INFO(u1H2CAoacRsvdPageParm, rsvdpageloc->LocPNOInfo);
+			FillH2CCmd8723B(padapter, H2C_AOAC_RSVDPAGE3, H2C_AOAC_RSVDPAGE_LOC_LEN, u1H2CAoacRsvdPageParm);
+			msleep(10);
+		}
+#endif
+	}
+
+#endif /*  CONFIG_WOWLAN */
+}
+
+#ifdef CONFIG_AP_WOWLAN
+static void rtl8723b_set_ap_wow_rsvdpage_cmd(
+	struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc
+)
+{
+	u8 header;
+	u8 rsvdparm[H2C_AOAC_RSVDPAGE_LOC_LEN] = {0};
+
+	header = rtw_read8(padapter, REG_BCNQ_BDNY);
+
+	DBG_871X("%s: beacon: %d, probeRsp: %d, header:0x%02x\n", __func__,
+			rsvdpageloc->LocApOffloadBCN,
+			rsvdpageloc->LocProbeRsp,
+			header);
+
+	SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_BCN(rsvdparm,
+			rsvdpageloc->LocApOffloadBCN + header);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_BCN_RSVDPAGE,
+			H2C_BCN_RSVDPAGE_LEN, rsvdparm);
+
+	msleep(10);
+
+	memset(&rsvdparm, 0, sizeof(rsvdparm));
+
+	SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_ProbeRsp(
+			rsvdparm,
+			rsvdpageloc->LocProbeRsp + header);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_PROBERSP_RSVDPAGE,
+			H2C_PROBERSP_RSVDPAGE_LEN, rsvdparm);
+
+	msleep(10);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+void rtl8723b_set_FwMediaStatusRpt_cmd(struct adapter *padapter, u8 mstatus, u8 macid)
+{
+	u8 u1H2CMediaStatusRptParm[H2C_MEDIA_STATUS_RPT_LEN] = {0};
+	u8 macid_end = 0;
+
+	DBG_871X("%s(): mstatus = %d macid =%d\n", __func__, mstatus, macid);
+
+	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);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CMediaStatusRptParm:", u1H2CMediaStatusRptParm, H2C_MEDIA_STATUS_RPT_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_MEDIA_STATUS_RPT, H2C_MEDIA_STATUS_RPT_LEN, u1H2CMediaStatusRptParm);
+}
+
+#ifdef CONFIG_WOWLAN
+static void rtl8723b_set_FwKeepAlive_cmd(struct adapter *padapter, u8 benable, u8 pkt_type)
+{
+	u8 u1H2CKeepAliveParm[H2C_KEEP_ALIVE_CTRL_LEN] = {0};
+	u8 adopt = 1, check_period = 5;
+
+	DBG_871X("%s(): benable = %d\n", __func__, benable);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_ENABLE(u1H2CKeepAliveParm, benable);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_ADOPT(u1H2CKeepAliveParm, adopt);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_PKT_TYPE(u1H2CKeepAliveParm, pkt_type);
+	SET_8723B_H2CCMD_KEEPALIVE_PARM_CHECK_PERIOD(u1H2CKeepAliveParm, check_period);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CKeepAliveParm:", u1H2CKeepAliveParm, H2C_KEEP_ALIVE_CTRL_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_KEEP_ALIVE, H2C_KEEP_ALIVE_CTRL_LEN, u1H2CKeepAliveParm);
+}
+
+static void rtl8723b_set_FwDisconDecision_cmd(struct adapter *padapter, u8 benable)
+{
+	u8 u1H2CDisconDecisionParm[H2C_DISCON_DECISION_LEN] = {0};
+	u8 adopt = 1, check_period = 10, trypkt_num = 0;
+
+	DBG_871X("%s(): benable = %d\n", __func__, benable);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_ENABLE(u1H2CDisconDecisionParm, benable);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_ADOPT(u1H2CDisconDecisionParm, adopt);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_CHECK_PERIOD(u1H2CDisconDecisionParm, check_period);
+	SET_8723B_H2CCMD_DISCONDECISION_PARM_TRY_PKT_NUM(u1H2CDisconDecisionParm, trypkt_num);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CDisconDecisionParm:", u1H2CDisconDecisionParm, H2C_DISCON_DECISION_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_DISCON_DECISION, H2C_DISCON_DECISION_LEN, u1H2CDisconDecisionParm);
+}
+#endif /*  CONFIG_WOWLAN */
+
+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};
+
+	DBG_871X("%s(): mac_id =%d raid = 0x%x bw =%d mask = 0x%x\n", __func__, mac_id, raid, bw, mask);
+
+	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));
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CMacIdConfigParm:", u1H2CMacIdConfigParm, H2C_MACID_CFG_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_MACID_CFG, H2C_MACID_CFG_LEN, u1H2CMacIdConfigParm);
+}
+
+static void rtl8723b_set_FwRssiSetting_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;
+
+	/* DBG_871X("%s(): param =%.2x-%.2x-%.2x\n", __func__, *param, *(param+1), *(param+2)); */
+	/* DBG_871X("%s(): mac_id =%d rssi =%d\n", __func__, mac_id, rssi); */
+
+	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);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_notice_, "u1H2CRssiSettingParm:", u1H2CRssiSettingParm, H2C_RSSI_SETTING_LEN);
+	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)
+		DBG_871X("%s(): FW LPS mode = %d, SmartPS =%d, dtim =%d\n", __func__, psmode, pwrpriv->smart_ps, pwrpriv->dtim);
+	else
+		DBG_871X("%s(): FW LPS mode = %d, SmartPS =%d\n", __func__, psmode, pwrpriv->smart_ps);
+
+#ifdef CONFIG_WOWLAN
+	if (psmode == PS_MODE_DTIM) { /* For WOWLAN LPS, DTIM = (awake_intvl - 1) */
+		awake_intvl = 3;/* DTIM =2 */
+		rlbm = 2;
+	} else
+#endif /* CONFIG_WOWLAN */
+	{
+		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 (rtw_btcoex_IsBtControlLps(padapter) == true) {
+			PowerState = rtw_btcoex_RpwmVal(padapter);
+			byte5 = rtw_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 == false && 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;
+
+			DBG_871X("%s(): bcn_cnt = %d\n", __func__, pmlmeext->bcn_cnt);
+
+			for (i = 0; i < 9; i++) {
+				pmlmeext->bcn_delay_ratio[i] = (pmlmeext->bcn_delay_cnt[i]*100)/pmlmeext->bcn_cnt;
+
+				DBG_871X(
+					"%s(): bcn_delay_cnt[%d]=%d, bcn_delay_ratio[%d] = %d\n",
+					__func__,
+					i,
+					pmlmeext->bcn_delay_cnt[i],
+					i,
+					pmlmeext->bcn_delay_ratio[i]
+				);
+
+				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;
+					DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, pmlmeext->DrvBcnEarly);
+				}
+
+				if (ratio_80_delay > 80 && pmlmeext->DrvBcnTimeOut == 0xff) {
+					pmlmeext->DrvBcnTimeOut = i;
+					DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, pmlmeext->DrvBcnTimeOut);
+				}
+
+				/* 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;
+
+		} else {
+			DBG_871X("%s(): DrvBcnEarly = %d\n", __func__, pmlmeext->DrvBcnEarly);
+			DBG_871X("%s(): DrvBcnTimeOut = %d\n", __func__, pmlmeext->DrvBcnTimeOut);
+		}
+
+/* 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) ;
+*/
+
+	}
+
+	rtw_btcoex_RecordPwrMode(padapter, u1H2CPwrModeParm, H2C_PWRMODE_LEN);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CPwrModeParm:", 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;
+
+	/* DBG_871X("%s(): FW LPS mode = %d\n", __func__, psmode); */
+
+	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);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CPsTuneParm:", u1H2CPsTuneParm, H2C_PSTUNEPARAM_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_PS_TUNING_PARA, H2C_PSTUNEPARAM_LEN, u1H2CPsTuneParm);
+}
+
+void rtl8723b_set_FwPwrModeInIPS_cmd(struct adapter *padapter, u8 cmd_param)
+{
+	/* BIT0:enable, BIT1:NoConnect32k */
+
+	DBG_871X("%s()\n", __func__);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_FWLPS_IN_IPS_, 1, &cmd_param);
+}
+
+#ifdef CONFIG_WOWLAN
+static void rtl8723b_set_FwWoWlanCtrl_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	struct security_priv *psecpriv = &padapter->securitypriv;
+	u8 u1H2CWoWlanCtrlParm[H2C_WOWLAN_LEN] = {0};
+	u8 discont_wake = 1, gpionum = 0, gpio_dur = 0, hw_unicast = 0;
+	u8 sdio_wakeup_enable = 1;
+	u8 gpio_high_active = 0; /* 0: low active, 1: high active */
+	u8 magic_pkt = 0;
+
+#ifdef CONFIG_GPIO_WAKEUP
+	gpionum = WAKEUP_GPIO_IDX;
+	sdio_wakeup_enable = 0;
+#endif
+
+#ifdef CONFIG_PNO_SUPPORT
+	if (!ppwrpriv->wowlan_pno_enable)
+		magic_pkt = 1;
+#endif
+
+	if (psecpriv->dot11PrivacyAlgrthm == _WEP40_ || psecpriv->dot11PrivacyAlgrthm == _WEP104_)
+		hw_unicast = 1;
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	SET_H2CCMD_WOWLAN_FUNC_ENABLE(u1H2CWoWlanCtrlParm, bFuncEn);
+	SET_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(u1H2CWoWlanCtrlParm, 0);
+	SET_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(u1H2CWoWlanCtrlParm, magic_pkt);
+	SET_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(u1H2CWoWlanCtrlParm, hw_unicast);
+	SET_H2CCMD_WOWLAN_ALL_PKT_DROP(u1H2CWoWlanCtrlParm, 0);
+	SET_H2CCMD_WOWLAN_GPIO_ACTIVE(u1H2CWoWlanCtrlParm, gpio_high_active);
+	SET_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(u1H2CWoWlanCtrlParm, discont_wake);
+	SET_H2CCMD_WOWLAN_GPIONUM(u1H2CWoWlanCtrlParm, gpionum);
+	SET_H2CCMD_WOWLAN_DATAPIN_WAKE_UP(u1H2CWoWlanCtrlParm, sdio_wakeup_enable);
+	SET_H2CCMD_WOWLAN_GPIO_DURATION(u1H2CWoWlanCtrlParm, gpio_dur);
+	/* SET_H2CCMD_WOWLAN_GPIO_PULSE_EN(u1H2CWoWlanCtrlParm, 1); */
+	SET_H2CCMD_WOWLAN_GPIO_PULSE_COUNT(u1H2CWoWlanCtrlParm, 0x09);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CWoWlanCtrlParm:", u1H2CWoWlanCtrlParm, H2C_WOWLAN_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_WOWLAN, H2C_WOWLAN_LEN, u1H2CWoWlanCtrlParm);
+}
+
+static void rtl8723b_set_FwRemoteWakeCtrl_Cmd(struct adapter *padapter, u8 benable)
+{
+	u8 u1H2CRemoteWakeCtrlParm[H2C_REMOTE_WAKE_CTRL_LEN] = {0};
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct pwrctrl_priv *ppwrpriv = adapter_to_pwrctl(padapter);
+
+	DBG_871X("%s(): Enable =%d\n", __func__, benable);
+
+	if (!ppwrpriv->wowlan_pno_enable) {
+		SET_H2CCMD_REMOTE_WAKECTRL_ENABLE(u1H2CRemoteWakeCtrlParm, benable);
+		SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 1);
+#ifdef CONFIG_GTK_OL
+		if (psecuritypriv->binstallKCK_KEK &&
+		    psecuritypriv->dot11PrivacyAlgrthm == _AES_) {
+			SET_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 1);
+		} else {
+			DBG_871X("no kck or security is not AES\n");
+			SET_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, 0);
+		}
+#endif /* CONFIG_GTK_OL */
+
+		SET_H2CCMD_REMOTE_WAKE_CTRL_FW_UNICAST_EN(u1H2CRemoteWakeCtrlParm, 1);
+
+		if ((psecuritypriv->dot11PrivacyAlgrthm == _AES_) ||
+		    (psecuritypriv->dot11PrivacyAlgrthm == _NO_PRIVACY_))
+			SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_ACTION(u1H2CRemoteWakeCtrlParm, 0);
+		else
+			SET_H2CCMD_REMOTE_WAKE_CTRL_ARP_ACTION(u1H2CRemoteWakeCtrlParm, 1);
+	}
+#ifdef CONFIG_PNO_SUPPORT
+	else {
+		SET_H2CCMD_REMOTE_WAKECTRL_ENABLE(u1H2CRemoteWakeCtrlParm, benable);
+		SET_H2CCMD_REMOTE_WAKE_CTRL_NLO_OFFLOAD_EN(u1H2CRemoteWakeCtrlParm, benable);
+	}
+#endif
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CRemoteWakeCtrlParm:", u1H2CRemoteWakeCtrlParm, H2C_REMOTE_WAKE_CTRL_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_REMOTE_WAKE_CTRL,
+		H2C_REMOTE_WAKE_CTRL_LEN, u1H2CRemoteWakeCtrlParm);
+#ifdef CONFIG_PNO_SUPPORT
+	if (ppwrpriv->wowlan_pno_enable && ppwrpriv->pno_in_resume == false) {
+		res = rtw_read8(padapter, REG_PNO_STATUS);
+		DBG_871X("cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", res);
+		while (!(res&BIT(7)) && count < 25) {
+			DBG_871X("[%d] cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", count, res);
+			res = rtw_read8(padapter, REG_PNO_STATUS);
+			count++;
+			msleep(2);
+		}
+		DBG_871X("cmd: 0x81 REG_PNO_STATUS: 0x%02x\n", res);
+	}
+#endif /* CONFIG_PNO_SUPPORT */
+}
+
+static void rtl8723b_set_FwAOACGlobalInfo_Cmd(struct adapter *padapter,  u8 group_alg, u8 pairwise_alg)
+{
+	u8 u1H2CAOACGlobalInfoParm[H2C_AOAC_GLOBAL_INFO_LEN] = {0};
+
+	DBG_871X("%s(): group_alg =%d pairwise_alg =%d\n", __func__, group_alg, pairwise_alg);
+
+	SET_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(u1H2CAOACGlobalInfoParm, pairwise_alg);
+	SET_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(u1H2CAOACGlobalInfoParm, group_alg);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CAOACGlobalInfoParm:", u1H2CAOACGlobalInfoParm, H2C_AOAC_GLOBAL_INFO_LEN);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AOAC_GLOBAL_INFO, H2C_AOAC_GLOBAL_INFO_LEN, u1H2CAOACGlobalInfoParm);
+}
+
+#ifdef CONFIG_PNO_SUPPORT
+static void rtl8723b_set_FwScanOffloadInfo_cmd(struct adapter *padapter, PRSVDPAGE_LOC rsvdpageloc, u8 enable)
+{
+	u8 u1H2CScanOffloadInfoParm[H2C_SCAN_OFFLOAD_CTRL_LEN] = {0};
+	u8 res = 0, count = 0;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	DBG_871X("%s: loc_probe_packet:%d, loc_scan_info: %d loc_ssid_info:%d\n",
+		__func__, rsvdpageloc->LocProbePacket, rsvdpageloc->LocScanInfo, rsvdpageloc->LocSSIDInfo);
+
+	SET_H2CCMD_AOAC_NLO_FUN_EN(u1H2CScanOffloadInfoParm, enable);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_SCAN_INFO(u1H2CScanOffloadInfoParm, rsvdpageloc->LocScanInfo);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_PROBE_PACKET(u1H2CScanOffloadInfoParm, rsvdpageloc->LocProbePacket);
+	SET_H2CCMD_AOAC_RSVDPAGE_LOC_SSID_INFO(u1H2CScanOffloadInfoParm, rsvdpageloc->LocSSIDInfo);
+
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_always_, "u1H2CScanOffloadInfoParm:", u1H2CScanOffloadInfoParm, H2C_SCAN_OFFLOAD_CTRL_LEN);
+	FillH2CCmd8723B(padapter, H2C_8723B_D0_SCAN_OFFLOAD_INFO, H2C_SCAN_OFFLOAD_CTRL_LEN, u1H2CScanOffloadInfoParm);
+
+	msleep(20);
+}
+#endif /* CONFIG_PNO_SUPPORT */
+
+static void rtl8723b_set_FwWoWlanRelated_cmd(struct adapter *padapter, u8 enable)
+{
+	struct security_priv *psecpriv = &padapter->securitypriv;
+	struct pwrctrl_priv *ppwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_info *psta = NULL;
+	u8 pkt_type = 0;
+
+	DBG_871X_LEVEL(_drv_always_, "+%s()+: enable =%d\n", __func__, enable);
+	if (enable) {
+		rtl8723b_set_FwAOACGlobalInfo_Cmd(padapter, psecpriv->dot118021XGrpPrivacy, psecpriv->dot11PrivacyAlgrthm);
+
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);	/* RT_MEDIA_CONNECT will confuse in the future */
+
+		if (!(ppwrpriv->wowlan_pno_enable)) {
+			psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(pmlmepriv));
+			if (psta != NULL)
+				rtl8723b_set_FwMediaStatusRpt_cmd(padapter, RT_MEDIA_CONNECT, psta->mac_id);
+		} else
+			DBG_871X("%s(): Disconnected, no FwMediaStatusRpt CONNECT\n", __func__);
+
+		msleep(2);
+
+		if (!(ppwrpriv->wowlan_pno_enable)) {
+			rtl8723b_set_FwDisconDecision_cmd(padapter, enable);
+			msleep(2);
+
+			if ((psecpriv->dot11PrivacyAlgrthm != _WEP40_) || (psecpriv->dot11PrivacyAlgrthm != _WEP104_))
+				pkt_type = 1;
+
+			rtl8723b_set_FwKeepAlive_cmd(padapter, enable, pkt_type);
+			msleep(2);
+		}
+
+		rtl8723b_set_FwWoWlanCtrl_Cmd(padapter, enable);
+		msleep(2);
+
+		rtl8723b_set_FwRemoteWakeCtrl_Cmd(padapter, enable);
+	} else {
+		rtl8723b_set_FwRemoteWakeCtrl_Cmd(padapter, enable);
+		msleep(2);
+		rtl8723b_set_FwWoWlanCtrl_Cmd(padapter, enable);
+	}
+
+	DBG_871X_LEVEL(_drv_always_, "-%s()-\n", __func__);
+}
+
+void rtl8723b_set_wowlan_cmd(struct adapter *padapter, u8 enable)
+{
+	rtl8723b_set_FwWoWlanRelated_cmd(padapter, enable);
+}
+#endif /* CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+static void rtl8723b_set_FwAPWoWlanCtrl_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	u8 u1H2CAPWoWlanCtrlParm[H2C_WOWLAN_LEN] = {0};
+	u8 gpionum = 0, gpio_dur = 0;
+	u8 gpio_high_active = 1; /* 0: low active, 1: high active */
+	u8 gpio_pulse = bFuncEn;
+#ifdef CONFIG_GPIO_WAKEUP
+	gpionum = WAKEUP_GPIO_IDX;
+#endif
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	if (bFuncEn)
+		gpio_dur = 16;
+	else
+		gpio_dur = 0;
+
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_INDEX(u1H2CAPWoWlanCtrlParm,
+			gpionum);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_PLUS(u1H2CAPWoWlanCtrlParm,
+			gpio_pulse);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_HIGH_ACTIVE(u1H2CAPWoWlanCtrlParm,
+			gpio_high_active);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_EN(u1H2CAPWoWlanCtrlParm,
+			bFuncEn);
+	SET_H2CCMD_AP_WOW_GPIO_CTRL_DURATION(u1H2CAPWoWlanCtrlParm,
+			gpio_dur);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AP_WOW_GPIO_CTRL,
+			H2C_AP_WOW_GPIO_CTRL_LEN, u1H2CAPWoWlanCtrlParm);
+}
+
+static void rtl8723b_set_Fw_AP_Offload_Cmd(struct adapter *padapter, u8 bFuncEn)
+{
+	u8 u1H2CAPOffloadCtrlParm[H2C_WOWLAN_LEN] = {0};
+
+	DBG_871X("%s(): bFuncEn =%d\n", __func__, bFuncEn);
+
+	SET_H2CCMD_AP_WOWLAN_EN(u1H2CAPOffloadCtrlParm, bFuncEn);
+
+	FillH2CCmd8723B(padapter, H2C_8723B_AP_OFFLOAD,
+			H2C_AP_OFFLOAD_LEN, u1H2CAPOffloadCtrlParm);
+}
+
+static void rtl8723b_set_AP_FwWoWlan_cmd(struct adapter *padapter, u8 enable)
+{
+	DBG_871X_LEVEL(_drv_always_, "+%s()+: enable =%d\n", __func__, enable);
+	if (enable) {
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);
+		issue_beacon(padapter, 0);
+	}
+
+	rtl8723b_set_FwAPWoWlanCtrl_Cmd(padapter, enable);
+	msleep(10);
+	rtl8723b_set_Fw_AP_Offload_Cmd(padapter, enable);
+	msleep(10);
+	DBG_871X_LEVEL(_drv_always_, "-%s()-\n", __func__);
+	return ;
+}
+
+void rtl8723b_set_ap_wowlan_cmd(struct adapter *padapter, u8 enable)
+{
+	rtl8723b_set_AP_FwWoWlan_cmd(padapter, enable);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+/*  */
+/*  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 lengh. */
+/* 			      true: At the second time, we should send the first packet (default:beacon) */
+/* 						to Hw again and set the lengh in descriptor to the real beacon lengh. */
+/*  2009.10.15 by tynli. */
+static void rtl8723b_set_FwRsvdPagePkt(
+	struct adapter *padapter, bool bDLFinished
+)
+{
+	struct hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pwrctrl_priv *pwrctl;
+	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;
+	RSVDPAGE_LOC RsvdPageLoc;
+#ifdef CONFIG_WOWLAN
+	u32 ARPLegnth = 0, GTKLegnth = 0;
+	u8 currentip[4];
+	u8 cur_dot11txpn[8];
+#ifdef CONFIG_GTK_OL
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	u8 kek[RTW_KEK_LEN];
+	u8 kck[RTW_KCK_LEN];
+#endif
+#endif
+
+	/* DBG_871X("%s---->\n", __func__); */
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	pwrctl = adapter_to_pwrctl(padapter);
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B + WOWLAN_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		DBG_871X("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(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);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: PS-POLL %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (PSPollLength+TxDescLen)); */
+
+	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);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (NullDataLength+TxDescLen)); */
+
+	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);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: QOS NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (QosNullLength+TxDescLen)); */
+
+	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);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: BT QOS NULL DATA %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (BTQosNullLength+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + BTQosNullLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+#ifdef CONFIG_WOWLAN
+	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+	/* if (pwrctl->wowlan_mode == true) { */
+		/* BufIndex += (CurtPktPageNum*PageSize); */
+
+	/* 3(7) ARP RSP */
+	rtw_get_current_ip_address(padapter, currentip);
+	RsvdPageLoc.LocArpRsp = TotalPageNum;
+	{
+	ConstructARPResponse(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&ARPLegnth,
+		currentip
+		);
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], ARPLegnth, false, false, true);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: ARP RSP %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (ARPLegnth+TxDescLen)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + ARPLegnth);
+	}
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3(8) SEC IV */
+	rtw_get_sec_iv(padapter, cur_dot11txpn, get_my_bssid(&pmlmeinfo->network));
+	RsvdPageLoc.LocRemoteCtrlInfo = TotalPageNum;
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen, cur_dot11txpn, _AES_IV_LEN_);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: SEC IV %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], _AES_IV_LEN_); */
+
+	CurtPktPageNum = (u8)PageNum_128(_AES_IV_LEN_);
+
+	TotalPageNum += CurtPktPageNum;
+
+#ifdef CONFIG_GTK_OL
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* if the ap staion info. exists, get the kek, kck from staion info. */
+	psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+	if (psta == NULL) {
+		memset(kek, 0, RTW_KEK_LEN);
+		memset(kck, 0, RTW_KCK_LEN);
+		DBG_8192C("%s, KEK, KCK download rsvd page all zero\n", __func__);
+	} else {
+		memcpy(kek, psta->kek, RTW_KEK_LEN);
+		memcpy(kck, psta->kck, RTW_KCK_LEN);
+	}
+
+	/* 3(9) KEK, KCK */
+	RsvdPageLoc.LocGTKInfo = TotalPageNum;
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen, kck, RTW_KCK_LEN);
+	memcpy(ReservedPagePacket+BufIndex-TxDescLen+RTW_KCK_LEN, kek, RTW_KEK_LEN);
+
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: KEK KCK %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (TxDescLen + RTW_KCK_LEN + RTW_KEK_LEN)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + RTW_KCK_LEN + RTW_KEK_LEN);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* 3(10) GTK Response */
+	RsvdPageLoc.LocGTKRsp = TotalPageNum;
+	ConstructGTKResponse(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&GTKLegnth
+	);
+
+	rtl8723b_fill_fake_txdesc(padapter, &ReservedPagePacket[BufIndex-TxDescLen], GTKLegnth, false, false, true);
+	/* DBG_871X("%s(): HW_VAR_SET_TX_CMD: GTK RSP %p %d\n", */
+	/* 	__func__, &ReservedPagePacket[BufIndex-TxDescLen], (TxDescLen + GTKLegnth)); */
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + GTKLegnth);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	/* below page is empty for GTK extension memory */
+	/* 3(11) GTK EXT MEM */
+	RsvdPageLoc.LocGTKEXTMEM = TotalPageNum;
+
+	CurtPktPageNum = 2;
+
+	TotalPageNum += CurtPktPageNum;
+
+	TotalPacketLen = BufIndex-TxDescLen + 256; /* extension memory for FW */
+#else
+	TotalPacketLen = BufIndex-TxDescLen + sizeof (union pn48); /* IV len */
+#endif /* CONFIG_GTK_OL */
+	} else
+#endif /* CONFIG_WOWLAN */
+	{
+#ifdef CONFIG_PNO_SUPPORT
+		if (pwrctl->pno_in_resume == false && pwrctl->pno_inited == true) {
+			/* Probe Request */
+			RsvdPageLoc.LocProbePacket = TotalPageNum;
+			ConstructProbeReq(
+				padapter,
+				&ReservedPagePacket[BufIndex],
+				&ProbeReqLength);
+
+			rtl8723b_fill_fake_txdesc(padapter,
+				&ReservedPagePacket[BufIndex-TxDescLen],
+				ProbeReqLength, false, false, false);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("probe req pkt =>\n");
+			for (gj = 0; gj < ProbeReqLength+TxDescLen; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj+1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum =
+				(u8)PageNum_128(TxDescLen + ProbeReqLength);
+
+			TotalPageNum += CurtPktPageNum;
+
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* PNO INFO Page */
+			RsvdPageLoc.LocPNOInfo = TotalPageNum;
+			ConstructPnoInfo(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &PNOLength);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("PNO pkt =>\n");
+			for (gj = 0; gj < PNOLength; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+
+			CurtPktPageNum = (u8)PageNum_128(PNOLength);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* SSID List Page */
+			RsvdPageLoc.LocSSIDInfo = TotalPageNum;
+			ConstructSSIDList(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &SSIDLegnth);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("SSID list pkt =>\n");
+			for (gj = 0; gj < SSIDLegnth; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum = (u8)PageNum_128(SSIDLegnth);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			/* Scan Info Page */
+			RsvdPageLoc.LocScanInfo = TotalPageNum;
+			ConstructScanInfo(padapter, &ReservedPagePacket[BufIndex-TxDescLen], &ScanInfoLength);
+#ifdef CONFIG_PNO_SET_DEBUG
+	{
+			int gj;
+			printk("Scan info pkt =>\n");
+			for (gj = 0; gj < ScanInfoLength; gj++) {
+				printk(" %02x ", ReservedPagePacket[BufIndex-TxDescLen+gj]);
+				if ((gj + 1)%8 == 0)
+					printk("\n");
+			}
+			printk(" <=end\n");
+	}
+#endif
+			CurtPktPageNum = (u8)PageNum_128(ScanInfoLength);
+			TotalPageNum += CurtPktPageNum;
+			BufIndex += (CurtPktPageNum*PageSize);
+
+			TotalPacketLen = BufIndex + ScanInfoLength;
+		} else {
+		TotalPacketLen = BufIndex + BTQosNullLength;
+	}
+#else /* CONFIG_PNO_SUPPORT */
+		TotalPacketLen = BufIndex + BTQosNullLength;
+#endif
+	}
+
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		DBG_871X("%s(): ERROR: The rsvd page size is not enough!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n", __func__,
+			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);
+	}
+
+	DBG_871X("%s: Set RSVD page location to Fw , TotalPacketLen(%d), TotalPageNum(%d)\n", __func__, TotalPacketLen, TotalPageNum);
+	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);
+#ifdef CONFIG_PNO_SUPPORT
+		if (pwrctl->pno_in_resume)
+			rtl8723b_set_FwScanOffloadInfo_cmd(padapter,
+					&RsvdPageLoc, 0);
+		else
+			rtl8723b_set_FwScanOffloadInfo_cmd(padapter,
+					&RsvdPageLoc, 1);
+#endif
+	}
+	return;
+
+error:
+
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+
+#ifdef CONFIG_AP_WOWLAN
+/*  */
+/* Description: Fill the reserved packets that FW will use to RSVD page. */
+/* Now we just send 2 types packet to rsvd page. (1)Beacon, (2)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 lengh. */
+/*  */
+/* true: At the second time, we should send the first packet (default:beacon) */
+/* 	to Hw again and set the lengh in descriptor to the real beacon lengh. */
+/*  2009.10.15 by tynli. */
+static void rtl8723b_set_AP_FwRsvdPagePkt(
+	struct adapter *padapter, bool bDLFinished
+)
+{
+	struct hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pwrctrl_priv *pwrctl;
+	u32 BeaconLength = 0, ProbeRspLength = 0;
+	u8 *ReservedPagePacket;
+	u8 TxDescLen = TXDESC_SIZE, TxDescOffset = TXDESC_OFFSET;
+	u8 TotalPageNum = 0, CurtPktPageNum = 0, RsvdPageNum = 0;
+	u8 currentip[4];
+	u16 BufIndex, PageSize = 128;
+	u32 TotalPacketLen = 0, MaxRsvdPageBufSize = 0;
+	RSVDPAGE_LOC RsvdPageLoc;
+
+	/* DBG_871X("%s---->\n", __func__); */
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter));
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	pwrctl = adapter_to_pwrctl(padapter);
+
+	RsvdPageNum = BCNQ_PAGE_NUM_8723B + AP_WOWLAN_PAGE_NUM_8723B;
+	MaxRsvdPageBufSize = RsvdPageNum*PageSize;
+
+	pcmdframe = rtw_alloc_cmdxmitframe(pxmitpriv);
+	if (pcmdframe == NULL) {
+		DBG_871X("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(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);
+
+	/* 2 (4) probe response */
+	RsvdPageLoc.LocProbeRsp = TotalPageNum;
+
+	rtw_get_current_ip_address(padapter, currentip);
+
+	ConstructProbeRsp(
+		padapter,
+		&ReservedPagePacket[BufIndex],
+		&ProbeRspLength,
+		currentip,
+		false);
+	rtl8723b_fill_fake_txdesc(padapter,
+			&ReservedPagePacket[BufIndex-TxDescLen],
+			ProbeRspLength,
+			false, false, false);
+
+	DBG_871X("%s(): HW_VAR_SET_TX_CMD: PROBE RSP %p %d\n",
+		__func__, &ReservedPagePacket[BufIndex-TxDescLen],
+		(ProbeRspLength+TxDescLen));
+
+	CurtPktPageNum = (u8)PageNum_128(TxDescLen + ProbeRspLength);
+
+	TotalPageNum += CurtPktPageNum;
+
+	BufIndex += (CurtPktPageNum*PageSize);
+
+	TotalPacketLen = BufIndex + ProbeRspLength;
+
+	if (TotalPacketLen > MaxRsvdPageBufSize) {
+		DBG_871X("%s(): ERROR: The rsvd page size is not enough \
+				!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n",
+				__func__, TotalPacketLen, MaxRsvdPageBufSize);
+		goto error;
+	} else {
+		/*  update attribute */
+		pattrib = &pcmdframe->attrib;
+		update_mgntframe_attrib(padapter, pattrib);
+		pattrib->qsel = 0x10;
+		pattrib->pktlen = TotalPacketLen - TxDescOffset;
+		pattrib->last_txcmdsz = TotalPacketLen - TxDescOffset;
+		dump_mgntframe_and_wait(padapter, pcmdframe, 100);
+	}
+
+	DBG_871X("%s: Set RSVD page location to Fw , TotalPacketLen(%d), TotalPageNum(%d)\n", __func__, TotalPacketLen, TotalPageNum);
+	rtl8723b_set_ap_wow_rsvdpage_cmd(padapter, &RsvdPageLoc);
+
+	return;
+error:
+	rtw_free_xmitframe(pxmitpriv, pcmdframe);
+}
+#endif /* CONFIG_AP_WOWLAN */
+
+void rtl8723b_download_rsvd_page(struct adapter *padapter, u8 mstatus)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+#ifdef CONFIG_AP_WOWLAN
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+#endif
+	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;
+
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d mstatus(%x)\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter), mstatus);
+
+	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 {
+#ifdef CONFIG_AP_WOWLAN
+			if (pwrpriv->wowlan_ap_mode)
+				rtl8723b_set_AP_FwRsvdPagePkt(padapter, 0);
+			else
+				rtl8723b_set_FwRsvdPagePkt(padapter, 0);
+#else
+			/*  download rsvd page. */
+			rtl8723b_set_FwRsvdPagePkt(padapter, 0);
+#endif
+			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 if (!bcn_valid)
+			DBG_871X(ADPT_FMT": 1 DL RSVD page failed! DLBcnCount:%u, poll:%u\n",
+				ADPT_ARG(padapter), DLBcnCount, poll);
+		else {
+			struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+			pwrctl->fw_psmode_iface_id = padapter->iface_id;
+			DBG_871X(ADPT_FMT": 1 DL RSVD page success! DLBcnCount:%u, poll:%u\n",
+				ADPT_ARG(padapter), DLBcnCount, poll);
+		}
+
+		/*  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_rssi_cmd(struct adapter *padapter, u8 *param)
+{
+	rtl8723b_set_FwRssiSetting_cmd(padapter, param);
+}
+
+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 == NULL)
+		return;
+
+	bw = psta->bw_mode;
+
+	if (rssi_level != DM_RATR_STA_INIT)
+		mask = ODM_Get_Rate_Bitmap(&pHalData->odmpriv, mac_id, mask, rssi_level);
+
+	DBG_871X("%s(): mac_id =%d raid = 0x%x bw =%d mask = 0x%x\n", __func__, mac_id, raid, bw, mask);
+	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;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	u8 bssid[ETH_ALEN];
+
+
+	DBG_871X("+" FUNC_ADPT_FMT ": qos =%d eosp =%d ps =%d\n",
+		FUNC_ADPT_ARG(padapter), bQoS, bEosp, bForcePowerSave);
+
+	pwlanhdr = (struct ieee80211_hdr *)pframe;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+
+	if (NULL == 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 == true) {
+		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 hal_com_data *pHalData;
+	struct xmit_frame *pcmdframe;
+	struct pkt_attrib *pattrib;
+	struct xmit_priv *pxmitpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	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;
+	RSVDPAGE_LOC RsvdPageLoc;
+
+
+/* 	DBG_8192C("+" FUNC_ADPT_FMT "\n", FUNC_ADPT_ARG(padapter)); */
+
+	pHalData = GET_HAL_DATA(padapter);
+	pxmitpriv = &padapter->xmitpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &pmlmeext->mlmext_info;
+	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 == NULL) {
+		DBG_8192C("%s: alloc ReservedPagePacket fail!\n", __func__);
+		return;
+	}
+
+	ReservedPagePacket = pcmdframe->buf_addr;
+	memset(&RsvdPageLoc, 0, sizeof(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) {
+		DBG_8192C(FUNC_ADPT_FMT ": ERROR: The rsvd page size is not enough!!TotalPacketLen %d, MaxRsvdPageBufSize %d\n",
+			FUNC_ADPT_ARG(padapter), 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);
+
+/* 	DBG_8192C(FUNC_ADPT_FMT ": Set RSVD page location to Fw, TotalPacketLen(%d), TotalPageNum(%d)\n", */
+/* 		FUNC_ADPT_ARG(padapter), TotalPacketLen, TotalPageNum); */
+	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;
+
+
+	DBG_8192C("+" FUNC_ADPT_FMT ": iface_type =%d fw_state = 0x%08X\n",
+		FUNC_ADPT_ARG(padapter), get_iface_type(padapter), get_fwstate(&padapter->mlmepriv));
+
+#ifdef DEBUG
+	if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == false) {
+		DBG_8192C(FUNC_ADPT_FMT ": [WARNING] not in AP mode!!\n",
+			FUNC_ADPT_ARG(padapter));
+	}
+#endif /*  DEBUG */
+
+	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 (true == bcn_valid) {
+		struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+		pwrctl->fw_psmode_iface_id = padapter->iface_id;
+		DBG_8192C(ADPT_FMT": DL RSVD page success! DLBcnCount:%d, poll:%d\n",
+			ADPT_ARG(padapter), DLBcnCount, poll);
+	} else {
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! DLBcnCount:%d, poll:%d\n",
+			ADPT_ARG(padapter), DLBcnCount, poll);
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! bSurpriseRemoved =%d\n",
+			ADPT_ARG(padapter), padapter->bSurpriseRemoved);
+		DBG_8192C(ADPT_FMT": DL RSVD page fail! bDriverStopped =%d\n",
+			ADPT_ARG(padapter), padapter->bDriverStopped);
+	}
+
+	/*  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 000000000000..b16255984f4e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_dm.c
@@ -0,0 +1,300 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*  Description: */
+/*  This file is for 92CE/92CU dynamic mechanism only */
+
+#define _RTL8723B_DM_C_
+
+#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);
+	PDM_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;
+
+	DBG_871X("%s(): fab_ver =%d cut_ver =%d\n", __func__, fab_ver, cut_ver);
+	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_MP_TEST_CHIP, IS_NORMAL_CHIP(pHalData->VersionID));
+
+	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);
+
+
+	if (pHalData->rf_type == RF_1T1R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T1R);
+	} else if (pHalData->rf_type == RF_2T2R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_2T2R);
+	} else if (pHalData->rf_type == RF_1T2R) {
+		ODM_CmnInfoUpdate(pDM_Odm, ODM_CMNINFO_RF_TYPE, ODM_1T2R);
+	}
+
+	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);
+	PDM_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
+#ifdef CONFIG_ODM_ADAPTIVITY
+		| ODM_BB_ADAPTIVITY
+#endif
+		;
+
+	/*  */
+	/*  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_BAND, &(pHalData->CurrentBandType));
+	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;
+	PDM_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 bFwCurrentInPSMode = 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;
+
+	bFwCurrentInPSMode = adapter_to_pwrctl(Adapter)->bFwCurrentInPSMode;
+	rtw_hal_get_hwreg(Adapter, HW_VAR_FWLPS_RF_ON, (u8 *)(&bFwPSAwake));
+
+	if (
+		(hw_init_completed == true) &&
+		((!bFwCurrentInPSMode) && 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;
+		u8 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 = rtw_btcoex_IsBtDisabled(Adapter);
+
+		ODM_CmnInfoUpdate(&pHalData->odmpriv, ODM_CMNINFO_BT_ENABLED, ((bBtDisabled == true)?false:true));
+
+		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);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta = NULL;
+
+	DBG_871X("%s, RSSI_Min =%d\n", __func__, pDM_Odm->RSSI_Min);
+
+	/* 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;
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	pDIG_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 == NULL)
+		goto skip_lps_dm;
+
+	pdmpriv->EntryMinUndecoratedSmoothedPWDB = psta->rssi_stat.UndecoratedSmoothedPWDB;
+
+	DBG_871X("CurIGValue =%d, EntryMinUndecoratedSmoothedPWDB = %d\n", pDM_DigTable->CurIGValue, pdmpriv->EntryMinUndecoratedSmoothedPWDB);
+
+	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 000000000000..163537faefd9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
@@ -0,0 +1,4549 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HAL_INIT_C_
+
+#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);
+
+		if (count > 0)
+			DBG_871X("%s: !!!!!!!!Write 0x80 Fail!: count = %d\n", __func__, count);
+
+		/*  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 = (u8 *)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;
+
+	if (blockCount_p1) {
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_notice_,
+			(
+				"_BlockWrite: [P1] buffSize(%d) blockSize_p1(%d) blockCount_p1(%d) remainSize_p1(%d)\n",
+				buffSize,
+				blockSize_p1,
+				blockCount_p1,
+				remainSize_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;
+
+		if (blockCount_p2) {
+				RT_TRACE(
+					_module_hal_init_c_,
+					_drv_notice_,
+					(
+						"_BlockWrite: [P2] buffSize_p2(%d) blockSize_p2(%d) blockCount_p2(%d) remainSize_p2(%d)\n",
+						(buffSize-offset),
+						blockSize_p2,
+						blockCount_p2,
+						remainSize_p2
+					)
+				);
+		}
+
+	}
+
+	/* 3 Phase #3 */
+	if (remainSize_p2) {
+		offset = (blockCount_p1 * blockSize_p1) + (blockCount_p2 * blockSize_p2);
+
+		blockCount_p3 = remainSize_p2 / blockSize_p3;
+
+		RT_TRACE(_module_hal_init_c_, _drv_notice_,
+				("_BlockWrite: [P3] buffSize_p3(%d) blockSize_p3(%d) blockCount_p3(%d)\n",
+				(buffSize-offset), blockSize_p3, blockCount_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 = (u8 *)buffer;
+
+	pageNums = size / MAX_DLFW_PAGE_SIZE;
+	/* RT_ASSERT((pageNums <= 4), ("Page numbers should not greater then 4\n")); */
+	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;
+		}
+	}
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("_WriteFW Done- for Normal chip.\n"));
+
+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);
+
+	DBG_8192C("%s: Finish\n", __func__);
+}
+
+u8 g_fwdl_chksum_fail = 0;
+
+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) {
+		DBG_871X("%s: fwdl test case: fwdl_chksum_fail\n", __func__);
+		g_fwdl_chksum_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+	DBG_871X(
+		"%s: Checksum report %s! (%u, %dms), REG_MCUFWDL:0x%08x\n",
+		__func__,
+		(ret == _SUCCESS) ? "OK" : "Fail",
+		cnt,
+		jiffies_to_msecs(jiffies-start),
+		value32
+	);
+
+	return ret;
+}
+
+u8 g_fwdl_wintint_rdy_fail = 0;
+
+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) {
+		DBG_871X("%s: fwdl test case: wintint_rdy_fail\n", __func__);
+		g_fwdl_wintint_rdy_fail--;
+		goto exit;
+	}
+
+	ret = _SUCCESS;
+
+exit:
+	DBG_871X(
+		"%s: Polling FW ready %s! (%u, %dms), REG_MCUFWDL:0x%08x\n",
+		__func__,
+		(ret == _SUCCESS) ? "OK" : "Fail",
+		cnt,
+		jiffies_to_msecs(jiffies-start),
+		value32
+	);
+
+	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);
+		}
+		RT_TRACE(_module_hal_init_c_, _drv_notice_, ("-%s: 8051 reset success (%d)\n", __func__, Delay));
+
+		if (Delay == 0) {
+			RT_TRACE(_module_hal_init_c_, _drv_notice_, ("%s: Force 8051 reset!!!\n", __func__));
+			/* 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;
+
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("+%s\n", __func__));
+#ifdef CONFIG_WOWLAN
+	RT_TRACE(_module_hal_init_c_, _drv_notice_, ("+%s, bUsedWoWLANFw:%d\n", __func__, bUsedWoWLANFw));
+#endif
+	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) {
+		DBG_871X(FUNC_ADPT_FMT" tmp_ps =%x\n", FUNC_ADPT_ARG(padapter), tmp_ps);
+		pdbgpriv->dbg_downloadfw_pwr_state_cnt++;
+	}
+
+#ifdef CONFIG_WOWLAN
+	if (bUsedWoWLANFw)
+		fwfilepath = "rtlwifi/rtl8723bs_wowlan.bin";
+	else
+#endif /*  CONFIG_WOWLAN */
+		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;
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_err_,
+			("Firmware size exceed 0x%X. Check it.\n", FW_8188E_SIZE)
+		);
+		goto exit;
+	}
+
+	pFirmware->szFwBuffer = kzalloc(fw->size, GFP_KERNEL);
+	if (!pFirmware->szFwBuffer) {
+		rtStatus = _FAIL;
+		goto exit;
+	}
+
+	memcpy(pFirmware->szFwBuffer, fw->data, fw->size);
+	pFirmware->ulFwLength = fw->size;
+	release_firmware(fw);
+	if (pFirmware->ulFwLength > FW_8723B_SIZE) {
+		rtStatus = _FAIL;
+		DBG_871X_LEVEL(_drv_emerg_, "Firmware size:%u exceed %u\n", pFirmware->ulFwLength, FW_8723B_SIZE);
+		goto release_fw1;
+	}
+
+	pFirmwareBuf = pFirmware->szFwBuffer;
+	FirmwareLen = pFirmware->ulFwLength;
+
+	/*  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);
+
+	DBG_871X(
+		"%s: fw_ver =%x fw_subver =%04x sig = 0x%x, Month =%02x, Date =%02x, Hour =%02x, Minute =%02x\n",
+		__func__,
+		pHalData->FirmwareVersion,
+		pHalData->FirmwareSubVersion,
+		pHalData->FirmwareSignature,
+		pFwHdr->Month,
+		pFwHdr->Date,
+		pFwHdr->Hour,
+		pFwHdr->Minute
+	);
+
+	if (IS_FW_HEADER_EXIST_8723B(pFwHdr)) {
+		DBG_871X("%s(): Shift for fw header!\n", __func__);
+		/*  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:
+	DBG_871X(
+		"FWDL %s. write_fw:%u, %dms\n",
+		(rtStatus == _SUCCESS)?"success":"fail",
+		write_fw,
+		jiffies_to_msecs(jiffies - fwdl_start_time)
+	);
+
+exit:
+	kfree(pFirmware->szFwBuffer);
+	kfree(pFirmware);
+release_fw1:
+	kfree(pBTFirmware);
+	DBG_871X(" <=== rtl8723b_FirmwareDownload()\n");
+	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)->bFwCurrentInPSMode = 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; */
+}
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+/*  */
+
+/*  */
+/*  Description: Prepare some information to Fw for WoWLAN. */
+/* (1) Download wowlan Fw. */
+/* (2) Download RSVD page packets. */
+/* (3) Enable AP offload if needed. */
+/*  */
+/*  2011.04.12 by tynli. */
+/*  */
+void SetFwRelatedForWoWLAN8723b(
+	struct adapter *padapter, u8 bHostIsGoingtoSleep
+)
+{
+	int	status = _FAIL;
+	/*  */
+	/*  1. Before WoWLAN we need to re-download WoWLAN Fw. */
+	/*  */
+	status = rtl8723b_FirmwareDownload(padapter, bHostIsGoingtoSleep);
+	if (status != _SUCCESS) {
+		DBG_871X("SetFwRelatedForWoWLAN8723b(): Re-Download Firmware failed!!\n");
+		return;
+	} else {
+		DBG_871X("SetFwRelatedForWoWLAN8723b(): Re-Download Firmware Success !!\n");
+	}
+	/*  */
+	/*  2. Re-Init the variables about Fw related setting. */
+	/*  */
+	rtl8723b_InitializeFirmwareVars(padapter);
+}
+#endif /* CONFIG_WOWLAN */
+
+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);
+	PEFUSE_HAL pEfuseHal = &pHalData->EfuseHal;
+#endif
+
+
+	DBG_8192C("%s: Efuse switch bank to %d\n", __func__, bank);
+	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 = (u8 *)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 = (u16 *)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 = (u16 *)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 = (u16 *)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 = (u16 *)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 = (u8 *)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 = (u16 *)pOut;
+
+			if (efuseType == EFUSE_WIFI)
+				*pu2Tmp = EFUSE_REAL_CONTENT_LEN_8723B;
+			else
+				*pu2Tmp = EFUSE_BT_REAL_BANK_CONTENT_LEN;
+		}
+		break;
+
+	default:
+		{
+			u8 *pu1Tmp;
+			pu1Tmp = (u8 *)pOut;
+			*pu1Tmp = 0;
+		}
+		break;
+	}
+}
+
+#define VOLTAGE_V25		0x03
+#define LDOE25_SHIFT	28
+
+/*  */
+/* 	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 EFUSE_ACCESS_OFF_8723			0x00	/*  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 == true) {
+		/*  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 wirte 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 wirte 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 == true) {
+		/*  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);
+
+			if (count >= 100) {
+				DBG_8192C(FUNC_ADPT_FMT ": Leave SDIO local register suspend fail! Local 0x86 =%#X\n",
+					FUNC_ADPT_ARG(padapter), tempval);
+			} else {
+				DBG_8192C(FUNC_ADPT_FMT ": Leave SDIO local register suspend OK! Local 0x86 =%#X\n",
+					FUNC_ADPT_ARG(padapter), tempval);
+			}
+		}
+
+		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 == true) {
+			/*  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 == true) {
+			/*  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);
+	PEFUSE_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;
+
+	/* DBG_871X("YJ: ====>%s():_offset =%d _size_byte =%d bPseudoTest =%d\n", __func__, _offset, _size_byte, bPseudoTest); */
+	/*  */
+	/*  Do NOT excess total size of EFuse table. Added by Roger, 2008.11.10. */
+	/*  */
+	if ((_offset+_size_byte) > EFUSE_MAX_MAP_LEN) {
+		DBG_8192C("%s: Invalid offset(%#x) with read bytes(%#x)!!\n", __func__, _offset, _size_byte);
+		return;
+	}
+
+	efuseTbl = (u8 *)rtw_malloc(EFUSE_MAX_MAP_LEN);
+	if (efuseTbl == NULL) {
+		DBG_8192C("%s: alloc efuseTbl fail!\n", __func__);
+		return;
+	}
+	/*  0xff will be efuse default value instead of 0x00. */
+	memset(efuseTbl, 0xFF, EFUSE_MAX_MAP_LEN);
+
+
+#ifdef DEBUG
+if (0) {
+	for (i = 0; i < 256; i++)
+		efuse_OneByteRead(padapter, i, &efuseTbl[i], false);
+	DBG_871X("Efuse Content:\n");
+	for (i = 0; i < 256; i++) {
+		if (i % 16 == 0)
+			printk("\n");
+		printk("%02X ", efuseTbl[i]);
+	}
+	printk("\n");
+}
+#endif
+
+
+	/*  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) {
+			DBG_8192C("%s: data end at address =%#x\n", __func__, eFuse_Addr-1);
+			break;
+		}
+		/* DBG_8192C("%s: efuse[0x%X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseHeader); */
+
+		/*  Check PG header for section num. */
+		if (EXT_HEADER(efuseHeader)) { /* extended header */
+			offset = GET_HDR_OFFSET_2_0(efuseHeader);
+			/* DBG_8192C("%s: extended header offset = 0x%X\n", __func__, offset); */
+
+			efuse_OneByteRead(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest);
+			/* DBG_8192C("%s: efuse[0x%X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseExtHdr); */
+			if (ALL_WORDS_DISABLED(efuseExtHdr))
+				continue;
+
+			offset |= ((efuseExtHdr & 0xF0) >> 1);
+			wden = (efuseExtHdr & 0x0F);
+		} else {
+			offset = ((efuseHeader >> 4) & 0x0f);
+			wden = (efuseHeader & 0x0f);
+		}
+		/* DBG_8192C("%s: Offset =%d Worden = 0x%X\n", __func__, offset, wden); */
+
+		if (offset < EFUSE_MAX_SECTION_8723B) {
+			u16 addr;
+			/*  Get word enable value from PG header */
+/* 			DBG_8192C("%s: Offset =%d Worden = 0x%X\n", __func__, offset, wden); */
+
+			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);
+/* 					DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseData); */
+					efuseTbl[addr] = efuseData;
+
+					efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+/* 					DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseData); */
+					efuseTbl[addr+1] = efuseData;
+				}
+				addr += 2;
+			}
+		} else {
+			DBG_8192C(KERN_ERR "%s: offset(%d) is illegal!!\n", __func__, offset);
+			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];
+
+#ifdef DEBUG
+if (1) {
+	DBG_871X("Efuse Realmap:\n");
+	for (i = 0; i < _size_byte; i++) {
+		if (i % 16 == 0)
+			printk("\n");
+		printk("%02X ", pbuf[i]);
+	}
+	printk("\n");
+}
+#endif
+	/*  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);
+	PEFUSE_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) {
+		DBG_8192C("%s: Invalid offset(%#x) with read bytes(%#x)!!\n", __func__, _offset, _size_byte);
+		return;
+	}
+
+	efuseTbl = rtw_malloc(EFUSE_BT_MAP_LEN);
+	if (efuseTbl == NULL) {
+		DBG_8192C("%s: efuseTbl malloc fail!\n", __func__);
+		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) {
+			DBG_8192C("%s: hal_EfuseSwitchToBank Fail!!\n", __func__);
+			goto exit;
+		}
+
+		eFuse_Addr = 0;
+
+		while (AVAILABLE_EFUSE_ADDR(eFuse_Addr)) {
+			efuse_OneByteRead(padapter, eFuse_Addr++, &efuseHeader, bPseudoTest);
+			if (efuseHeader == 0xFF)
+				break;
+			DBG_8192C("%s: efuse[%#X]= 0x%02x (header)\n", __func__, (((bank-1)*EFUSE_REAL_CONTENT_LEN_8723B)+eFuse_Addr-1), efuseHeader);
+
+			/*  Check PG header for section num. */
+			if (EXT_HEADER(efuseHeader)) { /* extended header */
+				offset = GET_HDR_OFFSET_2_0(efuseHeader);
+				DBG_8192C("%s: extended header offset_2_0 = 0x%X\n", __func__, offset);
+
+				efuse_OneByteRead(padapter, eFuse_Addr++, &efuseExtHdr, bPseudoTest);
+				DBG_8192C("%s: efuse[%#X]= 0x%02x (ext header)\n", __func__, (((bank-1)*EFUSE_REAL_CONTENT_LEN_8723B)+eFuse_Addr-1), efuseExtHdr);
+				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;
+
+				/*  Get word enable value from PG header */
+				DBG_8192C("%s: Offset =%d Worden =%#X\n", __func__, offset, wden);
+
+				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);
+						DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseData);
+						efuseTbl[addr] = efuseData;
+
+						efuse_OneByteRead(padapter, eFuse_Addr++, &efuseData, bPseudoTest);
+						DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, eFuse_Addr-1, efuseData);
+						efuseTbl[addr+1] = efuseData;
+					}
+					addr += 2;
+				}
+			} else {
+				DBG_8192C("%s: offset(%d) is illegal!!\n", __func__, offset);
+				eFuse_Addr += Efuse_CalculateWordCnts(wden)*2;
+			}
+		}
+
+		if ((eFuse_Addr-1) < total) {
+			DBG_8192C("%s: bank(%d) data end at %#x\n", __func__, bank, eFuse_Addr-1);
+			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;
+	DBG_8192C("%s: bank(%d) data end at %#x , used =%d\n", __func__, bank, eFuse_Addr-1, used);
+	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);
+	PEFUSE_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;
+	DBG_8192C("%s: start_efuse_addr = 0x%X\n", __func__, 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) {
+			DBG_8192C(KERN_ERR "%s: efuse_OneByteRead Fail! addr = 0x%X !!\n", __func__, efuse_addr);
+			goto error;
+		}
+
+		if (efuse_data == 0xFF)
+			break;
+
+		if ((start_addr != 0) && (efuse_addr == start_addr)) {
+			count++;
+			DBG_8192C(FUNC_ADPT_FMT ": [WARNING] efuse raw 0x%X = 0x%02X not 0xFF!!(%d times)\n",
+				FUNC_ADPT_ARG(padapter), efuse_addr, efuse_data, 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 wirte efuse */
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_AVAILABLE_EFUSE_BYTES_TOTAL, &efuse_addr, bPseudoTest);
+
+exit:
+	DBG_8192C("%s: CurrentSize =%d\n", __func__, efuse_addr);
+
+	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);
+	PEFUSE_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));
+
+	DBG_8192C("%s: start from bank =%d addr = 0x%X\n", __func__, startBank, efuse_addr);
+
+	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) {
+			DBG_8192C(KERN_ERR "%s: switch bank(%d) Fail!!\n", __func__, bank);
+			/* 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) {
+				DBG_8192C(KERN_ERR "%s: efuse_OneByteRead Fail! addr = 0x%X !!\n", __func__, efuse_addr);
+				/* bank = EFUSE_MAX_BANK; */
+				break;
+			}
+			DBG_8192C("%s: efuse_OneByteRead ! addr = 0x%X !efuse_data = 0x%X! bank =%d\n", __func__, efuse_addr, efuse_data, bank);
+
+			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);
+				DBG_8192C("%s: efuse_OneByteRead EXT_HEADER ! addr = 0x%X !efuse_data = 0x%X! bank =%d\n", __func__, efuse_addr, efuse_data, bank);
+
+				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;
+			}
+
+			DBG_8192C(FUNC_ADPT_FMT": Offset =%d Worden =%#X\n",
+				FUNC_ADPT_ARG(padapter), hoffset, hworden);
+
+			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;
+		/* RT_DISP(FEEPROM, EFUSE_PG, ("Hal_EfuseGetCurrentSize_BT92C(), already use %u bytes\n", pEfuseHal->fakeBTEfuseUsedBytes)); */
+	} else {
+		pEfuseHal->BTEfuseUsedBytes = retU2;
+		/* RT_DISP(FEEPROM, EFUSE_PG, ("Hal_EfuseGetCurrentSize_BT92C(), already use %u bytes\n", pEfuseHal->BTEfuseUsedBytes)); */
+	}
+
+	DBG_8192C("%s: CurrentSize =%d\n", __func__, 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];
+
+
+/* 	DBG_8192C("%s: efuse_addr =%#x word_en =%#x\n", __func__, efuse_addr, word_en); */
+	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 == NULL)
+		return false;
+
+	EFUSE_GetEfuseDefinition(padapter, EFUSE_WIFI, TYPE_EFUSE_MAX_SECTION, &max_section, bPseudoTest);
+	if (offset > max_section) {
+		DBG_8192C("%s: Packet offset(%d) is illegal(>%d)!\n", __func__, 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)) {
+				DBG_8192C("%s: Error!! All words disabled!\n", __func__);
+				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);
+/* 					DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, efuse_addr+tmpidx, efuse_data); */
+					data[i*2] = efuse_data;
+
+					efuse_OneByteRead(padapter, efuse_addr++, &efuse_data, bPseudoTest);
+/* 					DBG_8192C("%s: efuse[%#X]= 0x%02X\n", __func__, efuse_addr+tmpidx, efuse_data); */
+					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);
+/* 	DBG_8192C("%s: max_available =%d\n", __func__, max_available); */
+
+	current_size = Efuse_GetCurrentSize(padapter, efuseType, bPseudoTest);
+	if (current_size >= max_available) {
+		DBG_8192C("%s: Error!! current_size(%d)>max_available(%d)\n", __func__, current_size, max_available);
+		return false;
+	}
+	return true;
+}
+
+static void hal_EfuseConstructPGPkt(
+	u8 offset,
+	u8 word_en,
+	u8 *pData,
+	PPGPKT_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,
+	PPGPKT_STRUCT pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+	PEFUSE_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;
+	DBG_8192C("%s: startAddr =%#X\n", __func__, startAddr);
+
+	while (1) {
+		if (startAddr >= efuse_max_available_len) {
+			bRet = false;
+			DBG_8192C("%s: startAddr(%d) >= efuse_max_available_len(%d)\n", __func__, startAddr, efuse_max_available_len);
+			break;
+		}
+
+		if (efuse_OneByteRead(padapter, startAddr, &efuse_data, bPseudoTest) && (efuse_data != 0xFF)) {
+#if 1
+			bRet = false;
+			DBG_8192C("%s: Something Wrong! last bytes(%#X = 0x%02X) is not 0xFF\n",
+				__func__, startAddr, efuse_data);
+			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)) {
+					DBG_8192C("%s: Error condition, all words disabled!", __func__);
+					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
+			) {
+				DBG_8192C("%s: Need to partial write data by the previous wrote header\n", __func__);
+				/*  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;
+/* 			DBG_8192C("%s: Started from unused header offset =%d\n", __func__, startAddr)); */
+			bRet = true;
+			break;
+		}
+	}
+
+	return bRet;
+}
+
+static u8 hal_EfusePgPacketWrite1ByteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	PPGPKT_STRUCT pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u8 pg_header = 0, tmp_header = 0;
+	u16 efuse_addr = *pAddr;
+	u8 repeatcnt = 0;
+
+
+/* 	DBG_8192C("%s\n", __func__); */
+	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_) {
+			DBG_8192C("%s: Repeat over limit for pg_header!!\n", __func__);
+			return false;
+		}
+	} while (1);
+
+	if (tmp_header != pg_header) {
+		DBG_8192C(KERN_ERR "%s: PG Header Fail!!(pg = 0x%02X read = 0x%02X)\n", __func__, pg_header, tmp_header);
+		return false;
+	}
+
+	*pAddr = efuse_addr;
+
+	return true;
+}
+
+static u8 hal_EfusePgPacketWrite2ByteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	PPGPKT_STRUCT pTargetPkt,
+	u8 bPseudoTest
+)
+{
+	u16 efuse_addr, efuse_max_available_len = 0;
+	u8 pg_header = 0, tmp_header = 0;
+	u8 repeatcnt = 0;
+
+
+/* 	DBG_8192C("%s\n", __func__); */
+	EFUSE_GetEfuseDefinition(padapter, efuseType, TYPE_AVAILABLE_EFUSE_BYTES_BANK, &efuse_max_available_len, bPseudoTest);
+
+	efuse_addr = *pAddr;
+	if (efuse_addr >= efuse_max_available_len) {
+		DBG_8192C("%s: addr(%d) over avaliable(%d)!!\n", __func__, efuse_addr, efuse_max_available_len);
+		return false;
+	}
+
+	pg_header = ((pTargetPkt->offset & 0x07) << 5) | 0x0F;
+/* 	DBG_8192C("%s: pg_header = 0x%x\n", __func__, pg_header); */
+
+	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_) {
+			DBG_8192C("%s: Repeat over limit for pg_header!!\n", __func__);
+			return false;
+		}
+	} while (1);
+
+	if (tmp_header != pg_header) {
+		DBG_8192C(KERN_ERR "%s: PG Header Fail!!(pg = 0x%02X read = 0x%02X)\n", __func__, pg_header, tmp_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_) {
+			DBG_8192C("%s: Repeat over limit for ext_header!!\n", __func__);
+			return false;
+		}
+	} while (1);
+
+	if (tmp_header != pg_header) { /* offset PG fail */
+		DBG_8192C(KERN_ERR "%s: PG EXT Header Fail!!(pg = 0x%02X read = 0x%02X)\n", __func__, pg_header, tmp_header);
+		return false;
+	}
+
+	*pAddr = efuse_addr;
+
+	return true;
+}
+
+static u8 hal_EfusePgPacketWriteHeader(
+	struct adapter *padapter,
+	u8 efuseType,
+	u16 *pAddr,
+	PPGPKT_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,
+	PPGPKT_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) {
+		DBG_8192C("%s: Fail!!\n", __func__);
+		return false;
+	}
+
+/* 	DBG_8192C("%s: ok\n", __func__); */
+	return true;
+}
+
+static s32 Hal_EfusePgPacketWrite(
+	struct adapter *padapter,
+	u8 offset,
+	u8 word_en,
+	u8 *pData,
+	bool bPseudoTest
+)
+{
+	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
+)
+{
+	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 HAL_VERSION ReadChipVersion8723B(struct adapter *padapter)
+{
+	u32 value32;
+	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.RFType = RF_TYPE_1T1R;
+	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;
+	if (IS_1T2R(ChipVersion))
+		pHalData->rf_type = RF_1T2R;
+	else if (IS_2T2R(ChipVersion))
+		pHalData->rf_type = RF_2T2R;
+	else
+		pHalData->rf_type = RF_1T1R;
+
+	MSG_8192C("RF_Type is %x!!\n", pHalData->rf_type);
+
+	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);
+
+
+	/*  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. */
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+ResumeTxBeacon\n"));
+
+	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);
+
+
+	/*  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. */
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+StopTxBeacon\n"));
+
+	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) {
+		DBG_871X("Enable notch filter\n");
+		rtw_write8(adapter, rOFDM0_RxDSP+1, rtw_read8(adapter, rOFDM0_RxDSP+1) | BIT1);
+	} else {
+		DBG_871X("Disable notch filter\n");
+		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);
+
+	DBG_871X("%s(): mac_id =%d rssi_level =%d\n", __func__, mac_id, rssi_level);
+
+	if (mac_id >= NUM_STA) /* CAM_SIZE */
+		return;
+
+	psta = pmlmeinfo->FW_sta_info[mac_id].psta;
+	if (psta == NULL)
+		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);
+	DBG_871X("%s => mac_id:%d, networkType:0x%02x, mask:0x%08x\n\t ==> rssi_level:%d, rate_bitmap:0x%08x\n",
+			__func__, mac_id, psta->wireless_mode, mask, rssi_level, rate_bitmap);
+
+	mask &= rate_bitmap;
+
+	rate_bitmap = rtw_btcoex_GetRaMask(padapter);
+	mask &= ~rate_bitmap;
+
+#ifdef CONFIG_CMCC_TEST
+	if (pmlmeext->cur_wireless_mode & WIRELESS_11G) {
+		if (mac_id == 0) {
+			DBG_871X("CMCC_BT update raid entry, mask = 0x%x\n", mask);
+			mask &= 0xffffff00; /* disable CCK & <24M OFDM rate for 11G mode for CMCC */
+			DBG_871X("CMCC_BT update raid entry, mask = 0x%x\n", mask);
+		}
+	}
+#endif
+
+	if (pHalData->fw_ractrl == true) {
+		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;
+	DBG_871X("%s(): mac_id =%d raid = 0x%x bw =%d mask = 0x%x init_rate = 0x%x\n", __func__, mac_id, psta->raid, psta->bw_mode, mask, 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)
+{
+	struct hal_com_data *pHalData;
+	u8 val;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	val = rtw_read8(padapter, REG_LEDCFG2);
+	/*  Let 8051 take control antenna settting */
+	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;
+
+	MSG_8192C("EEPROM type is %s\n", size == 4 ? "E-FUSE" : "93C46");
+
+	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) {
+			DBG_8192C(
+				"%s: FAIL!! REG_AUTO_LLT(0x%X) =%08x\n",
+				__func__,
+				REG_AUTO_LLT,
+				val32
+			);
+			break;
+		}
+
+		msleep(1);
+	} while (1);
+
+	return ret;
+}
+
+static bool Hal_GetChnlGroup8723B(u8 Channel, u8 *pGroup)
+{
+	bool bIn24G = true;
+
+	if (Channel <= 14) {
+		bIn24G = true;
+
+		if (1  <= Channel && Channel <= 2)
+			*pGroup = 0;
+		else if (3  <= Channel && Channel <= 5)
+			*pGroup = 1;
+		else if (6  <= Channel && Channel <= 8)
+			*pGroup = 2;
+		else if (9  <= Channel && Channel <= 11)
+			*pGroup = 3;
+		else if (12 <= Channel && Channel <= 14)
+			*pGroup = 4;
+		else {
+			RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("==>Hal_GetChnlGroup8723B in 2.4 G, but Channel %d in Group not found\n", Channel));
+		}
+	} else {
+		bIn24G = false;
+
+		if (36   <= Channel && Channel <=  42)
+			*pGroup = 0;
+		else if (44   <= Channel && Channel <=  48)
+			*pGroup = 1;
+		else if (50   <= Channel && Channel <=  58)
+			*pGroup = 2;
+		else if (60   <= Channel && Channel <=  64)
+			*pGroup = 3;
+		else if (100  <= Channel && Channel <= 106)
+			*pGroup = 4;
+		else if (108  <= Channel && Channel <= 114)
+			*pGroup = 5;
+		else if (116  <= Channel && Channel <= 122)
+			*pGroup = 6;
+		else if (124  <= Channel && Channel <= 130)
+			*pGroup = 7;
+		else if (132  <= Channel && Channel <= 138)
+			*pGroup = 8;
+		else if (140  <= Channel && Channel <= 144)
+			*pGroup = 9;
+		else if (149  <= Channel && Channel <= 155)
+			*pGroup = 10;
+		else if (157  <= Channel && Channel <= 161)
+			*pGroup = 11;
+		else if (165  <= Channel && Channel <= 171)
+			*pGroup = 12;
+		else if (173  <= Channel && Channel <= 177)
+			*pGroup = 13;
+		else {
+			RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("==>Hal_GetChnlGroup8723B in 5G, but Channel %d in Group not found\n", Channel));
+		}
+
+	}
+	RT_TRACE(
+		_module_hci_hal_init_c_,
+		_drv_info_,
+		(
+			"<==Hal_GetChnlGroup8723B,  (%s) Channel = %d, Group =%d,\n",
+			bIn24G ? "2.4G" : "5G",
+			Channel,
+			*pGroup
+		)
+	);
+	return bIn24G;
+}
+
+void Hal_InitPGData(struct adapter *padapter, u8 *PROMContent)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+
+	if (false == 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 */
+		RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("AutoLoad Fail reported from CR9346!!\n"));
+		if (false == 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) {
+		DBG_8192C("EEPROM ID(%#x) is invalid!!\n", EEPROMId);
+		pEEPROM->bautoload_fail_flag = true;
+	} else
+		pEEPROM->bautoload_fail_flag = false;
+
+	RT_TRACE(_module_hal_init_c_, _drv_notice_, ("EEPROM ID = 0x%04x\n", EEPROMId));
+}
+
+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) {
+		DBG_871X("%s(): Use Default value!\n", __func__);
+		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_GetChnlGroup8723B(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];
+			}
+#ifdef DEBUG
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("======= Path %d, ChannelIndex %d, Group %d =======\n", rfPath, ch, group));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Index24G_CCK_Base[%d][%d] = 0x%x\n", rfPath, ch, pHalData->Index24G_CCK_Base[rfPath][ch]));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Index24G_BW40_Base[%d][%d] = 0x%x\n", rfPath, ch, pHalData->Index24G_BW40_Base[rfPath][ch]));
+#endif
+		}
+
+		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];
+
+#ifdef DEBUG
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("--------------------------------------- 2.4G ---------------------------------------\n"));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("CCK_24G_Diff[%d][%d]= %d\n", rfPath, TxCount, pHalData->CCK_24G_Diff[rfPath][TxCount]));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("OFDM_24G_Diff[%d][%d]= %d\n", rfPath, TxCount, pHalData->OFDM_24G_Diff[rfPath][TxCount]));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("BW20_24G_Diff[%d][%d]= %d\n", rfPath, TxCount, pHalData->BW20_24G_Diff[rfPath][TxCount]));
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("BW40_24G_Diff[%d][%d]= %d\n", rfPath, TxCount, pHalData->BW40_24G_Diff[rfPath][TxCount]));
+#endif
+		}
+	}
+
+	/*  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;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("EEPROMRegulatory = 0x%x\n", pHalData->EEPROMRegulatory));
+}
+
+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)-ODM_RF_PATH_A; */
+			/*  EFUSE_0xC3[6] == 1, S0(Aux)-ODM_RF_PATH_B */
+			pHalData->ant_path = (tempval & BIT(6))?ODM_RF_PATH_B:ODM_RF_PATH_A;
+		} else {
+			pHalData->EEPROMBluetoothAntNum = Ant_x1;
+			if (pHalData->PackageType == PACKAGE_QFN68)
+				pHalData->ant_path = ODM_RF_PATH_B;
+			else
+				pHalData->ant_path = ODM_RF_PATH_A;
+		}
+	} else {
+		pHalData->EEPROMBluetoothCoexist = false;
+		pHalData->EEPROMBluetoothType = BT_RTL8723B;
+		pHalData->EEPROMBluetoothAntNum = Ant_x1;
+		pHalData->ant_path = ODM_RF_PATH_A;
+	}
+
+	if (padapter->registrypriv.ant_num > 0) {
+		DBG_8192C(
+			"%s: Apply driver defined antenna number(%d) to replace origin(%d)\n",
+			__func__,
+			padapter->registrypriv.ant_num,
+			pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1
+		);
+
+		switch (padapter->registrypriv.ant_num) {
+		case 1:
+			pHalData->EEPROMBluetoothAntNum = Ant_x1;
+			break;
+		case 2:
+			pHalData->EEPROMBluetoothAntNum = Ant_x2;
+			break;
+		default:
+			DBG_8192C(
+				"%s: Discard invalid driver defined antenna number(%d)!\n",
+				__func__,
+				padapter->registrypriv.ant_num
+			);
+			break;
+		}
+	}
+
+	rtw_btcoex_SetBTCoexist(padapter, pHalData->EEPROMBluetoothCoexist);
+	rtw_btcoex_SetChipType(padapter, pHalData->EEPROMBluetoothType);
+	rtw_btcoex_SetPGAntNum(padapter, pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1);
+	if (pHalData->EEPROMBluetoothAntNum == Ant_x1)
+		rtw_btcoex_SetSingleAntPath(padapter, pHalData->ant_path);
+
+	DBG_8192C(
+		"%s: %s BT-coex, ant_num =%d\n",
+		__func__,
+		pHalData->EEPROMBluetoothCoexist == true ? "Enable" : "Disable",
+		pHalData->EEPROMBluetoothAntNum == Ant_x2 ? 2 : 1
+	);
+}
+
+void Hal_EfuseParseEEPROMVer_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+/* 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("%s(): AutoLoadFail = %d\n", __func__, AutoLoadFail)); */
+	if (!AutoLoadFail)
+		pHalData->EEPROMVersion = hwinfo[EEPROM_VERSION_8723B];
+	else
+		pHalData->EEPROMVersion = 1;
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("Hal_EfuseParseEEPROMVer(), EEVer = %d\n",
+		pHalData->EEPROMVersion));
+}
+
+
+
+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);
+	DBG_871X("%s phy efuse read 0x1FB =%x\n", __func__, efuseContent);
+	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;
+	}
+
+	DBG_871X("PackageType = 0x%X\n", pHalData->PackageType);
+}
+
+
+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); */
+	DBG_871X("%s hwinfo[EEPROM_Voltage_ADDR_8723B] =%02x\n", __func__, hwinfo[EEPROM_Voltage_ADDR_8723B]);
+	pEEPROM->adjuseVoltageVal = (hwinfo[EEPROM_Voltage_ADDR_8723B] & 0xf0) >> 4;
+	DBG_871X("%s pEEPROM->adjuseVoltageVal =%x\n", __func__, pEEPROM->adjuseVoltageVal);
+}
+
+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);
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("EEPROM ChannelPlan = 0x%02x\n", padapter->mlmepriv.ChannelPlan));
+}
+
+void Hal_EfuseParseCustomerID_8723B(
+	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
+)
+{
+	struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+
+/* 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("%s(): AutoLoadFail = %d\n", __func__, AutoLoadFail)); */
+	if (!AutoLoadFail)
+		pHalData->EEPROMCustomerID = hwinfo[EEPROM_CustomID_8723B];
+	else
+		pHalData->EEPROMCustomerID = 0;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("EEPROM Customer ID: 0x%2x\n", pHalData->EEPROMCustomerID));
+}
+
+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);
+
+/* 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("%s(): AutoLoadFail = %d\n", __func__, AutoLoadFail)); */
+	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;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("EEPROM CrystalCap: 0x%2x\n", pHalData->CrystalCap));
+}
+
+
+void Hal_EfuseParseThermalMeter_8723B(
+	struct adapter *padapter, u8 *PROMContent, u8 AutoLoadFail
+)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+/* 	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("%s(): AutoLoadFail = %d\n", __func__, AutoLoadFail)); */
+	/*  */
+	/*  ThermalMeter from EEPROM */
+	/*  */
+	if (false == AutoLoadFail)
+		pHalData->EEPROMThermalMeter = PROMContent[EEPROM_THERMAL_METER_8723B];
+	else
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_8723B;
+
+	if ((pHalData->EEPROMThermalMeter == 0xff) || (true == AutoLoadFail)) {
+		pHalData->bAPKThermalMeterIgnore = true;
+		pHalData->EEPROMThermalMeter = EEPROM_Default_ThermalMeter_8723B;
+	}
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("EEPROM ThermalMeter = 0x%x\n", pHalData->EEPROMThermalMeter));
+}
+
+
+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];
+		DBG_871X("AutoloadFail =%x,\n", AutoloadFail);
+		Adapter->eeprompriv.EEPROMRFGainVal = EFUSE_Read1Byte(Adapter, EEPROM_RF_GAIN_VAL);
+		DBG_871X("Adapter->eeprompriv.EEPROMRFGainVal =%x\n", Adapter->eeprompriv.EEPROMRFGainVal);
+	} else {
+		Adapter->eeprompriv.EEPROMRFGainOffset = 0;
+		Adapter->eeprompriv.EEPROMRFGainVal = 0xFF;
+		DBG_871X("else AutoloadFail =%x,\n", AutoloadFail);
+	}
+	DBG_871X("EEPRORFGainOffset = 0x%02x\n", Adapter->eeprompriv.EEPROMRFGainOffset);
+}
+
+u8 BWMapping_8723B(struct adapter *Adapter, struct pkt_attrib *pattrib)
+{
+	u8 BWSettingOfDesc = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	/* DBG_871X("BWMapping pHalData->CurrentChannelBW %d, pattrib->bwmode %d\n", pHalData->CurrentChannelBW, pattrib->bwmode); */
+
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_80) {
+		if (pattrib->bwmode == CHANNEL_WIDTH_80)
+			BWSettingOfDesc = 2;
+		else if (pattrib->bwmode == CHANNEL_WIDTH_40)
+			BWSettingOfDesc = 1;
+		else
+			BWSettingOfDesc = 0;
+	} else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		if ((pattrib->bwmode == CHANNEL_WIDTH_40) || (pattrib->bwmode == CHANNEL_WIDTH_80))
+			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);
+
+	/* DBG_871X("SCMapping: pHalData->CurrentChannelBW %d, pHalData->nCur80MhzPrimeSC %d, pHalData->nCur40MhzPrimeSC %d\n", pHalData->CurrentChannelBW, pHalData->nCur80MhzPrimeSC, pHalData->nCur40MhzPrimeSC); */
+
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_80) {
+		if (pattrib->bwmode == CHANNEL_WIDTH_80) {
+			SCSettingOfDesc = VHT_DATA_SC_DONOT_CARE;
+		} else if (pattrib->bwmode == CHANNEL_WIDTH_40) {
+			if (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+				SCSettingOfDesc = VHT_DATA_SC_40_LOWER_OF_80MHZ;
+			else if (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+				SCSettingOfDesc = VHT_DATA_SC_40_UPPER_OF_80MHZ;
+			else
+				DBG_871X("SCMapping: Not Correct Primary40MHz Setting\n");
+		} else {
+			if ((pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) && (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER))
+				SCSettingOfDesc = VHT_DATA_SC_20_LOWEST_OF_80MHZ;
+			else if ((pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) && (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER))
+				SCSettingOfDesc = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+			else if ((pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) && (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER))
+				SCSettingOfDesc = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+			else if ((pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) && (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER))
+				SCSettingOfDesc = VHT_DATA_SC_20_UPPERST_OF_80MHZ;
+			else
+				DBG_871X("SCMapping: Not Correct Primary40MHz Setting\n");
+		}
+	} else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		/* DBG_871X("SCMapping: HT Case: pHalData->CurrentChannelBW %d, pHalData->nCur40MhzPrimeSC %d\n", pHalData->CurrentChannelBW, pHalData->nCur40MhzPrimeSC); */
+
+		if (pattrib->bwmode == CHANNEL_WIDTH_40) {
+			SCSettingOfDesc = VHT_DATA_SC_DONOT_CARE;
+		} else if (pattrib->bwmode == CHANNEL_WIDTH_20) {
+			if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) {
+				SCSettingOfDesc = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+			} else if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) {
+				SCSettingOfDesc = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+			} else {
+				SCSettingOfDesc = VHT_DATA_SC_DONOT_CARE;
+			}
+		}
+	} else {
+		SCSettingOfDesc = VHT_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, PTXDESC_8723B ptxdesc)
+{
+	/* DBG_8192C("cvs_mode =%d\n", pattrib->vcs_mode); */
+
+	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, PTXDESC_8723B ptxdesc)
+{
+	/* DBG_8192C("bwmode =%d, ch_off =%d\n", pattrib->bwmode, pattrib->ch_offset); */
+
+	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 dm_priv *pdmpriv;
+	struct mlme_ext_priv *pmlmeext;
+	struct mlme_ext_info *pmlmeinfo;
+	struct pkt_attrib *pattrib;
+	PTXDESC_8723B ptxdesc;
+	s32 bmcst;
+
+	memset(pbuf, 0, TXDESC_SIZE);
+
+	padapter = pxmitframe->padapter;
+	pHalData = GET_HAL_DATA(padapter);
+	pdmpriv = &pHalData->dmpriv;
+	pmlmeext = &padapter->mlmeextpriv;
+	pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	pattrib = &pxmitframe->attrib;
+	bmcst = IS_MCAST(pattrib->ra);
+
+	ptxdesc = (PTXDESC_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)
+#ifdef CONFIG_AUTO_AP_MODE
+			&& (pattrib->pctrl != true)
+#endif
+		) {
+			/*  Non EAP & ARP & DHCP type data packet */
+
+			if (pattrib->ampdu_en == true) {
+				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 == false) {
+				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;
+
+#ifdef CONFIG_CMCC_TEST
+			ptxdesc->data_short = 1; /* use cck short premble */
+#endif
+		} 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);
+			DBG_871X("YJ: %s(): ARP Data: userate =%d, datarate = 0x%x\n", __func__, ptxdesc->userate, ptxdesc->datarate);
+		}
+
+		ptxdesc->usb_txagg_num = pxmitframe->agg_num;
+	} else if (pxmitframe->frame_tag == MGNT_FRAMETAG) {
+/* 		RT_TRACE(_module_hal_xmit_c_, _drv_notice_, ("%s: MGNT_FRAMETAG\n", __func__)); */
+
+		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 == true) {
+			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) {
+			#ifdef DBG_CCX
+			DBG_8192C("%s set spe_rpt\n", __func__);
+			#endif
+			ptxdesc->spe_rpt = 1;
+			ptxdesc->sw_define = (u8)(GET_PRIMARY_ADAPTER(padapter)->xmitpriv.seq_no);
+		}
+	} else if (pxmitframe->frame_tag == TXAGG_FRAMETAG) {
+		RT_TRACE(_module_hal_xmit_c_, _drv_warning_, ("%s: TXAGG_FRAMETAG\n", __func__));
+	} else {
+		RT_TRACE(_module_hal_xmit_c_, _drv_warning_, ("%s: frame_tag = 0x%x\n", __func__, pxmitframe->frame_tag));
+
+		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 controled 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;
+	pdesc->txdw0 = pdesc->txdw0;
+	pdesc->txdw1 = pdesc->txdw1;
+	pdesc->txdw2 = pdesc->txdw2;
+	pdesc->txdw3 = pdesc->txdw3;
+	pdesc->txdw4 = pdesc->txdw4;
+	pdesc->txdw5 = pdesc->txdw5;
+	pdesc->txdw6 = pdesc->txdw6;
+	pdesc->txdw7 = pdesc->txdw7;
+	pdesc->txdw8 = pdesc->txdw8;
+	pdesc->txdw9 = pdesc->txdw9;
+
+	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 (true == 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 (true == 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 (true == 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);
+		DBG_871X("#### %s() -%d iface_type(0) mode = %d ####\n", __func__, __LINE__, mode);
+
+		if ((mode == _HW_STATE_STATION_) || (mode == _HW_STATE_NOLINK_)) {
+			{
+				StopTxBeacon(padapter);
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN_EARLY_INT
+				rtw_write8(padapter, REG_DRVERLYINT, 0x05); /*  restore early int time to 5ms */
+				UpdateInterruptMask8812AU(padapter, true, 0, IMR_BCNDMAINT0_8723B);
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN_EARLY_INT */
+
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN_BCN_OK_ERR
+				UpdateInterruptMask8812AU(padapter, true, 0, (IMR_TXBCN0ERR_8723B|IMR_TXBCN0OK_8723B));
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN_BCN_OK_ERR */
+
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN */
+			}
+
+			/*  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_) /*|| (mode == _HW_STATE_AP_)*/) {
+			ResumeTxBeacon(padapter);
+			rtw_write8(padapter, REG_BCN_CTRL, DIS_TSF_UDT|EN_BCN_FUNCTION|DIS_BCNQ_SUB);
+		} else if (mode == _HW_STATE_AP_) {
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN_EARLY_INT
+			UpdateInterruptMask8723BU(padapter, true, IMR_BCNDMAINT0_8723B, 0);
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN_EARLY_INT */
+
+#ifdef CONFIG_INTERRUPT_BASED_TXBCN_BCN_OK_ERR
+			UpdateInterruptMask8723BU(padapter, true, (IMR_TXBCN0ERR_8723B|IMR_TXBCN0OK_8723B), 0);
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN_BCN_OK_ERR */
+
+#endif /*  CONFIG_INTERRUPT_BASED_TXBCN */
+
+			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-rtw_modular64(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 orignal 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 orignal 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 hal_com_data *pHalData;
+	struct mlme_priv *pmlmepriv;
+	struct eeprom_priv *pEEPROM;
+
+
+	RetryLimit = 0x30;
+	type = *(u8 *)val;
+	pHalData = GET_HAL_DATA(padapter);
+	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)
+{
+	u8 seq_no;
+
+#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)
+
+	/* DBG_871X("%s, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\n", __func__, */
+	/* 		*pdata, *(pdata+1), *(pdata+2), *(pdata+3), *(pdata+4), *(pdata+5), *(pdata+6), *(pdata+7)); */
+
+	seq_no = *(pdata+6);
+
+	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) {
+		DBG_871X("tx_seq_no =%d, rpt_seq_no =%d\n", padapter->xmitpriv.seq_no, 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;
+	u8 index = 0;
+
+	if (pC2hEvent == NULL) {
+		DBG_8192C("%s(): pC2hEventis NULL\n", __func__);
+		ret = _FAIL;
+		goto exit;
+	}
+
+	switch (pC2hEvent->id) {
+	case C2H_AP_RPT_RSP:
+		break;
+	case C2H_DBG:
+		{
+			RT_TRACE(_module_hal_init_c_, _drv_info_, ("c2h_handler_8723b: %s\n", pC2hEvent->payload));
+		}
+		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:
+		RT_TRACE(_module_hal_init_c_, _drv_info_, ("[BT], C2H_HW_INFO_EXCH\n"));
+		for (index = 0; index < pC2hEvent->plen; index++) {
+			RT_TRACE(_module_hal_init_c_, _drv_info_, ("[BT], tmpBuf[%d]= 0x%x\n", index, pC2hEvent->payload[index]));
+		}
+		break;
+
+	case C2H_8723B_BT_INFO:
+		rtw_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, PC2H_EVT_HDR pC2hEvent, u8 *c2hBuf)
+{
+	u8 index = 0;
+
+	if (c2hBuf == NULL) {
+		DBG_8192C("%s c2hbuff is NULL\n", __func__);
+		return;
+	}
+
+	switch (pC2hEvent->CmdID) {
+	case C2H_AP_RPT_RSP:
+		break;
+	case C2H_DBG:
+		{
+			RT_TRACE(_module_hal_init_c_, _drv_info_, ("C2HCommandHandler: %s\n", c2hBuf));
+		}
+		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:
+		RT_TRACE(_module_hal_init_c_, _drv_info_, ("[BT], C2H_HW_INFO_EXCH\n"));
+		for (index = 0; index < pC2hEvent->CmdLen; index++) {
+			RT_TRACE(_module_hal_init_c_, _drv_info_, ("[BT], tmpBuf[%d]= 0x%x\n", index, c2hBuf[index]));
+		}
+		break;
+
+	case C2H_8723B_BT_INFO:
+		rtw_btcoex_BtInfoNotify(padapter, pC2hEvent->CmdLen, c2hBuf);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void C2HPacketHandler_8723B(struct adapter *padapter, u8 *pbuffer, u16 length)
+{
+	C2H_EVT_HDR	C2hEvent;
+	u8 *tmpBuf = NULL;
+#ifdef CONFIG_WOWLAN
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+
+	if (pwrpriv->wowlan_mode == true) {
+		DBG_871X("%s(): return because wowolan_mode ==true! CMDID =%d\n", __func__, pbuffer[0]);
+		return;
+	}
+#endif
+	C2hEvent.CmdID = pbuffer[0];
+	C2hEvent.CmdSeq = pbuffer[1];
+	C2hEvent.CmdLen = length-2;
+	tmpBuf = pbuffer+2;
+
+	/* DBG_871X("%s C2hEvent.CmdID:%x C2hEvent.CmdLen:%x C2hEvent.CmdSeq:%x\n", */
+	/* 		__func__, C2hEvent.CmdID, C2hEvent.CmdLen, C2hEvent.CmdSeq); */
+	RT_PRINT_DATA(_module_hal_init_c_, _drv_notice_, "C2HPacketHandler_8723B(): Command Content:\n", tmpBuf, C2hEvent.CmdLen);
+
+	process_c2h_event(padapter, &C2hEvent, tmpBuf);
+	/* c2h_handler_8723b(padapter,&C2hEvent); */
+	return;
+}
+
+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 input_b = 0, masked = 0, ioted = 0, 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);
+		input_b = BrateCfg;
+
+		/* apply force and allow mask */
+		BrateCfg |= rrsr_2g_force_mask;
+		BrateCfg &= rrsr_2g_allow_mask;
+		masked = BrateCfg;
+
+		#ifdef CONFIG_CMCC_TEST
+		BrateCfg |= (RRSR_11M|RRSR_5_5M|RRSR_1M); /* use 11M to send ACK */
+		BrateCfg |= (RRSR_24M|RRSR_18M|RRSR_12M); /* CMCC_OFDM_ACK 12/18/24M */
+		#endif
+
+		/* 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;
+		}
+		ioted = BrateCfg;
+
+		pHalData->BasicRateSet = BrateCfg;
+
+		DBG_8192C("HW_VAR_BASIC_RATE: %#x -> %#x -> %#x\n", input_b, masked, ioted);
+
+		/*  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);
+
+		rtw_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 */
+			rtw_btcoex_ConnectNotify(padapter, true);
+			break;
+		case 1:
+			/*  joinbss_event callback when join res < 0 */
+			rtw_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);
+		DBG_8192C("%s, %d, RCR = %x\n", __func__, __LINE__, rtw_read32(padapter, REG_RCR));
+		break;
+
+	case HW_VAR_OFF_RCR_AM:
+		val32 = rtw_read32(padapter, REG_RCR);
+		val32 &= ~RCR_AM;
+		rtw_write32(padapter, REG_RCR, val32);
+		DBG_8192C("%s, %d, RCR = %x\n", __func__, __LINE__, rtw_read32(padapter, REG_RCR));
+		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); */
+				/* RT_TRACE(COMP_SEC, DBG_LOUD, ("CAM_empty_entry(): WRITE A4: %lx\n", ulContent)); */
+				rtw_write32(padapter, RWCAM, ulCommand);  /* mdelay(40); */
+				/* RT_TRACE(COMP_SEC, DBG_LOUD, ("CAM_empty_entry(): WRITE A0: %lx\n", ulCommand)); */
+			}
+		}
+		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;
+			}
+
+			DBG_8192C("[HW_VAR_ACM_CTRL] Write 0x%02X\n", hwctrl);
+			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:
+		{
+			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 != true) {
+				/* 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;
+
+					DBG_871X("%s: [HW_VAR_FIFO_CLEARN_UP] val =%x times:%d\n", __func__, val32, trycnt);
+				} while (--trycnt);
+
+				if (trycnt == 0) {
+					DBG_8192C("[HW_VAR_FIFO_CLEARN_UP] Stop RX DMA failed......\n");
+				}
+
+				/*  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;
+		DBG_8192C("%s: bMacPwrCtrlOn =%d\n", __func__, pHalData->bMacPwrCtrlOn);
+		break;
+
+	case HW_VAR_NAV_UPPER:
+		{
+			u32 usNavUpper = *((u32 *)val);
+
+			if (usNavUpper > HAL_NAV_UPPER_UNIT_8723B * 0xFF) {
+				RT_TRACE(_module_hal_init_c_, _drv_notice_, ("The setting value (0x%08X us) of NAV_UPPER is larger than (%d * 0xFF)!!!\n", usNavUpper, HAL_NAV_UPPER_UNIT_8723B));
+				break;
+			}
+
+			/*  The value of ((usNavUpper + HAL_NAV_UPPER_UNIT_8723B - 1) / HAL_NAV_UPPER_UNIT_8723B) */
+			/*  is getting the upper integer. */
+			usNavUpper = (usNavUpper + HAL_NAV_UPPER_UNIT_8723B - 1) / 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) {
+			DBG_8192C(FUNC_ADPT_FMT ": [HW_VAR_MACID_SLEEP] Invalid macid(%d)\n",
+				FUNC_ADPT_ARG(padapter), val32);
+			break;
+		}
+		val8 = (u8)val32; /*  macid is between 0~31 */
+
+		val32 = rtw_read32(padapter, REG_MACID_SLEEP);
+		DBG_8192C(FUNC_ADPT_FMT ": [HW_VAR_MACID_SLEEP] macid =%d, org MACID_SLEEP = 0x%08X\n",
+			FUNC_ADPT_ARG(padapter), val8, val32);
+		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) {
+			DBG_8192C(FUNC_ADPT_FMT ": [HW_VAR_MACID_WAKEUP] Invalid macid(%d)\n",
+				FUNC_ADPT_ARG(padapter), val32);
+			break;
+		}
+		val8 = (u8)val32; /*  macid is between 0~31 */
+
+		val32 = rtw_read32(padapter, REG_MACID_SLEEP);
+		DBG_8192C(FUNC_ADPT_FMT ": [HW_VAR_MACID_WAKEUP] macid =%d, org MACID_SLEEP = 0x%08X\n",
+			FUNC_ADPT_ARG(padapter), val8, val32);
+		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 == true) ||
+				(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;
+#ifdef CONFIG_WOWLAN
+	case HW_VAR_RPWM_TOG:
+		*val = rtw_read8(padapter, SDIO_LOCAL_BASE|SDIO_REG_HRPWM1) & BIT7;
+		break;
+	case HW_VAR_WAKEUP_REASON:
+		*val = rtw_read8(padapter, REG_WOWLAN_WAKE_REASON);
+		if (*val == 0xEA)
+			*val = 0;
+		break;
+	case HW_VAR_SYS_CLKR:
+		*val = rtw_read8(padapter, REG_SYS_CLKR);
+		break;
+#endif
+	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)
+{
+	struct hal_com_data *pHalData;
+	u8 bResult;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	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)
+{
+	struct hal_com_data *pHalData;
+	u8 bResult;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	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 = MAX_AMPDU_FACTOR_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;
+			u32 ra_info1, ra_info2;
+			u32 rate_mask1, rate_mask2;
+			u8 curr_tx_rate, curr_tx_sgi, hight_rate, lowest_rate;
+
+			DBG_8192C("============ RA status check  Mac_id:%d ===================\n", mac_id);
+
+			cmd = 0x40000100 | mac_id;
+			rtw_write32(padapter, REG_HMEBOX_DBG_2_8723B, cmd);
+			msleep(10);
+			ra_info1 = rtw_read32(padapter, 0x2F0);
+			curr_tx_rate = ra_info1&0x7F;
+			curr_tx_sgi = (ra_info1>>7)&0x01;
+			DBG_8192C("[ ra_info1:0x%08x ] =>cur_tx_rate = %s, cur_sgi:%d, PWRSTS = 0x%02x \n",
+				ra_info1,
+				HDATA_RATE(curr_tx_rate),
+				curr_tx_sgi,
+				(ra_info1>>8)  & 0x07);
+
+			cmd = 0x40000400 | mac_id;
+			rtw_write32(padapter, REG_HMEBOX_DBG_2_8723B, cmd);
+			msleep(10);
+			ra_info1 = rtw_read32(padapter, 0x2F0);
+			ra_info2 = rtw_read32(padapter, 0x2F4);
+			rate_mask1 = rtw_read32(padapter, 0x2F8);
+			rate_mask2 = rtw_read32(padapter, 0x2FC);
+			hight_rate = ra_info2&0xFF;
+			lowest_rate = (ra_info2>>8)  & 0xFF;
+
+			DBG_8192C("[ ra_info1:0x%08x ] =>RSSI =%d, BW_setting = 0x%02x, DISRA = 0x%02x, VHT_EN = 0x%02x\n",
+				ra_info1,
+				ra_info1&0xFF,
+				(ra_info1>>8)  & 0xFF,
+				(ra_info1>>16) & 0xFF,
+				(ra_info1>>24) & 0xFF);
+
+			DBG_8192C("[ ra_info2:0x%08x ] =>hight_rate =%s, lowest_rate =%s, SGI = 0x%02x, RateID =%d\n",
+				ra_info2,
+				HDATA_RATE(hight_rate),
+				HDATA_RATE(lowest_rate),
+				(ra_info2>>16) & 0xFF,
+				(ra_info2>>24) & 0xFF);
+
+			DBG_8192C("rate_mask2 = 0x%08x, rate_mask1 = 0x%08x\n", rate_mask2, rate_mask1);
+
+		}
+		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;
+}
+
+#ifdef CONFIG_WOWLAN
+void Hal_DetectWoWMode(struct adapter *padapter)
+{
+	adapter_to_pwrctl(padapter)->bSupportRemoteWakeup = true;
+	DBG_871X("%s\n", __func__);
+}
+#endif /* CONFIG_WOWLAN */
+
+void rtl8723b_start_thread(struct adapter *padapter)
+{
+#ifndef CONFIG_SDIO_TX_TASKLET
+	struct xmit_priv *xmitpriv = &padapter->xmitpriv;
+
+	xmitpriv->SdioXmitThread = kthread_run(rtl8723bs_xmit_thread, padapter, "RTWHALXT");
+	if (IS_ERR(xmitpriv->SdioXmitThread)) {
+		RT_TRACE(_module_hal_xmit_c_, _drv_err_, ("%s: start rtl8723bs_xmit_thread FAIL!!\n", __func__));
+	}
+#endif
+}
+
+void rtl8723b_stop_thread(struct adapter *padapter)
+{
+#ifndef CONFIG_SDIO_TX_TASKLET
+	struct xmit_priv *xmitpriv = &padapter->xmitpriv;
+
+	/*  stop xmit_buf_thread */
+	if (xmitpriv->SdioXmitThread) {
+		up(&xmitpriv->SdioXmitSema);
+		down(&xmitpriv->SdioXmitTerminateSema);
+		xmitpriv->SdioXmitThread = NULL;
+	}
+#endif
+}
+
+#if defined(CONFIG_CHECK_BT_HANG)
+extern void check_bt_status_work(void *data);
+void rtl8723bs_init_checkbthang_workqueue(struct adapter *adapter)
+{
+	adapter->priv_checkbt_wq = alloc_workqueue("sdio_wq", 0, 0);
+	INIT_DELAYED_WORK(&adapter->checkbt_work, (void *)check_bt_status_work);
+}
+
+void rtl8723bs_free_checkbthang_workqueue(struct adapter *adapter)
+{
+	if (adapter->priv_checkbt_wq) {
+		cancel_delayed_work_sync(&adapter->checkbt_work);
+		flush_workqueue(adapter->priv_checkbt_wq);
+		destroy_workqueue(adapter->priv_checkbt_wq);
+		adapter->priv_checkbt_wq = NULL;
+	}
+}
+
+void rtl8723bs_cancle_checkbthang_workqueue(struct adapter *adapter)
+{
+	if (adapter->priv_checkbt_wq)
+		cancel_delayed_work_sync(&adapter->checkbt_work);
+}
+
+void rtl8723bs_hal_check_bt_hang(struct adapter *adapter)
+{
+	if (adapter->priv_checkbt_wq)
+		queue_delayed_work(adapter->priv_checkbt_wq, &(adapter->checkbt_work), 0);
+}
+#endif
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
new file mode 100644
index 000000000000..28d1a229c3a6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_phycfg.c
@@ -0,0 +1,1050 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTL8723B_PHYCFG_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+
+/*---------------------------Define Local Constant---------------------------*/
+/* 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
+
+#define MAX_DOZE_WAITING_TIMES_9x 64
+
+/**
+* Function:	phy_CalculateBitShift
+*
+* OverView:	Get shifted position of the BitMask
+*
+* Input:
+*		u32 	BitMask,
+*
+* Output:	none
+* Return:		u32 	Return the shift bit 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;
+}
+
+
+/**
+* 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 PHY_QueryBBReg_8723B(struct adapter *Adapter, u32 RegAddr, u32 BitMask)
+{
+	u32 ReturnValue = 0, OriginalValue, BitShift;
+
+#if (DISABLE_BB_RF == 1)
+	return 0;
+#endif
+
+	/* RT_TRACE(COMP_RF, DBG_TRACE, ("--->PHY_QueryBBReg(): RegAddr(%#lx), BitMask(%#lx)\n", RegAddr, BitMask)); */
+
+	OriginalValue = rtw_read32(Adapter, RegAddr);
+	BitShift = phy_CalculateBitShift(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 PHY_SetBBReg_8723B(
+	struct adapter *Adapter,
+	u32 RegAddr,
+	u32 BitMask,
+	u32 Data
+)
+{
+	/* u16 BBWaitCounter	= 0; */
+	u32 OriginalValue, BitShift;
+
+#if (DISABLE_BB_RF == 1)
+	return;
+#endif
+
+	/* RT_TRACE(COMP_RF, DBG_TRACE, ("--->PHY_SetBBReg(): RegAddr(%#lx), BitMask(%#lx), Data(%#lx)\n", RegAddr, BitMask, Data)); */
+
+	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);
+
+		/* RT_DISP(FINIT, INIT_RF, ("Readback from RF-PI : 0x%x\n", retValue)); */
+	} else {
+		/* Read from BBreg8a0, 12 bits for 8190, 20 bits for T65 RF */
+		retValue = PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBack|MaskforPhySet, bLSSIReadBackData);
+
+		/* RT_DISP(FINIT, INIT_RF, ("Readback from RF-SI : 0x%x\n", retValue)); */
+	}
+	return retValue;
+
+}
+
+/**
+* Function:	phy_RFSerialWrite_8723B
+*
+* OverView:	Write data to RF register (page 8~)
+*
+* Input:
+*		struct adapter *	Adapter,
+*		RF_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_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 = (Data<<16) | (NewOffset&0x3f); */
+	DataAndAddr = ((NewOffset<<20) | (Data&0x000fffff)) & 0x0fffffff;	/*  T65 RF */
+
+	/*  */
+	/*  Write Operation */
+	/*  */
+	PHY_SetBBReg(Adapter, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
+	/* RTPRINT(FPHY, PHY_RFW, ("RFW-%d Addr[0x%lx]= 0x%lx\n", eRFPath, pPhyReg->rf3wireOffset, DataAndAddr)); */
+
+}
+
+
+/**
+* Function:	PHY_QueryRFReg
+*
+* OverView:	Query "Specific bits" to RF register (page 8~)
+*
+* Input:
+*		struct adapter *	Adapter,
+*		RF_PATH			eRFPath,	Radio path of A/B/C/D
+*		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 PHY_QueryRFReg_8723B(
+	struct adapter *Adapter,
+	u8 eRFPath,
+	u32 RegAddr,
+	u32 BitMask
+)
+{
+	u32 Original_Value, Readback_Value, BitShift;
+
+#if (DISABLE_BB_RF == 1)
+	return 0;
+#endif
+
+	Original_Value = phy_RFSerialRead_8723B(Adapter, eRFPath, RegAddr);
+
+	BitShift =  phy_CalculateBitShift(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,
+*		RF_PATH			eRFPath,	Radio path of A/B/C/D
+*		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 PHY_SetRFReg_8723B(
+	struct adapter *Adapter,
+	u8 eRFPath,
+	u32 RegAddr,
+	u32 BitMask,
+	u32 Data
+)
+{
+	u32 Original_Value, BitShift;
+
+#if (DISABLE_BB_RF == 1)
+	return;
+#endif
+
+	/*  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. */
+/*  */
+
+
+/*-----------------------------------------------------------------------------
+ * 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_MACConfig8723B(struct adapter *Adapter)
+{
+	int rtStatus = _SUCCESS;
+	struct hal_com_data	*pHalData = GET_HAL_DATA(Adapter);
+	s8 *pszMACRegFile;
+	s8 sz8723MACRegFile[] = RTL8723B_PHY_MACREG;
+
+
+	pszMACRegFile = sz8723MACRegFile;
+
+	/*  */
+	/*  Config MAC */
+	/*  */
+	rtStatus = phy_ConfigMACWithParaFile(Adapter, pszMACRegFile);
+	if (rtStatus == _FAIL)
+	{
+		ODM_ConfigMACWithHeaderFile(&pHalData->odmpriv);
+		rtStatus = _SUCCESS;
+	}
+
+	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_com_data		*pHalData = GET_HAL_DATA(Adapter);
+
+	/*  RF Interface Sowrtware Control */
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 LSBs if read 32-bit from 0x870 */
+	pHalData->PHYRegDef[ODM_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[ODM_RF_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x860 */
+	pHalData->PHYRegDef[ODM_RF_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x864 */
+
+	/*  RF Interface (Output and)  Enable */
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x860 (16-bit for 0x862) */
+	pHalData->PHYRegDef[ODM_RF_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x864 (16-bit for 0x866) */
+
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter; /* LSSI Parameter */
+	pHalData->PHYRegDef[ODM_RF_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
+
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;  /* wire control parameter2 */
+	pHalData->PHYRegDef[ODM_RF_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;  /* wire control parameter2 */
+
+	/*  Tranceiver Readback LSSI/HSPI mode */
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	pHalData->PHYRegDef[ODM_RF_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	pHalData->PHYRegDef[ODM_RF_PATH_A].rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
+	pHalData->PHYRegDef[ODM_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);
+	int rtStatus = _SUCCESS;
+	u8 sz8723BBRegFile[] = RTL8723B_PHY_REG;
+	u8 sz8723AGCTableFile[] = RTL8723B_AGC_TAB;
+	u8 sz8723BBBRegPgFile[] = RTL8723B_PHY_REG_PG;
+	u8 sz8723BBRegMpFile[] = RTL8723B_PHY_REG_MP;
+	u8 sz8723BRFTxPwrLmtFile[] = RTL8723B_TXPWR_LMT;
+	u8 *pszBBRegFile = NULL, *pszAGCTableFile = NULL, *pszBBRegPgFile = NULL, *pszBBRegMpFile = NULL, *pszRFTxPwrLmtFile = NULL;
+
+	pszBBRegFile = sz8723BBRegFile;
+	pszAGCTableFile = sz8723AGCTableFile;
+	pszBBRegPgFile = sz8723BBBRegPgFile;
+	pszBBRegMpFile = sz8723BBRegMpFile;
+	pszRFTxPwrLmtFile = sz8723BRFTxPwrLmtFile;
+
+	/*  Read Tx Power Limit File */
+	PHY_InitTxPowerLimit(Adapter);
+	if (
+		Adapter->registrypriv.RegEnableTxPowerLimit == 1 ||
+		(Adapter->registrypriv.RegEnableTxPowerLimit == 2 && pHalData->EEPROMRegulatory == 1)
+	) {
+		if (PHY_ConfigRFWithPowerLimitTableParaFile(Adapter, pszRFTxPwrLmtFile) == _FAIL)
+		{
+			if (HAL_STATUS_SUCCESS != ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_TXPWR_LMT, (ODM_RF_RADIO_PATH_E)0))
+				rtStatus = _FAIL;
+		}
+
+		if (rtStatus != _SUCCESS) {
+			DBG_871X("%s():Read Tx power limit fail\n", __func__);
+			goto phy_BB8190_Config_ParaFile_Fail;
+		}
+	}
+
+	/*  */
+	/*  1. Read PHY_REG.TXT BB INIT!! */
+	/*  */
+	if (phy_ConfigBBWithParaFile(Adapter, pszBBRegFile, CONFIG_BB_PHY_REG) == _FAIL)
+	{
+		if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG))
+			rtStatus = _FAIL;
+	}
+
+	if (rtStatus != _SUCCESS) {
+		DBG_8192C("%s():Write BB Reg Fail!!", __func__);
+		goto phy_BB8190_Config_ParaFile_Fail;
+	}
+
+	/*  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)
+	) {
+		if (phy_ConfigBBWithPgParaFile(Adapter, pszBBRegPgFile) == _FAIL)
+		{
+			if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG_PG))
+				rtStatus = _FAIL;
+		}
+
+		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);
+
+		if (rtStatus != _SUCCESS) {
+			DBG_8192C("%s():BB_PG Reg Fail!!\n", __func__);
+		}
+	}
+
+	/*  */
+	/*  2. Read BB AGC table Initialization */
+	/*  */
+	if (phy_ConfigBBWithParaFile(Adapter, pszAGCTableFile, CONFIG_BB_AGC_TAB) == _FAIL)
+	{
+		if (HAL_STATUS_SUCCESS != ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_AGC_TAB))
+			rtStatus = _FAIL;
+	}
+
+	if (rtStatus != _SUCCESS) {
+		DBG_8192C("%s():AGC Table Fail\n", __func__);
+		goto phy_BB8190_Config_ParaFile_Fail;
+	}
+
+phy_BB8190_Config_ParaFile_Fail:
+
+	return rtStatus;
+}
+
+
+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, ODM_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);
+	/* PHY_BB8723B_Config_1T(Adapter); */
+
+	return rtStatus;
+}
+
+/**************************************************************************************************************
+ *   Description:
+ *       The low-level interface to set TxAGC , called by both MP and Normal Driver.
+ *
+ *                                                                                    <20120830, Kordan>
+ **************************************************************************************************************/
+
+void PHY_SetTxPowerIndex_8723B(
+	struct adapter *Adapter,
+	u32 PowerIndex,
+	u8 RFPath,
+	u8 Rate
+)
+{
+	if (RFPath == ODM_RF_PATH_A || RFPath == ODM_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:
+			DBG_871X("Invalid Rate!!\n");
+			break;
+		}
+	} else {
+		RT_TRACE(_module_hal_init_c_, _drv_err_, ("Invalid RFPath!!\n"));
+	}
+}
+
+u8 PHY_GetTxPowerIndex_8723B(
+	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;
+	bool bIn24G = false;
+
+	/* DBG_871X("===>%s\n", __func__); */
+
+	txPower = (s8) PHY_GetTxPowerIndexBase(padapter, RFPath, Rate, BandWidth, Channel, &bIn24G);
+	powerDiffByRate = PHY_GetTxPowerByRate(padapter, BAND_ON_2_4G, ODM_RF_PATH_A, RF_1TX, Rate);
+
+	limit = PHY_GetTxPowerLimit(
+		padapter,
+		padapter->registrypriv.RegPwrTblSel,
+		(u8)(!bIn24G),
+		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;
+
+	/* DBG_871X("Final Tx Power(RF-%c, Channel: %d) = %d(0x%X)\n", ((RFPath == 0)?'A':'B'), Channel, txPower, txPower)); */
+	return (u8) txPower;
+}
+
+void PHY_SetTxPowerLevel8723B(struct adapter *Adapter, u8 Channel)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+	pFAT_T pDM_FatTable = &pDM_Odm->DM_FatTable;
+	u8 RFPath = ODM_RF_PATH_A;
+
+	if (pHalData->AntDivCfg) {/*  antenna diversity Enable */
+		RFPath = ((pDM_FatTable->RxIdleAnt == MAIN_ANT) ? ODM_RF_PATH_A : ODM_RF_PATH_B);
+	} else { /*  antenna diversity disable */
+		RFPath = pHalData->ant_path;
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("==>PHY_SetTxPowerLevel8723B()\n"));
+
+	PHY_SetTxPowerLevelByPath(Adapter, Channel, RFPath);
+
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("<==PHY_SetTxPowerLevel8723B()\n"));
+}
+
+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;
+
+	case CHANNEL_WIDTH_80:
+		u2tmp = RegRfMod_BW | BIT8;
+		rtw_write16(Adapter, REG_TRXPTCL_CTL_8723B, (u2tmp & 0xFF7F)); /*  BIT 7 = 0, BIT 8 = 1 */
+		break;
+
+	default:
+		DBG_871X("phy_PostSetBWMode8723B():	unknown Bandwidth: %#X\n", CurrentBW);
+		break;
+	}
+}
+
+static u8 phy_GetSecondaryChnl_8723B(struct adapter *Adapter)
+{
+	u8 SCSettingOf40 = 0, SCSettingOf20 = 0;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	RT_TRACE(
+		_module_hal_init_c_,
+		_drv_info_,
+		(
+			"SCMapping: VHT Case: pHalData->CurrentChannelBW %d, pHalData->nCur80MhzPrimeSC %d, pHalData->nCur40MhzPrimeSC %d\n",
+			pHalData->CurrentChannelBW,
+			pHalData->nCur80MhzPrimeSC,
+			pHalData->nCur40MhzPrimeSC
+		)
+	);
+	if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_80) {
+		if (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+			SCSettingOf40 = VHT_DATA_SC_40_LOWER_OF_80MHZ;
+		else if (pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+			SCSettingOf40 = VHT_DATA_SC_40_UPPER_OF_80MHZ;
+		else
+			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SCMapping: Not Correct Primary40MHz Setting\n"));
+
+		if (
+			(pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) &&
+			(pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+		)
+			SCSettingOf20 = VHT_DATA_SC_20_LOWEST_OF_80MHZ;
+		else if (
+			(pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) &&
+			(pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+		)
+			SCSettingOf20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else if (
+			(pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) &&
+			(pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+		)
+			SCSettingOf20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if (
+			(pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER) &&
+			(pHalData->nCur80MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+		)
+			SCSettingOf20 = VHT_DATA_SC_20_UPPERST_OF_80MHZ;
+		else
+			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SCMapping: Not Correct Primary40MHz Setting\n"));
+	} else if (pHalData->CurrentChannelBW == CHANNEL_WIDTH_40) {
+		RT_TRACE(
+			_module_hal_init_c_,
+			_drv_info_,
+			(
+				"SCMapping: VHT Case: pHalData->CurrentChannelBW %d, pHalData->nCur40MhzPrimeSC %d\n",
+				pHalData->CurrentChannelBW,
+				pHalData->nCur40MhzPrimeSC
+			)
+		);
+
+		if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_UPPER)
+			SCSettingOf20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER)
+			SCSettingOf20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else
+			RT_TRACE(_module_hal_init_c_, _drv_err_, ("SCMapping: Not Correct Primary40MHz Setting\n"));
+	}
+
+	RT_TRACE(_module_hal_init_c_, _drv_info_, ("SCMapping: SC Value %x\n", ((SCSettingOf40 << 4) | SCSettingOf20)));
+	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, rFPGA0_AnalogParameter2, BIT10, 1); */
+
+		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, rFPGA0_AnalogParameter2, BIT10, 0); */
+
+		PHY_SetBBReg(Adapter, 0x818, (BIT26|BIT27), (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+
+		break;
+
+	default:
+		/*RT_TRACE(COMP_DBG, DBG_LOUD, ("phy_SetBWMode8723B(): unknown Bandwidth: %#X\n"\
+					, pHalData->CurrentChannelBW));*/
+		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) {
+		/* RT_TRACE(COMP_MLME, DBG_LOUD, ("phy_SwChnl8723B: return for PSEUDO\n")); */
+		return;
+	}
+	pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff00) | channelToSW);
+	PHY_SetRFReg(padapter, ODM_RF_PATH_A, RF_CHNLBW, 0x3FF, pHalData->RfRegChnlVal[0]);
+	PHY_SetRFReg(padapter, ODM_RF_PATH_B, RF_CHNLBW, 0x3FF, pHalData->RfRegChnlVal[0]);
+
+	DBG_8192C("===>phy_SwChnl8723B: Channel = %d\n", channelToSW);
+}
+
+static void phy_SwChnlAndSetBwMode8723B(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	/* RT_TRACE(COMP_SCAN, DBG_LOUD, ("phy_SwChnlAndSetBwMode8723B(): bSwChnl %d, bSetChnlBW %d\n", pHalData->bSwChnl, pHalData->bSetChnlBW)); */
+	if (Adapter->bNotifyChannelChange) {
+		DBG_871X("[%s] bSwChnl =%d, ch =%d, bSetChnlBW =%d, bw =%d\n",
+			__func__,
+			pHalData->bSwChnl,
+			pHalData->CurrentChannel,
+			pHalData->bSetChnlBW,
+			pHalData->CurrentChannelBW);
+	}
+
+	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;
+
+	/* DBG_871X("=> PHY_HandleSwChnlAndSetBW8812: bSwitchChannel %d, bSetBandWidth %d\n", bSwitchChannel, bSetBandWidth); */
+
+	/* check is swchnl or setbw */
+	if (!bSwitchChannel && !bSetBandWidth) {
+		DBG_871X("PHY_HandleSwChnlAndSetBW8812:  not switch channel and not set bandwidth\n");
+		return;
+	}
+
+	/* skip change for channel or bandwidth is the same */
+	if (bSwitchChannel) {
+		/* if (pHalData->CurrentChannel != ChannelNum) */
+		{
+			if (HAL_IsLegalChannel(Adapter, ChannelNum))
+				pHalData->bSwChnl = true;
+		}
+	}
+
+	if (bSetBandWidth)
+		pHalData->bSetChnlBW = true;
+
+	if (!pHalData->bSetChnlBW && !pHalData->bSwChnl) {
+		/* DBG_871X("<= PHY_HandleSwChnlAndSetBW8812: bSwChnl %d, bSetChnlBW %d\n", pHalData->bSwChnl, pHalData->bSetChnlBW); */
+		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
+)
+{
+	/* DBG_871X("%s() ===>\n", __func__); */
+
+	PHY_HandleSwChnlAndSetBW8723B(Adapter, true, true, channel, Bandwidth, Offset40, Offset80, channel);
+
+	/* DBG_871X("<==%s()\n", __func__); */
+}
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
new file mode 100644
index 000000000000..3a85d0cddfda
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_rf6052.c
@@ -0,0 +1,224 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/******************************************************************************
+ *
+ *
+ * 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, ODM_RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		PHY_SetRFReg(Adapter, ODM_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, ODM_RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		PHY_SetRFReg(Adapter, ODM_RF_PATH_B, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
+		break;
+
+	default:
+		/* RT_TRACE(COMP_DBG, DBG_LOUD, ("PHY_SetRF8225Bandwidth(): unknown Bandwidth: %#X\n", Bandwidth)); */
+		break;
+	}
+
+}
+
+static int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
+{
+	u32 u4RegValue = 0;
+	u8 eRFPath;
+	struct bb_register_def *pPhyReg;
+
+	int rtStatus = _SUCCESS;
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+
+	static char sz8723RadioAFile[] = RTL8723B_PHY_RADIO_A;
+	static char sz8723RadioBFile[] = RTL8723B_PHY_RADIO_B;
+	static s8 sz8723BTxPwrTrackFile[] = RTL8723B_TXPWR_TRACK;
+	char *pszRadioAFile, *pszRadioBFile, *pszTxPwrTrackFile;
+
+	pszRadioAFile = sz8723RadioAFile;
+	pszRadioBFile = sz8723RadioBFile;
+	pszTxPwrTrackFile = sz8723BTxPwrTrackFile;
+
+	/* 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:
+		case RF_PATH_C:
+			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV);
+			break;
+		case RF_PATH_B:
+		case RF_PATH_D:
+			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:
+			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioAFile, eRFPath) == _FAIL)
+			{
+				if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_RADIO, (ODM_RF_RADIO_PATH_E)eRFPath))
+					rtStatus = _FAIL;
+			}
+			break;
+		case RF_PATH_B:
+			if (PHY_ConfigRFWithParaFile(Adapter, pszRadioBFile, eRFPath) == _FAIL)
+			{
+				if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, CONFIG_RF_RADIO, (ODM_RF_RADIO_PATH_E)eRFPath))
+					rtStatus = _FAIL;
+			}
+			break;
+		case RF_PATH_C:
+			break;
+		case RF_PATH_D:
+			break;
+		}
+
+		/*----Restore RFENV control type----*/;
+		switch (eRFPath) {
+		case RF_PATH_A:
+		case RF_PATH_C:
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
+			break;
+		case RF_PATH_B:
+		case RF_PATH_D:
+			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV<<16, u4RegValue);
+			break;
+		}
+
+		if (rtStatus != _SUCCESS) {
+			/* RT_TRACE(COMP_FPGA, DBG_LOUD, ("phy_RF6052_Config_ParaFile():Radio[%d] Fail!!", eRFPath)); */
+			goto phy_RF6052_Config_ParaFile_Fail;
+		}
+
+	}
+
+	/* 3 ----------------------------------------------------------------- */
+	/* 3 Configuration of Tx Power Tracking */
+	/* 3 ----------------------------------------------------------------- */
+
+	if (PHY_ConfigRFWithTxPwrTrackParaFile(Adapter, pszTxPwrTrackFile) == _FAIL)
+	{
+		ODM_ConfigRFWithTxPwrTrackHeaderFile(&pHalData->odmpriv);
+	}
+
+	/* RT_TRACE(COMP_INIT, DBG_LOUD, ("<---phy_RF6052_Config_ParaFile()\n")); */
+	return rtStatus;
+
+phy_RF6052_Config_ParaFile_Fail:
+	return rtStatus;
+}
+
+
+int PHY_RF6052_Config8723B(struct adapter *Adapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);
+	int rtStatus = _SUCCESS;
+
+	/*  */
+	/*  Initialize general global value */
+	/*  */
+	/*  TODO: Extend RF_PATH_C and RF_PATH_D in the future */
+	if (pHalData->rf_type == RF_1T1R)
+		pHalData->NumTotalRFPath = 1;
+	else
+		pHalData->NumTotalRFPath = 2;
+
+	/*  */
+	/*  Config BB and RF */
+	/*  */
+	rtStatus = phy_RF6052_Config_ParaFile(Adapter);
+	return rtStatus;
+
+}
+
+/* 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 000000000000..92e5a0e7aa59
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_rxdesc.c
@@ -0,0 +1,86 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTL8723B_REDESC_C_
+
+#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;
+
+	/* DBG_8192C("process_rssi => pattrib->rssil(%d) signal_strength(%d)\n ", pattrib->RecvSignalPower, pattrib->signal_strength); */
+	/* 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 == NULL || padapter == NULL)
+		return;
+
+	pattrib = &prframe->u.hdr.attrib;
+	signal_stat = &padapter->recvpriv.signal_qual_data;
+
+	/* DBG_8192C("process_link_qual => pattrib->signal_qual(%d)\n ", pattrib->signal_qual); */
+
+	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 = (union recv_frame *)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 000000000000..b002eb446b2c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
@@ -0,0 +1,490 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTL8723BS_RECV_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+
+static s32 initrecvbuf(struct recv_buf *precvbuf, struct adapter *padapter)
+{
+	INIT_LIST_HEAD(&precvbuf->list);
+	spin_lock_init(&precvbuf->recvbuf_lock);
+
+	precvbuf->adapter = padapter;
+
+	return _SUCCESS;
+}
+
+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;
+	PRXREPORT prxreport = (PRXREPORT)&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;
+/* 	DBG_871X("%s: pkt_rpt_type =%d\n", __func__, pattrib->pkt_rpt_type); */
+
+	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 *pHalData = GET_HAL_DATA(padapter);
+	PODM_PHY_INFO_T pPHYInfo = (PODM_PHY_INFO_T)(&pattrib->phy_info);
+
+	u8 *wlanhdr;
+	ODM_PACKET_INFO_T pkt_info;
+	u8 *sa = NULL;
+	/* _irqL		irqL; */
+	struct sta_priv *pstapriv;
+	struct sta_info *psta;
+
+	pkt_info.bPacketMatchBSSID = false;
+	pkt_info.bPacketToSelf = false;
+	pkt_info.bPacketBeacon = false;
+
+
+	wlanhdr = get_recvframe_data(precvframe);
+
+	pkt_info.bPacketMatchBSSID = ((!IsFrameTypeCtrl(wlanhdr)) &&
+		!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_ra(wlanhdr), myid(&padapter->eeprompriv), ETH_ALEN));
+
+	pkt_info.bPacketBeacon = pkt_info.bPacketMatchBSSID && (GetFrameSubType(wlanhdr) == WIFI_BEACON);
+
+	sa = get_ta(wlanhdr);
+
+	pkt_info.StationID = 0xFF;
+
+	pstapriv = &padapter->stapriv;
+	psta = rtw_get_stainfo(pstapriv, sa);
+	if (psta) {
+		pkt_info.StationID = psta->mac_id;
+		/* DBG_8192C("%s ==> StationID(%d)\n", __func__, pkt_info.StationID); */
+	}
+	pkt_info.DataRate = pattrib->data_rate;
+
+	/* rtl8723b_query_rx_phy_status(precvframe, pphy_status); */
+	/* spin_lock_bh(&pHalData->odm_stainfo_lock); */
+	ODM_PhyStatusQuery(&pHalData->odmpriv, pPHYInfo, (u8 *)pphy_status, &(pkt_info));
+	if (psta)
+		psta->rssi = pattrib->phy_info.RecvSignalPower;
+	/* spin_unlock_bh(&pHalData->odm_stainfo_lock); */
+	precvframe->u.hdr.psta = NULL;
+	if (
+		pkt_info.bPacketMatchBSSID &&
+		(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.bPacketToSelf || pkt_info.bPacketBeacon) {
+		if (check_fwstate(&padapter->mlmepriv, WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE) == true)
+			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 *tmpBuf = NULL;
+	u8 res = false;
+
+	if (length == 0)
+		return;
+
+	/* DBG_871X("+%s() length =%d\n", __func__, length); */
+
+	tmpBuf = rtw_zmalloc(length);
+	if (tmpBuf == NULL)
+		return;
+
+	memcpy(tmpBuf, pbuf, length);
+
+	res = rtw_c2h_packet_wk_cmd(padapter, tmpBuf, length);
+
+	if (res == false)
+		kfree(tmpBuf);
+
+	/* DBG_871X("-%s res(%d)\n", __func__, res); */
+
+	return;
+}
+
+static void rtl8723bs_recv_tasklet(void *priv)
+{
+	struct adapter *padapter;
+	struct hal_com_data *pHalData;
+	struct recv_priv *precvpriv;
+	struct recv_buf *precvbuf;
+	union recv_frame *precvframe;
+	struct rx_pkt_attrib *pattrib;
+	u8 *ptr;
+	u32 pkt_offset, skb_len, alloc_sz;
+	_pkt *pkt_copy = NULL;
+	u8 shift_sz = 0, rx_report_sz = 0;
+
+
+	padapter = (struct adapter *)priv;
+	pHalData = GET_HAL_DATA(padapter);
+	precvpriv = &padapter->recvpriv;
+
+	do {
+		precvbuf = rtw_dequeue_recvbuf(&precvpriv->recv_buf_pending_queue);
+		if (NULL == precvbuf)
+			break;
+
+		ptr = precvbuf->pdata;
+
+		while (ptr < precvbuf->ptail) {
+			precvframe = rtw_alloc_recvframe(&precvpriv->free_recv_queue);
+			if (precvframe == NULL) {
+				DBG_8192C("%s: no enough recv frame!\n", __func__);
+				rtw_enqueue_recvbuf_to_head(precvbuf, &precvpriv->recv_buf_pending_queue);
+
+				/*  The case of can't allocte recvframe should be temporary, */
+				/*  schedule again and hope recvframe is available next time. */
+				tasklet_schedule(&precvpriv->recv_tasklet);
+				return;
+			}
+
+			/* rx desc parsing */
+			update_recvframe_attrib(padapter, precvframe, (struct recv_stat *)ptr);
+
+			pattrib = &precvframe->u.hdr.attrib;
+
+			/*  fix Hardware RX data error, drop whole recv_buffer */
+			if ((!(pHalData->ReceiveConfig & RCR_ACRC32)) && pattrib->crc_err) {
+				DBG_8192C("%s()-%d: RX Warning! rx CRC ERROR !!\n", __func__, __LINE__);
+				rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+				break;
+			}
+
+			rx_report_sz = RXDESC_SIZE + pattrib->drvinfo_sz;
+			pkt_offset = rx_report_sz + pattrib->shift_sz + pattrib->pkt_len;
+
+			if ((ptr + pkt_offset) > precvbuf->ptail) {
+				DBG_8192C("%s()-%d: : next pkt len(%p,%d) exceed ptail(%p)!\n", __func__, __LINE__, ptr, pkt_offset, precvbuf->ptail);
+				rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+				break;
+			}
+
+			if ((pattrib->crc_err) || (pattrib->icv_err)) {
+				{
+					DBG_8192C("%s: crc_err =%d icv_err =%d, skip!\n", __func__, 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) {
+					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);
+				} else {
+					if ((pattrib->mfrag == 1) && (pattrib->frag_num == 0)) {
+						DBG_8192C("%s: alloc_skb fail, drop frag frame\n", __func__);
+						rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+						break;
+					}
+
+					precvframe->u.hdr.pkt = rtw_skb_clone(precvbuf->pskb);
+					if (precvframe->u.hdr.pkt) {
+						_pkt *pkt_clone = precvframe->u.hdr.pkt;
+
+						pkt_clone->data = ptr + rx_report_sz + pattrib->shift_sz;
+						skb_reset_tail_pointer(pkt_clone);
+						precvframe->u.hdr.rx_head = precvframe->u.hdr.rx_data = precvframe->u.hdr.rx_tail
+							= pkt_clone->data;
+						precvframe->u.hdr.rx_end =	pkt_clone->data + skb_len;
+					} else {
+						DBG_8192C("%s: rtw_skb_clone fail\n", __func__);
+						rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+						break;
+					}
+				}
+
+				recvframe_put(precvframe, skb_len);
+				/* recvframe_pull(precvframe, drvinfo_sz + RXDESC_SIZE); */
+
+				if (pHalData->ReceiveConfig & RCR_APPFCS)
+					recvframe_pull_tail(precvframe, IEEE80211_FCS_LEN);
+
+				/*  move to drv info position */
+				ptr += RXDESC_SIZE;
+
+				/*  update drv info */
+				if (pHalData->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);
+
+					if (rtw_recv_entry(precvframe) != _SUCCESS) {
+						RT_TRACE(_module_rtl871x_recv_c_, _drv_dump_, ("%s: rtw_recv_entry(precvframe) != _SUCCESS\n", __func__));
+					}
+				} else if (pattrib->pkt_rpt_type == C2H_PACKET) {
+					C2H_EVT_HDR	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 = _RND8(pkt_offset);
+			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 */
+	_rtw_init_queue(&precvpriv->free_recv_buf_queue);
+	_rtw_init_queue(&precvpriv->recv_buf_pending_queue);
+
+	n = NR_RECVBUFF * sizeof(struct recv_buf) + 4;
+	precvpriv->pallocated_recv_buf = rtw_zmalloc(n);
+	if (precvpriv->pallocated_recv_buf == NULL) {
+		res = _FAIL;
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("alloc recv_buf fail!\n"));
+		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++) {
+		res = initrecvbuf(precvbuf, padapter);
+		if (res == _FAIL)
+			break;
+
+		if (precvbuf->pskb == NULL) {
+			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));
+			}
+
+			if (precvbuf->pskb == NULL) {
+				DBG_871X("%s: alloc_skb fail!\n", __func__);
+			}
+		}
+
+		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_init(
+		&precvpriv->recv_tasklet,
+		(void(*)(unsigned long))rtl8723bs_recv_tasklet,
+		(unsigned long)padapter
+	);
+
+	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;
+	}
+
+	if (precvpriv->pallocated_recv_buf) {
+		n = NR_RECVBUFF * sizeof(struct recv_buf) + 4;
+		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, n;
+	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) {
+		n = NR_RECVBUFF;
+		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;
+	}
+
+	if (precvpriv->pallocated_recv_buf) {
+		n = NR_RECVBUFF * sizeof(struct recv_buf) + 4;
+		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 000000000000..ca6ad9659b09
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_xmit.c
@@ -0,0 +1,686 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RTL8723BS_XMIT_C_
+
+#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 == true) ||
+			(padapter->bDriverStopped == true)
+		) {
+			DBG_871X("%s: bSurpriseRemoved or bDriverStopped (wait TxOQT)\n", __func__);
+			return false;
+		}
+
+		HalQueryTxOQTBufferStatus8723BSdio(padapter);
+
+		if ((++n % 60) == 0) {
+			if ((n % 300) == 0) {
+				DBG_871X("%s(%d): QOT free space(%d), agg_num: %d\n",
+				__func__, n, pHalData->SdioTxOQTFreeSpace, agg_num);
+			}
+			msleep(1);
+			/* yield(); */
+		}
+	}
+
+	pHalData->SdioTxOQTFreeSpace -= agg_num;
+
+	/* if (n > 1) */
+	/* 	++priv->pshare->nr_out_of_txoqt_space; */
+
+	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 (true == ret)
+		pxmitbuf = dequeue_pending_xmitbuf_under_survey(pxmitpriv);
+	else
+		pxmitbuf = dequeue_pending_xmitbuf(pxmitpriv);
+
+	if (pxmitbuf == NULL)
+		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 (false == 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 == true) ||
+		(padapter->bDriverStopped == true)
+	) {
+		RT_TRACE(
+			_module_hal_xmit_c_,
+			_drv_notice_,
+			("%s: bSurpriseRemoved(wirte port)\n", __func__)
+		);
+		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);
+
+#ifdef CONFIG_SDIO_TX_TASKLET
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+#endif
+
+	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 (down_interruptible(&pxmitpriv->xmit_sema)) {
+		DBG_871X_LEVEL(_drv_emerg_, "%s: down SdioXmitBufSema fail!\n", __func__);
+		return _FAIL;
+	}
+
+	ret = (padapter->bDriverStopped == true) || (padapter->bSurpriseRemoved == true);
+	if (ret) {
+		RT_TRACE(
+			_module_hal_xmit_c_,
+			_drv_err_,
+			(
+				"%s: bDriverStopped(%d) bSurpriseRemoved(%d)!\n",
+				__func__,
+				padapter->bDriverStopped,
+				padapter->bSurpriseRemoved
+			)
+		);
+		return _FAIL;
+	}
+
+	queue_pending = check_pending_xmitbuf(pxmitpriv);
+
+	if (queue_pending == false)
+		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 no_res, idx, hwentry;
+	struct tx_servq *ptxservq;
+	struct list_head *sta_plist, *sta_phead, *frame_plist, *frame_phead;
+	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;
+	no_res = false;
+	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) == true) &&
+			(padapter->mlmepriv.LinkDetectInfo.bHigherBusyTxTraffic == true)
+		) {
+			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);
+		sta_plist = get_next(sta_phead);
+		/* because stop_sta_xmit may delete sta_plist at any time */
+		/* so we should add lock here, or while loop can not exit */
+		while (sta_phead != sta_plist) {
+			ptxservq = LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending);
+			sta_plist = get_next(sta_plist);
+
+#ifdef DBG_XMIT_BUF
+			DBG_871X(
+				"%s idx:%d hwxmit_pkt_num:%d ptxservq_pkt_num:%d\n",
+				__func__,
+				idx,
+				phwxmit->accnt,
+				ptxservq->qcnt
+			);
+			DBG_871X(
+				"%s free_xmit_extbuf_cnt =%d free_xmitbuf_cnt =%d free_xmitframe_cnt =%d\n",
+				__func__,
+				pxmitpriv->free_xmit_extbuf_cnt,
+				pxmitpriv->free_xmitbuf_cnt,
+				pxmitpriv->free_xmitframe_cnt
+			);
+#endif
+			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 = LIST_CONTAINOR(frame_plist, struct xmit_frame, list);
+
+				/*  check xmit_buf size enough or not */
+				txlen = txdesc_size + rtw_wlan_pkt_size(pxmitframe);
+				if (
+					(NULL == 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 == NULL) {
+#ifdef DBG_XMIT_BUF
+						DBG_871X_LEVEL(_drv_err_, "%s: xmit_buf is not enough!\n", __func__);
+#endif
+						err = -2;
+						up(&(pxmitpriv->xmit_sema));
+						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)
+					) {
+						DBG_871X(
+							"%s: one not triggered pkt in queue when this STA sleep,"
+							" break and goto next sta\n",
+							__func__
+						);
+						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) {
+					DBG_871X_LEVEL(_drv_err_, "%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;
+				    /* if (k != 1) */
+					/* 	((struct xmit_frame*)pxmitbuf->priv_data)->pg_num += pxmitframe->pg_num; */
+					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) {
+			RT_TRACE(_module_hal_xmit_c_, _drv_info_, ("pxmitbuf->len =%d enqueue\n", pxmitbuf->len));
+
+			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 (down_interruptible(&pxmitpriv->SdioXmitSema)) {
+		DBG_871X_LEVEL(_drv_emerg_, "%s: down sema fail!\n", __func__);
+		return _FAIL;
+	}
+
+next:
+	if (
+		(padapter->bDriverStopped == true) ||
+		(padapter->bSurpriseRemoved == true)
+	) {
+		RT_TRACE(
+			_module_hal_xmit_c_,
+			_drv_notice_,
+			(
+				"%s: bDriverStopped(%d) bSurpriseRemoved(%d)\n",
+				__func__,
+				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] = "RTWHALXT";
+
+
+	ret = _SUCCESS;
+	padapter = (struct adapter *)context;
+	pxmitpriv = &padapter->xmitpriv;
+
+	rtw_sprintf(thread_name, 20, "%s-"ADPT_FMT, thread_name, ADPT_ARG(padapter));
+	thread_enter(thread_name);
+
+	DBG_871X("start "FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	/*  For now, no one would down sema to check thread is running, */
+	/*  so mark this temporary, Lucas@20130820 */
+/* 	up(&pxmitpriv->SdioXmitTerminateSema); */
+
+	do {
+		ret = rtl8723bs_xmit_handler(padapter);
+		if (signal_pending(current)) {
+			flush_signals(current);
+		}
+	} while (_SUCCESS == ret);
+
+	up(&pxmitpriv->SdioXmitTerminateSema);
+
+	RT_TRACE(_module_hal_xmit_c_, _drv_notice_, ("-%s\n", __func__));
+
+	thread_exit();
+}
+
+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;
+
+	RT_TRACE(_module_hal_xmit_c_, _drv_info_, ("+%s\n", __func__));
+
+	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 == true)
+			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) {
+		RT_TRACE(_module_hal_xmit_c_, _drv_err_, ("rtl8723bs_hal_xmit: enqueue xmitframe fail\n"));
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		pxmitpriv->tx_drop++;
+		return true;
+	}
+
+	up(&pxmitpriv->SdioXmitSema);
+
+	return false;
+}
+
+s32	rtl8723bs_hal_xmitframe_enqueue(
+	struct adapter *padapter, struct xmit_frame *pxmitframe
+)
+{
+	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
+	s32 err;
+
+	if ((err = rtw_xmitframe_enqueue(padapter, pxmitframe)) != _SUCCESS) {
+		rtw_free_xmitframe(pxmitpriv, pxmitframe);
+
+		pxmitpriv->tx_drop++;
+	} else {
+#ifdef CONFIG_SDIO_TX_TASKLET
+		tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+#else
+		up(&pxmitpriv->SdioXmitSema);
+#endif
+	}
+
+	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);
+	sema_init(&xmitpriv->SdioXmitSema, 0);
+	sema_init(&xmitpriv->SdioXmitTerminateSema, 0);
+
+	return _SUCCESS;
+}
+
+void rtl8723bs_free_xmit_priv(struct adapter *padapter)
+{
+	struct hal_com_data *phal;
+	struct xmit_priv *pxmitpriv;
+	struct xmit_buf *pxmitbuf;
+	struct __queue *pqueue;
+	struct list_head *plist, *phead;
+	struct list_head tmplist;
+
+
+	phal = GET_HAL_DATA(padapter);
+	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 = LIST_CONTAINOR(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 000000000000..6dfb06a49d41
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
@@ -0,0 +1,1928 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _SDIO_HALINIT_C_
+
+#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 == false) {
+		/*  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;
+}
+
+#ifdef CONFIG_GPIO_WAKEUP
+/* we set it high under init and fw will */
+/* give us Low Pulse when host wake up */
+void HostWakeUpGpioClear(struct adapter *Adapter)
+{
+	u32 value32;
+
+	value32 = rtw_read32(Adapter, REG_GPIO_PIN_CTRL_2);
+
+	/* set GPIO 12 1 */
+	value32 |= BIT(12);/* 4+8 */
+	/* GPIO 12 out put */
+	value32 |= BIT(20);/* 4+16 */
+
+	rtw_write32(Adapter, REG_GPIO_PIN_CTRL_2, value32);
+} /* HostWakeUpGpioClear */
+
+void HalSetOutPutGPIO(struct adapter *padapter, u8 index, u8 OutPutValue)
+{
+	if (index <= 7) {
+		/* config GPIO mode */
+		rtw_write8(padapter, REG_GPIO_PIN_CTRL + 3, rtw_read8(padapter, REG_GPIO_PIN_CTRL + 3) & ~BIT(index));
+
+		/* config GPIO Sel */
+		/* 0: input */
+		/* 1: output */
+		rtw_write8(padapter, REG_GPIO_PIN_CTRL + 2, rtw_read8(padapter, REG_GPIO_PIN_CTRL + 2) | BIT(index));
+
+		/* set output value */
+		if (OutPutValue)
+			rtw_write8(padapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(padapter, REG_GPIO_PIN_CTRL + 1) | BIT(index));
+		else
+			rtw_write8(padapter, REG_GPIO_PIN_CTRL + 1, rtw_read8(padapter, REG_GPIO_PIN_CTRL + 1) & ~BIT(index));
+	} else {
+		/* 88C Series: */
+		/* index: 11~8 transform to 3~0 */
+		/* 8723 Series: */
+		/* index: 12~8 transform to 4~0 */
+		index -= 8;
+
+		/* config GPIO mode */
+		rtw_write8(padapter, REG_GPIO_PIN_CTRL_2 + 3, rtw_read8(padapter, REG_GPIO_PIN_CTRL_2 + 3) & ~BIT(index));
+
+		/* config GPIO Sel */
+		/* 0: input */
+		/* 1: output */
+		rtw_write8(padapter, REG_GPIO_PIN_CTRL_2 + 2, rtw_read8(padapter, REG_GPIO_PIN_CTRL_2 + 2) | BIT(index));
+
+		/* set output value */
+		if (OutPutValue)
+			rtw_write8(padapter, REG_GPIO_PIN_CTRL_2 + 1, rtw_read8(padapter, REG_GPIO_PIN_CTRL_2 + 1) | BIT(index));
+		else
+			rtw_write8(padapter, REG_GPIO_PIN_CTRL_2 + 1, rtw_read8(padapter, REG_GPIO_PIN_CTRL_2 + 1) & ~BIT(index));
+	}
+}
+#endif
+
+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 */
+#ifdef CONFIG_EXT_CLK
+	/*  Use external crystal(XTAL) */
+	value8 = rtw_read8(padapter, REG_PAD_CTRL1_8723B+2);
+	value8 |=  BIT(7);
+	rtw_write8(padapter, REG_PAD_CTRL1_8723B+2, value8);
+
+	/*  CLK_REQ High active or Low Active */
+	/*  Request GPIO polarity: */
+	/*  0: low active */
+	/*  1: high active */
+	value8 = rtw_read8(padapter, REG_MULTI_FUNC_CTRL+1);
+	value8 |= BIT(5);
+	rtw_write8(padapter, REG_MULTI_FUNC_CTRL+1, value8);
+#endif /*  CONFIG_EXT_CLK */
+
+	/*  only cmd52 can be used before power on(card enable) */
+	ret = CardEnable(padapter);
+	if (ret == false) {
+		RT_TRACE(
+			_module_hci_hal_init_c_,
+			_drv_emerg_,
+			("%s: run power on flow fail\n", __func__)
+		);
+		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);
+
+	rtw_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);
+/* 	DBG_8192C("%s: REG_PWR_DATA(0x%x) = 0x%04X\n", __func__, REG_PWR_DATA, rtw_read16(padapter, REG_PWR_DATA)); */
+
+	value32 = rtw_read32(padapter, REG_LEDCFG0);
+	value32 |= BIT(23); /*  DPDT_SEL_EN, 1 for SW control */
+	rtw_write32(padapter, REG_LEDCFG0, value32);
+/* 	DBG_8192C("%s: REG_LEDCFG0(0x%x) = 0x%08X\n", __func__, REG_LEDCFG0, rtw_read32(padapter, REG_LEDCFG0)); */
+
+	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);
+/* 	DBG_8192C("%s: REG_PAD_CTRL1(0x%x) = 0x%02X\n", __func__, REG_PAD_CTRL1_8723B, rtw_read8(padapter, REG_PAD_CTRL1_8723B)); */
+
+#ifdef CONFIG_GPIO_WAKEUP
+	HostWakeUpGpioClear(padapter);
+#endif
+
+	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);
+	DBG_8192C("%s(): Enable Tx FIFO Page Threshold H:0x%x, N:0x%x, L:0x%x\n", __func__, HQ_threshold, NQ_threshold, 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 writting 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:
+		/* RT_ASSERT(false, ("Shall not reach here!\n")); */
+		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:
+		/* RT_ASSERT(false, ("Shall not reach here!\n")); */
+		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 _InitNormalChipQueuePriority(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:
+		/* RT_ASSERT(false, ("Shall not reach here!\n")); */
+		break;
+	}
+
+
+}
+
+static void _InitQueuePriority(struct adapter *padapter)
+{
+	_InitNormalChipQueuePriority(padapter);
+}
+
+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)
+{
+	struct registry_priv *pregistrypriv;
+	u8 valueDMATimeout;
+	u8 valueDMAPageCount;
+
+
+	pregistrypriv = &padapter->registrypriv;
+
+	if (pregistrypriv->wifi_spec) {
+		/*  2010.04.27 hpfan */
+		/*  Adjust RxAggrTimeout to close to zero disable RxAggr, suggested by designer */
+		/*  Timeout value is calculated by 34 / (2^n) */
+		valueDMATimeout = 0x06;
+		valueDMAPageCount = 0x06;
+	} else {
+		/*  20130530, Isaac@SD1 suggest 3 kinds of parameter */
+		/*  TX/RX Balance */
+		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)
+{
+	struct hal_com_data *pHalData;
+	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... */
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	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 hal_com_data *pHalData;
+	struct mlme_ext_priv *pmlmeext;
+	u8 regBwOpMode = 0;
+	u32 regRATR = 0, regRRSR = 0;
+
+	pHalData = GET_HAL_DATA(padapter);
+	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;
+		regRATR = RATE_ALL_CCK;
+		regRRSR = RATE_ALL_CCK;
+		break;
+	case WIRELESS_MODE_A:
+/* 			RT_ASSERT(false, ("Error wireless a mode\n")); */
+		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:
+/* 			RT_ASSERT(false, ("Error wireless mode")); */
+		regBwOpMode = BW_OPMODE_5G;
+		regRATR = RATE_ALL_OFDM_AG | RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
+		regRRSR = RATE_ALL_OFDM_AG;
+		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);
+
+#if	DISABLE_BB_RF
+	pHalData->rf_chip	= RF_PSEUDO_11N;
+	return;
+#endif
+
+	pHalData->rf_chip	= RF_6052;
+
+	pHalData->rf_type = RF_1T1R;
+	DBG_8192C("Set RF Chip ID to RF_6052 and RF type to 1T1R.\n");
+}
+
+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;
+
+	DBG_8192C("HalDetectPwrDownMode(): PDN =%d\n", pHalData->pwrdown);
+
+	return pHalData->pwrdown;
+}	/*  HalDetectPwrDownMode */
+
+static u32 rtl8723bs_hal_init(struct adapter *padapter)
+{
+	s32 ret;
+	struct hal_com_data *pHalData;
+	struct pwrctrl_priv *pwrctrlpriv;
+	struct registry_priv *pregistrypriv;
+	u32 NavUpper = WiFiNavUpperUs;
+	u8 u1bTmp;
+
+	pHalData = GET_HAL_DATA(padapter);
+	pwrctrlpriv = adapter_to_pwrctl(padapter);
+	pregistrypriv = &padapter->registrypriv;
+
+	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;
+
+		DBG_871X("%s: Leaving IPS in FWLPS state\n", __func__);
+
+		/* 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);
+		DBG_871X("%s: write rpwm =%02x\n", __func__, 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) {
+				DBG_871X("%s: polling cpwm timeout when leaving IPS in FWLPS state\n", __func__);
+				break;
+			}
+		} while (1);
+
+		rtl8723b_set_FwPwrModeInIPS_cmd(padapter, 0);
+
+		rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+
+		rtw_btcoex_HAL_Initialize(padapter, false);
+
+		return _SUCCESS;
+	}
+
+#ifdef CONFIG_WOWLAN
+	if (rtw_read8(padapter, REG_MCUFWDL)&BIT7) {
+		u8 reg_val = 0;
+		DBG_871X("+Reset Entry+\n");
+		rtw_write8(padapter, REG_MCUFWDL, 0x00);
+		_8051Reset8723(padapter);
+		/* reset BB */
+		reg_val = rtw_read8(padapter, REG_SYS_FUNC_EN);
+		reg_val &= ~(BIT(0) | BIT(1));
+		rtw_write8(padapter, REG_SYS_FUNC_EN, reg_val);
+		/* reset RF */
+		rtw_write8(padapter, REG_RF_CTRL, 0);
+		/* reset TRX path */
+		rtw_write16(padapter, REG_CR, 0);
+		/* reset MAC, Digital Core */
+		reg_val = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		reg_val &= ~(BIT(4) | BIT(7));
+		rtw_write8(padapter, REG_SYS_FUNC_EN+1, reg_val);
+		reg_val = rtw_read8(padapter, REG_SYS_FUNC_EN+1);
+		reg_val |= BIT(4) | BIT(7);
+		rtw_write8(padapter, REG_SYS_FUNC_EN+1, reg_val);
+		DBG_871X("-Reset Entry-\n");
+	}
+#endif /* CONFIG_WOWLAN */
+	/*  Disable Interrupt first. */
+/* 	rtw_hal_disable_interrupt(padapter); */
+
+	ret = _InitPowerOn_8723BS(padapter);
+	if (_FAIL == ret) {
+		RT_TRACE(_module_hci_hal_init_c_, _drv_err_, ("Failed to init Power On!\n"));
+		return _FAIL;
+	}
+
+	rtw_write8(padapter, REG_EARLY_MODE_CONTROL, 0);
+
+	ret = rtl8723b_FirmwareDownload(padapter, false);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_hci_hal_init_c_, _drv_err_, ("%s: Download Firmware failed!!\n", __func__));
+		padapter->bFWReady = false;
+		pHalData->fw_ractrl = false;
+		return ret;
+	} else {
+		RT_TRACE(_module_hci_hal_init_c_, _drv_notice_, ("rtl8723bs_hal_init(): Download Firmware Success!!\n"));
+		padapter->bFWReady = true;
+		pHalData->fw_ractrl = true;
+	}
+
+	rtl8723b_InitializeFirmwareVars(padapter);
+
+/* 	SIC_Init(padapter); */
+
+	if (pwrctrlpriv->reg_rfoff == true)
+		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;
+
+#if (HAL_MAC_ENABLE == 1)
+	ret = PHY_MACConfig8723B(padapter);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Initializepadapter8192CSdio(): Fail to configure MAC!!\n"));
+		return ret;
+	}
+#endif
+	/*  */
+	/* d. Initialize BB related configurations. */
+	/*  */
+#if (HAL_BB_ENABLE == 1)
+	ret = PHY_BBConfig8723B(padapter);
+	if (ret != _SUCCESS) {
+		RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Initializepadapter8192CSdio(): Fail to configure BB!!\n"));
+		return ret;
+	}
+#endif
+
+	/*  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) */
+	{
+#if (HAL_RF_ENABLE == 1)
+		ret = PHY_RFConfig8723B(padapter);
+		if (ret != _SUCCESS) {
+			RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Initializepadapter8192CSdio(): Fail to configure RF!!\n"));
+			return ret;
+		}
+#endif
+	}
+
+	/*  */
+	/*  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 (_SUCCESS != ret) {
+		DBG_8192C("%s: Failed to init LLT Table!\n", __func__);
+		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);
+
+	/* DbgPrint("pHalData->DefaultTxPwrDbm = %d\n", pHalData->DefaultTxPwrDbm); */
+
+	/*  */
+	/*  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);
+
+			rtw_btcoex_IQKNotify(padapter, true);
+
+			restore_iqk_rst = (pwrpriv->bips_processing == true) ? true : false;
+			b2Ant = pHalData->EEPROMBluetoothAntNum == Ant_x2 ? true : false;
+			PHY_IQCalibrate_8723B(padapter, false, restore_iqk_rst, b2Ant, pHalData->ant_path);
+			pHalData->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
+
+			rtw_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. */
+	rtw_btcoex_HAL_Initialize(padapter, false);
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("-%s\n", __func__));
+
+	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;
+	u8 ret = _FAIL;
+
+	/*  Run LPS WL RFOFF flow */
+	ret = HalPwrSeqCmdParsing(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, rtl8723B_enter_lps_flow);
+	if (ret == _FAIL) {
+		DBG_8192C(KERN_ERR "%s: run RF OFF flow fail!\n", __func__);
+	}
+
+	/* 	==== 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 */
+	ret = false;
+	rtw_hal_set_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	ret = HalPwrSeqCmdParsing(padapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, rtl8723B_card_disable_flow);
+	if (ret == false) {
+		DBG_8192C(KERN_ERR "%s: run CARD DISABLE flow fail!\n", __func__);
+	}
+}
+
+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 == true) {
+		if (adapter_to_pwrctl(padapter)->bips_processing == true) {
+			if (padapter->netif_up == true) {
+				int cnt = 0;
+				u8 val8 = 0;
+
+				DBG_871X("%s: issue H2C to FW when entering IPS\n", __func__);
+
+				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++;
+					DBG_871X("%s  polling REG_HMETFR = 0x%x, cnt =%d\n", __func__, val8, 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);
+					DBG_871X("%s: write rpwm =%02x\n", __func__, val8);
+					adapter_to_pwrctl(padapter)->tog = (val8 + 0x80) & 0x80;
+					cnt = val8 = 0;
+					do {
+						val8 = rtw_read8(padapter, REG_CR);
+						cnt++;
+						DBG_871X("%s  polling 0x100 = 0x%x, cnt =%d\n", __func__, val8, cnt);
+						mdelay(10);
+					} while (cnt < 100 && (val8 != 0xEA));
+				} else {
+					DBG_871X(
+						"MAC_1C0 =%08x, MAC_1C4 =%08x, MAC_1C8 =%08x, MAC_1CC =%08x\n",
+						rtw_read32(padapter, 0x1c0),
+						rtw_read32(padapter, 0x1c4),
+						rtw_read32(padapter, 0x1c8),
+						rtw_read32(padapter, 0x1cc)
+					);
+				}
+
+				DBG_871X(
+					"polling done when entering IPS, check result : 0x100 = 0x%x, cnt =%d, MAC_1cc = 0x%02x\n",
+					rtw_read8(padapter, REG_CR),
+					cnt,
+					rtw_read8(padapter, REG_HMETFR)
+				);
+
+				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)
+{
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+rtl8723bs_inirp_deinit\n"));
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("-rtl8723bs_inirp_deinit\n"));
+
+	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);
+
+#if DISABLE_BB_RF
+	pHalData->rf_chip = RF_PSEUDO_11N;
+#else
+	pHalData->rf_chip = RF_6052;
+#endif
+}
+
+
+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);
+	}
+/* 	NicIFSetMacAddress(padapter, padapter->PermanentAddress); */
+
+	RT_TRACE(
+		_module_hci_hal_init_c_,
+		_drv_notice_,
+		(
+			"Hal_EfuseParseMACAddr_8723BS: Permanent Address = %02x-%02x-%02x-%02x-%02x-%02x\n",
+			pEEPROM->mac_addr[0],
+			pEEPROM->mac_addr[1],
+			pEEPROM->mac_addr[2],
+			pEEPROM->mac_addr[3],
+			pEEPROM->mac_addr[4],
+			pEEPROM->mac_addr[5]
+		)
+	);
+}
+
+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;
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("Board Type: 0x%2x\n", pHalData->BoardType));
+}
+
+static void _ReadEfuseInfo8723BS(struct adapter *padapter)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
+	u8 *hwinfo = NULL;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("====>_ReadEfuseInfo8723BS()\n"));
+
+	/*  */
+	/*  This part read and parse the eeprom/efuse content */
+	/*  */
+
+	if (sizeof(pEEPROM->efuse_eeprom_data) < HWSET_MAX_SIZE_8723B)
+		DBG_871X("[WARNING] size of efuse_eeprom_data is less than HWSET_MAX_SIZE_8723B!\n");
+
+	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);
+
+#ifdef CONFIG_WOWLAN
+	Hal_DetectWoWMode(padapter);
+#endif
+
+	Hal_ReadRFGainOffset(padapter, hwinfo, pEEPROM->bautoload_fail_flag);
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("<==== _ReadEfuseInfo8723BS()\n"));
+}
+
+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;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_,
+		 ("%s: 9346CR = 0x%02X, Boot from %s, Autoload %s\n",
+		  __func__, eeValue,
+		  (pEEPROM->EepromOrEfuse ? "EEPROM" : "EFUSE"),
+		  (pEEPROM->bautoload_fail_flag ? "Fail" : "OK")));
+
+/* 	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;
+	unsigned long start;
+
+	RT_TRACE(_module_hci_hal_init_c_, _drv_info_, ("+_ReadAdapterInfo8723BS\n"));
+
+	/*  before access eFuse, make sure card enable has been called */
+	if (padapter->hw_init_completed == false)
+		_InitPowerOn_8723BS(padapter);
+
+
+	val8 = rtw_read8(padapter, 0x4e);
+	MSG_8192C("%s, 0x4e = 0x%x\n", __func__, val8);
+	val8 |= BIT(6);
+	rtw_write8(padapter, 0x4e, val8);
+
+
+	start = jiffies;
+
+	_EfuseCellSel(padapter);
+	_ReadRFType(padapter);
+	_ReadPROMContent(padapter);
+	_InitOtherVariable(padapter);
+
+	if (padapter->hw_init_completed == false) {
+		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 */
+	}
+
+
+	MSG_8192C("<==== _ReadAdapterInfo8723BS in %d ms\n", jiffies_to_msecs(jiffies - start));
+
+	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)
+{
+	struct hal_com_data *pHalData;
+	u8 val8;
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	struct wowlan_ioctl_param *poidparam;
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+	int res;
+	u32 tmp;
+	u16 len = 0;
+	u8 trycnt = 100;
+	u32 himr = 0;
+#if defined(CONFIG_WOWLAN)
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct sta_info *psta = NULL;
+	u64 iv_low = 0, iv_high = 0;
+	u8 mstatus = (*(u8 *)val);
+#endif
+#endif
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	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;
+
+#ifdef CONFIG_WOWLAN
+	case HW_VAR_WOWLAN:
+	{
+		poidparam = (struct wowlan_ioctl_param *)val;
+		switch (poidparam->subcode) {
+		case WOWLAN_ENABLE:
+			DBG_871X_LEVEL(_drv_always_, "WOWLAN_ENABLE\n");
+
+			/* backup data rate to register 0x8b for wowlan FW */
+			rtw_write8(padapter, 0x8d, 1);
+			rtw_write8(padapter, 0x8c, 0);
+			rtw_write8(padapter, 0x8f, 0x40);
+			rtw_write8(padapter, 0x8b,
+			rtw_read8(padapter, 0x2f0));
+
+			/*  1. Download WOWLAN FW */
+			DBG_871X_LEVEL(_drv_always_, "Re-download WoWlan FW!\n");
+			SetFwRelatedForWoWLAN8723b(padapter, true);
+
+			/*  2. RX DMA stop */
+			DBG_871X_LEVEL(_drv_always_, "Pause DMA\n");
+			rtw_write32(padapter, REG_RXPKT_NUM, (rtw_read32(padapter, REG_RXPKT_NUM)|RW_RELEASE_EN));
+			do {
+				if ((rtw_read32(padapter, REG_RXPKT_NUM)&RXDMA_IDLE)) {
+					DBG_871X_LEVEL(_drv_always_, "RX_DMA_IDLE is true\n");
+					break;
+				} else {
+					/*  If RX_DMA is not idle, receive one pkt from DMA */
+					res = sdio_local_read(padapter, SDIO_REG_RX0_REQ_LEN, 4, (u8 *)&tmp);
+					len = le16_to_cpu(tmp);
+					DBG_871X_LEVEL(_drv_always_, "RX len:%d\n", len);
+					if (len > 0)
+						res = RecvOnePkt(padapter, len);
+					else
+						DBG_871X_LEVEL(_drv_always_, "read length fail %d\n", len);
+
+					DBG_871X_LEVEL(_drv_always_, "RecvOnePkt Result: %d\n", res);
+				}
+			} while (trycnt--);
+			if (trycnt == 0)
+				DBG_871X_LEVEL(_drv_always_, "Stop RX DMA failed......\n");
+
+			/*  3. Clear IMR and ISR */
+			DBG_871X_LEVEL(_drv_always_, "Clear IMR and ISR\n");
+			tmp = 0;
+			sdio_local_write(padapter, SDIO_REG_HIMR_ON, 4, (u8 *)&tmp);
+			sdio_local_write(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			sdio_local_read(padapter, SDIO_REG_HISR, 4, (u8 *)&tmp);
+			sdio_local_write(padapter, SDIO_REG_HISR, 4, (u8 *)&tmp);
+
+			/*  4. Enable CPWM2 only */
+			DBG_871X_LEVEL(_drv_always_, "Enable only CPWM2\n");
+			sdio_local_read(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			DBG_871X("DisableInterruptButCpwm28723BSdio(): Read SDIO_REG_HIMR: 0x%08x\n", tmp);
+
+			himr = cpu_to_le32(SDIO_HIMR_DISABLED)|SDIO_HIMR_CPWM2_MSK;
+			sdio_local_write(padapter, SDIO_REG_HIMR, 4, (u8 *)&himr);
+
+			sdio_local_read(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			DBG_871X("DisableInterruptButCpwm28723BSdio(): Read again SDIO_REG_HIMR: 0x%08x\n", tmp);
+
+			/*  5. Set Enable WOWLAN H2C command. */
+			DBG_871X_LEVEL(_drv_always_, "Set Enable WOWLan cmd\n");
+			rtl8723b_set_wowlan_cmd(padapter, 1);
+
+			/*  6. Check EnableWoWlan CMD is ready */
+			if (!pwrctl->wowlan_pno_enable) {
+				DBG_871X_LEVEL(_drv_always_, "Check EnableWoWlan CMD is ready\n");
+				mstatus = rtw_read8(padapter, REG_WOW_CTRL);
+				trycnt = 10;
+				while (!(mstatus&BIT1) && trycnt > 1) {
+					mstatus = rtw_read8(padapter, REG_WOW_CTRL);
+					DBG_871X("Loop index: %d :0x%02x\n", trycnt, mstatus);
+					trycnt--;
+					msleep(2);
+				}
+			}
+			break;
+
+		case WOWLAN_DISABLE:
+			DBG_871X_LEVEL(_drv_always_, "WOWLAN_DISABLE\n");
+
+			psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(pmlmepriv));
+			if (psta != NULL)
+				rtl8723b_set_FwMediaStatusRpt_cmd(padapter, RT_MEDIA_DISCONNECT, psta->mac_id);
+			else
+				DBG_871X("psta is null\n");
+
+			/*  1. Read wakeup reason */
+			pwrctl->wowlan_wake_reason = rtw_read8(padapter, REG_WOWLAN_WAKE_REASON);
+			DBG_871X_LEVEL(
+				_drv_always_,
+				"wakeup_reason: 0x%02x, mac_630 = 0x%08x, mac_634 = 0x%08x, mac_1c0 = 0x%08x, mac_1c4 = 0x%08x"
+				", mac_494 = 0x%08x, , mac_498 = 0x%08x, mac_49c = 0x%08x, mac_608 = 0x%08x, mac_4a0 = 0x%08x, mac_4a4 = 0x%08x\n"
+				", mac_1cc = 0x%08x, mac_2f0 = 0x%08x, mac_2f4 = 0x%08x, mac_2f8 = 0x%08x, mac_2fc = 0x%08x, mac_8c = 0x%08x",
+				pwrctl->wowlan_wake_reason,
+				rtw_read32(padapter, REG_WOWLAN_GTK_DBG1),
+				rtw_read32(padapter, REG_WOWLAN_GTK_DBG2),
+				rtw_read32(padapter, 0x1c0),
+				rtw_read32(padapter, 0x1c4),
+				rtw_read32(padapter, 0x494),
+				rtw_read32(padapter, 0x498),
+				rtw_read32(padapter, 0x49c),
+				rtw_read32(padapter, 0x608),
+				rtw_read32(padapter, 0x4a0),
+				rtw_read32(padapter, 0x4a4),
+				rtw_read32(padapter, 0x1cc),
+				rtw_read32(padapter, 0x2f0),
+				rtw_read32(padapter, 0x2f4),
+				rtw_read32(padapter, 0x2f8),
+				rtw_read32(padapter, 0x2fc),
+				rtw_read32(padapter, 0x8c)
+			);
+#ifdef CONFIG_PNO_SET_DEBUG
+			DBG_871X("0x1b9: 0x%02x, 0x632: 0x%02x\n", rtw_read8(padapter, 0x1b9), rtw_read8(padapter, 0x632));
+			DBG_871X("0x4fc: 0x%02x, 0x4fd: 0x%02x\n", rtw_read8(padapter, 0x4fc), rtw_read8(padapter, 0x4fd));
+			DBG_871X("TXDMA STATUS: 0x%08x\n", rtw_read32(padapter, REG_TXDMA_STATUS));
+#endif
+
+			{
+				/*  2.  Set Disable WOWLAN H2C command. */
+				DBG_871X_LEVEL(_drv_always_, "Set Disable WOWLan cmd\n");
+				rtl8723b_set_wowlan_cmd(padapter, 0);
+
+				/*  3. Check Disable WoWlan CMD ready. */
+				DBG_871X_LEVEL(_drv_always_, "Check DisableWoWlan CMD is ready\n");
+				mstatus = rtw_read8(padapter, REG_WOW_CTRL);
+				trycnt = 50;
+				while (mstatus&BIT1 && trycnt > 1) {
+					mstatus = rtw_read8(padapter, REG_WOW_CTRL);
+					DBG_871X_LEVEL(_drv_always_, "Loop index: %d :0x%02x\n", trycnt, mstatus);
+					trycnt--;
+					msleep(10);
+				}
+
+				if (mstatus & BIT1) {
+					DBG_871X_LEVEL(_drv_always_, "Disable WOW mode fail!!\n");
+					DBG_871X("Set 0x690 = 0x00\n");
+					rtw_write8(padapter, REG_WOW_CTRL, (rtw_read8(padapter, REG_WOW_CTRL)&0xf0));
+					DBG_871X_LEVEL(_drv_always_, "Release RXDMA\n");
+					rtw_write32(padapter, REG_RXPKT_NUM, (rtw_read32(padapter, REG_RXPKT_NUM)&(~RW_RELEASE_EN)));
+				}
+
+				/*  3.1 read fw iv */
+				iv_low = rtw_read32(padapter, REG_TXPKTBUF_IV_LOW);
+					/* only low two bytes is PN, check AES_IV macro for detail */
+				iv_low &= 0xffff;
+				iv_high = rtw_read32(padapter, REG_TXPKTBUF_IV_HIGH);
+					/* get the real packet number */
+				pwrctl->wowlan_fw_iv = iv_high << 16 | iv_low;
+				DBG_871X_LEVEL(_drv_always_, "fw_iv: 0x%016llx\n", pwrctl->wowlan_fw_iv);
+				/* Update TX iv data. */
+				rtw_set_sec_pn(padapter);
+
+				/*  3.2 read GTK index and key */
+				if (
+					psecuritypriv->binstallKCK_KEK == true &&
+					psecuritypriv->dot11PrivacyAlgrthm == _AES_
+				) {
+					u8 gtk_keyindex = 0;
+					u8 get_key[16];
+					/* read gtk key index */
+					gtk_keyindex = rtw_read8(padapter, 0x48c);
+
+					if (gtk_keyindex < 4) {
+						psecuritypriv->dot118021XGrpKeyid = gtk_keyindex;
+						read_cam(padapter, gtk_keyindex, get_key);
+						memcpy(psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].skey, get_key, 16);
+						DBG_871X_LEVEL(
+							_drv_always_,
+							"GTK (%d) = 0x%08x, 0x%08x, 0x%08x, 0x%08x\n",
+							gtk_keyindex,
+							psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].lkey[0],
+							psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].lkey[1],
+							psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].lkey[2],
+							psecuritypriv->dot118021XGrpKey[psecuritypriv->dot118021XGrpKeyid].lkey[3]
+						);
+					} else
+						DBG_871X_LEVEL(_drv_always_, "GTK index =%d\n", gtk_keyindex);
+				}
+
+				/*  4. Re-download Normal FW. */
+				DBG_871X_LEVEL(_drv_always_, "Re-download Normal FW!\n");
+				SetFwRelatedForWoWLAN8723b(padapter, false);
+			}
+#ifdef CONFIG_GPIO_WAKEUP
+			DBG_871X_LEVEL(_drv_always_, "Set Wake GPIO to high for default.\n");
+			HalSetOutPutGPIO(padapter, WAKEUP_GPIO_IDX, 1);
+#endif
+
+			/*  5. Download reserved pages and report media status if needed. */
+			if (
+				(pwrctl->wowlan_wake_reason != FWDecisionDisconnect) &&
+				(pwrctl->wowlan_wake_reason != Rx_Pairwisekey) &&
+				(pwrctl->wowlan_wake_reason != Rx_DisAssoc) &&
+				(pwrctl->wowlan_wake_reason != Rx_DeAuth)
+			) {
+				rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);
+				if (psta != NULL)
+					rtl8723b_set_FwMediaStatusRpt_cmd(padapter, RT_MEDIA_CONNECT, psta->mac_id);
+			}
+#ifdef CONFIG_PNO_SUPPORT
+			rtw_write8(padapter, 0x1b8, 0);
+			DBG_871X("reset 0x1b8: %d\n", rtw_read8(padapter, 0x1b8));
+			rtw_write8(padapter, 0x1b9, 0);
+			DBG_871X("reset 0x1b9: %d\n", rtw_read8(padapter, 0x1b9));
+			rtw_write8(padapter, REG_PNO_STATUS, 0);
+			DBG_871X("reset REG_PNO_STATUS: %d\n", rtw_read8(padapter, REG_PNO_STATUS));
+#endif
+			break;
+
+		default:
+			break;
+		}
+	}
+	break;
+#endif /* CONFIG_WOWLAN */
+#ifdef CONFIG_AP_WOWLAN
+	case HW_VAR_AP_WOWLAN:
+	{
+		poidparam = (struct wowlan_ioctl_param *)val;
+		switch (poidparam->subcode) {
+		case WOWLAN_AP_ENABLE:
+			DBG_871X("%s, WOWLAN_AP_ENABLE\n", __func__);
+			/*  1. Download WOWLAN FW */
+			DBG_871X_LEVEL(_drv_always_, "Re-download WoWlan FW!\n");
+			SetFwRelatedForWoWLAN8723b(padapter, true);
+
+			/*  2. RX DMA stop */
+			DBG_871X_LEVEL(_drv_always_, "Pause DMA\n");
+			rtw_write32(padapter, REG_RXPKT_NUM,
+				(rtw_read32(padapter, REG_RXPKT_NUM)|RW_RELEASE_EN));
+			do {
+				if ((rtw_read32(padapter, REG_RXPKT_NUM)&RXDMA_IDLE)) {
+					DBG_871X_LEVEL(_drv_always_, "RX_DMA_IDLE is true\n");
+					break;
+				} else {
+					/*  If RX_DMA is not idle, receive one pkt from DMA */
+					res = sdio_local_read(padapter, SDIO_REG_RX0_REQ_LEN, 4, (u8 *)&tmp);
+					len = le16_to_cpu(tmp);
+
+					DBG_871X_LEVEL(_drv_always_, "RX len:%d\n", len);
+					if (len > 0)
+						res = RecvOnePkt(padapter, len);
+					else
+						DBG_871X_LEVEL(_drv_always_, "read length fail %d\n", len);
+
+					DBG_871X_LEVEL(_drv_always_, "RecvOnePkt Result: %d\n", res);
+				}
+			} while (trycnt--);
+
+			if (trycnt == 0)
+				DBG_871X_LEVEL(_drv_always_, "Stop RX DMA failed......\n");
+
+			/*  3. Clear IMR and ISR */
+			DBG_871X_LEVEL(_drv_always_, "Clear IMR and ISR\n");
+			tmp = 0;
+			sdio_local_write(padapter, SDIO_REG_HIMR_ON, 4, (u8 *)&tmp);
+			sdio_local_write(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			sdio_local_read(padapter, SDIO_REG_HISR, 4, (u8 *)&tmp);
+			sdio_local_write(padapter, SDIO_REG_HISR, 4, (u8 *)&tmp);
+
+			/*  4. Enable CPWM2 only */
+			DBG_871X_LEVEL(_drv_always_, "Enable only CPWM2\n");
+			sdio_local_read(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			DBG_871X("DisableInterruptButCpwm28723BSdio(): Read SDIO_REG_HIMR: 0x%08x\n", tmp);
+
+			himr = cpu_to_le32(SDIO_HIMR_DISABLED)|SDIO_HIMR_CPWM2_MSK;
+			sdio_local_write(padapter, SDIO_REG_HIMR, 4, (u8 *)&himr);
+
+			sdio_local_read(padapter, SDIO_REG_HIMR, 4, (u8 *)&tmp);
+			DBG_871X("DisableInterruptButCpwm28723BSdio(): Read again SDIO_REG_HIMR: 0x%08x\n", tmp);
+
+			/*  5. Set Enable WOWLAN H2C command. */
+			DBG_871X_LEVEL(_drv_always_, "Set Enable AP WOWLan cmd\n");
+			rtl8723b_set_ap_wowlan_cmd(padapter, 1);
+			/*  6. add some delay for H2C cmd ready */
+			msleep(10);
+
+			rtw_write8(padapter, REG_WOWLAN_WAKE_REASON, 0);
+			break;
+		case WOWLAN_AP_DISABLE:
+			DBG_871X("%s, WOWLAN_AP_DISABLE\n", __func__);
+			/*  1. Read wakeup reason */
+			pwrctl->wowlan_wake_reason =
+				rtw_read8(padapter, REG_WOWLAN_WAKE_REASON);
+
+			DBG_871X_LEVEL(_drv_always_, "wakeup_reason: 0x%02x\n",
+					pwrctl->wowlan_wake_reason);
+
+			/*  2.  Set Disable WOWLAN H2C command. */
+			DBG_871X_LEVEL(_drv_always_, "Set Disable WOWLan cmd\n");
+			rtl8723b_set_ap_wowlan_cmd(padapter, 0);
+			/*  6. add some delay for H2C cmd ready */
+			msleep(2);
+
+			DBG_871X_LEVEL(_drv_always_, "Release RXDMA\n");
+
+			rtw_write32(padapter, REG_RXPKT_NUM,
+				(rtw_read32(padapter, REG_RXPKT_NUM) & (~RW_RELEASE_EN)));
+
+			SetFwRelatedForWoWLAN8723b(padapter, false);
+
+#ifdef CONFIG_GPIO_WAKEUP
+		DBG_871X_LEVEL(_drv_always_, "Set Wake GPIO to high for default.\n");
+		HalSetOutPutGPIO(padapter, WAKEUP_GPIO_IDX, 1);
+#endif /* CONFIG_GPIO_WAKEUP */
+		rtl8723b_set_FwJoinBssRpt_cmd(padapter, RT_MEDIA_CONNECT);
+		issue_beacon(padapter, 0);
+		break;
+		default:
+			break;
+	}
+}
+	break;
+#endif /* CONFIG_AP_WOWLAN */
+	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:
+		/* DBG_8192C("%s len =%d\n", __func__, len); */
+		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 = MAX_AMPDU_FACTOR_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;
+#ifdef CONFIG_WOWLAN
+	pHalFunc->clear_interrupt = &ClearInterrupt8723BSdio;
+#endif
+	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;
+
+#if defined(CONFIG_CHECK_BT_HANG)
+	pHalFunc->hal_init_checkbthang_workqueue = &rtl8723bs_init_checkbthang_workqueue;
+	pHalFunc->hal_free_checkbthang_workqueue = &rtl8723bs_free_checkbthang_workqueue;
+	pHalFunc->hal_cancle_checkbthang_workqueue = &rtl8723bs_cancle_checkbthang_workqueue;
+	pHalFunc->hal_checke_bt_hang = &rtl8723bs_hal_check_bt_hang;
+#endif
+}
diff --git a/drivers/staging/rtl8723bs/hal/sdio_ops.c b/drivers/staging/rtl8723bs/hal/sdio_ops.c
new file mode 100644
index 000000000000..6285b72faa9a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/hal/sdio_ops.c
@@ -0,0 +1,1294 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ *******************************************************************************/
+#define _SDIO_OPS_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtl8723b_hal.h>
+
+/* define SDIO_DEBUG_IO 1 */
+
+
+/*  */
+/*  Description: */
+/* 	The following mapping is for SDIO host local register space. */
+/*  */
+/*  Creadted by Roger, 2011.01.31. */
+/*  */
+static void HalSdioGetCmdAddr8723BSdio(
+	struct adapter *padapter,
+	u8 DeviceID,
+	u32 Addr,
+	u32 *pCmdAddr
+)
+{
+	switch (DeviceID) {
+	case SDIO_LOCAL_DEVICE_ID:
+		*pCmdAddr = ((SDIO_LOCAL_DEVICE_ID << 13) | (Addr & SDIO_LOCAL_MSK));
+		break;
+
+	case WLAN_IOREG_DEVICE_ID:
+		*pCmdAddr = ((WLAN_IOREG_DEVICE_ID << 13) | (Addr & WLAN_IOREG_MSK));
+		break;
+
+	case WLAN_TX_HIQ_DEVICE_ID:
+		*pCmdAddr = ((WLAN_TX_HIQ_DEVICE_ID << 13) | (Addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_TX_MIQ_DEVICE_ID:
+		*pCmdAddr = ((WLAN_TX_MIQ_DEVICE_ID << 13) | (Addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_TX_LOQ_DEVICE_ID:
+		*pCmdAddr = ((WLAN_TX_LOQ_DEVICE_ID << 13) | (Addr & WLAN_FIFO_MSK));
+		break;
+
+	case WLAN_RX0FF_DEVICE_ID:
+		*pCmdAddr = ((WLAN_RX0FF_DEVICE_ID << 13) | (Addr & WLAN_RX0FF_MSK));
+		break;
+
+	default:
+		break;
+	}
+}
+
+static u8 get_deviceid(u32 addr)
+{
+	u8 devideId;
+	u16 pseudoId;
+
+
+	pseudoId = (u16)(addr >> 16);
+	switch (pseudoId) {
+	case 0x1025:
+		devideId = SDIO_LOCAL_DEVICE_ID;
+		break;
+
+	case 0x1026:
+		devideId = WLAN_IOREG_DEVICE_ID;
+		break;
+
+/* 		case 0x1027: */
+/* 			devideId = SDIO_FIRMWARE_FIFO; */
+/* 			break; */
+
+	case 0x1031:
+		devideId = WLAN_TX_HIQ_DEVICE_ID;
+		break;
+
+	case 0x1032:
+		devideId = WLAN_TX_MIQ_DEVICE_ID;
+		break;
+
+	case 0x1033:
+		devideId = WLAN_TX_LOQ_DEVICE_ID;
+		break;
+
+	case 0x1034:
+		devideId = WLAN_RX0FF_DEVICE_ID;
+		break;
+
+	default:
+/* 			devideId = (u8)((addr >> 13) & 0xF); */
+		devideId = WLAN_IOREG_DEVICE_ID;
+		break;
+	}
+
+	return devideId;
+}
+
+/*
+ * Ref:
+ *HalSdioGetCmdAddr8723BSdio()
+ */
+static u32 _cvrt2ftaddr(const u32 addr, u8 *pdeviceId, u16 *poffset)
+{
+	u8 deviceId;
+	u16 offset;
+	u32 ftaddr;
+
+
+	deviceId = get_deviceid(addr);
+	offset = 0;
+
+	switch (deviceId) {
+	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:
+		deviceId = WLAN_IOREG_DEVICE_ID;
+		offset = addr & WLAN_IOREG_MSK;
+		break;
+	}
+	ftaddr = (deviceId << 13) | offset;
+
+	if (pdeviceId)
+		*pdeviceId = deviceId;
+	if (poffset)
+		*poffset = offset;
+
+	return ftaddr;
+}
+
+static u8 sdio_read8(struct intf_hdl *pintfhdl, u32 addr)
+{
+	u32 ftaddr;
+	u8 val;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	val = sd_read8(pintfhdl, ftaddr, NULL);
+	return val;
+}
+
+static u16 sdio_read16(struct intf_hdl *pintfhdl, u32 addr)
+{
+	u32 ftaddr;
+	u16 val;
+	__le16 le_tmp;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	sd_cmd52_read(pintfhdl, ftaddr, 2, (u8 *)&le_tmp);
+	val = le16_to_cpu(le_tmp);
+	return val;
+}
+
+static u32 sdio_read32(struct intf_hdl *pintfhdl, u32 addr)
+{
+	struct adapter *padapter;
+	u8 bMacPwrCtrlOn;
+	u8 deviceId;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	u32 val;
+	s32 err;
+	__le32 le_tmp;
+
+	padapter = pintfhdl->padapter;
+	ftaddr = _cvrt2ftaddr(addr, &deviceId, &offset);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		((deviceId == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(false == bMacPwrCtrlOn) ||
+		(true == adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		err = sd_cmd52_read(pintfhdl, ftaddr, 4, (u8 *)&le_tmp);
+#ifdef SDIO_DEBUG_IO
+		if (!err) {
+#endif
+			val = le32_to_cpu(le_tmp);
+			return val;
+#ifdef SDIO_DEBUG_IO
+		}
+
+		DBG_8192C(KERN_ERR "%s: Mac Power off, Read FAIL(%d)! addr = 0x%x\n", __func__, err, addr);
+		return SDIO_ERR_VAL32;
+#endif
+	}
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		val = sd_read32(pintfhdl, ftaddr, NULL);
+	} else {
+		u8 *ptmpbuf;
+
+		ptmpbuf = (u8 *)rtw_malloc(8);
+		if (NULL == ptmpbuf) {
+			DBG_8192C(KERN_ERR "%s: Allocate memory FAIL!(size =8) addr = 0x%x\n", __func__, addr);
+			return SDIO_ERR_VAL32;
+		}
+
+		ftaddr &= ~(u16)0x3;
+		sd_read(pintfhdl, ftaddr, 8, ptmpbuf);
+		memcpy(&le_tmp, ptmpbuf+shift, 4);
+		val = le32_to_cpu(le_tmp);
+
+		kfree(ptmpbuf);
+	}
+	return val;
+}
+
+static s32 sdio_readN(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pbuf)
+{
+	struct adapter *padapter;
+	u8 bMacPwrCtrlOn;
+	u8 deviceId;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+
+	padapter = pintfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &deviceId, &offset);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		((deviceId == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(false == bMacPwrCtrlOn) ||
+		(true == adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		err = sd_cmd52_read(pintfhdl, ftaddr, cnt, pbuf);
+		return err;
+	}
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		err = sd_read(pintfhdl, ftaddr, cnt, pbuf);
+	} else {
+		u8 *ptmpbuf;
+		u32 n;
+
+		ftaddr &= ~(u16)0x3;
+		n = cnt + shift;
+		ptmpbuf = rtw_malloc(n);
+		if (NULL == ptmpbuf)
+			return -1;
+
+		err = sd_read(pintfhdl, ftaddr, n, ptmpbuf);
+		if (!err)
+			memcpy(pbuf, ptmpbuf+shift, cnt);
+		kfree(ptmpbuf);
+	}
+	return err;
+}
+
+static s32 sdio_write8(struct intf_hdl *pintfhdl, u32 addr, u8 val)
+{
+	u32 ftaddr;
+	s32 err;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	sd_write8(pintfhdl, ftaddr, val, &err);
+
+	return err;
+}
+
+static s32 sdio_write16(struct intf_hdl *pintfhdl, u32 addr, u16 val)
+{
+	u32 ftaddr;
+	s32 err;
+	__le16 le_tmp;
+
+	ftaddr = _cvrt2ftaddr(addr, NULL, NULL);
+	le_tmp = cpu_to_le16(val);
+	err = sd_cmd52_write(pintfhdl, ftaddr, 2, (u8 *)&le_tmp);
+
+	return err;
+}
+
+static s32 sdio_write32(struct intf_hdl *pintfhdl, u32 addr, u32 val)
+{
+	struct adapter *padapter;
+	u8 bMacPwrCtrlOn;
+	u8 deviceId;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+	__le32 le_tmp;
+
+	padapter = pintfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &deviceId, &offset);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		((deviceId == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(!bMacPwrCtrlOn) ||
+		(adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		le_tmp = cpu_to_le32(val);
+		err = sd_cmd52_write(pintfhdl, ftaddr, 4, (u8 *)&le_tmp);
+		return err;
+	}
+
+	/*  4 bytes alignment */
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		sd_write32(pintfhdl, ftaddr, val, &err);
+	} else {
+		le_tmp = cpu_to_le32(val);
+		err = sd_cmd52_write(pintfhdl, ftaddr, 4, (u8 *)&le_tmp);
+	}
+	return err;
+}
+
+static s32 sdio_writeN(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pbuf)
+{
+	struct adapter *padapter;
+	u8 bMacPwrCtrlOn;
+	u8 deviceId;
+	u16 offset;
+	u32 ftaddr;
+	u8 shift;
+	s32 err;
+
+	padapter = pintfhdl->padapter;
+	err = 0;
+
+	ftaddr = _cvrt2ftaddr(addr, &deviceId, &offset);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		((deviceId == WLAN_IOREG_DEVICE_ID) && (offset < 0x100)) ||
+		(false == bMacPwrCtrlOn) ||
+		(true == adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		err = sd_cmd52_write(pintfhdl, ftaddr, cnt, pbuf);
+		return err;
+	}
+
+	shift = ftaddr & 0x3;
+	if (shift == 0) {
+		err = sd_write(pintfhdl, ftaddr, cnt, pbuf);
+	} else {
+		u8 *ptmpbuf;
+		u32 n;
+
+		ftaddr &= ~(u16)0x3;
+		n = cnt + shift;
+		ptmpbuf = rtw_malloc(n);
+		if (NULL == ptmpbuf)
+			return -1;
+		err = sd_read(pintfhdl, ftaddr, 4, ptmpbuf);
+		if (err) {
+			kfree(ptmpbuf);
+			return err;
+		}
+		memcpy(ptmpbuf+shift, pbuf, cnt);
+		err = sd_write(pintfhdl, ftaddr, n, ptmpbuf);
+		kfree(ptmpbuf);
+	}
+	return err;
+}
+
+static u8 sdio_f0_read8(struct intf_hdl *pintfhdl, u32 addr)
+{
+	return sd_f0_read8(pintfhdl, addr, NULL);
+}
+
+static void sdio_read_mem(
+	struct intf_hdl *pintfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *rmem
+)
+{
+	s32 err;
+
+	err = sdio_readN(pintfhdl, addr, cnt, rmem);
+	/* TODO: Report error is err not zero */
+}
+
+static void sdio_write_mem(
+	struct intf_hdl *pintfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *wmem
+)
+{
+	sdio_writeN(pintfhdl, 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:
+ *pintfhdl	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 *pintfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *mem
+)
+{
+	struct adapter *padapter;
+	PSDIO_DATA psdio;
+	struct hal_com_data *phal;
+	u32 oldcnt;
+#ifdef SDIO_DYNAMIC_ALLOC_MEM
+	u8 *oldmem;
+#endif
+	s32 err;
+
+
+	padapter = pintfhdl->padapter;
+	psdio = &adapter_to_dvobj(padapter)->intf_data;
+	phal = GET_HAL_DATA(padapter);
+
+	HalSdioGetCmdAddr8723BSdio(padapter, addr, phal->SdioRxFIFOCnt++, &addr);
+
+	oldcnt = cnt;
+	if (cnt > psdio->block_transfer_len)
+		cnt = _RND(cnt, psdio->block_transfer_len);
+/* 	cnt = sdio_align_size(cnt); */
+
+	if (oldcnt != cnt) {
+#ifdef SDIO_DYNAMIC_ALLOC_MEM
+		oldmem = mem;
+		mem = rtw_malloc(cnt);
+		if (mem == NULL) {
+			DBG_8192C(KERN_WARNING "%s: allocate memory %d bytes fail!\n", __func__, cnt);
+			mem = oldmem;
+			oldmem == NULL;
+		}
+#else
+		/*  in this case, caller should gurante the buffer is big enough */
+		/*  to receive data after alignment */
+#endif
+	}
+
+	err = _sd_read(pintfhdl, addr, cnt, mem);
+
+#ifdef SDIO_DYNAMIC_ALLOC_MEM
+	if ((oldcnt != cnt) && (oldmem)) {
+		memcpy(oldmem, mem, oldcnt);
+		kfree(mem);
+	}
+#endif
+
+	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:
+ *pintfhdl	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 *pintfhdl,
+	u32 addr,
+	u32 cnt,
+	u8 *mem
+)
+{
+	struct adapter *padapter;
+	PSDIO_DATA psdio;
+	s32 err;
+	struct xmit_buf *xmitbuf = (struct xmit_buf *)mem;
+
+	padapter = pintfhdl->padapter;
+	psdio = &adapter_to_dvobj(padapter)->intf_data;
+
+	if (padapter->hw_init_completed == false) {
+		DBG_871X("%s [addr = 0x%x cnt =%d] padapter->hw_init_completed == false\n", __func__, addr, cnt);
+		return _FAIL;
+	}
+
+	cnt = _RND4(cnt);
+	HalSdioGetCmdAddr8723BSdio(padapter, addr, cnt >> 2, &addr);
+
+	if (cnt > psdio->block_transfer_len)
+		cnt = _RND(cnt, psdio->block_transfer_len);
+/* 	cnt = sdio_align_size(cnt); */
+
+	err = sd_write(pintfhdl, 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 *padapter, struct _io_ops *pops)
+{
+	pops->_read8 = &sdio_read8;
+	pops->_read16 = &sdio_read16;
+	pops->_read32 = &sdio_read32;
+	pops->_read_mem = &sdio_read_mem;
+	pops->_read_port = &sdio_read_port;
+
+	pops->_write8 = &sdio_write8;
+	pops->_write16 = &sdio_write16;
+	pops->_write32 = &sdio_write32;
+	pops->_writeN = &sdio_writeN;
+	pops->_write_mem = &sdio_write_mem;
+	pops->_write_port = &sdio_write_port;
+
+	pops->_sd_f0_read8 = sdio_f0_read8;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+static s32 _sdio_local_read(
+	struct adapter *padapter,
+	u32 addr,
+	u32 cnt,
+	u8 *pbuf
+)
+{
+	struct intf_hdl *pintfhdl;
+	u8 bMacPwrCtrlOn;
+	s32 err;
+	u8 *ptmpbuf;
+	u32 n;
+
+
+	pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (false == bMacPwrCtrlOn) {
+		err = _sd_cmd52_read(pintfhdl, addr, cnt, pbuf);
+		return err;
+	}
+
+	n = RND4(cnt);
+	ptmpbuf = (u8 *)rtw_malloc(n);
+	if (!ptmpbuf)
+		return (-1);
+
+	err = _sd_read(pintfhdl, addr, n, ptmpbuf);
+	if (!err)
+		memcpy(pbuf, ptmpbuf, cnt);
+
+	kfree(ptmpbuf);
+
+	return err;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+s32 sdio_local_read(
+	struct adapter *padapter,
+	u32 addr,
+	u32 cnt,
+	u8 *pbuf
+)
+{
+	struct intf_hdl *pintfhdl;
+	u8 bMacPwrCtrlOn;
+	s32 err;
+	u8 *ptmpbuf;
+	u32 n;
+
+	pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		(false == bMacPwrCtrlOn) ||
+		(true == adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		err = sd_cmd52_read(pintfhdl, addr, cnt, pbuf);
+		return err;
+	}
+
+	n = RND4(cnt);
+	ptmpbuf = (u8 *)rtw_malloc(n);
+	if (!ptmpbuf)
+		return (-1);
+
+	err = sd_read(pintfhdl, addr, n, ptmpbuf);
+	if (!err)
+		memcpy(pbuf, ptmpbuf, cnt);
+
+	kfree(ptmpbuf);
+
+	return err;
+}
+
+/*
+ * Todo: align address to 4 bytes.
+ */
+s32 sdio_local_write(
+	struct adapter *padapter,
+	u32 addr,
+	u32 cnt,
+	u8 *pbuf
+)
+{
+	struct intf_hdl *pintfhdl;
+	u8 bMacPwrCtrlOn;
+	s32 err;
+	u8 *ptmpbuf;
+
+	if (addr & 0x3)
+		DBG_8192C("%s, address must be 4 bytes alignment\n", __func__);
+
+	if (cnt  & 0x3)
+		DBG_8192C("%s, size must be the multiple of 4\n", __func__);
+
+	pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (
+		(false == bMacPwrCtrlOn) ||
+		(true == adapter_to_pwrctl(padapter)->bFwCurrentInPSMode)
+	) {
+		err = sd_cmd52_write(pintfhdl, addr, cnt, pbuf);
+		return err;
+	}
+
+	ptmpbuf = (u8 *)rtw_malloc(cnt);
+	if (!ptmpbuf)
+		return (-1);
+
+	memcpy(ptmpbuf, pbuf, cnt);
+
+	err = sd_write(pintfhdl, addr, cnt, ptmpbuf);
+
+	kfree(ptmpbuf);
+
+	return err;
+}
+
+u8 SdioLocalCmd52Read1Byte(struct adapter *padapter, u32 addr)
+{
+	u8 val = 0;
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_read(pintfhdl, addr, 1, &val);
+
+	return val;
+}
+
+static u16 SdioLocalCmd52Read2Byte(struct adapter *padapter, u32 addr)
+{
+	__le16 val = 0;
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_read(pintfhdl, addr, 2, (u8 *)&val);
+
+	return le16_to_cpu(val);
+}
+
+static u32 SdioLocalCmd53Read4Byte(struct adapter *padapter, u32 addr)
+{
+
+	u8 bMacPwrCtrlOn;
+	u32 val = 0;
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+	__le32 le_tmp;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	rtw_hal_get_hwreg(padapter, HW_VAR_APFM_ON_MAC, &bMacPwrCtrlOn);
+	if (!bMacPwrCtrlOn || adapter_to_pwrctl(padapter)->bFwCurrentInPSMode) {
+		sd_cmd52_read(pintfhdl, addr, 4, (u8 *)&le_tmp);
+		val = le32_to_cpu(le_tmp);
+	} else {
+		val = sd_read32(pintfhdl, addr, NULL);
+	}
+	return val;
+}
+
+void SdioLocalCmd52Write1Byte(struct adapter *padapter, u32 addr, u8 v)
+{
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	sd_cmd52_write(pintfhdl, addr, 1, &v);
+}
+
+static void SdioLocalCmd52Write4Byte(struct adapter *padapter, u32 addr, u32 v)
+{
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+	__le32 le_tmp;
+
+	HalSdioGetCmdAddr8723BSdio(padapter, SDIO_LOCAL_DEVICE_ID, addr, &addr);
+	le_tmp = cpu_to_le32(v);
+	sd_cmd52_write(pintfhdl, addr, 4, (u8 *)&le_tmp);
+}
+
+static s32 ReadInterrupt8723BSdio(struct adapter *padapter, u32 *phisr)
+{
+	u32 hisr, himr;
+	u8 val8, hisr_len;
+
+
+	if (phisr == NULL)
+		return false;
+
+	himr = GET_HAL_DATA(padapter)->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(padapter, 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 *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+	pHalData->sdio_himr = (u32)(		\
+								SDIO_HIMR_RX_REQUEST_MSK			|
+								SDIO_HIMR_AVAL_MSK					|
+/* 								SDIO_HIMR_TXERR_MSK				| */
+/* 								SDIO_HIMR_RXERR_MSK				| */
+/* 								SDIO_HIMR_TXFOVW_MSK				| */
+/* 								SDIO_HIMR_RXFOVW_MSK				| */
+/* 								SDIO_HIMR_TXBCNOK_MSK				| */
+/* 								SDIO_HIMR_TXBCNERR_MSK			| */
+/* 								SDIO_HIMR_BCNERLY_INT_MSK			| */
+/* 								SDIO_HIMR_C2HCMD_MSK				| */
+/* 								SDIO_HIMR_HSISR_IND_MSK			| */
+/* 								SDIO_HIMR_GTINT3_IND_MSK			| */
+/* 								SDIO_HIMR_GTINT4_IND_MSK			| */
+/* 								SDIO_HIMR_PSTIMEOUT_MSK			| */
+/* 								SDIO_HIMR_OCPINT_MSK				| */
+/* 								SDIO_HIMR_ATIMEND_MSK				| */
+/* 								SDIO_HIMR_ATIMEND_E_MSK			| */
+/* 								SDIO_HIMR_CTWEND_MSK				| */
+								0);
+}
+
+/*  */
+/* 	Description: */
+/* 		Initialize System Host Interrupt Mask configuration variables for future use. */
+/*  */
+/* 	Created by Roger, 2011.08.03. */
+/*  */
+void InitSysInterrupt8723BSdio(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->SysIntrMask = (		\
+/* 							HSIMR_GPIO12_0_INT_EN			| */
+/* 							HSIMR_SPS_OCP_INT_EN			| */
+/* 							HSIMR_RON_INT_EN				| */
+/* 							HSIMR_PDNINT_EN				| */
+/* 							HSIMR_GPIO9_INT_EN				| */
+							0);
+}
+
+#ifdef CONFIG_WOWLAN
+/*  */
+/* 	Description: */
+/* 		Clear corresponding SDIO Host ISR interrupt service. */
+/*  */
+/* 	Assumption: */
+/* 		Using SDIO Local register ONLY for configuration. */
+/*  */
+/* 	Created by Roger, 2011.02.11. */
+/*  */
+void ClearInterrupt8723BSdio(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData;
+	u8 *clear;
+
+
+	if (true == padapter->bSurpriseRemoved)
+		return;
+
+	pHalData = GET_HAL_DATA(padapter);
+	clear = rtw_zmalloc(4);
+
+	/*  Clear corresponding HISR Content if needed */
+	*(__le32 *)clear = cpu_to_le32(pHalData->sdio_hisr & MASK_SDIO_HISR_CLEAR);
+	if (*(__le32 *)clear) {
+		/*  Perform write one clear operation */
+		sdio_local_write(padapter, SDIO_REG_HISR, 4, clear);
+	}
+
+	kfree(clear);
+}
+#endif
+
+/*  */
+/* 	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 *padapter)
+{
+	struct hal_com_data *pHalData;
+	__le32 himr;
+	u32 tmp;
+
+	pHalData = GET_HAL_DATA(padapter);
+
+	himr = cpu_to_le32(pHalData->sdio_himr);
+	sdio_local_write(padapter, SDIO_REG_HIMR, 4, (u8 *)&himr);
+
+	RT_TRACE(
+		_module_hci_ops_c_,
+		_drv_notice_,
+		(
+			"%s: enable SDIO HIMR = 0x%08X\n",
+			__func__,
+			pHalData->sdio_himr
+		)
+	);
+
+	/*  Update current system IMR settings */
+	tmp = rtw_read32(padapter, REG_HSIMR);
+	rtw_write32(padapter, REG_HSIMR, tmp | pHalData->SysIntrMask);
+
+	RT_TRACE(
+		_module_hci_ops_c_,
+		_drv_notice_,
+		(
+			"%s: enable HSIMR = 0x%08X\n",
+			__func__,
+			pHalData->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(padapter, 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 *padapter)
+{
+	__le32 himr;
+
+	himr = cpu_to_le32(SDIO_HIMR_DISABLED);
+	sdio_local_write(padapter, 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 *padapter)
+{
+	DBG_871X(
+		"%s(): Read 0x100 = 0x%02x 0x86 = 0x%02x\n",
+		__func__,
+		rtw_read8(padapter, 0x100),
+		rtw_read8(padapter, 0x86)
+	);
+
+	if (rtw_read8(padapter, 0x100) == 0xEA)
+		return true;
+	else
+		return false;
+}
+
+static struct recv_buf *sd_recv_rxfifo(struct adapter *padapter, u32 size)
+{
+	u32 readsize, ret;
+	u8 *preadbuf;
+	struct recv_priv *precvpriv;
+	struct recv_buf	*precvbuf;
+
+
+	/*  Patch for some SDIO Host 4 bytes issue */
+	/*  ex. RK3188 */
+	readsize = RND4(size);
+
+	/* 3 1. alloc recvbuf */
+	precvpriv = &padapter->recvpriv;
+	precvbuf = rtw_dequeue_recvbuf(&precvpriv->free_recv_buf_queue);
+	if (precvbuf == NULL) {
+		DBG_871X_LEVEL(_drv_err_, "%s: alloc recvbuf FAIL!\n", __func__);
+		return NULL;
+	}
+
+	/* 3 2. alloc skb */
+	if (precvbuf->pskb == NULL) {
+		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));
+		}
+
+		if (precvbuf->pskb == NULL) {
+			DBG_871X("%s: alloc_skb fail! read =%d\n", __func__, readsize);
+			return NULL;
+		}
+	}
+
+	/* 3 3. read data from rxfifo */
+	preadbuf = precvbuf->pskb->data;
+	ret = sdio_read_port(&padapter->iopriv.intf, WLAN_RX0FF_DEVICE_ID, readsize, preadbuf);
+	if (ret == _FAIL) {
+		RT_TRACE(_module_hci_ops_os_c_, _drv_err_, ("%s: read port FAIL!\n", __func__));
+		return NULL;
+	}
+
+
+	/* 3 4. init recvbuf */
+	precvbuf->len = size;
+	precvbuf->phead = precvbuf->pskb->head;
+	precvbuf->pdata = precvbuf->pskb->data;
+	skb_set_tail_pointer(precvbuf->pskb, size);
+	precvbuf->ptail = skb_tail_pointer(precvbuf->pskb);
+	precvbuf->pend = skb_end_pointer(precvbuf->pskb);
+
+	return precvbuf;
+}
+
+static void sd_rxhandler(struct adapter *padapter, struct recv_buf *precvbuf)
+{
+	struct recv_priv *precvpriv;
+	struct __queue *ppending_queue;
+
+	precvpriv = &padapter->recvpriv;
+	ppending_queue = &precvpriv->recv_buf_pending_queue;
+
+	/* 3 1. enqueue recvbuf */
+	rtw_enqueue_recvbuf(precvbuf, ppending_queue);
+
+	/* 3 2. schedule tasklet */
+	tasklet_schedule(&precvpriv->recv_tasklet);
+}
+
+void sd_int_dpc(struct adapter *padapter)
+{
+	struct hal_com_data *phal;
+	struct dvobj_priv *dvobj;
+	struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
+	struct pwrctrl_priv *pwrctl;
+
+
+	phal = GET_HAL_DATA(padapter);
+	dvobj = adapter_to_dvobj(padapter);
+	pwrctl = dvobj_to_pwrctl(dvobj);
+
+	if (phal->sdio_hisr & SDIO_HISR_AVAL) {
+		u8 freepage[4];
+
+		_sdio_local_read(padapter, SDIO_REG_FREE_TXPG, 4, freepage);
+		up(&(padapter->xmitpriv.xmit_sema));
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_CPWM1) {
+		struct reportpwrstate_parm report;
+
+		u8 bcancelled;
+		_cancel_timer(&(pwrctl->pwr_rpwm_timer), &bcancelled);
+
+		report.state = SdioLocalCmd52Read1Byte(padapter, SDIO_REG_HCPWM1_8723B);
+
+		/* cpwm_int_hdl(padapter, &report); */
+		_set_workitem(&(pwrctl->cpwm_event));
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_TXERR) {
+		u8 *status;
+		u32 addr;
+
+		status = rtw_malloc(4);
+		if (status) {
+			addr = REG_TXDMA_STATUS;
+			HalSdioGetCmdAddr8723BSdio(padapter, WLAN_IOREG_DEVICE_ID, addr, &addr);
+			_sd_read(pintfhdl, addr, 4, status);
+			_sd_write(pintfhdl, addr, 4, status);
+			DBG_8192C("%s: SDIO_HISR_TXERR (0x%08x)\n", __func__, le32_to_cpu(*(u32 *)status));
+			kfree(status);
+		} else {
+			DBG_8192C("%s: SDIO_HISR_TXERR, but can't allocate memory to read status!\n", __func__);
+		}
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_TXBCNOK) {
+		DBG_8192C("%s: SDIO_HISR_TXBCNOK\n", __func__);
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_TXBCNERR) {
+		DBG_8192C("%s: SDIO_HISR_TXBCNERR\n", __func__);
+	}
+#ifndef CONFIG_C2H_PACKET_EN
+	if (phal->sdio_hisr & SDIO_HISR_C2HCMD) {
+		struct c2h_evt_hdr_88xx *c2h_evt;
+
+		DBG_8192C("%s: C2H Command\n", __func__);
+		c2h_evt = (struct c2h_evt_hdr_88xx *)rtw_zmalloc(16);
+		if (c2h_evt != NULL) {
+			if (rtw_hal_c2h_evt_read(padapter, (u8 *)c2h_evt) == _SUCCESS) {
+				if (c2h_id_filter_ccx_8723b((u8 *)c2h_evt)) {
+					/* Handle CCX report here */
+					rtw_hal_c2h_handler(padapter, (u8 *)c2h_evt);
+					kfree((u8 *)c2h_evt);
+				} else {
+					rtw_c2h_wk_cmd(padapter, (u8 *)c2h_evt);
+				}
+			}
+		} else {
+			/* Error handling for malloc fail */
+			if (rtw_cbuf_push(padapter->evtpriv.c2h_queue, (void *)NULL) != _SUCCESS)
+				DBG_871X("%s rtw_cbuf_push fail\n", __func__);
+			_set_workitem(&padapter->evtpriv.c2h_wk);
+		}
+	}
+#endif
+
+	if (phal->sdio_hisr & SDIO_HISR_RXFOVW) {
+		DBG_8192C("%s: Rx Overflow\n", __func__);
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_RXERR) {
+		DBG_8192C("%s: Rx Error\n", __func__);
+	}
+
+	if (phal->sdio_hisr & SDIO_HISR_RX_REQUEST) {
+		struct recv_buf *precvbuf;
+		int alloc_fail_time = 0;
+		u32 hisr;
+
+/* 		DBG_8192C("%s: RX Request, size =%d\n", __func__, phal->SdioRxFIFOSize); */
+		phal->sdio_hisr ^= SDIO_HISR_RX_REQUEST;
+		do {
+			phal->SdioRxFIFOSize = SdioLocalCmd52Read2Byte(padapter, SDIO_REG_RX0_REQ_LEN);
+			if (phal->SdioRxFIFOSize != 0) {
+				precvbuf = sd_recv_rxfifo(padapter, phal->SdioRxFIFOSize);
+				if (precvbuf)
+					sd_rxhandler(padapter, precvbuf);
+				else {
+					alloc_fail_time++;
+					DBG_871X("precvbuf is Null for %d times because alloc memory failed\n", alloc_fail_time);
+					if (alloc_fail_time >= 10)
+						break;
+				}
+				phal->SdioRxFIFOSize = 0;
+			} else
+				break;
+
+			hisr = 0;
+			ReadInterrupt8723BSdio(padapter, &hisr);
+			hisr &= SDIO_HISR_RX_REQUEST;
+			if (!hisr)
+				break;
+		} while (1);
+
+		if (alloc_fail_time == 10)
+			DBG_871X("exit because alloc memory failed more than 10 times\n");
+
+	}
+}
+
+void sd_int_hdl(struct adapter *padapter)
+{
+	struct hal_com_data *phal;
+
+
+	if (
+		(padapter->bDriverStopped == true) ||
+		(padapter->bSurpriseRemoved == true)
+	)
+		return;
+
+	phal = GET_HAL_DATA(padapter);
+
+	phal->sdio_hisr = 0;
+	ReadInterrupt8723BSdio(padapter, &phal->sdio_hisr);
+
+	if (phal->sdio_hisr & phal->sdio_himr) {
+		u32 v32;
+
+		phal->sdio_hisr &= phal->sdio_himr;
+
+		/*  clear HISR */
+		v32 = phal->sdio_hisr & MASK_SDIO_HISR_CLEAR;
+		if (v32) {
+			SdioLocalCmd52Write4Byte(padapter, SDIO_REG_HISR, v32);
+		}
+
+		sd_int_dpc(padapter);
+	} else {
+		RT_TRACE(_module_hci_ops_c_, _drv_err_,
+				("%s: HISR(0x%08x) and HIMR(0x%08x) not match!\n",
+				__func__, phal->sdio_hisr, phal->sdio_himr));
+	}
+}
+
+/*  */
+/* 	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 *padapter)
+{
+	struct hal_com_data *phal;
+	u32 NumOfFreePage;
+	/* _irqL irql; */
+
+
+	phal = GET_HAL_DATA(padapter);
+
+	NumOfFreePage = SdioLocalCmd53Read4Byte(padapter, SDIO_REG_FREE_TXPG);
+
+	/* spin_lock_bh(&phal->SdioTxFIFOFreePageLock); */
+	memcpy(phal->SdioTxFIFOFreePage, &NumOfFreePage, 4);
+	RT_TRACE(_module_hci_ops_c_, _drv_notice_,
+			("%s: Free page for HIQ(%#x), MIDQ(%#x), LOWQ(%#x), PUBQ(%#x)\n",
+			__func__,
+			phal->SdioTxFIFOFreePage[HI_QUEUE_IDX],
+			phal->SdioTxFIFOFreePage[MID_QUEUE_IDX],
+			phal->SdioTxFIFOFreePage[LOW_QUEUE_IDX],
+			phal->SdioTxFIFOFreePage[PUBLIC_QUEUE_IDX]));
+	/* spin_unlock_bh(&phal->SdioTxFIFOFreePageLock); */
+
+	return true;
+}
+
+/*  */
+/* 	Description: */
+/* 		Query SDIO Local register to get the current number of TX OQT Free Space. */
+/*  */
+u8 HalQueryTxOQTBufferStatus8723BSdio(struct adapter *padapter)
+{
+	struct hal_com_data *pHalData = GET_HAL_DATA(padapter);
+
+	pHalData->SdioTxOQTFreeSpace = SdioLocalCmd52Read1Byte(padapter, SDIO_REG_OQT_FREE_PG);
+	return true;
+}
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+u8 RecvOnePkt(struct adapter *padapter, u32 size)
+{
+	struct recv_buf *precvbuf;
+	struct dvobj_priv *psddev;
+	PSDIO_DATA psdio_data;
+	struct sdio_func *func;
+
+	u8 res = false;
+
+	DBG_871X("+%s: size: %d+\n", __func__, size);
+
+	if (padapter == NULL) {
+		DBG_871X(KERN_ERR "%s: padapter is NULL!\n", __func__);
+		return false;
+	}
+
+	psddev = adapter_to_dvobj(padapter);
+	psdio_data = &psddev->intf_data;
+	func = psdio_data->func;
+
+	if (size) {
+		sdio_claim_host(func);
+		precvbuf = sd_recv_rxfifo(padapter, size);
+
+		if (precvbuf) {
+			/* printk("Completed Recv One Pkt.\n"); */
+			sd_rxhandler(padapter, precvbuf);
+			res = true;
+		} else {
+			res = false;
+		}
+		sdio_release_host(func);
+	}
+	DBG_871X("-%s-\n", __func__);
+	return res;
+}
+#endif /* CONFIG_WOWLAN */
diff --git a/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h b/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h
new file mode 100644
index 000000000000..fbb83db4fb60
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/Hal8192CPhyReg.h
@@ -0,0 +1,1126 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*****************************************************************************
+ *
+ * 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 Regsiter 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 defintion 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		rTxAGC_B_Mcs11_Mcs08			0x84c
+
+#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_Mcs15_Mcs12			0x868
+#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	/*  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		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 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		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		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
+
+#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/Hal8723BPhyCfg.h b/drivers/staging/rtl8723bs/include/Hal8723BPhyCfg.h
new file mode 100644
index 000000000000..22250411ed6c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/Hal8723BPhyCfg.h
@@ -0,0 +1,128 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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_8723B(
+struct adapter *		Adapter,
+u32 				PowerIndex,
+u8 			RFPath,
+u8 			Rate
+	);
+
+u8
+PHY_GetTxPowerIndex_8723B(
+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,	/*  20M or 40M */
+unsigned char 			Offset		/*  Upper, Lower, or Don't care */
+);
+
+void
+PHY_SwChnl8723B(/*  Call after initialization */
+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/Hal8723BPhyReg.h b/drivers/staging/rtl8723bs/include/Hal8723BPhyReg.h
new file mode 100644
index 000000000000..84a08e0092dd
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/Hal8723BPhyReg.h
@@ -0,0 +1,77 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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/Hal8723BPwrSeq.h b/drivers/staging/rtl8723bs/include/Hal8723BPwrSeq.h
new file mode 100644
index 000000000000..796449c3f430
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/Hal8723BPwrSeq.h
@@ -0,0 +1,232 @@
+#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 transision 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 WLAN_PWR_CFG rtl8723B_power_on_flow[RTL8723B_TRANS_CARDEMU_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_radio_off_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_card_disable_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_card_enable_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_suspend_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_resume_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_SUS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_hwpdn_flow[RTL8723B_TRANS_ACT_TO_CARDEMU_STEPS+RTL8723B_TRANS_CARDEMU_TO_PDN_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_enter_lps_flow[RTL8723B_TRANS_ACT_TO_LPS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_leave_lps_flow[RTL8723B_TRANS_LPS_TO_ACT_STEPS+RTL8723B_TRANS_END_STEPS];
+extern WLAN_PWR_CFG rtl8723B_enter_swlps_flow[RTL8723B_TRANS_ACT_TO_SWLPS_STEPS+RTL8723B_TRANS_END_STEPS];
+extern 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/HalPwrSeqCmd.h b/drivers/staging/rtl8723bs/include/HalPwrSeqCmd.h
new file mode 100644
index 000000000000..5bb9f5a04734
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/HalPwrSeqCmd.h
@@ -0,0 +1,132 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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
+
+
+typedef enum _PWRSEQ_CMD_DELAY_UNIT_
+{
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+} PWRSEQ_DELAY_UNIT;
+
+typedef struct _WL_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;
+} WLAN_PWR_CFG, *PWLAN_PWR_CFG;
+
+
+#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,
+	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 000000000000..a9e8609303b9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/HalVerDef.h
@@ -0,0 +1,127 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HAL_VERSION_DEF_H__
+#define __HAL_VERSION_DEF_H__
+
+/*  HAL_IC_TYPE_E */
+typedef enum tag_HAL_IC_Type_Definition
+{
+	CHIP_8192S	=	0,
+	CHIP_8188C	=	1,
+	CHIP_8192C	=	2,
+	CHIP_8192D	=	3,
+	CHIP_8723A	=	4,
+	CHIP_8188E	=	5,
+	CHIP_8812	=	6,
+	CHIP_8821	=	7,
+	CHIP_8723B	=	8,
+	CHIP_8192E	=	9,
+}HAL_IC_TYPE_E;
+
+/* HAL_CHIP_TYPE_E */
+typedef enum tag_HAL_CHIP_Type_Definition
+{
+	TEST_CHIP		=	0,
+	NORMAL_CHIP	=	1,
+	FPGA			=	2,
+}HAL_CHIP_TYPE_E;
+
+/* HAL_CUT_VERSION_E */
+typedef enum 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_CUT_VERSION_E;
+
+/*  HAL_Manufacturer */
+typedef enum tag_HAL_Manufacturer_Version_Definition
+{
+	CHIP_VENDOR_TSMC	=	0,
+	CHIP_VENDOR_UMC		=	1,
+	CHIP_VENDOR_SMIC	=	2,
+}HAL_VENDOR_E;
+
+typedef enum tag_HAL_RF_Type_Definition
+{
+	RF_TYPE_1T1R	=	0,
+	RF_TYPE_1T2R	=	1,
+	RF_TYPE_2T2R	=	2,
+	RF_TYPE_2T3R	=	3,
+	RF_TYPE_2T4R	=	4,
+	RF_TYPE_3T3R	=	5,
+	RF_TYPE_3T4R	=	6,
+	RF_TYPE_4T4R	=	7,
+}HAL_RF_TYPE_E;
+
+typedef	struct tag_HAL_VERSION
+{
+	HAL_IC_TYPE_E		ICType;
+	HAL_CHIP_TYPE_E		ChipType;
+	HAL_CUT_VERSION_E	CUTVersion;
+	HAL_VENDOR_E		VendorType;
+	HAL_RF_TYPE_E		RFType;
+	u8 			ROMVer;
+}HAL_VERSION,*PHAL_VERSION;
+
+/* VERSION_8192C			VersionID; */
+/* HAL_VERSION			VersionID; */
+
+/*  Get element */
+#define GET_CVID_IC_TYPE(version)			((HAL_IC_TYPE_E)((version).ICType)	)
+#define GET_CVID_CHIP_TYPE(version)			((HAL_CHIP_TYPE_E)((version).ChipType)	)
+#define GET_CVID_RF_TYPE(version)			((HAL_RF_TYPE_E)((version).RFType))
+#define GET_CVID_MANUFACTUER(version)		((HAL_VENDOR_E)((version).VendorType))
+#define GET_CVID_CUT_VERSION(version)		((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)
+
+/* HAL_RF_TYPE_E */
+#define IS_1T1R(version)					((GET_CVID_RF_TYPE(version) == RF_TYPE_1T1R)? true : false)
+#define IS_1T2R(version)					((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)? true : false)
+#define IS_2T2R(version)					((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)? true : false)
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/autoconf.h b/drivers/staging/rtl8723bs/include/autoconf.h
new file mode 100644
index 000000000000..09ed29f4efbd
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/autoconf.h
@@ -0,0 +1,73 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+
+
+/*
+ * Automatically generated C config: don't edit
+ */
+
+/*
+ * Functions Config
+ */
+/* define DEBUG_CFG80211 */
+
+#ifndef CONFIG_WIRELESS_EXT
+#error CONFIG_WIRELESS_EXT needs to be enabled for this driver to work
+#endif
+
+/*
+ * Auto Config Section
+ */
+#define LPS_RPWM_WAIT_MS 300
+#ifndef DISABLE_BB_RF
+#define DISABLE_BB_RF	0
+#endif
+
+#if DISABLE_BB_RF
+	#define HAL_MAC_ENABLE	0
+	#define HAL_BB_ENABLE		0
+	#define HAL_RF_ENABLE		0
+#else
+	#define HAL_MAC_ENABLE	1
+	#define HAL_BB_ENABLE		1
+	#define HAL_RF_ENABLE		1
+#endif
+
+/*
+ * Platform dependent
+ */
+#define WAKEUP_GPIO_IDX	12	/* WIFI Chip Side */
+#ifdef CONFIG_WOWLAN
+#define CONFIG_GTK_OL
+#endif /* CONFIG_WOWLAN */
+
+/*
+ * Debug Related Config
+ */
+#undef DEBUG
+
+#ifdef DEBUG
+#define DBG	1	/*  for ODM & BTCOEX debug */
+/*#define DEBUG_RTL871X */
+#else /*  !DEBUG */
+#define DBG	0	/*  for ODM & BTCOEX debug */
+#endif /*  !DEBUG */
+
+#ifdef CONFIG_PROC_FS
+#define PROC_DEBUG
+#endif
+
+/* define DBG_XMIT_BUF */
+/* define DBG_XMIT_BUF_EXT */
diff --git a/drivers/staging/rtl8723bs/include/basic_types.h b/drivers/staging/rtl8723bs/include/basic_types.h
new file mode 100644
index 000000000000..abadea07466d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/basic_types.h
@@ -0,0 +1,209 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __BASIC_TYPES_H__
+#define __BASIC_TYPES_H__
+
+
+#define SUCCESS	0
+#define FAIL	(-1)
+
+#include <linux/types.h>
+
+typedef	signed int sint;
+
+#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 aligment 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 000000000000..3ad3ace86fd4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/cmd_osdep.h
@@ -0,0 +1,26 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __CMD_OSDEP_H_
+#define __CMD_OSDEP_H_
+
+
+extern sint _rtw_init_cmd_priv (struct	cmd_priv *pcmdpriv);
+extern sint _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);
+extern sint _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_conf.h b/drivers/staging/rtl8723bs/include/drv_conf.h
new file mode 100644
index 000000000000..3e1ed0717ed8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/drv_conf.h
@@ -0,0 +1,37 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __DRV_CONF_H__
+#define __DRV_CONF_H__
+#include "autoconf.h"
+
+//About USB VENDOR REQ
+#if defined(CONFIG_USB_VENDOR_REQ_BUFFER_PREALLOC) && !defined(CONFIG_USB_VENDOR_REQ_MUTEX)
+	#warning "define CONFIG_USB_VENDOR_REQ_MUTEX for CONFIG_USB_VENDOR_REQ_BUFFER_PREALLOC automatically"
+	#define CONFIG_USB_VENDOR_REQ_MUTEX
+#endif
+#if defined(CONFIG_VENDOR_REQ_RETRY) &&  !defined(CONFIG_USB_VENDOR_REQ_MUTEX)
+	#warning "define CONFIG_USB_VENDOR_REQ_MUTEX for CONFIG_VENDOR_REQ_RETRY automatically"
+	#define CONFIG_USB_VENDOR_REQ_MUTEX
+#endif
+
+#define DYNAMIC_CAMID_ALLOC
+
+#ifndef CONFIG_RTW_HIQ_FILTER
+	#define CONFIG_RTW_HIQ_FILTER 1
+#endif
+
+//#include <rtl871x_byteorder.h>
+
+#endif // __DRV_CONF_H__
diff --git a/drivers/staging/rtl8723bs/include/drv_types.h b/drivers/staging/rtl8723bs/include/drv_types.h
new file mode 100644
index 000000000000..4d14fbc5a1fe
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/drv_types.h
@@ -0,0 +1,720 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*-------------------------------------------------------------------------------
+
+	For type defines and data structure defines
+
+--------------------------------------------------------------------------------*/
+
+
+#ifndef __DRV_TYPES_H__
+#define __DRV_TYPES_H__
+
+#include <linux/version.h>
+#include <linux/sched/signal.h>
+#include <autoconf.h>
+#include <basic_types.h>
+#include <osdep_service.h>
+#include <rtw_byteorder.h>
+#include <wlan_bssdef.h>
+#include <wifi.h>
+#include <ieee80211.h>
+
+enum _NIC_VERSION {
+
+	RTL8711_NIC,
+	RTL8712_NIC,
+	RTL8713_NIC,
+	RTL8716_NIC
+
+};
+
+#include <rtw_rf.h>
+
+#include <rtw_ht.h>
+
+#ifdef CONFIG_INTEL_WIDI
+#include <rtw_intel_widi.h>
+#endif
+
+#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.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_efuse.h>
+#include <rtw_version.h>
+#include <rtw_odm.h>
+
+#include "ioctl_cfg80211.h"
+
+#include <linux/ip.h>
+#include <linux/if_ether.h>
+#include <ethernet.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 specific_device_id{
+
+	u32 	flags;
+
+	u16 	idVendor;
+	u16 	idProduct;
+
+};
+
+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: 80 MHz, 3: 160MHz */
+	/*  2.4G use bit 0 ~ 3, 5G use bit 4 ~ 7 */
+	/*  0x21 means enable 2.4G 40MHz & 5G 80MHz */
+	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 rf_config ;
+	u8 low_power ;
+
+	u8 wifi_spec;/*  !turbo_mode */
+
+	u8 channel_plan;
+
+	u8 btcoex;
+	u8 bt_iso;
+	u8 bt_sco;
+	u8 bt_ampdu;
+	s8	ant_num;
+
+	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 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;
+	s8	TxBBSwing_5G;
+	u8 AmplifierType_2G;
+	u8 AmplifierType_5G;
+	u8 bEn_RFE;
+	u8 RFE_Type;
+	u8  check_fw_ps;
+
+	u8 load_phy_file;
+	u8 RegDecryptCustomFile;
+
+#ifdef CONFIG_MULTI_VIR_IFACES
+	u8 ext_iface_num;/* primary/secondary iface is excluded */
+#endif
+	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 INTF_DATA SDIO_DATA
+
+#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])
+
+#ifdef CONFIG_DBG_COUNTER
+
+struct rx_logs {
+	u32 intf_rx;
+	u32 intf_rx_err_recvframe;
+	u32 intf_rx_err_skb;
+	u32 intf_rx_report;
+	u32 core_rx;
+	u32 core_rx_pre;
+	u32 core_rx_pre_ver_err;
+	u32 core_rx_pre_mgmt;
+	u32 core_rx_pre_mgmt_err_80211w;
+	u32 core_rx_pre_mgmt_err;
+	u32 core_rx_pre_ctrl;
+	u32 core_rx_pre_ctrl_err;
+	u32 core_rx_pre_data;
+	u32 core_rx_pre_data_wapi_seq_err;
+	u32 core_rx_pre_data_wapi_key_err;
+	u32 core_rx_pre_data_handled;
+	u32 core_rx_pre_data_err;
+	u32 core_rx_pre_data_unknown;
+	u32 core_rx_pre_unknown;
+	u32 core_rx_enqueue;
+	u32 core_rx_dequeue;
+	u32 core_rx_post;
+	u32 core_rx_post_decrypt;
+	u32 core_rx_post_decrypt_wep;
+	u32 core_rx_post_decrypt_tkip;
+	u32 core_rx_post_decrypt_aes;
+	u32 core_rx_post_decrypt_wapi;
+	u32 core_rx_post_decrypt_hw;
+	u32 core_rx_post_decrypt_unknown;
+	u32 core_rx_post_decrypt_err;
+	u32 core_rx_post_defrag_err;
+	u32 core_rx_post_portctrl_err;
+	u32 core_rx_post_indicate;
+	u32 core_rx_post_indicate_in_oder;
+	u32 core_rx_post_indicate_reoder;
+	u32 core_rx_post_indicate_err;
+	u32 os_indicate;
+	u32 os_indicate_ap_mcast;
+	u32 os_indicate_ap_forward;
+	u32 os_indicate_ap_self;
+	u32 os_indicate_err;
+	u32 os_netif_ok;
+	u32 os_netif_err;
+};
+
+struct tx_logs {
+	u32 os_tx;
+	u32 os_tx_err_up;
+	u32 os_tx_err_xmit;
+	u32 os_tx_m2u;
+	u32 os_tx_m2u_ignore_fw_linked;
+	u32 os_tx_m2u_ignore_self;
+	u32 os_tx_m2u_entry;
+	u32 os_tx_m2u_entry_err_xmit;
+	u32 os_tx_m2u_entry_err_skb;
+	u32 os_tx_m2u_stop;
+	u32 core_tx;
+	u32 core_tx_err_pxmitframe;
+	u32 core_tx_err_brtx;
+	u32 core_tx_upd_attrib;
+	u32 core_tx_upd_attrib_adhoc;
+	u32 core_tx_upd_attrib_sta;
+	u32 core_tx_upd_attrib_ap;
+	u32 core_tx_upd_attrib_unknown;
+	u32 core_tx_upd_attrib_dhcp;
+	u32 core_tx_upd_attrib_icmp;
+	u32 core_tx_upd_attrib_active;
+	u32 core_tx_upd_attrib_err_ucast_sta;
+	u32 core_tx_upd_attrib_err_ucast_ap_link;
+	u32 core_tx_upd_attrib_err_sta;
+	u32 core_tx_upd_attrib_err_link;
+	u32 core_tx_upd_attrib_err_sec;
+	u32 core_tx_ap_enqueue_warn_fwstate;
+	u32 core_tx_ap_enqueue_warn_sta;
+	u32 core_tx_ap_enqueue_warn_nosta;
+	u32 core_tx_ap_enqueue_warn_link;
+	u32 core_tx_ap_enqueue_warn_trigger;
+	u32 core_tx_ap_enqueue_mcast;
+	u32 core_tx_ap_enqueue_ucast;
+	u32 core_tx_ap_enqueue;
+	u32 intf_tx;
+	u32 intf_tx_pending_ac;
+	u32 intf_tx_pending_fw_under_survey;
+	u32 intf_tx_pending_fw_under_linking;
+	u32 intf_tx_pending_xmitbuf;
+	u32 intf_tx_enqueue;
+	u32 core_tx_enqueue;
+	u32 core_tx_enqueue_class;
+	u32 core_tx_enqueue_class_err_sta;
+	u32 core_tx_enqueue_class_err_nosta;
+	u32 core_tx_enqueue_class_err_fwlink;
+	u32 intf_tx_direct;
+	u32 intf_tx_direct_err_coalesce;
+	u32 intf_tx_dequeue;
+	u32 intf_tx_dequeue_err_coalesce;
+	u32 intf_tx_dump_xframe;
+	u32 intf_tx_dump_xframe_err_txdesc;
+	u32 intf_tx_dump_xframe_err_port;
+};
+
+struct int_logs {
+	u32 all;
+	u32 err;
+	u32 tbdok;
+	u32 tbder;
+	u32 bcnderr;
+	u32 bcndma;
+	u32 bcndma_e;
+	u32 rx;
+	u32 rx_rdu;
+	u32 rx_fovw;
+	u32 txfovw;
+	u32 mgntok;
+	u32 highdok;
+	u32 bkdok;
+	u32 bedok;
+	u32 vidok;
+	u32 vodok;
+};
+
+#endif /*  CONFIG_DBG_COUNTER */
+
+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_toogle_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 {
+	_lock lock;
+	u64 bitmap;
+};
+
+struct cam_entry_cache {
+	u16 ctrl;
+	u8 mac[ETH_ALEN];
+	u8 key[16];
+};
+
+#define KEY_FMT "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
+#define KEY_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5], \
+	((u8 *)(x))[6], ((u8 *)(x))[7], ((u8 *)(x))[8], ((u8 *)(x))[9], ((u8 *)(x))[10], ((u8 *)(x))[11], \
+	((u8 *)(x))[12], ((u8 *)(x))[13], ((u8 *)(x))[14], ((u8 *)(x))[15]
+
+struct dvobj_priv
+{
+	/*-------- below is common data --------*/
+	struct adapter *if1; /* PRIMARY_ADAPTER */
+	struct adapter *if2; /* SECONDARY_ADAPTER */
+
+	s32	processing_dev_remove;
+
+	struct debug_priv drv_dbg;
+
+	/* for local/global synchronization */
+	/*  */
+	_lock	lock;
+	int macid[NUM_STA];
+
+	_mutex hw_init_mutex;
+	_mutex h2c_fwcmd_mutex;
+	_mutex setch_mutex;
+	_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];
+
+	/* For 92D, DMDP have 2 interface. */
+	u8 InterfaceNumber;
+	u8 NumInterfaces;
+
+	/* 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 --------*/
+
+#ifdef INTF_DATA
+	INTF_DATA intf_data;
+#endif
+};
+
+#define dvobj_to_pwrctl(dvobj) (&(dvobj->pwrctl_priv))
+#define pwrctl_to_dvobj(pwrctl) container_of(pwrctl, struct dvobj_priv, pwrctl_priv)
+
+__inline static 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_TYPE {
+	IFACE_PORT0, /* mapping to port0 for C/D series chips */
+	IFACE_PORT1, /* mapping to port1 for C/D series chip */
+	MAX_IFACE_PORT,
+};
+
+enum ADAPTER_TYPE {
+	PRIMARY_ADAPTER,
+	SECONDARY_ADAPTER,
+	MAX_ADAPTER = 0xFF,
+};
+
+typedef enum _DRIVER_STATE{
+	DRIVER_NORMAL = 0,
+	DRIVER_DISAPPEAR = 1,
+	DRIVER_REPLACE_DONGLE = 2,
+}DRIVER_STATE;
+
+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;
+	_lock   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);
+
+	_nic_hdl pnetdev;
+	char old_ifname[IFNAMSIZ];
+
+	/*  used by rtw_rereg_nd_name related function */
+	struct rereg_nd_name_data {
+		_nic_hdl 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 */
+	struct proc_dir_entry *dir_odm;
+
+	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 inteface 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;
+
+#ifdef CONFIG_DBG_COUNTER
+	struct rx_logs rx_logs;
+	struct tx_logs tx_logs;
+	struct int_logs int_logs;
+#endif
+};
+
+#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))) */
+__inline static 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);
+}
+
+__inline static 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))
+
+#ifdef CONFIG_GPIO_API
+int rtw_get_gpio(struct net_device *netdev, int gpio_num);
+int rtw_set_gpio_output_value(struct net_device *netdev, int gpio_num, bool isHigh);
+int rtw_config_gpio(struct net_device *netdev, int gpio_num, bool isOutput);
+#endif
+
+#ifdef CONFIG_WOWLAN
+int rtw_suspend_wow(struct adapter *padapter);
+int rtw_resume_process_wow(struct adapter *padapter);
+#endif
+
+__inline static 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>
+
+void rtw_indicate_wx_disassoc_event(struct adapter *padapter);
+void rtw_indicate_wx_assoc_event(struct adapter *padapter);
+void rtw_indicate_wx_disassoc_event(struct adapter *padapter);
+void indicate_wx_scan_complete_event(struct adapter *padapter);
+int rtw_change_ifname(struct adapter *padapter, const char *ifname);
+
+extern char *rtw_phy_file_path;
+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 000000000000..aef9bf71ab25
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/drv_types_sdio.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __DRV_TYPES_SDIO_H__
+#define __DRV_TYPES_SDIO_H__
+
+/*  SDIO Header Files */
+	#include <linux/mmc/sdio_func.h>
+	#include <linux/mmc/sdio_ids.h>
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+	#include <linux/mmc/host.h>
+	#include <linux/mmc/card.h>
+#endif
+
+typedef 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;
+} SDIO_DATA, *PSDIO_DATA;
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/ethernet.h b/drivers/staging/rtl8723bs/include/ethernet.h
new file mode 100644
index 000000000000..bd7099497b95
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/ethernet.h
@@ -0,0 +1,22 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+/*! \file */
+#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/rtl8723bs/include/hal_btcoex.h b/drivers/staging/rtl8723bs/include/hal_btcoex.h
new file mode 100644
index 000000000000..7ee59c07fbf9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_btcoex.h
@@ -0,0 +1,68 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HAL_BTCOEX_H__
+#define __HAL_BTCOEX_H__
+
+#include <drv_types.h>
+
+/*  Some variables can't get from outsrc BT-Coex, */
+/*  so we need to save here */
+typedef struct _BT_COEXIST
+{
+	u8 bBtExist;
+	u8 btTotalAntNum;
+	u8 btChipType;
+	u8 bInitlized;
+} BT_COEXIST, *PBT_COEXIST;
+
+void DBG_BT_INFO(u8 *dbgmsg);
+
+void hal_btcoex_SetBTCoexist(struct adapter *padapter, u8 bBtExist);
+u8 hal_btcoex_IsBtExist(struct adapter *padapter);
+u8 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);
+
+u8 hal_btcoex_Initialize(struct adapter *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_Hanlder(struct adapter *padapter);
+
+s32 hal_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *padapter);
+void hal_btcoex_SetManualControl(struct adapter *padapter, u8 bmanual);
+u8 hal_btcoex_IsBtControlLps(struct adapter *);
+u8 hal_btcoex_IsLpsOn(struct adapter *);
+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);
+void hal_btcoex_DisplayBtCoexInfo(struct adapter *, u8 *pbuf, u32 bufsize);
+void hal_btcoex_SetDBG(struct adapter *, u32 *pDbgModule);
+u32 hal_btcoex_GetDBG(struct adapter *, u8 *pStrBuf, u32 bufSize);
+
+#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 000000000000..3e9ed3b66632
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com.h
@@ -0,0 +1,309 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 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_RATEMCS16				0x1C
+#define DESC_RATEMCS17				0x1D
+#define DESC_RATEMCS18				0x1E
+#define DESC_RATEMCS19				0x1F
+#define DESC_RATEMCS20				0x20
+#define DESC_RATEMCS21				0x21
+#define DESC_RATEMCS22				0x22
+#define DESC_RATEMCS23				0x23
+#define DESC_RATEMCS24				0x24
+#define DESC_RATEMCS25				0x25
+#define DESC_RATEMCS26				0x26
+#define DESC_RATEMCS27				0x27
+#define DESC_RATEMCS28				0x28
+#define DESC_RATEMCS29				0x29
+#define DESC_RATEMCS30				0x2A
+#define DESC_RATEMCS31				0x2B
+#define DESC_RATEVHTSS1MCS0		0x2C
+#define DESC_RATEVHTSS1MCS1		0x2D
+#define DESC_RATEVHTSS1MCS2		0x2E
+#define DESC_RATEVHTSS1MCS3		0x2F
+#define DESC_RATEVHTSS1MCS4		0x30
+#define DESC_RATEVHTSS1MCS5		0x31
+#define DESC_RATEVHTSS1MCS6		0x32
+#define DESC_RATEVHTSS1MCS7		0x33
+#define DESC_RATEVHTSS1MCS8		0x34
+#define DESC_RATEVHTSS1MCS9		0x35
+#define DESC_RATEVHTSS2MCS0		0x36
+#define DESC_RATEVHTSS2MCS1		0x37
+#define DESC_RATEVHTSS2MCS2		0x38
+#define DESC_RATEVHTSS2MCS3		0x39
+#define DESC_RATEVHTSS2MCS4		0x3A
+#define DESC_RATEVHTSS2MCS5		0x3B
+#define DESC_RATEVHTSS2MCS6		0x3C
+#define DESC_RATEVHTSS2MCS7		0x3D
+#define DESC_RATEVHTSS2MCS8		0x3E
+#define DESC_RATEVHTSS2MCS9		0x3F
+#define DESC_RATEVHTSS3MCS0		0x40
+#define DESC_RATEVHTSS3MCS1		0x41
+#define DESC_RATEVHTSS3MCS2		0x42
+#define DESC_RATEVHTSS3MCS3		0x43
+#define DESC_RATEVHTSS3MCS4		0x44
+#define DESC_RATEVHTSS3MCS5		0x45
+#define DESC_RATEVHTSS3MCS6		0x46
+#define DESC_RATEVHTSS3MCS7		0x47
+#define DESC_RATEVHTSS3MCS8		0x48
+#define DESC_RATEVHTSS3MCS9		0x49
+#define DESC_RATEVHTSS4MCS0		0x4A
+#define DESC_RATEVHTSS4MCS1		0x4B
+#define DESC_RATEVHTSS4MCS2		0x4C
+#define DESC_RATEVHTSS4MCS3		0x4D
+#define DESC_RATEVHTSS4MCS4		0x4E
+#define DESC_RATEVHTSS4MCS5		0x4F
+#define DESC_RATEVHTSS4MCS6		0x50
+#define DESC_RATEVHTSS4MCS7		0x51
+#define DESC_RATEVHTSS4MCS8		0x52
+#define DESC_RATEVHTSS4MCS9		0x53
+
+#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":\
+(rate ==DESC_RATEMCS8)?"MCS8":\
+(rate ==DESC_RATEMCS9)?"MCS9":\
+(rate ==DESC_RATEMCS10)?"MCS10":\
+(rate ==DESC_RATEMCS11)?"MCS11":\
+(rate ==DESC_RATEMCS12)?"MCS12":\
+(rate ==DESC_RATEMCS13)?"MCS13":\
+(rate ==DESC_RATEMCS14)?"MCS14":\
+(rate ==DESC_RATEMCS15)?"MCS15":\
+(rate ==DESC_RATEVHTSS1MCS0)?"VHTSS1MCS0":\
+(rate ==DESC_RATEVHTSS1MCS1)?"VHTSS1MCS1":\
+(rate ==DESC_RATEVHTSS1MCS2)?"VHTSS1MCS2":\
+(rate ==DESC_RATEVHTSS1MCS3)?"VHTSS1MCS3":\
+(rate ==DESC_RATEVHTSS1MCS4)?"VHTSS1MCS4":\
+(rate ==DESC_RATEVHTSS1MCS5)?"VHTSS1MCS5":\
+(rate ==DESC_RATEVHTSS1MCS6)?"VHTSS1MCS6":\
+(rate ==DESC_RATEVHTSS1MCS7)?"VHTSS1MCS7":\
+(rate ==DESC_RATEVHTSS1MCS8)?"VHTSS1MCS8":\
+(rate ==DESC_RATEVHTSS1MCS9)?"VHTSS1MCS9":\
+(rate ==DESC_RATEVHTSS2MCS0)?"VHTSS2MCS0":\
+(rate ==DESC_RATEVHTSS2MCS1)?"VHTSS2MCS1":\
+(rate ==DESC_RATEVHTSS2MCS2)?"VHTSS2MCS2":\
+(rate ==DESC_RATEVHTSS2MCS3)?"VHTSS2MCS3":\
+(rate ==DESC_RATEVHTSS2MCS4)?"VHTSS2MCS4":\
+(rate ==DESC_RATEVHTSS2MCS5)?"VHTSS2MCS5":\
+(rate ==DESC_RATEVHTSS2MCS6)?"VHTSS2MCS6":\
+(rate ==DESC_RATEVHTSS2MCS7)?"VHTSS2MCS7":\
+(rate ==DESC_RATEVHTSS2MCS8)?"VHTSS2MCS8":\
+(rate ==DESC_RATEVHTSS2MCS9)?"VHTSS2MCS9":"UNKNOW"
+
+
+enum{
+	UP_LINK,
+	DOWN_LINK,
+};
+typedef enum _RT_MEDIA_STATUS {
+	RT_MEDIA_DISCONNECT = 0,
+	RT_MEDIA_CONNECT       = 1
+} RT_MEDIA_STATUS;
+
+#define MAX_DLFW_PAGE_SIZE			4096	/*  @ page : 4k bytes */
+enum FIRMWARE_SOURCE {
+	FW_SOURCE_IMG_FILE = 0,
+	FW_SOURCE_HEADER_FILE = 1,		/* from header file */
+};
+
+/*  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))
+#define PageNum_256(_Len)		(u32)(((_Len)>>8) + ((_Len)&0xFF ? 1:0))
+#define PageNum_512(_Len)		(u32)(((_Len)>>9) + ((_Len)&0x1FF ? 1:0))
+#define PageNum(_Len, _Size)		(u32)(((_Len)/(_Size)) + ((_Len)&((_Size) - 1) ? 1:0))
+
+
+u8 rtw_hal_data_init(struct adapter *padapter);
+void rtw_hal_data_deinit(struct adapter *padapter);
+
+void dump_chip_info(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_hal_networktype_to_raid(struct adapter *adapter, struct sta_info *psta);
+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);
+
+bool IsHexDigit(char chTmp);
+
+u32 MapCharToHexDigit(char chTmp);
+
+bool GetHexValueFromString(char *szStr, u32 *pu4bVal, u32 *pu4bMove);
+
+bool GetFractionValueFromString(char *szStr, u8 *pInteger, u8 *pFraction,
+				u32 *pu4bMove);
+
+bool IsCommentString(char *szStr);
+
+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);
+
+#ifdef CONFIG_BACKGROUND_NOISE_MONITOR
+struct noise_info
+{
+	u8 bPauseDIG;
+	u8 IGIValue;
+	u32 max_time;/* ms */
+	u8 chan;
+};
+#endif
+
+#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 000000000000..86b0c42295c2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_h2c.h
@@ -0,0 +1,293 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __COMMON_H2C_H__
+#define __COMMON_H2C_H__
+
+/*  */
+/*     H2C CMD DEFINITION    ------------------------------------------------ */
+/*  */
+/*  88e, 8723b, 8812, 8821, 92e use the same FW code base */
+enum h2c_cmd{
+	/* Common Class: 000 */
+	H2C_RSVD_PAGE = 0x00,
+	H2C_MEDIA_STATUS_RPT = 0x01,
+	H2C_SCAN_ENABLE = 0x02,
+	H2C_KEEP_ALIVE = 0x03,
+	H2C_DISCON_DECISION = 0x04,
+	H2C_PSD_OFFLOAD = 0x05,
+	H2C_AP_OFFLOAD = 0x08,
+	H2C_BCN_RSVDPAGE = 0x09,
+	H2C_PROBERSP_RSVDPAGE = 0x0A,
+	H2C_FCS_RSVDPAGE = 0x10,
+	H2C_FCS_INFO = 0x11,
+	H2C_AP_WOW_GPIO_CTRL = 0x13,
+
+	/* PoweSave Class: 001 */
+	H2C_SET_PWR_MODE = 0x20,
+	H2C_PS_TUNING_PARA = 0x21,
+	H2C_PS_TUNING_PARA2 = 0x22,
+	H2C_P2P_LPS_PARAM = 0x23,
+	H2C_P2P_PS_OFFLOAD = 0x24,
+	H2C_PS_SCAN_ENABLE = 0x25,
+	H2C_SAP_PS_ = 0x26,
+	H2C_INACTIVE_PS_ = 0x27, /* Inactive_PS */
+	H2C_FWLPS_IN_IPS_ = 0x28,
+
+	/* Dynamic Mechanism Class: 010 */
+	H2C_MACID_CFG = 0x40,
+	H2C_TXBF = 0x41,
+	H2C_RSSI_SETTING = 0x42,
+	H2C_AP_REQ_TXRPT = 0x43,
+	H2C_INIT_RATE_COLLECT = 0x44,
+
+	/* BT Class: 011 */
+	H2C_B_TYPE_TDMA = 0x60,
+	H2C_BT_INFO = 0x61,
+	H2C_FORCE_BT_TXPWR = 0x62,
+	H2C_BT_IGNORE_WLANACT = 0x63,
+	H2C_DAC_SWING_VALUE = 0x64,
+	H2C_ANT_SEL_RSV = 0x65,
+	H2C_WL_OPMODE = 0x66,
+	H2C_BT_MP_OPER = 0x67,
+	H2C_BT_CONTROL = 0x68,
+	H2C_BT_WIFI_CTRL = 0x69,
+	H2C_BT_FW_PATCH = 0x6A,
+
+	/* WOWLAN Class: 100 */
+	H2C_WOWLAN = 0x80,
+	H2C_REMOTE_WAKE_CTRL = 0x81,
+	H2C_AOAC_GLOBAL_INFO = 0x82,
+	H2C_AOAC_RSVD_PAGE = 0x83,
+	H2C_AOAC_RSVD_PAGE2 = 0x84,
+	H2C_D0_SCAN_OFFLOAD_CTRL = 0x85,
+	H2C_D0_SCAN_OFFLOAD_INFO = 0x86,
+	H2C_CHNL_SWITCH_OFFLOAD = 0x87,
+	H2C_AOAC_RSVDPAGE3 = 0x88,
+
+	H2C_RESET_TSF = 0xC0,
+	H2C_MAXID,
+};
+
+#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
+
+#ifdef CONFIG_WOWLAN
+#define eqMacAddr(a, b)		(((a)[0]==(b)[0] && (a)[1]==(b)[1] && (a)[2]==(b)[2] && (a)[3]==(b)[3] && (a)[4]==(b)[4] && (a)[5]==(b)[5]) ? 1:0)
+#define cpMacAddr(des, src)	((des)[0]=(src)[0], (des)[1]=(src)[1], (des)[2]=(src)[2], (des)[3]=(src)[3], (des)[4]=(src)[4], (des)[5]=(src)[5])
+#define cpIpAddr(des, src)	((des)[0]=(src)[0], (des)[1]=(src)[1], (des)[2]=(src)[2], (des)[3]=(src)[3])
+
+/*  */
+/*  ARP packet */
+/*  */
+/*  LLC Header */
+#define GET_ARP_PKT_LLC_TYPE(__pHeader)			ReadEF2Byte(((u8 *)(__pHeader)) + 6)
+
+/* ARP element */
+#define GET_ARP_PKT_OPERATION(__pHeader)		ReadEF2Byte(((u8 *)(__pHeader)) + 6)
+#define GET_ARP_PKT_SENDER_MAC_ADDR(__pHeader, _val)	cpMacAddr((u8 *)(_val), ((u8 *)(__pHeader))+8)
+#define GET_ARP_PKT_SENDER_IP_ADDR(__pHeader, _val)	cpIpAddr((u8 *)(_val), ((u8 *)(__pHeader))+14)
+#define GET_ARP_PKT_TARGET_MAC_ADDR(__pHeader, _val)	cpMacAddr((u8 *)(_val), ((u8 *)(__pHeader))+18)
+#define GET_ARP_PKT_TARGET_IP_ADDR(__pHeader, _val)	cpIpAddr((u8 *)(_val), ((u8 *)(__pHeader))+24)
+
+#define SET_ARP_PKT_HW(__pHeader, __Value)		WRITEEF2BYTE(((u8 *)(__pHeader)) + 0, __Value)
+#define SET_ARP_PKT_PROTOCOL(__pHeader, __Value)	WRITEEF2BYTE(((u8 *)(__pHeader)) + 2, __Value)
+#define SET_ARP_PKT_HW_ADDR_LEN(__pHeader, __Value)	WRITEEF1BYTE(((u8 *)(__pHeader)) + 4, __Value)
+#define SET_ARP_PKT_PROTOCOL_ADDR_LEN(__pHeader, __Value)	WRITEEF1BYTE(((u8 *)(__pHeader)) + 5, __Value)
+#define SET_ARP_PKT_OPERATION(__pHeader, __Value)	WRITEEF2BYTE(((u8 *)(__pHeader)) + 6, __Value)
+#define SET_ARP_PKT_SENDER_MAC_ADDR(__pHeader, _val)	cpMacAddr(((u8 *)(__pHeader))+8, (u8 *)(_val))
+#define SET_ARP_PKT_SENDER_IP_ADDR(__pHeader, _val)	cpIpAddr(((u8 *)(__pHeader))+14, (u8 *)(_val))
+#define SET_ARP_PKT_TARGET_MAC_ADDR(__pHeader, _val)	cpMacAddr(((u8 *)(__pHeader))+18, (u8 *)(_val))
+#define SET_ARP_PKT_TARGET_IP_ADDR(__pHeader, _val)	cpIpAddr(((u8 *)(__pHeader))+24, (u8 *)(_val))
+
+#define FW_WOWLAN_FUN_EN			BIT(0)
+#define FW_WOWLAN_PATTERN_MATCH			BIT(1)
+#define FW_WOWLAN_MAGIC_PKT			BIT(2)
+#define FW_WOWLAN_UNICAST			BIT(3)
+#define FW_WOWLAN_ALL_PKT_DROP			BIT(4)
+#define FW_WOWLAN_GPIO_ACTIVE			BIT(5)
+#define FW_WOWLAN_REKEY_WAKEUP			BIT(6)
+#define FW_WOWLAN_DEAUTH_WAKEUP			BIT(7)
+
+#define FW_WOWLAN_GPIO_WAKEUP_EN		BIT(0)
+#define FW_FW_PARSE_MAGIC_PKT			BIT(1)
+
+#define FW_REMOTE_WAKE_CTRL_EN			BIT(0)
+#define FW_REALWOWLAN_EN			BIT(5)
+
+#define FW_WOWLAN_KEEP_ALIVE_EN			BIT(0)
+#define FW_ADOPT_USER				BIT(1)
+#define FW_WOWLAN_KEEP_ALIVE_PKT_TYPE		BIT(2)
+
+#define FW_REMOTE_WAKE_CTRL_EN			BIT(0)
+#define FW_ARP_EN				BIT(1)
+#define FW_REALWOWLAN_EN			BIT(5)
+#define FW_WOW_FW_UNICAST_EN			BIT(7)
+
+#endif /* CONFIG_WOWLAN */
+
+/* _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)
+
+#ifdef CONFIG_AP_WOWLAN
+/* _AP_Offload 0x08 */
+#define SET_H2CCMD_AP_WOWLAN_EN(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+/* _BCN_RsvdPage	0x09 */
+#define SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_BCN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+/* _Probersp_RsvdPage 0x0a */
+#define SET_H2CCMD_AP_WOWLAN_RSVDPAGE_LOC_ProbeRsp(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT(__pH2CCmd, 0, 8, __Value)
+/* _Probersp_RsvdPage 0x13 */
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_INDEX(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 4, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_C2H_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 1, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_PLUS(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 5, 1, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_HIGH_ACTIVE(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 6, 1, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 7, 1, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_DURATION(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_H2CCMD_AP_WOW_GPIO_CTRL_C2H_DURATION(__pH2CCmd, __Value)SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+/* _AP_PS 0x26 */
+#define SET_H2CCMD_AP_WOW_PS_EN(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
+#define SET_H2CCMD_AP_WOW_PS_32K_EN(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
+#define SET_H2CCMD_AP_WOW_PS_RF(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
+#define SET_H2CCMD_AP_WOW_PS_DURATION(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#endif
+
+/*  _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)
+#ifdef CONFIG_GTK_OL
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+5, 0, 8, __Value)
+#endif /* CONFIG_GTK_OL */
+#ifdef CONFIG_PNO_SUPPORT
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_NLO_INFO(__pH2CCmd, __Value)		SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd), 0, 8, __Value)
+#endif
+
+#ifdef CONFIG_PNO_SUPPORT
+/*  D0_Scan_Offload_Info_0x86 */
+#define SET_H2CCMD_AOAC_NLO_FUN_EN(__pH2CCmd, __Value)			SET_BITS_TO_LE_1BYTE((__pH2CCmd), 3, 1, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_PROBE_PACKET(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+1, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_SCAN_INFO(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+2, 0, 8, __Value)
+#define SET_H2CCMD_AOAC_RSVDPAGE_LOC_SSID_INFO(__pH2CCmd, __Value)	SET_BITS_TO_LE_1BYTE_8BIT((__pH2CCmd)+3, 0, 8, __Value)
+#endif /* CONFIG_PNO_SUPPORT */
+
+/*  */
+/*     Structure    -------------------------------------------------- */
+/*  */
+typedef struct _RSVDPAGE_LOC {
+	u8 LocProbeRsp;
+	u8 LocPsPoll;
+	u8 LocNullData;
+	u8 LocQosNull;
+	u8 LocBTQosNull;
+#ifdef CONFIG_WOWLAN
+	u8 LocRemoteCtrlInfo;
+	u8 LocArpRsp;
+	u8 LocNbrAdv;
+	u8 LocGTKRsp;
+	u8 LocGTKInfo;
+	u8 LocProbeReq;
+	u8 LocNetList;
+#ifdef CONFIG_GTK_OL
+	u8 LocGTKEXTMEM;
+#endif /* CONFIG_GTK_OL */
+#ifdef CONFIG_PNO_SUPPORT
+	u8 LocPNOInfo;
+	u8 LocScanInfo;
+	u8 LocSSIDInfo;
+	u8 LocProbePacket;
+#endif /* CONFIG_PNO_SUPPORT */
+#endif /* CONFIG_WOWLAN */
+#ifdef CONFIG_AP_WOWLAN
+	u8 LocApOffloadBCN;
+#endif /* CONFIG_AP_WOWLAN */
+} RSVDPAGE_LOC, *PRSVDPAGE_LOC;
+
+#endif
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+void rtw_get_current_ip_address(struct adapter *padapter, u8 *pcurrentip);
+void rtw_get_sec_iv(struct adapter *padapter, u8*pcur_dot11txpn, u8 *StaAddr);
+void rtw_set_sec_pn(struct adapter *padapter);
+#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 000000000000..bcd81f50581f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_phycfg.h
@@ -0,0 +1,273 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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,
+	HT_MCS8_MCS15,
+	HT_MCS16_MCS23,
+	HT_MCS24_MCS31,
+	VHT_1SSMCS0_1SSMCS9,
+	VHT_2SSMCS0_2SSMCS9,
+	VHT_3SSMCS0_3SSMCS9,
+	VHT_4SSMCS0_4SSMCS9,
+};
+
+enum RF_TX_NUM {
+	RF_1TX = 0,
+	RF_2TX,
+	RF_3TX,
+	RF_4TX,
+	RF_MAX_TX_NUM,
+	RF_TX_NUM_NONIMPLEMENT,
+};
+
+#define MAX_POWER_INDEX			0x3F
+
+enum _REGULATION_TXPWR_LMT {
+	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 		Band,
+u8 		RfPath,
+u8 		TxNum,
+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 	Band,
+u8 	RFPath,
+u8 	TxNum,
+u8 	RateIndex
+	);
+
+void
+PHY_SetTxPowerByRate(
+struct adapter *padapter,
+u8 	Band,
+u8 	RFPath,
+u8 	TxNum,
+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 		Band,
+u32 		RfPath,
+u32 		TxNum,
+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,
+	bool		*bIn24G
+	);
+
+s8 PHY_GetTxPowerLimit (struct adapter *adapter, u32 RegPwrTblSel,
+			enum BAND_TYPE Band, enum CHANNEL_WIDTH Bandwidth,
+u8 		RfPath,
+u8 		DataRate,
+u8 		Channel
+	);
+
+void
+PHY_SetTxPowerLimit(
+struct adapter *		Adapter,
+u8 			*Regulation,
+u8 			*Band,
+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
+	);
+
+u8
+PHY_GetTxPowerIndex(
+struct adapter *		padapter,
+u8 			RFPath,
+u8 			Rate,
+enum CHANNEL_WIDTH		BandWidth,
+u8 			Channel
+	);
+
+void
+PHY_SetTxPowerIndex(
+struct adapter *	padapter,
+u32 			PowerIndex,
+u8 		RFPath,
+u8 		Rate
+	);
+
+void
+Hal_ChannelPlanToRegulation(
+struct adapter *	Adapter,
+u16 			ChannelPlan
+	);
+
+#define MAX_PARA_FILE_BUF_LEN	25600
+
+#define LOAD_MAC_PARA_FILE				BIT0
+#define LOAD_BB_PARA_FILE					BIT1
+#define LOAD_BB_PG_PARA_FILE				BIT2
+#define LOAD_BB_MP_PARA_FILE				BIT3
+#define LOAD_RF_PARA_FILE					BIT4
+#define LOAD_RF_TXPWR_TRACK_PARA_FILE	BIT5
+#define LOAD_RF_TXPWR_LMT_PARA_FILE		BIT6
+
+int phy_ConfigMACWithParaFile(struct adapter *Adapter, char*pFileName);
+
+int phy_ConfigBBWithParaFile(struct adapter *Adapter, char*pFileName, u32 ConfigType);
+
+int phy_ConfigBBWithPgParaFile(struct adapter *Adapter, char*pFileName);
+
+int phy_ConfigBBWithMpParaFile(struct adapter *Adapter, char*pFileName);
+
+int PHY_ConfigRFWithParaFile(struct adapter *Adapter, char*pFileName, u8 eRFPath);
+
+int PHY_ConfigRFWithTxPwrTrackParaFile(struct adapter *Adapter, char*pFileName);
+
+int PHY_ConfigRFWithPowerLimitTableParaFile(struct adapter *Adapter, char*pFileName);
+
+void phy_free_filebuf(struct adapter *padapter);
+
+#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 000000000000..fbf33dba1757
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_com_reg.h
@@ -0,0 +1,1725 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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_XCK_OUT_CTRL				0x007c /* RTL8188E */
+#define REG_MCUFWDL					0x0080
+#define REG_WOL_EVENT					0x0081 /* RTL8188E */
+#define REG_MCUTSTCFG					0x0084
+#define REG_FDHM0						0x0088
+#define REG_HOST_SUSP_CNT				0x00BC	/*  RTL8192C Host suspend counter on FPGA platform */
+#define REG_SYSTEM_ON_CTRL			0x00CC	/*  For 8723AE Reset after S3 */
+#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_FTISR						0x013C /* RTL8192C */
+#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_32K_CTRL					0x0194 /* RTL8188E */
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
+#define REG_C2HEVT_CLEAR				0x01AF
+#define REG_MCUTST_1					0x01c0
+#define REG_MCUTST_WOWLAN			0x01C7	/*  Defined after 8188E series. */
+#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
+
+/*  */
+/*        8192C Cmd9346CR bits					(Offset 0xA, 16bit) */
+/*  */
+#define CmdEEPROM_En				BIT5	 /*  EEPROM enable when set 1 */
+#define CmdEERPOMSEL				BIT4	/*  System EEPROM select, 0: boot from E-FUSE, 1: The EEPROM used is 9346 */
+#define Cmd9346CR_9356SEL			BIT4
+
+/*  */
+/*        8192C GPIO MUX Configuration Register (offset 0x40, 4 byte) */
+/*  */
+#define GPIOSEL_GPIO				0
+#define GPIOSEL_ENBT				BIT5
+
+/*  */
+/*        8192C GPIO PIN Control Register (offset 0x44, 4 byte) */
+/*  */
+#define GPIO_IN					REG_GPIO_PIN_CTRL		/*  GPIO pins input value */
+#define GPIO_OUT				(REG_GPIO_PIN_CTRL+1)	/*  GPIO pins output value */
+#define GPIO_IO_SEL				(REG_GPIO_PIN_CTRL+2)	/*  GPIO pins output enable when a bit is set to "1"; otherwise, input is configured. */
+#define GPIO_MOD				(REG_GPIO_PIN_CTRL+3)
+
+/*  */
+/*        8811A GPIO PIN Control Register (offset 0x60, 4 byte) */
+/*  */
+#define GPIO_IN_8811A			REG_GPIO_PIN_CTRL_2		/*  GPIO pins input value */
+#define GPIO_OUT_8811A			(REG_GPIO_PIN_CTRL_2+1)	/*  GPIO pins output value */
+#define GPIO_IO_SEL_8811A		(REG_GPIO_PIN_CTRL_2+2)	/*  GPIO pins output enable when a bit is set to "1"; otherwise, input is configured. */
+#define GPIO_MOD_8811A			(REG_GPIO_PIN_CTRL_2+3)
+
+/*  */
+/*        8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+/*  */
+#define HSIMR_GPIO12_0_INT_EN			BIT0
+#define HSIMR_SPS_OCP_INT_EN			BIT5
+#define HSIMR_RON_INT_EN				BIT6
+#define HSIMR_PDN_INT_EN				BIT7
+#define HSIMR_GPIO9_INT_EN				BIT25
+
+/*  */
+/*        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
+
+/*  */
+/*        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
+
+/*  */
+/*        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
+/* 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
+
+#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
+#define BW_OPMODE_5G				BIT1
+
+/*  */
+/*        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 Interrup 8 */
+#define IMR_BCNDOK7				BIT24		/*  Beacon Queue DMA OK Interrup 7 */
+#define IMR_BCNDOK6				BIT23		/*  Beacon Queue DMA OK Interrup 6 */
+#define IMR_BCNDOK5				BIT22		/*  Beacon Queue DMA OK Interrup 5 */
+#define IMR_BCNDOK4				BIT21		/*  Beacon Queue DMA OK Interrup 4 */
+#define IMR_BCNDOK3				BIT20		/*  Beacon Queue DMA OK Interrup 3 */
+#define IMR_BCNDOK2				BIT19		/*  Beacon Queue DMA OK Interrup 2 */
+#define IMR_BCNDOK1				BIT18		/*  Beacon Queue DMA OK Interrup 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 Interrup */
+#define IMR_HIGHDOK				BIT8		/*  High Queue DMA OK Interrupt */
+#define IMR_TBDOK				BIT7		/*  Transmit Beacon OK interrup */
+#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
+
+/*  */
+/*  8723E series PCIE Host IMR/ISR bit */
+/*  */
+/*  IMR DW0 Bit 0-31 */
+#define PHIMR_TIMEOUT2				BIT31
+#define PHIMR_TIMEOUT1				BIT30
+#define PHIMR_PSTIMEOUT			BIT29
+#define PHIMR_GTINT4				BIT28
+#define PHIMR_GTINT3				BIT27
+#define PHIMR_TXBCNERR				BIT26
+#define PHIMR_TXBCNOK				BIT25
+#define PHIMR_TSF_BIT32_TOGGLE	BIT24
+#define PHIMR_BCNDMAINT3			BIT23
+#define PHIMR_BCNDMAINT2			BIT22
+#define PHIMR_BCNDMAINT1			BIT21
+#define PHIMR_BCNDMAINT0			BIT20
+#define PHIMR_BCNDOK3				BIT19
+#define PHIMR_BCNDOK2				BIT18
+#define PHIMR_BCNDOK1				BIT17
+#define PHIMR_BCNDOK0				BIT16
+#define PHIMR_HSISR_IND_ON			BIT15
+#define PHIMR_BCNDMAINT_E			BIT14
+#define PHIMR_ATIMEND_E			BIT13
+#define PHIMR_ATIM_CTW_END		BIT12
+#define PHIMR_HISRE_IND			BIT11	/*  RO. HISRE Indicator (HISRE & HIMRE is true, this bit is set to 1) */
+#define PHIMR_C2HCMD				BIT10
+#define PHIMR_CPWM2				BIT9
+#define PHIMR_CPWM					BIT8
+#define PHIMR_HIGHDOK				BIT7		/*  High Queue DMA OK Interrupt */
+#define PHIMR_MGNTDOK				BIT6		/*  Management Queue DMA OK Interrupt */
+#define PHIMR_BKDOK					BIT5		/*  AC_BK DMA OK Interrupt */
+#define PHIMR_BEDOK					BIT4		/*  AC_BE DMA OK Interrupt */
+#define PHIMR_VIDOK					BIT3		/*  AC_VI DMA OK Interrupt */
+#define PHIMR_VODOK				BIT2		/*  AC_VO DMA Interrupt */
+#define PHIMR_RDU					BIT1		/*  Receive Descriptor Unavailable */
+#define PHIMR_ROK					BIT0		/*  Receive DMA OK Interrupt */
+
+/*  PCIE Host Interrupt Status Extension bit */
+#define PHIMR_BCNDMAINT7			BIT23
+#define PHIMR_BCNDMAINT6			BIT22
+#define PHIMR_BCNDMAINT5			BIT21
+#define PHIMR_BCNDMAINT4			BIT20
+#define PHIMR_BCNDOK7				BIT19
+#define PHIMR_BCNDOK6				BIT18
+#define PHIMR_BCNDOK5				BIT17
+#define PHIMR_BCNDOK4				BIT16
+/*  bit12 15: RSVD */
+#define PHIMR_TXERR					BIT11
+#define PHIMR_RXERR					BIT10
+#define PHIMR_TXFOVW				BIT9
+#define PHIMR_RXFOVW				BIT8
+/*  bit2-7: RSVD */
+#define PHIMR_OCPINT				BIT1
+/*  bit0: RSVD */
+
+#define UHIMR_TIMEOUT2				BIT31
+#define UHIMR_TIMEOUT1				BIT30
+#define UHIMR_PSTIMEOUT			BIT29
+#define UHIMR_GTINT4				BIT28
+#define UHIMR_GTINT3				BIT27
+#define UHIMR_TXBCNERR				BIT26
+#define UHIMR_TXBCNOK				BIT25
+#define UHIMR_TSF_BIT32_TOGGLE	BIT24
+#define UHIMR_BCNDMAINT3			BIT23
+#define UHIMR_BCNDMAINT2			BIT22
+#define UHIMR_BCNDMAINT1			BIT21
+#define UHIMR_BCNDMAINT0			BIT20
+#define UHIMR_BCNDOK3				BIT19
+#define UHIMR_BCNDOK2				BIT18
+#define UHIMR_BCNDOK1				BIT17
+#define UHIMR_BCNDOK0				BIT16
+#define UHIMR_HSISR_IND			BIT15
+#define UHIMR_BCNDMAINT_E			BIT14
+/* RSVD	BIT13 */
+#define UHIMR_CTW_END				BIT12
+/* RSVD	BIT11 */
+#define UHIMR_C2HCMD				BIT10
+#define UHIMR_CPWM2				BIT9
+#define UHIMR_CPWM					BIT8
+#define UHIMR_HIGHDOK				BIT7		/*  High Queue DMA OK Interrupt */
+#define UHIMR_MGNTDOK				BIT6		/*  Management Queue DMA OK Interrupt */
+#define UHIMR_BKDOK				BIT5		/*  AC_BK DMA OK Interrupt */
+#define UHIMR_BEDOK				BIT4		/*  AC_BE DMA OK Interrupt */
+#define UHIMR_VIDOK					BIT3		/*  AC_VI DMA OK Interrupt */
+#define UHIMR_VODOK				BIT2		/*  AC_VO DMA Interrupt */
+#define UHIMR_RDU					BIT1		/*  Receive Descriptor Unavailable */
+#define UHIMR_ROK					BIT0		/*  Receive DMA OK Interrupt */
+
+/*  USB Host Interrupt Status Extension bit */
+#define UHIMR_BCNDMAINT7			BIT23
+#define UHIMR_BCNDMAINT6			BIT22
+#define UHIMR_BCNDMAINT5			BIT21
+#define UHIMR_BCNDMAINT4			BIT20
+#define UHIMR_BCNDOK7				BIT19
+#define UHIMR_BCNDOK6				BIT18
+#define UHIMR_BCNDOK5				BIT17
+#define UHIMR_BCNDOK4				BIT16
+/*  bit14-15: RSVD */
+#define UHIMR_ATIMEND_E			BIT13
+#define UHIMR_ATIMEND				BIT12
+#define UHIMR_TXERR					BIT11
+#define UHIMR_RXERR					BIT10
+#define UHIMR_TXFOVW				BIT9
+#define UHIMR_RXFOVW				BIT8
+/*  bit2-7: RSVD */
+#define UHIMR_OCPINT				BIT1
+/*  bit0: RSVD */
+
+
+#define HAL_NIC_UNPLUG_ISR			0xFFFFFFFF	/*  The value when the NIC is unplugged for PCI. */
+#define HAL_NIC_UNPLUG_PCI_ISR		0xEAEAEAEA	/*  The value when the NIC is unplugged for PCI in PCI interrupt (page 3). */
+
+/*  */
+/*        8188 IMR/ISR bits */
+/*  */
+#define IMR_DISABLED_88E			0x0
+/*  IMR DW0(0x0060-0063) Bit 0-31 */
+#define IMR_TXCCK_88E				BIT30		/*  TXRPT interrupt when CCX bit of the packet is set */
+#define IMR_PSTIMEOUT_88E			BIT29		/*  Power Save Time Out Interrupt */
+#define IMR_GTINT4_88E				BIT28		/*  When GTIMER4 expires, this bit is set to 1 */
+#define IMR_GTINT3_88E				BIT27		/*  When GTIMER3 expires, this bit is set to 1 */
+#define IMR_TBDER_88E				BIT26		/*  Transmit Beacon0 Error */
+#define IMR_TBDOK_88E				BIT25		/*  Transmit Beacon0 OK */
+#define IMR_TSF_BIT32_TOGGLE_88E	BIT24		/*  TSF Timer BIT32 toggle indication interrupt */
+#define IMR_BCNDMAINT0_88E		BIT20		/*  Beacon DMA Interrupt 0 */
+#define IMR_BCNDERR0_88E			BIT16		/*  Beacon Queue DMA Error 0 */
+#define IMR_HSISR_IND_ON_INT_88E	BIT15		/*  HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define IMR_BCNDMAINT_E_88E		BIT14		/*  Beacon DMA Interrupt Extension for Win7 */
+#define IMR_ATIMEND_88E			BIT12		/*  CTWidnow End or ATIM Window End */
+#define IMR_HISR1_IND_INT_88E		BIT11		/*  HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1) */
+#define IMR_C2HCMD_88E				BIT10		/*  CPU to Host Command INT Status, Write 1 clear */
+#define IMR_CPWM2_88E				BIT9			/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define IMR_CPWM_88E				BIT8			/*  CPU power Mode exchange INT Status, Write 1 clear */
+#define IMR_HIGHDOK_88E			BIT7			/*  High Queue DMA OK */
+#define IMR_MGNTDOK_88E			BIT6			/*  Management Queue DMA OK */
+#define IMR_BKDOK_88E				BIT5			/*  AC_BK DMA OK */
+#define IMR_BEDOK_88E				BIT4			/*  AC_BE DMA OK */
+#define IMR_VIDOK_88E				BIT3			/*  AC_VI DMA OK */
+#define IMR_VODOK_88E				BIT2			/*  AC_VO DMA OK */
+#define IMR_RDU_88E					BIT1			/*  Rx Descriptor Unavailable */
+#define IMR_ROK_88E					BIT0			/*  Receive DMA OK */
+
+/*  IMR DW1(0x00B4-00B7) Bit 0-31 */
+#define IMR_BCNDMAINT7_88E		BIT27		/*  Beacon DMA Interrupt 7 */
+#define IMR_BCNDMAINT6_88E		BIT26		/*  Beacon DMA Interrupt 6 */
+#define IMR_BCNDMAINT5_88E		BIT25		/*  Beacon DMA Interrupt 5 */
+#define IMR_BCNDMAINT4_88E		BIT24		/*  Beacon DMA Interrupt 4 */
+#define IMR_BCNDMAINT3_88E		BIT23		/*  Beacon DMA Interrupt 3 */
+#define IMR_BCNDMAINT2_88E		BIT22		/*  Beacon DMA Interrupt 2 */
+#define IMR_BCNDMAINT1_88E		BIT21		/*  Beacon DMA Interrupt 1 */
+#define IMR_BCNDOK7_88E			BIT20		/*  Beacon Queue DMA OK Interrup 7 */
+#define IMR_BCNDOK6_88E			BIT19		/*  Beacon Queue DMA OK Interrup 6 */
+#define IMR_BCNDOK5_88E			BIT18		/*  Beacon Queue DMA OK Interrup 5 */
+#define IMR_BCNDOK4_88E			BIT17		/*  Beacon Queue DMA OK Interrup 4 */
+#define IMR_BCNDOK3_88E			BIT16		/*  Beacon Queue DMA OK Interrup 3 */
+#define IMR_BCNDOK2_88E			BIT15		/*  Beacon Queue DMA OK Interrup 2 */
+#define IMR_BCNDOK1_88E			BIT14		/*  Beacon Queue DMA OK Interrup 1 */
+#define IMR_ATIMEND_E_88E			BIT13		/*  ATIM Window End Extension for Win7 */
+#define IMR_TXERR_88E				BIT11		/*  Tx Error Flag Interrupt Status, write 1 clear. */
+#define IMR_RXERR_88E				BIT10		/*  Rx Error Flag INT Status, Write 1 clear */
+#define IMR_TXFOVW_88E				BIT9			/*  Transmit FIFO Overflow */
+#define IMR_RXFOVW_88E				BIT8			/*  Receive FIFO Overflow */
+
+/*===================================================================
+=====================================================================
+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 the bits of stoping 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 eralier version. Confirmed with DD Tim. */
+/*  By Bruce, 2011-09-22. */
+#define StopBecon		BIT6
+#define StopHigh			BIT5
+#define StopMgt			BIT4
+#define StopBK			BIT3
+#define StopBE			BIT2
+#define StopVI			BIT1
+#define StopVO			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
+
+/* RTL8188E SDIO Specific */
+#define SDIO_HIMR_MCU_ERR_MSK			BIT28
+#define SDIO_HIMR_TSF_BIT32_TOGGLE_MSK		BIT29
+
+/*  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
+
+/* RTL8188E SDIO Specific */
+#define SDIO_HISR_MCU_ERR				BIT28
+#define SDIO_HISR_TSF_BIT32_TOGGLE	BIT29
+
+#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 */
+
+/* 3 REG_LIFECTRL_CTRL */
+#define HAL92C_EN_PKT_LIFE_TIME_BK		BIT3
+#define HAL92C_EN_PKT_LIFE_TIME_BE		BIT2
+#define HAL92C_EN_PKT_LIFE_TIME_VI		BIT1
+#define HAL92C_EN_PKT_LIFE_TIME_VO		BIT0
+
+#define HAL92C_MSDU_LIFE_TIME_UNIT		128	/*  in us, said by Tim. */
+
+/* 2 8192D PartNo. */
+#define PARTNO_92D_NIC							(BIT7|BIT6)
+#define PARTNO_92D_NIC_REMARK				(BIT5|BIT4)
+#define PARTNO_SINGLE_BAND_VS				BIT3
+#define PARTNO_SINGLE_BAND_VS_REMARK		BIT1
+#define PARTNO_CONCURRENT_BAND_VC			(BIT3|BIT2)
+#define PARTNO_CONCURRENT_BAND_VC_REMARK	(BIT1|BIT0)
+
+/*  */
+/*  General definitions */
+/*  */
+
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_8188E		176
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_8812			255
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_8723B		255
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_8192C		255
+#define LAST_ENTRY_OF_TX_PKT_BUFFER_DUAL_MAC	127
+
+#define POLLING_LLT_THRESHOLD				20
+#define POLLING_READY_TIMEOUT_COUNT		1000
+
+
+/*  GPIO BIT */
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT2
+#define	HAL_8192EU_HW_GPIO_WPS_BIT	BIT7
+#define	HAL_8188E_HW_GPIO_WPS_BIT	BIT7
+
+#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 000000000000..74a1db1eac38
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_data.h
@@ -0,0 +1,483 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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+24+21	/*  14 is the max channel number */
+#define CHANNEL_MAX_NUMBER_2G		14
+#define CHANNEL_MAX_NUMBER_5G		54			/*  Please refer to "phy_GetChnlGroup8812A" and "Hal_ReadTxPowerInfo8812A" */
+#define CHANNEL_MAX_NUMBER_5G_80M	7
+#define CHANNEL_GROUP_MAX		3+9	/*  ch1~3, ch4~9, ch10~14 total three groups */
+#define MAX_PG_GROUP			13
+
+/*  Tx Power Limit Table Size */
+#define MAX_REGULATION_NUM			4
+#define MAX_RF_PATH_NUM_IN_POWER_LIMIT_TABLE	4
+#define MAX_2_4G_BANDWITH_NUM			4
+#define MAX_RATE_SECTION_NUM			10
+#define MAX_5G_BANDWITH_NUM			4
+
+#define MAX_BASE_NUM_IN_PHY_REG_PG_2_4G		10 /*   CCK:1, OFDM:1, HT:4, VHT:4 */
+#define MAX_BASE_NUM_IN_PHY_REG_PG_5G		9 /*  OFDM:1, HT:4, VHT:4 */
+
+
+/*  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 IQK_BB_REG_NUM_92C	9
+#define IQK_BB_REG_NUM_92D	10
+#define IQK_BB_REG_NUM_test	6
+
+#define IQK_Matrix_Settings_NUM_92D	1+24+21
+
+/* 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 {
+	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;
+	enum BAND_TYPE CurrentBandType;	/* 0:2.4G, 1:5G */
+	enum BAND_TYPE BandSet;
+	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 rf_type;
+	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;
+	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];
+	/* 3 [5G] */
+	u8 Index5G_BW40_Base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
+	u8 Index5G_BW80_Base[MAX_RF_PATH][CHANNEL_MAX_NUMBER_5G_80M];
+	s8	OFDM_5G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	BW20_5G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	BW40_5G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+	s8	BW80_5G_Diff[MAX_RF_PATH][MAX_TX_COUNT];
+
+	u8 Regulation2_4G;
+	u8 Regulation5G;
+
+	u8 TxPwrInPercentage;
+
+	u8 TxPwrCalibrateRate;
+	/*  TX power by rate table at most 4RF path. */
+	/*  The register is */
+	/*  VHT TX power by rate off setArray = */
+	/*  Band:-2G&5G = 0 / 1 */
+	/*  RF: at most 4*4 = ABCD = 0/1/2/3 */
+	/*  CCK = 0 OFDM = 1/2 HT-MCS 0-15 =3/4/56 VHT =7/8/9/10/11 */
+	u8 TxPwrByRateTable;
+	u8 TxPwrByRateBand;
+	s8	TxPwrByRateOffset[TX_PWR_BY_RATE_NUM_BAND]
+						 [TX_PWR_BY_RATE_NUM_RF]
+						 [TX_PWR_BY_RATE_NUM_RF]
+						 [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_BANDWITH_NUM]
+	                                [MAX_RATE_SECTION_NUM]
+	                                [CHANNEL_MAX_NUMBER_2G]
+						[MAX_RF_PATH_NUM];
+
+	/*  Power Limit Table for 5G */
+	s8	TxPwrLimit_5G[MAX_REGULATION_NUM]
+						[MAX_5G_BANDWITH_NUM]
+						[MAX_RATE_SECTION_NUM]
+						[CHANNEL_MAX_NUMBER_5G]
+						[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[TX_PWR_BY_RATE_NUM_RF]
+						[TX_PWR_BY_RATE_NUM_RF]
+						[MAX_BASE_NUM_IN_PHY_REG_PG_2_4G];
+	u8 TxPwrByRateBase5G[TX_PWR_BY_RATE_NUM_RF]
+						[TX_PWR_BY_RATE_NUM_RF]
+						[MAX_BASE_NUM_IN_PHY_REG_PG_5G];
+
+	/*  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 PAType_5G;
+	u8 LNAType_2G;
+	u8 LNAType_5G;
+	u8 ExternalPA_2G;
+	u8 ExternalLNA_2G;
+	u8 ExternalPA_5G;
+	u8 ExternalLNA_5G;
+	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 aggreation mode dynamic shceme. */
+	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];
+	_lock		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;
+	DM_ODM_T		odmpriv;
+
+	/*  For bluetooth co-existance */
+	BT_COEXIST		bt_coexist;
+
+	/*  Interrupt related register information. */
+	u32 		SysIntrStatus;
+	u32 		SysIntrMask;
+
+
+	char para_file_buf[MAX_PARA_FILE_BUF_LEN];
+	char *mac_reg;
+	u32 mac_reg_len;
+	char *bb_phy_reg;
+	u32 bb_phy_reg_len;
+	char *bb_agc_tab;
+	u32 bb_agc_tab_len;
+	char *bb_phy_reg_pg;
+	u32 bb_phy_reg_pg_len;
+	char *bb_phy_reg_mp;
+	u32 bb_phy_reg_mp_len;
+	char *rf_radio_a;
+	u32 rf_radio_a_len;
+	char *rf_radio_b;
+	u32 rf_radio_b_len;
+	char *rf_tx_pwr_track;
+	u32 rf_tx_pwr_track_len;
+	char *rf_tx_pwr_lmt;
+	u32 rf_tx_pwr_lmt_len;
+
+#ifdef CONFIG_BACKGROUND_NOISE_MONITOR
+	s16 noise[ODM_MAX_CHANNEL_NUM];
+#endif
+
+};
+
+#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)
+#define GET_RF_TYPE(__padapter)		(GET_HAL_DATA(__padapter)->rf_type)
+
+#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 000000000000..276089ad4864
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_intf.h
@@ -0,0 +1,410 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HAL_INTF_H__
+#define __HAL_INTF_H__
+
+
+enum RTL871X_HCI_TYPE {
+	RTW_PCIE	= BIT0,
+	RTW_USB		= BIT1,
+	RTW_SDIO	= BIT2,
+	RTW_GSPI	= BIT3,
+};
+
+enum HW_VARIABLES {
+	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. */
+#ifdef CONFIG_WOWLAN
+	HW_VAR_WOWLAN,
+	HW_VAR_WAKEUP_REASON,
+	HW_VAR_RPWM_TOG,
+#endif
+#ifdef CONFIG_AP_WOWLAN
+	HW_VAR_AP_WOWLAN,
+#endif
+	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);
+};
+
+enum RT_EEPROM_TYPE {
+	EEPROM_93C46,
+	EEPROM_93C56,
+	EEPROM_BOOT_EFUSE,
+};
+
+#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)
+
+enum wowlan_subcode {
+	WOWLAN_PATTERN_MATCH	= 1,
+	WOWLAN_MAGIC_PACKET		= 2,
+	WOWLAN_UNICAST			= 3,
+	WOWLAN_SET_PATTERN		= 4,
+	WOWLAN_DUMP_REG			= 5,
+	WOWLAN_ENABLE			= 6,
+	WOWLAN_DISABLE			= 7,
+	WOWLAN_STATUS			= 8,
+	WOWLAN_DEBUG_RELOAD_FW	= 9,
+	WOWLAN_DEBUG_1			= 10,
+	WOWLAN_DEBUG_2			= 11,
+	WOWLAN_AP_ENABLE		= 12,
+	WOWLAN_AP_DISABLE		= 13
+};
+
+struct wowlan_ioctl_param{
+	unsigned int subcode;
+	unsigned int subcode_value;
+	unsigned int wakeup_reason;
+	unsigned int len;
+	unsigned char pattern[0];
+};
+
+#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 rtw_hal_bcn_related_reg_setting(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_evt_read(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_is_disable_sw_channel_plan(struct adapter *padapter);
+
+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 000000000000..ba2a0b0c5b2f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_pg.h
@@ -0,0 +1,81 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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
+
+/*  For VHT series TX power by rate table. */
+/*  VHT TX power by rate off setArray = */
+/*  Band:-2G&5G = 0 / 1 */
+/*  RF: at most 4*4 = ABCD = 0/1/2/3 */
+/*  CCK = 0 OFDM = 1/2 HT-MCS 0-15 =3/4/56 VHT =7/8/9/10/11 */
+#define TX_PWR_BY_RATE_NUM_BAND			2
+#define TX_PWR_BY_RATE_NUM_RF			4
+#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 000000000000..15f192697957
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy.h
@@ -0,0 +1,183 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __HAL_PHY_H__
+#define __HAL_PHY_H__
+
+
+#if DISABLE_BB_RF
+#define	HAL_FW_ENABLE				0
+#define	HAL_MAC_ENABLE			0
+#define	HAL_BB_ENABLE				0
+#define	HAL_RF_ENABLE				0
+#else /*  FPGA_PHY and ASIC */
+#define		HAL_FW_ENABLE				1
+#define	HAL_MAC_ENABLE			1
+#define	HAL_BB_ENABLE				1
+#define	HAL_RF_ENABLE				1
+#endif
+
+#define	RF6052_MAX_TX_PWR			0x3F
+#define	RF6052_MAX_REG_88E			0xFF
+#define	RF6052_MAX_REG_92C			0x7F
+
+#define	RF6052_MAX_REG	\
+		(RF6052_MAX_REG_88E > RF6052_MAX_REG_92C) ? RF6052_MAX_REG_88E: RF6052_MAX_REG_92C
+
+#define GET_RF6052_REAL_MAX_REG(_Adapter)	RF6052_MAX_REG_92C
+
+#define	RF6052_MAX_PATH				2
+
+/*  */
+/*  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 BAND_TYPE {
+	BAND_ON_2_4G = 0,
+	BAND_ON_5G,
+	BAND_ON_BOTH,
+	BANDMAX
+};
+
+enum RF_TYPE {
+	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_C,
+	RF_PATH_D
+};
+
+#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 ANTENNA_PATH {
+       ANTENNA_NONE	= 0,
+	ANTENNA_D	= 1,
+	ANTENNA_C	= 2,
+	ANTENNA_CD	= 3,
+	ANTENNA_B	= 4,
+	ANTENNA_BD	= 5,
+	ANTENNA_BC	= 6,
+	ANTENNA_BCD	= 7,
+	ANTENNA_A	= 8,
+	ANTENNA_AD	= 9,
+	ANTENNA_AC	= 10,
+	ANTENNA_ACD	= 11,
+	ANTENNA_AB	= 12,
+	ANTENNA_ABD	= 13,
+	ANTENNA_ABC	= 14,
+	ANTENNA_ABCD	= 15
+};
+
+enum RF_CONTENT {
+	radioa_txt = 0x1000,
+	radiob_txt = 0x1001,
+	radioc_txt = 0x1002,
+	radiod_txt = 0x1003
+};
+
+enum BaseBand_Config_Type {
+	BaseBand_Config_PHY_REG = 0,			/* Radio Path A */
+	BaseBand_Config_AGC_TAB = 1,			/* Radio Path B */
+	BaseBand_Config_AGC_TAB_2G = 2,
+	BaseBand_Config_AGC_TAB_5G = 3,
+	BaseBand_Config_PHY_REG_PG
+};
+
+enum HW_BLOCK {
+	HW_BLOCK_MAC = 0,
+	HW_BLOCK_PHY0 = 1,
+	HW_BLOCK_PHY1 = 2,
+	HW_BLOCK_RF = 3,
+	HW_BLOCK_MAXIMUM = 4, /*  Never use this */
+};
+
+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_AC_5G = 0x40,
+	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;
+};
+
+struct R_ANTENNA_SELECT_OFDM {
+#ifdef __LITTLE_ENDIAN
+	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;
+#else
+	u32 		Reserved:2;
+	u32 		OFDM_TXSC:2;
+	u32 		r_ant_non_ht_s1:4;
+	u32 		r_ant_ht_s1:4;
+	u32 		r_ant_ht2:4;
+	u32 		r_ant_ht1:4;
+	u32 		r_ant_non_ht:4;
+	u32 		r_ant_l:4;
+	u32 		r_tx_antenna:4;
+#endif
+};
+
+/*--------------------------Exported Function prototype---------------------*/
+
+#endif /* __HAL_COMMON_H__ */
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 000000000000..518095269497
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_phy_reg.h
@@ -0,0 +1,25 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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_sdio.h b/drivers/staging/rtl8723bs/include/hal_sdio.h
new file mode 100644
index 000000000000..691a02e980a3
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/hal_sdio.h
@@ -0,0 +1,26 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 000000000000..6dc6dc73d72f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/ieee80211.h
@@ -0,0 +1,1345 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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_VHT BIT(14)
+#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[];
+
+
+typedef enum _RATEID_IDX_ {
+	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,
+	RATEID_IDX_VHT_2SS = 9,
+	RATEID_IDX_VHT_1SS = 10,
+} RATEID_IDX, *PRATEID_IDX;
+
+typedef 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,
+}RATR_TABLE_MODE, *PRATR_TABLE_MODE;
+
+
+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_11A = BIT(2), /*  tx: ofdm only, rx: ofdm only, hw: ofdm only */
+	WIRELESS_11_24N = BIT(3), /*  tx: MCS only, rx: MCS & cck, hw: MCS & cck */
+	WIRELESS_11_5N = BIT(4), /*  tx: MCS only, rx: MCS & ofdm, hw: ofdm only */
+	WIRELESS_AUTO = BIT(5),
+	WIRELESS_11AC = BIT(6),
+
+	/* 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_11A_5N = (WIRELESS_11A|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	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 */
+	WIRELESS_11_24AC = (WIRELESS_11G|WIRELESS_11AC),
+	WIRELESS_11_5AC = (WIRELESS_11A|WIRELESS_11AC),
+
+
+	/* Type for registry default wireless mode */
+	WIRELESS_11AGN = (WIRELESS_11A|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N), /*  tx: ofdm & MCS, rx: ofdm & MCS, hw: ofdm only */
+	WIRELESS_11ABGN = (WIRELESS_11A|WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N),
+	WIRELESS_MODE_24G = (WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11AC),
+	WIRELESS_MODE_MAX = (WIRELESS_11A|WIRELESS_11B|WIRELESS_11G|WIRELESS_11_24N|WIRELESS_11_5N|WIRELESS_11AC),
+};
+
+#define SUPPORTED_24G_NETTYPE_MSK (WIRELESS_11B | WIRELESS_11G | WIRELESS_11_24N)
+
+#define IsLegacyOnly(NetType)  ((NetType) == ((NetType) & (WIRELESS_11BG|WIRELESS_11A)))
+
+#define IsSupported24G(NetType) ((NetType) & SUPPORTED_24G_NETTYPE_MSK ? true : false)
+
+#define IsEnableHWCCK(NetType) IsSupported24G(NetType)
+#define IsEnableHWOFDM(NetType) (((NetType) & (WIRELESS_11G|WIRELESS_11_24N)) ? true : false)
+
+#define IsSupportedRxCCK(NetType) IsEnableHWCCK(NetType)
+#define IsSupportedRxOFDM(NetType) IsEnableHWOFDM(NetType)
+#define IsSupportedRxHT(NetType) IsEnableHWOFDM(NetType)
+
+#define IsSupportedTxCCK(NetType) (((NetType) & (WIRELESS_11B)) ? true : false)
+#define IsSupportedTxOFDM(NetType) (((NetType) & (WIRELESS_11G|WIRELESS_11A)) ? true : false)
+#define IsSupportedHT(NetType) (((NetType) & (WIRELESS_11_24N|WIRELESS_11_5N)) ? true : false)
+
+#define IsSupportedVHT(NetType) (((NetType) & (WIRELESS_11AC)) ? true : false)
+
+
+typedef 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[0];
+		} 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[0];
+		} crypt;
+		struct {
+			u16 aid;
+			u16 capability;
+			int flags;
+			u8 tx_supp_rates[16];
+			struct rtw_ieee80211_ht_cap ht_cap;
+		} add_sta;
+		struct {
+			u8 reserved[2];/* for set max_num_sta */
+			u8 buf[0];
+		} bcn_ie;
+	} u;
+}ieee_param;
+
+typedef struct ieee_param_ex {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	u8 data[0];
+}ieee_param_ex;
+
+struct sta_data{
+	u16 aid;
+	u16 capability;
+	int flags;
+	u32 sta_set;
+	u8 tx_supp_rates[16];
+	u32 tx_supp_rates_len;
+	struct rtw_ieee80211_ht_cap ht_cap;
+	u64	rx_pkts;
+	u64	rx_bytes;
+	u64	rx_drops;
+	u64	tx_pkts;
+	u64	tx_bytes;
+	u64	tx_drops;
+};
+
+#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
+
+struct ieee_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num;
+	u16 frag_num;
+	unsigned long packet_time;
+	struct list_head	list;
+};
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __attribute__ ((packed));
+
+enum eap_type {
+	EAP_PACKET = 0,
+	EAPOL_START,
+	EAPOL_LOGOFF,
+	EAPOL_KEY,
+	EAPOL_ENCAP_ASF_ALERT
+};
+
+#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 */
+} __attribute__ ((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_ACTIVE_ROAM 65533
+#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 WLAN_EID_VHT_CAPABILITY 191
+#define WLAN_EID_VHT_OPERATION 192
+#define WLAN_EID_VHT_OP_MODE_NOTIFY 199
+
+#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_RATE{
+	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_MCS8,
+	MGN_MCS9,
+	MGN_MCS10,
+	MGN_MCS11,
+	MGN_MCS12,
+	MGN_MCS13,
+	MGN_MCS14,
+	MGN_MCS15,
+	MGN_MCS16,
+	MGN_MCS17,
+	MGN_MCS18,
+	MGN_MCS19,
+	MGN_MCS20,
+	MGN_MCS21,
+	MGN_MCS22,
+	MGN_MCS23,
+	MGN_MCS24,
+	MGN_MCS25,
+	MGN_MCS26,
+	MGN_MCS27,
+	MGN_MCS28,
+	MGN_MCS29,
+	MGN_MCS30,
+	MGN_MCS31,
+	MGN_VHT1SS_MCS0,
+	MGN_VHT1SS_MCS1,
+	MGN_VHT1SS_MCS2,
+	MGN_VHT1SS_MCS3,
+	MGN_VHT1SS_MCS4,
+	MGN_VHT1SS_MCS5,
+	MGN_VHT1SS_MCS6,
+	MGN_VHT1SS_MCS7,
+	MGN_VHT1SS_MCS8,
+	MGN_VHT1SS_MCS9,
+	MGN_VHT2SS_MCS0,
+	MGN_VHT2SS_MCS1,
+	MGN_VHT2SS_MCS2,
+	MGN_VHT2SS_MCS3,
+	MGN_VHT2SS_MCS4,
+	MGN_VHT2SS_MCS5,
+	MGN_VHT2SS_MCS6,
+	MGN_VHT2SS_MCS7,
+	MGN_VHT2SS_MCS8,
+	MGN_VHT2SS_MCS9,
+	MGN_VHT3SS_MCS0,
+	MGN_VHT3SS_MCS1,
+	MGN_VHT3SS_MCS2,
+	MGN_VHT3SS_MCS3,
+	MGN_VHT3SS_MCS4,
+	MGN_VHT3SS_MCS5,
+	MGN_VHT3SS_MCS6,
+	MGN_VHT3SS_MCS7,
+	MGN_VHT3SS_MCS8,
+	MGN_VHT3SS_MCS9,
+	MGN_VHT4SS_MCS0,
+	MGN_VHT4SS_MCS1,
+	MGN_VHT4SS_MCS2,
+	MGN_VHT4SS_MCS3,
+	MGN_VHT4SS_MCS4,
+	MGN_VHT4SS_MCS5,
+	MGN_VHT4SS_MCS6,
+	MGN_VHT4SS_MCS7,
+	MGN_VHT4SS_MCS8,
+	MGN_VHT4SS_MCS9,
+	MGN_UNKNOWN
+};
+
+#define IS_HT_RATE(_rate)				(_rate >= MGN_MCS0 && _rate <= MGN_MCS31)
+#define IS_VHT_RATE(_rate)				(_rate >= MGN_VHT1SS_MCS0 && _rate <= MGN_VHT4SS_MCS9)
+#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. */
+struct ieee80211_rx_stats {
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u8 received_channel;
+	u16 rate; /* in 100 kbps */
+	u8 mask;
+	u8 freq;
+	u16 len;
+};
+
+/* 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 {
+	u32 first_frag_time;
+	uint seq;
+	uint last_frag;
+	uint qos;   /* jackson */
+	uint tid;	/* jackson */
+	struct sk_buff *skb;
+	u8 src_addr[ETH_ALEN];
+	u8 dst_addr[ETH_ALEN];
+};
+
+struct ieee80211_stats {
+	uint tx_unicast_frames;
+	uint tx_multicast_frames;
+	uint tx_fragments;
+	uint tx_unicast_octets;
+	uint tx_multicast_octets;
+	uint tx_deferred_transmissions;
+	uint tx_single_retry_frames;
+	uint tx_multiple_retry_frames;
+	uint tx_retry_limit_exceeded;
+	uint tx_discards;
+	uint rx_unicast_frames;
+	uint rx_multicast_frames;
+	uint rx_fragments;
+	uint rx_unicast_octets;
+	uint rx_multicast_octets;
+	uint rx_fcs_errors;
+	uint rx_discards_no_buffer;
+	uint tx_discards_wrong_sa;
+	uint rx_discards_undecryptable;
+	uint rx_message_in_msg_fragments;
+	uint rx_message_in_bad_msg_fragments;
+};
+
+struct ieee80211_softmac_stats {
+	uint rx_ass_ok;
+	uint rx_ass_err;
+	uint rx_probe_rq;
+	uint tx_probe_rs;
+	uint tx_beacons;
+	uint rx_auth_rq;
+	uint rx_auth_rs_ok;
+	uint rx_auth_rs_err;
+	uint tx_auth_rq;
+	uint no_auth_rs;
+	uint no_ass_rs;
+	uint tx_ass_rq;
+	uint rx_ass_rq;
+	uint tx_probe_rq;
+	uint reassoc;
+	uint swtxstop;
+	uint swtxawake;
+};
+
+#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
+
+struct ieee80211_security {
+	u16 active_key:2,
+            enabled:1,
+	    auth_mode:2,
+            auth_algo:4,
+            unicast_uses_group:1;
+	u8 key_sizes[WEP_KEYS];
+	u8 keys[WEP_KEYS][WEP_KEY_LEN];
+	u8 level;
+	u16 flags;
+} __attribute__ ((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
+
+*/
+
+struct ieee80211_header_data {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	u16 seq_ctrl;
+};
+
+#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
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((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;
+	} __attribute__ ((packed));
+	u32 time_stamp[2];
+	u16 reason;
+	u16 status;
+*/
+
+#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
+#define IEEE80211_DEFAULT_BASIC_RATE 10
+
+
+struct ieee80211_authentication {
+	struct ieee80211_header_data header;
+	u16 algorithm;
+	u16 transaction;
+	u16 status;
+	/* struct ieee80211_info_element_hdr info_element; */
+} __attribute__ ((packed));
+
+
+struct ieee80211_probe_response {
+	struct ieee80211_header_data header;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 capability;
+	struct ieee80211_info_element info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_probe_request {
+	struct ieee80211_header_data header;
+	/*struct ieee80211_info_element info_element;*/
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_request_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 listen_interval;
+	/* u8 current_ap[ETH_ALEN]; */
+	struct ieee80211_info_element_hdr info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_response_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 status;
+	u16 aid;
+} __attribute__ ((packed));
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u16 reserved;
+	u16 frag_size;
+	u16 payload_size;
+	struct sk_buff *fragments[0];
+};
+
+
+/* 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
+*/
+
+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 MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5]
+#define IP_FMT "%d.%d.%d.%d"
+#define IP_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3]
+
+extern __inline int is_multicast_mac_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+
+extern __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));
+}
+
+extern __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)
+
+typedef struct tx_pending_t{
+	int frag;
+	struct ieee80211_txb *txb;
+}tx_pending_t;
+
+
+
+#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_ieee80211_category {
+	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_VHT = 21,
+	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
+};
+
+/* BACK action code */
+enum rtw_ieee80211_back_actioncode {
+	RTW_WLAN_ACTION_ADDBA_REQ = 0,
+	RTW_WLAN_ACTION_ADDBA_RESP = 1,
+	RTW_WLAN_ACTION_DELBA = 2,
+};
+
+/* HT features action code */
+enum rtw_ieee80211_ht_actioncode {
+	RTW_WLAN_ACTION_HT_NOTI_CHNL_WIDTH = 0,
+       RTW_WLAN_ACTION_HT_SM_PS = 1,
+       RTW_WLAN_ACTION_HT_PSMP = 2,
+       RTW_WLAN_ACTION_HT_SET_PCO_PHASE = 3,
+       RTW_WLAN_ACTION_HT_CSI = 4,
+       RTW_WLAN_ACTION_HT_NON_COMPRESS_BEAMFORMING = 5,
+       RTW_WLAN_ACTION_HT_COMPRESS_BEAMFORMING = 6,
+       RTW_WLAN_ACTION_HT_ASEL_FEEDBACK = 7,
+};
+
+/* BACK (block-ack) parties */
+enum rtw_ieee80211_back_parties {
+	RTW_WLAN_BACK_RECIPIENT = 0,
+	RTW_WLAN_BACK_INITIATOR = 1,
+	RTW_WLAN_BACK_TIMER = 2,
+};
+
+/* VHT features action code */
+enum rtw_ieee80211_vht_actioncode{
+	RTW_WLAN_ACTION_VHT_COMPRESSED_BEAMFORMING = 0,
+       RTW_WLAN_ACTION_VHT_GROUPID_MANAGEMENT = 1,
+       RTW_WLAN_ACTION_VHT_OPMODE_NOTIFICATION = 2,
+};
+
+
+#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;
+};
+
+typedef enum { ParseOK = 0, ParseUnknown = 1, ParseFailed = -1 } ParseRes;
+
+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, sint index, uint len, u8 *source, uint *frlen);
+
+enum secondary_ch_offset {
+	SCN = 0, /* no secondary channel */
+	SCA = 1, /* secondary channel above */
+	SCB = 3,  /* secondary channel below */
+};
+
+u8 *rtw_get_ie(u8*pbuf, sint index, sint *len, sint 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);
+
+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))
+
+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);
+
+uint	rtw_is_cckrates_included(u8 *rate);
+
+uint	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 rf_type, 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 000000000000..2d42e0c2deff
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/ioctl_cfg80211.h
@@ -0,0 +1,128 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __IOCTL_CFG80211_H__
+#define __IOCTL_CFG80211_H__
+
+#include <linux/version.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;
+	_lock 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 000000000000..69fd5543a3b1
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/mlme_osdep.h
@@ -0,0 +1,27 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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 000000000000..cd738da3bec9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_intf.h
@@ -0,0 +1,88 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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.
+*/
+
+	_mutex ioctl_mutex;
+};
+
+
+#ifdef CONFIG_R871X_TEST
+int rtw_start_pseudo_adhoc(struct adapter *padapter);
+int rtw_stop_pseudo_adhoc(struct adapter *padapter);
+#endif
+
+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);
+u8 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);
+
+#include "../os_dep/rtw_proc.h"
+
+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(_nic_hdl ndev);
+
+int rtw_suspend_common(struct adapter *padapter);
+int rtw_resume_common(struct adapter *padapter);
+
+#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 000000000000..fdeabc1daeca
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_service.h
@@ -0,0 +1,281 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __OSDEP_SERVICE_H_
+#define __OSDEP_SERVICE_H_
+
+
+#define _FAIL		0
+#define _SUCCESS	1
+#define RTW_RX_HANDLED 2
+
+#include <osdep_service_linux.h>
+
+#ifndef BIT
+	#define BIT(x)	(1 << (x))
+#endif
+
+#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);
+
+/* flags used for rtw_mstat_update() */
+enum mstat_f {
+	/* type: 0x00ff */
+	MSTAT_TYPE_VIR = 0x00,
+	MSTAT_TYPE_PHY = 0x01,
+	MSTAT_TYPE_SKB = 0x02,
+	MSTAT_TYPE_USB = 0x03,
+	MSTAT_TYPE_MAX = 0x04,
+
+	/* func: 0xff00 */
+	MSTAT_FUNC_UNSPECIFIED = 0x00<<8,
+	MSTAT_FUNC_IO = 0x01<<8,
+	MSTAT_FUNC_TX_IO = 0x02<<8,
+	MSTAT_FUNC_RX_IO = 0x03<<8,
+	MSTAT_FUNC_TX = 0x04<<8,
+	MSTAT_FUNC_RX = 0x05<<8,
+	MSTAT_FUNC_MAX = 0x06<<8,
+};
+
+#define mstat_tf_idx(flags) ((flags)&0xff)
+#define mstat_ff_idx(flags) (((flags)&0xff00) >> 8)
+
+typedef enum mstat_status{
+	MSTAT_ALLOC_SUCCESS = 0,
+	MSTAT_ALLOC_FAIL,
+	MSTAT_FREE
+} MSTAT_STATUS;
+
+#define rtw_mstat_update(flag, status, sz) do {} while (0)
+#define rtw_mstat_dump(sel) do {} while (0)
+u8*_rtw_zmalloc(u32 sz);
+u8*_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);
+struct sk_buff *_rtw_skb_clone(struct sk_buff *skb);
+int _rtw_netif_rx(_nic_hdl 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_clone(skb)	_rtw_skb_clone((skb))
+#define rtw_skb_copy_f(skb, mstat_f)	_rtw_skb_copy((skb))
+#define rtw_skb_clone_f(skb, mstat_f)	_rtw_skb_clone((skb))
+#define rtw_netif_rx(ndev, skb) _rtw_netif_rx(ndev, skb)
+
+extern void _rtw_init_queue(struct __queue	*pqueue);
+
+extern void rtw_init_timer(_timer *ptimer, void *padapter, void *pfunc);
+
+static __inline void thread_enter(char *name)
+{
+	allow_signal(SIGTERM);
+}
+
+__inline static void flush_signals_thread(void)
+{
+	if (signal_pending (current))
+	{
+		flush_signals(current);
+	}
+}
+
+#define rtw_warn_on(condition) WARN_ON(condition)
+
+__inline static 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))
+#define RND4(x)	(((x >> 2) + (((x & 3) == 0) ?  0: 1)) << 2)
+
+__inline static u32 _RND4(u32 sz)
+{
+
+	u32 val;
+
+	val = ((sz >> 2) + ((sz & 3) ? 1: 0)) << 2;
+
+	return val;
+
+}
+
+__inline static u32 _RND8(u32 sz)
+{
+
+	u32 val;
+
+	val = ((sz >> 3) + ((sz & 7) ? 1: 0)) << 3;
+
+	return val;
+
+}
+
+#ifndef MAC_FMT
+#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
+#endif
+#ifndef MAC_ARG
+#define MAC_ARG(x) ((u8 *)(x))[0], ((u8 *)(x))[1], ((u8 *)(x))[2], ((u8 *)(x))[3], ((u8 *)(x))[4], ((u8 *)(x))[5]
+#endif
+
+
+#ifdef CONFIG_AP_WOWLAN
+extern void rtw_softap_lock_suspend(void);
+extern void rtw_softap_unlock_suspend(void);
+#endif
+
+/* File operation APIs, just for linux now */
+extern int rtw_is_file_readable(char *path);
+extern int rtw_retrive_from_file(char *path, u8 *buf, u32 sz);
+
+extern void rtw_free_netdev(struct net_device * netdev);
+
+
+extern u64 rtw_modular64(u64 x, u64 y);
+
+/* 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_GET_LE16(a) ((u16) (((a)[1] << 8) | (a)[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_BE24(a, val)					\
+	do {							\
+		(a)[0] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
+		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
+		(a)[2] = (u8) (((u32) (val)) & 0xff);		\
+	} while (0)
+
+#define RTW_GET_BE32(a) ((((u32) (a)[0]) << 24) | (((u32) (a)[1]) << 16) | \
+			 (((u32) (a)[2]) << 8) | ((u32) (a)[3]))
+#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)
+
+#define RTW_GET_LE32(a) ((((u32) (a)[3]) << 24) | (((u32) (a)[2]) << 16) | \
+			 (((u32) (a)[1]) << 8) | ((u32) (a)[0]))
+#define RTW_PUT_LE32(a, val)					\
+	do {							\
+		(a)[3] = (u8) ((((u32) (val)) >> 24) & 0xff);	\
+		(a)[2] = (u8) ((((u32) (val)) >> 16) & 0xff);	\
+		(a)[1] = (u8) ((((u32) (val)) >> 8) & 0xff);	\
+		(a)[0] = (u8) (((u32) (val)) & 0xff);		\
+	} while (0)
+
+#define RTW_GET_BE64(a) ((((u64) (a)[0]) << 56) | (((u64) (a)[1]) << 48) | \
+			 (((u64) (a)[2]) << 40) | (((u64) (a)[3]) << 32) | \
+			 (((u64) (a)[4]) << 24) | (((u64) (a)[5]) << 16) | \
+			 (((u64) (a)[6]) << 8) | ((u64) (a)[7]))
+#define RTW_PUT_BE64(a, val)				\
+	do {						\
+		(a)[0] = (u8) (((u64) (val)) >> 56);	\
+		(a)[1] = (u8) (((u64) (val)) >> 48);	\
+		(a)[2] = (u8) (((u64) (val)) >> 40);	\
+		(a)[3] = (u8) (((u64) (val)) >> 32);	\
+		(a)[4] = (u8) (((u64) (val)) >> 24);	\
+		(a)[5] = (u8) (((u64) (val)) >> 16);	\
+		(a)[6] = (u8) (((u64) (val)) >> 8);	\
+		(a)[7] = (u8) (((u64) (val)) & 0xff);	\
+	} while (0)
+
+#define RTW_GET_LE64(a) ((((u64) (a)[7]) << 56) | (((u64) (a)[6]) << 48) | \
+			 (((u64) (a)[5]) << 40) | (((u64) (a)[4]) << 32) | \
+			 (((u64) (a)[3]) << 24) | (((u64) (a)[2]) << 16) | \
+			 (((u64) (a)[1]) << 8) | ((u64) (a)[0]))
+
+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[0];
+};
+
+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 000000000000..486e8184b0b2
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/osdep_service_linux.h
@@ -0,0 +1,178 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 <asm/uaccess.h>
+	#include <asm/byteorder.h>
+	#include <asm/atomic.h>
+	#include <asm/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/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>
+
+	typedef struct	semaphore _sema;
+	typedef	spinlock_t	_lock;
+	typedef struct mutex		_mutex;
+	typedef struct timer_list _timer;
+
+	struct	__queue	{
+		struct	list_head	queue;
+		_lock	lock;
+	};
+
+	typedef	struct sk_buff	_pkt;
+	typedef unsigned char _buffer;
+
+	typedef	int	_OS_STATUS;
+	/* typedef u32 _irqL; */
+	typedef unsigned long _irqL;
+	typedef	struct	net_device * _nic_hdl;
+
+	#define thread_exit() complete_and_exit(NULL, 0)
+
+	typedef void timer_hdl_return;
+	typedef void* timer_hdl_context;
+
+	typedef struct work_struct _workitem;
+
+__inline static struct list_head *get_next(struct list_head	*list)
+{
+	return list->next;
+}
+
+__inline static struct list_head	*get_list_head(struct __queue	*queue)
+{
+	return (&(queue->queue));
+}
+
+
+#define LIST_CONTAINOR(ptr, type, member) \
+        ((type *)((char *)(ptr)-(__kernel_size_t)(&((type *)0)->member)))
+
+#define RTW_TIMER_HDL_ARGS void *FunctionContext
+
+__inline static void _init_timer(_timer *ptimer, _nic_hdl nic_hdl, void *pfunc, void* cntx)
+{
+	/* setup_timer(ptimer, pfunc, (u32)cntx); */
+	ptimer->function = pfunc;
+	ptimer->data = (unsigned long)cntx;
+	init_timer(ptimer);
+}
+
+__inline static void _set_timer(_timer *ptimer, u32 delay_time)
+{
+	mod_timer(ptimer , (jiffies+(delay_time*HZ/1000)));
+}
+
+__inline static void _cancel_timer(_timer *ptimer, u8 *bcancelled)
+{
+	del_timer_sync(ptimer);
+	*bcancelled =  true;/* true == 1; false == 0 */
+}
+
+
+__inline static void _init_workitem(_workitem *pwork, void *pfunc, void *cntx)
+{
+	INIT_WORK(pwork, pfunc);
+}
+
+__inline static void _set_workitem(_workitem *pwork)
+{
+	schedule_work(pwork);
+}
+
+__inline static void _cancel_workitem_sync(_workitem *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);
+}
+
+static inline void rtw_merge_string(char *dst, int dst_len, char *src1, char *src2)
+{
+	int	len = 0;
+	len += snprintf(dst+len, dst_len - len, "%s", src1);
+	len += snprintf(dst+len, dst_len - len, "%s", src2);
+}
+
+#define rtw_signal_process(pid, sig) kill_pid(find_vpid((pid)), (sig), 1)
+
+#define rtw_netdev_priv(netdev) (((struct rtw_netdev_priv_indicator *)netdev_priv(netdev))->priv)
+
+#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
+
+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);
+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 000000000000..a480874a2f2a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/recv_osdep.h
@@ -0,0 +1,48 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RECV_OSDEP_H_
+#define __RECV_OSDEP_H_
+
+
+extern sint _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(_nic_hdl cnxt, _pkt *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);
+
+
+int 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);
+
+
+int rtw_os_recvbuf_resource_free(struct adapter *padapter, struct recv_buf *precvbuf);
+
+_pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata);
+void rtw_os_recv_indicate_pkt(struct adapter *padapter, _pkt *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 000000000000..3e1be0092df4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8192c_recv.h
@@ -0,0 +1,50 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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/rtl8192c_rf.h b/drivers/staging/rtl8723bs/include/rtl8192c_rf.h
new file mode 100644
index 000000000000..0dbee562d19b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8192c_rf.h
@@ -0,0 +1,39 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _RTL8192C_RF_H_
+#define _RTL8192C_RF_H_
+
+
+/*  */
+/*  RF RL6052 Series API */
+/*  */
+void 	rtl8192c_RF_ChangeTxPath(struct adapter *Adapter,
+				u16 	DataRate);
+void 	rtl8192c_PHY_RF6052SetBandwidth(
+				struct adapter *			Adapter,
+				enum CHANNEL_WIDTH		Bandwidth);
+void rtl8192c_PHY_RF6052SetCckTxPower(
+				struct adapter *Adapter,
+				u8*	pPowerlevel);
+void rtl8192c_PHY_RF6052SetOFDMTxPower(
+				struct adapter *Adapter,
+				u8*	pPowerLevel,
+				u8 Channel);
+int	PHY_RF6052_Config8192C(struct adapter *	Adapter	);
+
+/*--------------------------Exported Function prototype---------------------*/
+
+
+#endif/* End of HalRf.h */
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h b/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h
new file mode 100644
index 000000000000..8d610646ad17
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_cmd.h
@@ -0,0 +1,199 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTL8723B_CMD_H__
+#define __RTL8723B_CMD_H__
+
+/*  */
+/*     H2C CMD DEFINITION    ------------------------------------------------ */
+/*  */
+
+enum h2c_cmd_8723B{
+	/* 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);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+void rtl8723b_set_wowlan_cmd(struct adapter *padapter, u8 enable);
+void rtl8723b_set_ap_wowlan_cmd(struct adapter *padapter, u8 enable);
+void SetFwRelatedForWoWLAN8723b(struct adapter *padapter, u8 bHostIsGoingtoSleep);
+#endif/* CONFIG_WOWLAN */
+
+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 000000000000..cc64b38ead2a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_dm.h
@@ -0,0 +1,41 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 000000000000..adaeea115e4d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
@@ -0,0 +1,279 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 "Hal8723BPwrSeq.h"
+#include "Hal8723BPhyReg.h"
+#include "Hal8723BPhyCfg.h"
+
+/*  */
+/* 		RTL8723B From file */
+/*  */
+	#define RTL8723B_FW_IMG					"rtl8723b/FW_NIC.bin"
+	#define RTL8723B_FW_WW_IMG				"rtl8723b/FW_WoWLAN.bin"
+	#define RTL8723B_PHY_REG					"rtl8723b/PHY_REG.txt"
+	#define RTL8723B_PHY_RADIO_A				"rtl8723b/RadioA.txt"
+	#define RTL8723B_PHY_RADIO_B				"rtl8723b/RadioB.txt"
+	#define RTL8723B_TXPWR_TRACK				"rtl8723b/TxPowerTrack.txt"
+	#define RTL8723B_AGC_TAB					"rtl8723b/AGC_TAB.txt"
+	#define RTL8723B_PHY_MACREG				"rtl8723b/MAC_REG.txt"
+	#define RTL8723B_PHY_REG_PG				"rtl8723b/PHY_REG_PG.txt"
+	#define RTL8723B_PHY_REG_MP				"rtl8723b/PHY_REG_MP.txt"
+	#define RTL8723B_TXPWR_LMT				"rtl8723b/TXPWR_LMT.txt"
+
+/*  */
+/* 		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(_pFwHdr)	((le16_to_cpu(_pFwHdr->Signature)&0xFFF0) == 0x5300)
+
+struct rt_firmware {
+	u32 ulFwLength;
+	u8 *szFwBuffer;
+};
+
+/*  This structure must be cared byte-ordering */
+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 indcation, 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		RamCodeSize;	/*  The size of RAM code */
+	__le16		Rsvd2;
+
+	/*  LONG WORD 2 ---- */
+	__le32		SvnIdx;	/*  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 page equally for each queue other than 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
+
+#ifdef CONFIG_PNO_SUPPORT
+#undef BCNQ1_PAGE_NUM_8723B
+#define BCNQ1_PAGE_NUM_8723B		0x00 /*  0x04 */
+#endif
+#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 */
+#ifdef CONFIG_WOWLAN
+#define WOWLAN_PAGE_NUM_8723B	0x07
+#else
+#define WOWLAN_PAGE_NUM_8723B	0x00
+#endif
+
+#ifdef CONFIG_PNO_SUPPORT
+#undef WOWLAN_PAGE_NUM_8723B
+#define WOWLAN_PAGE_NUM_8723B	0x0d
+#endif
+
+#ifdef CONFIG_AP_WOWLAN
+#define AP_WOWLAN_PAGE_NUM_8723B	0x02
+#endif
+
+#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. */
+/*  Fisrt constructed by tynli. 2009.10.09. */
+typedef enum _C2H_EVT
+{
+	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
+} C2H_EVT;
+
+typedef struct _C2H_EVT_HDR
+{
+	u8 CmdID;
+	u8 CmdLen;
+	u8 CmdSeq;
+} __attribute__((__packed__)) C2H_EVT_HDR, *PC2H_EVT_HDR;
+
+typedef enum tag_Package_Definition
+{
+    PACKAGE_DEFAULT,
+    PACKAGE_QFN68,
+    PACKAGE_TFBGA90,
+    PACKAGE_TFBGA80,
+    PACKAGE_TFBGA79
+}PACKAGE_TYPE_E;
+
+#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);
+#ifdef CONFIG_WOWLAN
+void Hal_DetectWoWMode(struct adapter *padapter);
+#endif /* CONFIG_WOWLAN */
+
+void rtl8723b_start_thread(struct adapter *padapter);
+void rtl8723b_stop_thread(struct adapter *padapter);
+
+#if defined(CONFIG_CHECK_BT_HANG)
+void rtl8723bs_init_checkbthang_workqueue(struct adapter * adapter);
+void rtl8723bs_free_checkbthang_workqueue(struct adapter * adapter);
+void rtl8723bs_cancle_checkbthang_workqueue(struct adapter * adapter);
+void rtl8723bs_hal_check_bt_hang(struct adapter * adapter);
+#endif
+
+#ifdef CONFIG_GPIO_WAKEUP
+void HalSetOutPutGPIO(struct adapter *padapter, u8 index, u8 OutPutValue);
+#endif
+
+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 000000000000..7218424dae99
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_recv.h
@@ -0,0 +1,144 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTL8723B_RECV_H__
+#define __RTL8723B_RECV_H__
+
+#include <rtl8192c_recv.h>
+
+typedef 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;
+} RXREPORT, *PRXREPORT;
+
+typedef struct phystatus_8723b
+{
+	u32 rxgain_a:7;
+	u32 trsw_a:1;
+	u32 rxgain_b:7;
+	u32 trsw_b:1;
+	u32 chcorr_l:16;
+
+	u32 sigqualcck:8;
+	u32 cfo_a:8;
+	u32 cfo_b:8;
+	u32 chcorr_h:8;
+
+	u32 noisepwrdb_h:8;
+	u32 cfo_tail_a:8;
+	u32 cfo_tail_b:8;
+	u32 rsvd0824:8;
+
+	u32 rsvd1200:8;
+	u32 rxevm_a:8;
+	u32 rxevm_b:8;
+	u32 rxsnr_a:8;
+
+	u32 rxsnr_b:8;
+	u32 noisepwrdb_l:8;
+	u32 rsvd1616:8;
+	u32 postsnr_a:8;
+
+	u32 postsnr_b:8;
+	u32 csi_a:8;
+	u32 csi_b:8;
+	u32 targetcsi_a:8;
+
+	u32 targetcsi_b:8;
+	u32 sigevm:8;
+	u32 maxexpwr:8;
+	u32 exintflag:1;
+	u32 sgien:1;
+	u32 rxsc:2;
+	u32 idlelong:1;
+	u32 anttrainen:1;
+	u32 antselb:1;
+	u32 antsel:1;
+} PHYSTATUS, *PPHYSTATUS;
+
+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 000000000000..f5aa1b09a608
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_rf.h
@@ -0,0 +1,26 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTL8723B_RF_H__
+#define __RTL8723B_RF_H__
+
+#include "rtl8192c_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 000000000000..8d78f4ef5438
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_spec.h
@@ -0,0 +1,262 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTL8723B_SPEC_H__
+#define __RTL8723B_SPEC_H__
+
+#include <autoconf.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
+#ifdef CONFIG_WOWLAN
+#define REG_TXPKTBUF_IV_LOW             0x0484
+#define REG_TXPKTBUF_IV_HIGH            0x0488
+#endif
+#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 Regsiter 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)
+
+#ifdef CONFIG_WOWLAN
+#define RXPKT_RELEASE_POLL		BIT(16)
+#define RXDMA_IDLE				BIT(17)
+#define RW_RELEASE_EN			BIT(18)
+#endif
+
+/*  */
+/*  */
+/* 	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 Interrup 7 */
+#define	IMR_BCNDOK6_8723B					BIT19		/*  Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK5_8723B					BIT18		/*  Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK4_8723B					BIT17		/*  Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK3_8723B					BIT16		/*  Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK2_8723B					BIT15		/*  Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK1_8723B					BIT14		/*  Beacon Queue DMA OK Interrup 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 000000000000..3bea5d5dd82b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_xmit.h
@@ -0,0 +1,458 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 */
+/*  */
+typedef 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;
+}TXDESC_8723B, *PTXDESC_8723B;
+
+#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 DESC8723B_RATEMCS8				0x14
+#define DESC8723B_RATEMCS9				0x15
+#define DESC8723B_RATEMCS10		0x16
+#define DESC8723B_RATEMCS11		0x17
+#define DESC8723B_RATEMCS12		0x18
+#define DESC8723B_RATEMCS13		0x19
+#define DESC8723B_RATEMCS14		0x1a
+#define DESC8723B_RATEMCS15		0x1b
+#define DESC8723B_RATEVHTSS1MCS0		0x2c
+#define DESC8723B_RATEVHTSS1MCS1		0x2d
+#define DESC8723B_RATEVHTSS1MCS2		0x2e
+#define DESC8723B_RATEVHTSS1MCS3		0x2f
+#define DESC8723B_RATEVHTSS1MCS4		0x30
+#define DESC8723B_RATEVHTSS1MCS5		0x31
+#define DESC8723B_RATEVHTSS1MCS6		0x32
+#define DESC8723B_RATEVHTSS1MCS7		0x33
+#define DESC8723B_RATEVHTSS1MCS8		0x34
+#define DESC8723B_RATEVHTSS1MCS9		0x35
+#define DESC8723B_RATEVHTSS2MCS0		0x36
+#define DESC8723B_RATEVHTSS2MCS1		0x37
+#define DESC8723B_RATEVHTSS2MCS2		0x38
+#define DESC8723B_RATEVHTSS2MCS3		0x39
+#define DESC8723B_RATEVHTSS2MCS4		0x3a
+#define DESC8723B_RATEVHTSS2MCS5		0x3b
+#define DESC8723B_RATEVHTSS2MCS6		0x3c
+#define DESC8723B_RATEVHTSS2MCS7		0x3d
+#define DESC8723B_RATEVHTSS2MCS8		0x3e
+#define DESC8723B_RATEVHTSS2MCS9		0x3f
+
+
+#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 000000000000..3c2d1e912a9d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ap.h
@@ -0,0 +1,47 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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, u8 *pbuf);
+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);
+int 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);
+int 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_beamforming.h b/drivers/staging/rtl8723bs/include/rtw_beamforming.h
new file mode 100644
index 000000000000..69711e41c50b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_beamforming.h
@@ -0,0 +1,135 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_BEAMFORMING_H_
+#define __RTW_BEAMFORMING_H_
+
+#define BEAMFORMING_ENTRY_NUM		2
+#define GET_BEAMFORM_INFO(_pmlmepriv)	((struct beamforming_info *)(&(_pmlmepriv)->beamforming_info))
+
+typedef enum _BEAMFORMING_ENTRY_STATE
+{
+	BEAMFORMING_ENTRY_STATE_UNINITIALIZE,
+	BEAMFORMING_ENTRY_STATE_INITIALIZEING,
+	BEAMFORMING_ENTRY_STATE_INITIALIZED,
+	BEAMFORMING_ENTRY_STATE_PROGRESSING,
+	BEAMFORMING_ENTRY_STATE_PROGRESSED,
+}BEAMFORMING_ENTRY_STATE, *PBEAMFORMING_ENTRY_STATE;
+
+
+typedef enum _BEAMFORMING_STATE
+{
+	BEAMFORMING_STATE_IDLE,
+	BEAMFORMING_STATE_START,
+	BEAMFORMING_STATE_END,
+}BEAMFORMING_STATE, *PBEAMFORMING_STATE;
+
+
+typedef enum _BEAMFORMING_CAP
+{
+	BEAMFORMING_CAP_NONE = 0x0,
+	BEAMFORMER_CAP_HT_EXPLICIT = 0x1,
+	BEAMFORMEE_CAP_HT_EXPLICIT = 0x2,
+	BEAMFORMER_CAP_VHT_SU = 0x4,			/*  Self has er Cap, because Reg er  & peer ee */
+	BEAMFORMEE_CAP_VHT_SU = 0x8,			/*  Self has ee Cap, because Reg ee & peer er */
+	BEAMFORMER_CAP = 0x10,
+	BEAMFORMEE_CAP = 0x20,
+}BEAMFORMING_CAP, *PBEAMFORMING_CAP;
+
+
+typedef enum _SOUNDING_MODE
+{
+	SOUNDING_SW_VHT_TIMER = 0x0,
+	SOUNDING_SW_HT_TIMER = 0x1,
+	SOUNDING_STOP_All_TIMER = 0x2,
+	SOUNDING_HW_VHT_TIMER = 0x3,
+	SOUNDING_HW_HT_TIMER = 0x4,
+	SOUNDING_STOP_OID_TIMER = 0x5,
+	SOUNDING_AUTO_VHT_TIMER = 0x6,
+	SOUNDING_AUTO_HT_TIMER = 0x7,
+	SOUNDING_FW_VHT_TIMER = 0x8,
+	SOUNDING_FW_HT_TIMER = 0x9,
+}SOUNDING_MODE, *PSOUNDING_MODE;
+
+
+enum BEAMFORMING_CTRL_TYPE
+{
+	BEAMFORMING_CTRL_ENTER = 0,
+	BEAMFORMING_CTRL_LEAVE = 1,
+	BEAMFORMING_CTRL_START_PERIOD = 2,
+	BEAMFORMING_CTRL_END_PERIOD = 3,
+	BEAMFORMING_CTRL_SOUNDING_FAIL =4,
+	BEAMFORMING_CTRL_SOUNDING_CLK =5,
+};
+
+struct beamforming_entry {
+	bool	bUsed;
+	bool	bSound;
+	u16 aid;			/*  Used to construct AID field of NDPA packet. */
+	u16 mac_id;		/*  Used to Set Reg42C in IBSS mode. */
+	u16 p_aid;		/*  Used to fill Reg42C & Reg714 to compare with P_AID of Tx DESC. */
+	u8 mac_addr[6];/*  Used to fill Reg6E4 to fill Mac address of CSI report frame. */
+	enum CHANNEL_WIDTH	sound_bw;	/*  Sounding BandWidth */
+	u16 sound_period;
+	BEAMFORMING_CAP	beamforming_entry_cap;
+	BEAMFORMING_ENTRY_STATE	beamforming_entry_state;
+	u8 LogSeq;
+	u8 LogRetryCnt;
+	u8 LogSuccessCnt;
+	u8 LogStatusFailCnt;
+	u8 PreCsiReport[327];
+	u8 DefaultCsiCnt;
+	bool	bDefaultCSI;
+};
+
+struct sounding_info {
+	u8 		sound_idx;
+	enum CHANNEL_WIDTH	sound_bw;
+	SOUNDING_MODE	sound_mode;
+	u16 			sound_period;
+};
+
+struct beamforming_info {
+	BEAMFORMING_CAP		beamforming_cap;
+	BEAMFORMING_STATE		beamforming_state;
+	struct beamforming_entry	beamforming_entry[BEAMFORMING_ENTRY_NUM];
+	u8 				beamforming_cur_idx;
+	u8 				beamforming_in_progress;
+	u8 				sounding_sequence;
+	struct sounding_info 	sounding_info;
+};
+
+struct rtw_ndpa_sta_info {
+	u16 aid:12;
+	u16 feedback_type:1;
+	u16 nc_index:3;
+};
+
+BEAMFORMING_CAP beamforming_get_entry_beam_cap_by_mac_id(void *pmlmepriv , u8 mac_id);
+void beamforming_notify(struct adapter * adapter);
+BEAMFORMING_CAP beamforming_get_beamform_cap(struct beamforming_info *pBeamInfo);
+
+u32 beamforming_get_report_frame(struct adapter * Adapter, union recv_frame *precv_frame);
+
+bool	beamforming_send_ht_ndpa_packet(struct adapter * Adapter, u8 *ra, enum CHANNEL_WIDTH bw, u8 qidx);
+bool	beamforming_send_vht_ndpa_packet(struct adapter * Adapter, u8 *ra, u16 aid, enum CHANNEL_WIDTH bw, u8 qidx);
+
+void beamforming_check_sounding_success(struct adapter * Adapter, bool status);
+
+void beamforming_watchdog(struct adapter * Adapter);
+
+void beamforming_wk_hdl(struct adapter *padapter, u8 type, u8 *pbuf);
+u8 beamforming_wk_cmd(struct adapter *padapter, s32 type, u8 *pbuf, s32 size, u8 enqueue);
+
+#endif
diff --git a/drivers/staging/rtl8723bs/include/rtw_br_ext.h b/drivers/staging/rtl8723bs/include/rtw_br_ext.h
new file mode 100644
index 000000000000..c942535a7961
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_br_ext.h
@@ -0,0 +1,63 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _RTW_BR_EXT_H_
+#define _RTW_BR_EXT_H_
+
+#define MACADDRLEN		6
+#define _DEBUG_ERR		DBG_8192C
+#define _DEBUG_INFO		/* DBG_8192C */
+#define DEBUG_WARN		DBG_8192C
+#define DEBUG_INFO		/* DBG_8192C */
+#define DEBUG_ERR		DBG_8192C
+/* define GET_MY_HWADDR		((GET_MIB(priv))->dot11OperationEntry.hwaddr) */
+#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_LOOKUP,
+	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[MACADDRLEN];
+	unsigned int	nat25sc_disable;
+};
+
+void nat25_db_cleanup(struct adapter *priv);
+
+#endif /*  _RTW_BR_EXT_H_ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_btcoex.h b/drivers/staging/rtl8723bs/include/rtw_btcoex.h
new file mode 100644
index 000000000000..9a5c3f40bddb
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_btcoex.h
@@ -0,0 +1,64 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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_Initialize(struct adapter *);
+void rtw_btcoex_PowerOnSetting(struct adapter *padapter);
+void rtw_btcoex_HAL_Initialize(struct adapter *padapter, u8 bWifiOnly);
+void rtw_btcoex_IpsNotify(struct adapter *, u8 type);
+void rtw_btcoex_LpsNotify(struct adapter *, u8 type);
+void rtw_btcoex_ScanNotify(struct adapter *, u8 type);
+void rtw_btcoex_ConnectNotify(struct adapter *, u8 action);
+void rtw_btcoex_MediaStatusNotify(struct adapter *, u8 mediaStatus);
+void rtw_btcoex_SpecialPacketNotify(struct adapter *, u8 pktType);
+void rtw_btcoex_IQKNotify(struct adapter *padapter, u8 state);
+void rtw_btcoex_BtInfoNotify(struct adapter *, u8 length, u8 *tmpBuf);
+void rtw_btcoex_SuspendNotify(struct adapter *, u8 state);
+void rtw_btcoex_HaltNotify(struct adapter *);
+u8 rtw_btcoex_IsBtDisabled(struct adapter *);
+void rtw_btcoex_Handler(struct adapter *);
+s32 rtw_btcoex_IsBTCoexCtrlAMPDUSize(struct adapter *);
+void rtw_btcoex_SetManualControl(struct adapter *, u8 bmanual);
+u8 rtw_btcoex_IsBtControlLps(struct adapter *);
+u8 rtw_btcoex_IsLpsOn(struct adapter *);
+u8 rtw_btcoex_RpwmVal(struct adapter *);
+u8 rtw_btcoex_LpsVal(struct adapter *);
+void rtw_btcoex_SetBTCoexist(struct adapter *, u8 bBtExist);
+void rtw_btcoex_SetChipType(struct adapter *, u8 chipType);
+void rtw_btcoex_SetPGAntNum(struct adapter *, u8 antNum);
+void rtw_btcoex_SetSingleAntPath(struct adapter *padapter, u8 singleAntPath);
+u32 rtw_btcoex_GetRaMask(struct adapter *);
+void rtw_btcoex_RecordPwrMode(struct adapter *, u8 *pCmdBuf, u8 cmdLen);
+void rtw_btcoex_DisplayBtCoexInfo(struct adapter *, u8 *pbuf, u32 bufsize);
+void rtw_btcoex_SetDBG(struct adapter *, u32 *pDbgModule);
+u32 rtw_btcoex_GetDBG(struct adapter *, u8 *pStrBuf, u32 bufSize);
+
+/*  ================================================== */
+/*  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 000000000000..ffbbcec77a0d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_byteorder.h
@@ -0,0 +1,24 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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 000000000000..286d329b7a4f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_cmd.h
@@ -0,0 +1,980 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_CMD_H_
+#define __RTW_CMD_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;
+		/* _sema		cmd_sem; */
+		struct list_head	list;
+	};
+
+	/* cmd flags */
+	enum {
+		RTW_CMDF_DIRECTLY = BIT0,
+		RTW_CMDF_WAIT_ACK = BIT1,
+	};
+
+	struct cmd_priv {
+		_sema	cmd_queue_sema;
+		/* _sema	cmd_done_sema; */
+		_sema	terminate_cmdthread_sema;
+		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;
+		_mutex sctx_mutex;
+	};
+
+	struct	evt_priv {
+		_workitem 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[0];
+};
+
+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)
+
+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 */
+};
+
+extern u32 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 u32 rtw_init_cmd_priv (struct cmd_priv *pcmdpriv);
+extern void rtw_free_cmd_priv (struct cmd_priv *pcmdpriv);
+
+extern u32 rtw_init_evt_priv (struct evt_priv *pevtpriv);
+extern void rtw_free_evt_priv (struct evt_priv *pevtpriv);
+extern void rtw_evt_notify_isr(struct evt_priv *pevtpriv);
+
+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,
+	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 bandwith change. */
+	BEAMFORMING_WK_CID,
+	LPS_CHANGE_DTIM_CID,
+	BTINFO_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,
+	LPS_CTRL_TRAFFIC_BUSY = 6,
+};
+
+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
+
+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 {
+	sint 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;
+};
+
+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 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 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_rsp{
+	unsigned short tx_power;
+};
+
+struct setprobereqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[0];
+};
+
+struct setassocreqextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[0];
+};
+
+struct setproberspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[0];
+};
+
+struct setassocrspextraie_parm {
+	unsigned char e_id;
+	unsigned char ie_len;
+	unsigned char ie[0];
+};
+
+
+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
+{
+	void *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;
+};
+
+/*H2C Handler index: 64 */
+struct RunInThread_param
+{
+	void (*func)(void*);
+	void *context;
+};
+
+
+#define GEN_CMD_CODE(cmd)	cmd ## _CMD_
+
+
+/*
+
+Result:
+0x00: success
+0x01: sucess, 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);
+u8 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_setdatarate_cmd(struct adapter  *padapter, u8 *rateset);
+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_set_chplan_cmd(struct adapter *padapter, u8 chplan, u8 enqueue, u8 swconfig);
+
+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 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*/
+	GEN_CMD_CODE(_ChkBMCSleepq), /*63*/
+
+	GEN_CMD_CODE(_RunInThreadCMD), /*64*/
+
+	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 000000000000..625e2a39a861
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_debug.h
@@ -0,0 +1,355 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_DEBUG_H__
+#define __RTW_DEBUG_H__
+
+#include <linux/trace_seq.h>
+
+#define _drv_always_		1
+#define _drv_emerg_			2
+#define _drv_alert_			3
+#define _drv_crit_			4
+#define _drv_err_			5
+#define	_drv_warning_		6
+#define _drv_notice_		7
+#define _drv_info_			8
+#define _drv_dump_			9
+#define	_drv_debug_			10
+
+
+#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_ioctl_query_c_	BIT(18)
+#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_mp_			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_efuse_			BIT(27) */
+#define	_module_rtl8192c_xmit_c_ BIT(28)
+#define _module_hal_xmit_c_	BIT(28)
+#define _module_efuse_			BIT(29)
+#define _module_rtl8712_recv_c_		BIT(30)
+#define _module_rtl8712_led_c_		BIT(31)
+
+#undef _MODULE_DEFINE_
+
+#if defined _RTW_XMIT_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_xmit_c_
+#elif defined _XMIT_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_xmit_osdep_c_
+#elif defined _RTW_RECV_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_recv_c_
+#elif defined _RECV_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_recv_osdep_c_
+#elif defined _RTW_MLME_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_mlme_c_
+#elif defined _MLME_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_mlme_osdep_c_
+#elif defined _RTW_MLME_EXT_C_
+	#define _MODULE_DEFINE_ 1
+#elif defined _RTW_STA_MGT_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_sta_mgt_c_
+#elif defined _RTW_CMD_C_
+	#define _MODULE_DEFINE_	_module_rtl871x_cmd_c_
+#elif defined _CMD_OSDEP_C_
+	#define _MODULE_DEFINE_	_module_cmd_osdep_c_
+#elif defined _RTW_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 _RTW_SECURITY_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_security_c_
+#elif defined _RTW_EEPROM_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_eeprom_c_
+#elif defined _HAL_INTF_C_
+	#define	_MODULE_DEFINE_	_module_hal_init_c_
+#elif (defined _HCI_HAL_INIT_C_) || (defined _SDIO_HALINIT_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_IOCTL_QUERY_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_ioctl_query_c_
+#elif defined _RTL871X_PWRCTRL_C_
+	#define	_MODULE_DEFINE_	_module_rtl871x_pwrctrl_c_
+#elif defined _RTW_PWRCTRL_C_
+	#define	_MODULE_DEFINE_	1
+#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 _SDIO_OPS_C_
+	#define	_MODULE_DEFINE_ 1
+#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 _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 _RTL8712_CMD_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_cmd_c_
+#elif defined _RTL8192C_XMIT_C_
+	#define	_MODULE_DEFINE_	1
+#elif defined _RTL8723AS_XMIT_C_
+	#define	_MODULE_DEFINE_	1
+#elif defined _RTL8712_RECV_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_recv_c_
+#elif defined _RTL8192CU_RECV_C_
+	#define	_MODULE_DEFINE_	_module_rtl8712_recv_c_
+#elif defined _RTL871X_MLME_EXT_C_
+	#define _MODULE_DEFINE_	_module_mlme_osdep_c_
+#elif defined _RTW_EFUSE_C_
+	#define	_MODULE_DEFINE_	_module_efuse_
+#endif
+
+#define RT_TRACE(_Comp, _Level, Fmt) do{}while (0)
+#define RT_PRINT_DATA(_Comp, _Level, _TitleString, _HexData, _HexDataLen) do{}while (0)
+
+#define DBG_871X(x, ...) do {} while (0)
+#define MSG_8192C(x, ...) do {} while (0)
+#define DBG_8192C(x,...) do {} while (0)
+#define DBG_871X_LEVEL(x,...) do {} while (0)
+
+#undef _dbgdump
+
+#ifndef _RTL871X_DEBUG_C_
+	extern u32 GlobalDebugLevel;
+	extern u64 GlobalDebugComponents;
+#endif
+
+#define _dbgdump printk
+
+#define DRIVER_PREFIX "RTL8723BS: "
+
+#if defined(_dbgdump)
+
+/* with driver-defined prefix */
+#undef DBG_871X_LEVEL
+#define DBG_871X_LEVEL(level, fmt, arg...)     \
+	do {\
+		if (level <= GlobalDebugLevel) {\
+			if (level <= _drv_err_ && level > _drv_always_) \
+				_dbgdump(DRIVER_PREFIX"ERROR " fmt, ##arg);\
+			else \
+				_dbgdump(DRIVER_PREFIX fmt, ##arg);\
+		}\
+	}while (0)
+
+/* without driver-defined prefix */
+#undef _DBG_871X_LEVEL
+#define _DBG_871X_LEVEL(level, fmt, arg...)	   \
+	do {\
+		if (level <= GlobalDebugLevel) {\
+			if (level <= _drv_err_ && level > _drv_always_) \
+				_dbgdump("ERROR " fmt, ##arg);\
+			else \
+				_dbgdump(fmt, ##arg);\
+		}\
+	}while (0)
+
+#define RTW_DBGDUMP NULL /* 'stream' for _dbgdump */
+
+/* dump message to selected 'stream' */
+#define DBG_871X_SEL(sel, fmt, arg...)					\
+	do {								\
+		if (sel == RTW_DBGDUMP)					\
+			_DBG_871X_LEVEL(_drv_always_, fmt, ##arg);	\
+		else							\
+			seq_printf(sel, fmt, ##arg);			\
+	} while (0)
+
+/* dump message to selected 'stream' with driver-defined prefix */
+#define DBG_871X_SEL_NL(sel, fmt, arg...)				\
+	do {								\
+		if (sel == RTW_DBGDUMP)					\
+			DBG_871X_LEVEL(_drv_always_, fmt, ##arg);	\
+		else							\
+			seq_printf(sel, fmt, ##arg);			\
+	} while (0)
+
+#endif /* defined(_dbgdump) */
+
+#ifdef DEBUG
+#if	defined(_dbgdump)
+	#undef DBG_871X
+	#define DBG_871X(...)     do {\
+		_dbgdump(DRIVER_PREFIX __VA_ARGS__);\
+	}while (0)
+
+	#undef MSG_8192C
+	#define MSG_8192C(...)     do {\
+		_dbgdump(DRIVER_PREFIX __VA_ARGS__);\
+	}while (0)
+
+	#undef DBG_8192C
+	#define DBG_8192C(...)     do {\
+		_dbgdump(DRIVER_PREFIX __VA_ARGS__);\
+	}while (0)
+#endif /* defined(_dbgdump) */
+#endif /* DEBUG */
+
+#ifdef DEBUG_RTL871X
+
+#if	defined(_dbgdump) && defined(_MODULE_DEFINE_)
+
+	#undef RT_TRACE
+	#define RT_TRACE(_Comp, _Level, Fmt)\
+	do {\
+		if ((_Comp & GlobalDebugComponents) && (_Level <= GlobalDebugLevel)) {\
+			_dbgdump("%s [0x%08x,%d]", DRIVER_PREFIX, (unsigned int)_Comp, _Level);\
+			_dbgdump Fmt;\
+		}\
+	}while (0)
+
+#endif /* defined(_dbgdump) && defined(_MODULE_DEFINE_) */
+
+
+#if	defined(_dbgdump)
+	#undef RT_PRINT_DATA
+	#define RT_PRINT_DATA(_Comp, _Level, _TitleString, _HexData, _HexDataLen)			\
+		if (((_Comp) & GlobalDebugComponents) && (_Level <= GlobalDebugLevel))	\
+		{									\
+			int __i;								\
+			u8 *ptr = (u8 *)_HexData;				\
+			_dbgdump("%s", DRIVER_PREFIX);						\
+			_dbgdump(_TitleString);						\
+			for (__i = 0; __i<(int)_HexDataLen; __i++)				\
+			{								\
+				_dbgdump("%02X%s", ptr[__i], (((__i + 1) % 4) == 0)?"  ":" ");	\
+				if (((__i + 1) % 16) == 0)	_dbgdump("\n");			\
+			}								\
+			_dbgdump("\n");							\
+		}
+#endif /* defined(_dbgdump) */
+#endif /* DEBUG_RTL871X */
+
+#ifdef CONFIG_DBG_COUNTER
+#define DBG_COUNTER(counter) counter++
+#else
+#define DBG_COUNTER(counter) do {} while (0)
+#endif
+
+void dump_drv_version(void *sel);
+void dump_log_level(void *sel);
+
+void sd_f0_reg_dump(void *sel, struct adapter *adapter);
+
+void mac_reg_dump(void *sel, struct adapter *adapter);
+void bb_reg_dump(void *sel, struct adapter *adapter);
+void rf_reg_dump(void *sel, struct adapter *adapter);
+
+#ifdef PROC_DEBUG
+ssize_t proc_set_write_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_read_reg(struct seq_file *m, void *v);
+ssize_t proc_set_read_reg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_fwstate(struct seq_file *m, void *v);
+int proc_get_sec_info(struct seq_file *m, void *v);
+int proc_get_mlmext_state(struct seq_file *m, void *v);
+
+int proc_get_roam_flags(struct seq_file *m, void *v);
+ssize_t proc_set_roam_flags(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_roam_param(struct seq_file *m, void *v);
+ssize_t proc_set_roam_param(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+ssize_t proc_set_roam_tgt_addr(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_qos_option(struct seq_file *m, void *v);
+int proc_get_ht_option(struct seq_file *m, void *v);
+int proc_get_rf_info(struct seq_file *m, void *v);
+int proc_get_survey_info(struct seq_file *m, void *v);
+int proc_get_ap_info(struct seq_file *m, void *v);
+int proc_get_adapter_state(struct seq_file *m, void *v);
+int proc_get_trx_info(struct seq_file *m, void *v);
+int proc_get_rate_ctl(struct seq_file *m, void *v);
+ssize_t proc_set_rate_ctl(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_suspend_resume_info(struct seq_file *m, void *v);
+
+ssize_t proc_set_fwdl_test_case(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+ssize_t proc_set_wait_hiq_empty(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_all_sta_info(struct seq_file *m, void *v);
+
+int proc_get_rx_signal(struct seq_file *m, void *v);
+ssize_t proc_set_rx_signal(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_hw_status(struct seq_file *m, void *v);
+
+int proc_get_ht_enable(struct seq_file *m, void *v);
+ssize_t proc_set_ht_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_bw_mode(struct seq_file *m, void *v);
+ssize_t proc_set_bw_mode(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_ampdu_enable(struct seq_file *m, void *v);
+ssize_t proc_set_ampdu_enable(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_rx_ampdu(struct seq_file *m, void *v);
+ssize_t proc_set_rx_ampdu(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_rx_stbc(struct seq_file *m, void *v);
+ssize_t proc_set_rx_stbc(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_en_fwps(struct seq_file *m, void *v);
+ssize_t proc_set_en_fwps(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+/* int proc_get_two_path_rssi(struct seq_file *m, void *v); */
+int proc_get_rssi_disp(struct seq_file *m, void *v);
+ssize_t proc_set_rssi_disp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_btcoex_dbg(struct seq_file *m, void *v);
+ssize_t proc_set_btcoex_dbg(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_btcoex_info(struct seq_file *m, void *v);
+
+int proc_get_odm_dbg_comp(struct seq_file *m, void *v);
+ssize_t proc_set_odm_dbg_comp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+int proc_get_odm_dbg_level(struct seq_file *m, void *v);
+ssize_t proc_set_odm_dbg_level(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+int proc_get_odm_adaptivity(struct seq_file *m, void *v);
+ssize_t proc_set_odm_adaptivity(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+
+#ifdef CONFIG_DBG_COUNTER
+int proc_get_rx_logs(struct seq_file *m, void *v);
+int proc_get_tx_logs(struct seq_file *m, void *v);
+int proc_get_int_logs(struct seq_file *m, void *v);
+#endif
+
+#endif /* PROC_DEBUG */
+
+#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 000000000000..2e292bf9cf51
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_eeprom.h
@@ -0,0 +1,128 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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. */
+/*  */
+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,	/*  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 */
+}RT_CUSTOMER_ID, *PRT_CUSTOMER_ID;
+
+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 000000000000..5d3778a3204b
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_efuse.h
@@ -0,0 +1,132 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 _EFUSE_DEF_TYPE {
+	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----------------------------*/
+typedef struct PG_PKT_STRUCT_A{
+	u8 offset;
+	u8 word_en;
+	u8 data[8];
+	u8 word_cnts;
+}PGPKT_STRUCT,*PPGPKT_STRUCT;
+
+/*------------------------------Define structure----------------------------*/
+typedef 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];
+}EFUSE_HAL, *PEFUSE_HAL;
+
+
+/*------------------------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 000000000000..2bf23de7e516
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_event.h
@@ -0,0 +1,117 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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 addba_event
+{
+	unsigned int tid;
+};
+
+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;
+};
+
+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];
+};
+
+
+#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 000000000000..20ca0b7b481c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ht.h
@@ -0,0 +1,118 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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 rtw_ieee80211_ht_cap ht_cap;
+
+};
+
+typedef enum AGGRE_SIZE{
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+	VHT_AGG_SIZE_128K = 4,
+	VHT_AGG_SIZE_256K = 5,
+	VHT_AGG_SIZE_512K = 6,
+	VHT_AGG_SIZE_1024K = 7,
+}AGGRE_SIZE_E, *PAGGRE_SIZE_E;
+
+typedef enum _RT_HT_INF0_CAP{
+	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 */
+}RT_HT_INF0_CAPBILITY, *PRT_HT_INF0_CAPBILITY;
+
+typedef enum _RT_HT_INF1_CAP{
+	RT_HT_CAP_USE_VIDEO_CLIENT = 0x01,
+	RT_HT_CAP_USE_JAGUAR_BCUT = 0x02,
+	RT_HT_CAP_USE_JAGUAR_CCUT = 0x04,
+}RT_HT_INF1_CAPBILITY, *PRT_HT_INF1_CAPBILITY;
+
+#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 000000000000..0341d0d35375
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_io.h
@@ -0,0 +1,373 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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);
+
+		u8 (*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr);
+};
+
+struct io_req {
+	struct list_head	list;
+	u32 addr;
+	volatile u32 val;
+	u32 command;
+	u32 status;
+	u8 *pbuf;
+	_sema	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;/* 	pointer to &(padapter->dvobjpriv); */
+
+	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 */
+	/* 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
+
+};
+
+
+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
+
+};
+#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 {
+	_lock	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 u8 _rtw_sd_f0_read8(struct adapter *adapter, u32 addr);
+
+extern u32 _rtw_write_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem);
+
+#define rtw_read8(adapter, addr) _rtw_read8((adapter), (addr))
+#define rtw_read16(adapter, addr) _rtw_read16((adapter), (addr))
+#define rtw_read32(adapter, addr) _rtw_read32((adapter), (addr))
+
+#define  rtw_write8(adapter, addr, val) _rtw_write8((adapter), (addr), (val))
+#define  rtw_write16(adapter, addr, val) _rtw_write16((adapter), (addr), (val))
+#define  rtw_write32(adapter, addr, val) _rtw_write32((adapter), (addr), (val))
+
+#define rtw_write_port(adapter, addr, cnt, mem) _rtw_write_port((adapter), (addr), (cnt), (mem))
+
+#define rtw_sd_f0_read8(adapter, addr) _rtw_sd_f0_read8((adapter), (addr))
+
+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);
+
+#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/rtl8723bs/include/rtw_ioctl.h b/drivers/staging/rtl8723bs/include/rtw_ioctl.h
new file mode 100644
index 000000000000..c19e179fc426
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ioctl.h
@@ -0,0 +1,80 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _RTW_IOCTL_H_
+#define _RTW_IOCTL_H_
+
+/* 	00 - Success */
+/* 	11 - Error */
+#define STATUS_SUCCESS				(0x00000000L)
+#define STATUS_PENDING				(0x00000103L)
+
+#define STATUS_UNSUCCESSFUL			(0xC0000001L)
+#define STATUS_INSUFFICIENT_RESOURCES		(0xC000009AL)
+#define STATUS_NOT_SUPPORTED			(0xC00000BBL)
+
+#define NDIS_STATUS_SUCCESS			((uint)STATUS_SUCCESS)
+#define NDIS_STATUS_PENDING			((uint)STATUS_PENDING)
+#define NDIS_STATUS_NOT_RECOGNIZED		((uint)0x00010001L)
+#define NDIS_STATUS_NOT_COPIED			((uint)0x00010002L)
+#define NDIS_STATUS_NOT_ACCEPTED		((uint)0x00010003L)
+#define NDIS_STATUS_CALL_ACTIVE			((uint)0x00010007L)
+
+#define NDIS_STATUS_FAILURE			((uint)STATUS_UNSUCCESSFUL)
+#define NDIS_STATUS_RESOURCES			((uint)STATUS_INSUFFICIENT_RESOURCES)
+#define NDIS_STATUS_CLOSING			((uint)0xC0010002L)
+#define NDIS_STATUS_BAD_VERSION			((uint)0xC0010004L)
+#define NDIS_STATUS_BAD_CHARACTERISTICS		((uint)0xC0010005L)
+#define NDIS_STATUS_ADAPTER_NOT_FOUND		((uint)0xC0010006L)
+#define NDIS_STATUS_OPEN_FAILED			((uint)0xC0010007L)
+#define NDIS_STATUS_DEVICE_FAILED		((uint)0xC0010008L)
+#define NDIS_STATUS_MULTICAST_FULL		((uint)0xC0010009L)
+#define NDIS_STATUS_MULTICAST_EXISTS		((uint)0xC001000AL)
+#define NDIS_STATUS_MULTICAST_NOT_FOUND		((uint)0xC001000BL)
+#define NDIS_STATUS_REQUEST_ABORTED		((uint)0xC001000CL)
+#define NDIS_STATUS_RESET_IN_PROGRESS		((uint)0xC001000DL)
+#define NDIS_STATUS_CLOSING_INDICATING		((uint)0xC001000EL)
+#define NDIS_STATUS_NOT_SUPPORTED		((uint)STATUS_NOT_SUPPORTED)
+#define NDIS_STATUS_INVALID_PACKET		((uint)0xC001000FL)
+#define NDIS_STATUS_OPEN_LIST_FULL		((uint)0xC0010010L)
+#define NDIS_STATUS_ADAPTER_NOT_READY		((uint)0xC0010011L)
+#define NDIS_STATUS_ADAPTER_NOT_OPEN		((uint)0xC0010012L)
+#define NDIS_STATUS_NOT_INDICATING		((uint)0xC0010013L)
+#define NDIS_STATUS_INVALID_LENGTH		((uint)0xC0010014L)
+#define NDIS_STATUS_INVALID_DATA		((uint)0xC0010015L)
+#define NDIS_STATUS_BUFFER_TOO_SHORT		((uint)0xC0010016L)
+#define NDIS_STATUS_INVALID_OID			((uint)0xC0010017L)
+#define NDIS_STATUS_ADAPTER_REMOVED		((uint)0xC0010018L)
+#define NDIS_STATUS_UNSUPPORTED_MEDIA		((uint)0xC0010019L)
+#define NDIS_STATUS_GROUP_ADDRESS_IN_USE	((uint)0xC001001AL)
+#define NDIS_STATUS_FILE_NOT_FOUND		((uint)0xC001001BL)
+#define NDIS_STATUS_ERROR_READING_FILE		((uint)0xC001001CL)
+#define NDIS_STATUS_ALREADY_MAPPED		((uint)0xC001001DL)
+#define NDIS_STATUS_RESOURCE_CONFLICT		((uint)0xC001001EL)
+#define NDIS_STATUS_NO_CABLE			((uint)0xC001001FL)
+
+#define NDIS_STATUS_INVALID_SAP			((uint)0xC0010020L)
+#define NDIS_STATUS_SAP_IN_USE			((uint)0xC0010021L)
+#define NDIS_STATUS_INVALID_ADDRESS		((uint)0xC0010022L)
+#define NDIS_STATUS_VC_NOT_ACTIVATED		((uint)0xC0010023L)
+#define NDIS_STATUS_DEST_OUT_OF_ORDER		((uint)0xC0010024L)  /*  cause 27 */
+#define NDIS_STATUS_VC_NOT_AVAILABLE		((uint)0xC0010025L)  /*  cause 35, 45 */
+#define NDIS_STATUS_CELLRATE_NOT_AVAILABLE	((uint)0xC0010026L)  /*  cause 37 */
+#define NDIS_STATUS_INCOMPATABLE_QOS		((uint)0xC0010027L)  /*  cause 49 */
+#define NDIS_STATUS_AAL_PARAMS_UNSUPPORTED	((uint)0xC0010028L)  /*  cause 93 */
+#define NDIS_STATUS_NO_ROUTE_TO_DESTINATION	((uint)0xC0010029L)  /*  cause 3 */
+
+extern struct iw_handler_def  rtw_handlers_def;
+
+#endif /*  #ifndef __INC_CEINFO_ */
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 000000000000..ebf233559f67
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_ioctl_set.h
@@ -0,0 +1,41 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_IOCTL_SET_H_
+#define __RTW_IOCTL_SET_H_
+
+
+typedef u8 NDIS_802_11_PMKID_VALUE[16];
+
+typedef struct _BSSIDInfo {
+	NDIS_802_11_MAC_ADDRESS  BSSID;
+	NDIS_802_11_PMKID_VALUE  PMKID;
+} BSSIDInfo, *PBSSIDInfo;
+
+
+u8 rtw_set_802_11_authentication_mode(struct adapter *pdapter, enum NDIS_802_11_AUTHENTICATION_MODE authmode);
+u8 rtw_set_802_11_bssid(struct adapter *padapter, u8 *bssid);
+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);
+
+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 000000000000..d88ef67ce8d6
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme.h
@@ -0,0 +1,695 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 continous tx background */
+#define	WIFI_MP_CTX_ST						0x00040000	/*  in continous tx with single-tone */
+#define	WIFI_MP_CTX_BACKGROUND_PENDING	0x00080000	/*  pending in continous tx background due to out of skb */
+#define	WIFI_MP_CTX_CCK_HW					0x00100000	/*  in continous tx */
+#define	WIFI_MP_CTX_CCK_CS					0x00200000	/*  in continous 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 dot11AuthAlgrthmNum {
+ dot11AuthAlgrthm_Open = 0,
+ dot11AuthAlgrthm_Shared,
+ dot11AuthAlgrthm_8021X,
+ dot11AuthAlgrthm_Auto,
+ dot11AuthAlgrthm_WAPI,
+ dot11AuthAlgrthm_MaxNum
+};
+
+/*  Scan type including active and passive scan. */
+typedef enum _RT_SCAN_TYPE
+{
+	SCAN_PASSIVE,
+	SCAN_ACTIVE,
+	SCAN_MIX,
+}RT_SCAN_TYPE, *PRT_SCAN_TYPE;
+
+enum  _BAND
+{
+	GHZ24_50 = 0,
+	GHZ_50,
+	GHZ_24,
+	GHZ_MAX,
+};
+
+#define rtw_band_valid(band) ((band) >= GHZ24_50 && (band) < GHZ_MAX)
+
+enum DriverInterface {
+	DRIVER_WEXT =  1,
+	DRIVER_CFG80211 = 2
+};
+
+enum SCAN_RESULT_TYPE
+{
+	SCAN_RESULT_P2P_ONLY = 0,		/* 	Will return all the P2P devices. */
+	SCAN_RESULT_ALL = 1,			/* 	Will return all the scanned device, include AP. */
+	SCAN_RESULT_WFD_TYPE = 2		/* 	Will just return the correct WFD device. */
+									/* 	If this device is Miracast sink device, it will just return all the Miracast source devices. */
+};
+
+/*
+
+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 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 locks 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;
+	_timer	sitesurvey_ctrl_timer;
+};
+
+typedef 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;
+}RT_LINK_DETECT_T, *PRT_LINK_DETECT_T;
+
+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 informations */
+	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 negoitation 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 cfg80211_wifidirect_info{
+	_timer					remain_on_ch_timer;
+	u8 				restore_channel;
+	struct ieee80211_channel	remain_on_ch_channel;
+	enum nl80211_channel_type	remain_on_ch_type;
+	u64						remain_on_ch_cookie;
+	bool is_ro_ch;
+	unsigned long last_ro_ch_time; /* this will be updated at the beginning and end of ro_ch */
+};
+
+struct wifidirect_info{
+	struct adapter *			padapter;
+	_timer					find_phase_timer;
+	_timer					restore_p2p_state_timer;
+
+	/* 	Used to do the scanning. After confirming the peer is availalble, the driver transmits the P2P frame to peer. */
+	_timer					pre_tx_scan_timer;
+	_timer					reset_ch_sitesurvey;
+	_timer					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 negotation */
+	u8 				negotiation_dialog_token;
+	u8 				nego_ssid[ WLAN_SSID_MAXLEN ];	/* 	SSID information for group negotitation */
+	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 RxPWDBAll;
+	u8 is_tdls_sta;	/*  true: direct link sta, false: else */
+};
+
+struct tdls_info{
+	u8 			ap_prohibited;
+	u8 			link_established;
+	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 			ch_sensing;
+	u8 			cur_channel;
+	u8 			candidate_ch;
+	u8 			collect_pkt_num[MAX_CHANNEL_NUM];
+	_lock				cmd_lock;
+	_lock				hdl_lock;
+	u8 			watchdog_count;
+	u8 			dev_discovered;		/* WFD_TDLS: for sigma test */
+	u8 			tdls_enable;
+	u8 			external_setup;	/*  true: setup is handled by wpa_supplicant */
+};
+
+struct tdls_txmgmt {
+	u8 peer[ETH_ALEN];
+	u8 action_code;
+	u8 dialog_token;
+	u16 status_code;
+	u8 *buf;
+	size_t len;
+	u8 external_support;
+};
+
+/* used for mlme_priv.roam_flags */
+enum {
+	RTW_ROAM_ON_EXPIRED = BIT0,
+	RTW_ROAM_ON_RESUME = BIT1,
+	RTW_ROAM_ACTIVE = BIT2,
+};
+
+struct mlme_priv {
+
+	_lock	lock;
+	sint	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;
+
+	_timer assoc_timer;
+
+	uint assoc_by_bssid;
+	uint assoc_by_rssi;
+
+	_timer scan_to_timer; /*  driver itself handles scan_timeout status. */
+	unsigned long scan_start_time; /*  used to evaluate the time spent in scanning */
+
+	_timer 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;
+
+	RT_LINK_DETECT_T	LinkDetectInfo;
+	_timer	dynamic_chk_timer; /* dynamic/periodic check timer */
+
+	u8 acm_mask; /*  for wmm acm mask */
+	u8 ChannelPlan;
+	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;
+
+	_lock	bcn_update_lock;
+	u8 update_bcn;
+
+#ifdef CONFIG_INTEL_WIDI
+	int	widi_state;
+	int	listen_state;
+	_timer	listen_timer;
+	atomic_t	rx_probe_rsp; /*  1:receive probe respone from RDS source. */
+	u8 *l2sdTaBuffer;
+	u8 channel_idx;
+	u8 group_cnt;	/* In WiDi 3.5, they specified another scan algo. for WFD/RDS co-existed */
+	u8 sa_ext[L2SDTA_SERVICE_VE_LEN];
+
+	u8 widi_enable;
+	/**
+	 * For WiDi 4; upper layer would set
+	 * p2p_primary_device_type_category_id
+	 * p2p_primary_device_type_sub_category_id
+	 * p2p_secondary_device_type_category_id
+	 * p2p_secondary_device_type_sub_category_id
+	 */
+	u16 p2p_pdt_cid;
+	u16 p2p_pdt_scid;
+	u8 num_p2p_sdt;
+	u16 p2p_sdt_cid[MAX_NUM_P2P_SDT];
+	u16 p2p_sdt_scid[MAX_NUM_P2P_SDT];
+	u8 p2p_reject_disable;	/* When starting NL80211 wpa_supplicant/hostapd, it will call netdev_close */
+							/* such that it will cause p2p disabled. Use this flag to reject. */
+#endif /*  CONFIG_INTEL_WIDI */
+
+	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(RTW_TIMER_HDL_ARGS);
+extern void _rtw_scan_timeout_handler(RTW_TIMER_HDL_ARGS);
+
+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 sint rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv);
+extern sint rtw_set_key(struct adapter *adapter, struct security_priv *psecuritypriv, sint keyid, u8 set_tx, bool enqueue);
+extern sint rtw_set_auth(struct adapter *adapter, struct security_priv *psecuritypriv);
+
+__inline static 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;
+}
+
+__inline static sint check_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	if (pmlmepriv->fw_state & state)
+		return true;
+
+	return false;
+}
+
+__inline static 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:#### (!!!!)
+ * MUST TAKE CARE THAT BEFORE CALLING THIS FUNC, YOU SHOULD HAVE LOCKED pmlmepriv->lock
+ */
+__inline static void set_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	pmlmepriv->fw_state |= state;
+	/* FOR HW integration */
+	if (_FW_UNDER_SURVEY ==state) {
+		pmlmepriv->bScanInProcess = true;
+	}
+}
+
+__inline static void _clr_fwstate_(struct mlme_priv *pmlmepriv, sint 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...
+ */
+__inline static void clr_fwstate(struct mlme_priv *pmlmepriv, sint state)
+{
+	spin_lock_bh(&pmlmepriv->lock);
+	if (check_fwstate(pmlmepriv, state) == true)
+		pmlmepriv->fw_state ^= state;
+	spin_unlock_bh(&pmlmepriv->lock);
+}
+
+__inline static void set_scanned_network_val(struct mlme_priv *pmlmepriv, sint 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 adapter *adapter);
+extern void rtw_scan_timeout_handler(struct adapter *adapter);
+
+extern void rtw_dynamic_check_timer_handlder(struct adapter *adapter);
+bool rtw_is_scan_deny(struct adapter *adapter);
+void rtw_clear_scan_deny(struct adapter *adapter);
+void rtw_set_scan_deny_timer_hdl(struct adapter *adapter);
+void rtw_set_scan_deny(struct adapter *adapter, u32 ms);
+
+extern int _rtw_init_mlme_priv(struct adapter *padapter);
+
+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 void _rtw_free_network_queue(struct adapter *padapter, u8 isfreeall);
+
+extern sint rtw_if_up(struct adapter *padapter);
+
+sint 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 000000000000..f3952463697e
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
@@ -0,0 +1,888 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 wnat to customize the acitions(scan period or join actions) about one of the channel plan, */
+/* 	customize them in RT_CHANNEL_INFO in the RT_CHANNEL_LIST. */
+/*  */
+typedef 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_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,
+}RT_CHANNEL_DOMAIN, *PRT_CHANNEL_DOMAIN;
+
+typedef enum _RT_CHANNEL_DOMAIN_2G
+{
+	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,
+}RT_CHANNEL_DOMAIN_2G, *PRT_CHANNEL_DOMAIN_2G;
+
+typedef enum _RT_CHANNEL_DOMAIN_5G
+{
+	RT_CHANNEL_DOMAIN_5G_NULL = 0x00,
+	RT_CHANNEL_DOMAIN_5G_ETSI1 = 0x01,		/* Europe */
+	RT_CHANNEL_DOMAIN_5G_ETSI2 = 0x02,		/* Australia, New Zealand */
+	RT_CHANNEL_DOMAIN_5G_ETSI3 = 0x03,		/* Russia */
+	RT_CHANNEL_DOMAIN_5G_FCC1 = 0x04,		/* US */
+	RT_CHANNEL_DOMAIN_5G_FCC2 = 0x05,		/* FCC o/w DFS Channels */
+	RT_CHANNEL_DOMAIN_5G_FCC3 = 0x06,		/* India, Mexico */
+	RT_CHANNEL_DOMAIN_5G_FCC4 = 0x07,		/* Venezuela */
+	RT_CHANNEL_DOMAIN_5G_FCC5 = 0x08,		/* China */
+	RT_CHANNEL_DOMAIN_5G_FCC6 = 0x09,		/* Israel */
+	RT_CHANNEL_DOMAIN_5G_FCC7_IC1 = 0x0A,	/* US, Canada */
+	RT_CHANNEL_DOMAIN_5G_KCC1 = 0x0B,		/* Korea */
+	RT_CHANNEL_DOMAIN_5G_MKK1 = 0x0C,		/* Japan */
+	RT_CHANNEL_DOMAIN_5G_MKK2 = 0x0D,		/* Japan (W52, W53) */
+	RT_CHANNEL_DOMAIN_5G_MKK3 = 0x0E,		/* Japan (W56) */
+	RT_CHANNEL_DOMAIN_5G_NCC1 = 0x0F,		/* Taiwan */
+	RT_CHANNEL_DOMAIN_5G_NCC2 = 0x10,		/* Taiwan o/w DFS */
+	RT_CHANNEL_DOMAIN_5G_NCC3 = 0x11,		/* Taiwan w/o DFS, Band4 only */
+	RT_CHANNEL_DOMAIN_5G_ETSI4 = 0x12,		/* Europe w/o DFS, Band1 only */
+	RT_CHANNEL_DOMAIN_5G_ETSI5 = 0x13,		/* Australia, New Zealand(w/o Weather radar) */
+	RT_CHANNEL_DOMAIN_5G_FCC8 = 0x14,		/* Latin America */
+	RT_CHANNEL_DOMAIN_5G_ETSI6 = 0x15,		/* Israel, Bahrain, Egypt, India, China, Malaysia */
+	RT_CHANNEL_DOMAIN_5G_ETSI7 = 0x16,		/* China */
+	RT_CHANNEL_DOMAIN_5G_ETSI8 = 0x17,		/* Jordan */
+	RT_CHANNEL_DOMAIN_5G_ETSI9 = 0x18,		/* Lebanon */
+	RT_CHANNEL_DOMAIN_5G_ETSI10 = 0x19,		/* Qatar */
+	RT_CHANNEL_DOMAIN_5G_ETSI11 = 0x1A,		/* Russia */
+	RT_CHANNEL_DOMAIN_5G_NCC4 = 0x1B,		/* Taiwan, (w/o Weather radar) */
+	RT_CHANNEL_DOMAIN_5G_ETSI12 = 0x1C,		/* Indonesia */
+	RT_CHANNEL_DOMAIN_5G_FCC9 = 0x1D,		/* w/o Weather radar) */
+	RT_CHANNEL_DOMAIN_5G_ETSI13 = 0x1E,		/* w/o Weather radar) */
+	RT_CHANNEL_DOMAIN_5G_FCC10 = 0x1F,		/* Argentina (w/o Weather radar) */
+	/*  Add new channel plan above this line =============== */
+	/*  Driver Self Defined ===== */
+	RT_CHANNEL_DOMAIN_5G_FCC = 0x20,
+	RT_CHANNEL_DOMAIN_5G_JAPAN_NO_DFS = 0x21,
+	RT_CHANNEL_DOMAIN_5G_FCC4_NO_DFS = 0x22,
+	RT_CHANNEL_DOMAIN_5G_MAX,
+}RT_CHANNEL_DOMAIN_5G, *PRT_CHANNEL_DOMAIN_5G;
+
+#define rtw_is_channel_plan_valid(chplan) (chplan<RT_CHANNEL_DOMAIN_MAX || chplan == RT_CHANNEL_DOMAIN_REALTEK_DEFINE)
+
+typedef struct _RT_CHANNEL_PLAN
+{
+	unsigned char Channel[MAX_CHANNEL_NUM];
+	unsigned char Len;
+}RT_CHANNEL_PLAN, *PRT_CHANNEL_PLAN;
+
+typedef struct _RT_CHANNEL_PLAN_2G
+{
+	unsigned char Channel[MAX_CHANNEL_NUM_2G];
+	unsigned char Len;
+}RT_CHANNEL_PLAN_2G, *PRT_CHANNEL_PLAN_2G;
+
+typedef struct _RT_CHANNEL_PLAN_5G
+{
+	unsigned char Channel[MAX_CHANNEL_NUM_5G];
+	unsigned char Len;
+}RT_CHANNEL_PLAN_5G, *PRT_CHANNEL_PLAN_5G;
+
+typedef struct _RT_CHANNEL_PLAN_MAP
+{
+	unsigned char Index2G;
+	unsigned char Index5G;
+}RT_CHANNEL_PLAN_MAP, *PRT_CHANNEL_PLAN_MAP;
+
+enum Associated_AP
+{
+	atherosAP	= 0,
+	broadcomAP	= 1,
+	ciscoAP		= 2,
+	marvellAP	= 3,
+	ralinkAP	= 4,
+	realtekAP	= 5,
+	airgocapAP	= 6,
+	unknownAP	= 7,
+	maxAP,
+};
+
+typedef 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, 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
+}HT_IOT_PEER_E, *PHTIOT_PEER_E;
+
+
+enum SCAN_STATE
+{
+	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 + 5GHz 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.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 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 bAcceptAddbaReq;
+	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. */
+typedef struct _RT_CHANNEL_INFO
+{
+	u8 		ChannelNum;		/*  The channel number. */
+	RT_SCAN_TYPE	ScanType;		/*  Scan type such as passive or active scan. */
+}RT_CHANNEL_INFO, *PRT_CHANNEL_INFO;
+
+int rtw_ch_set_search_ch(RT_CHANNEL_INFO *ch_set, const u32 ch);
+bool rtw_mlme_band_check(struct adapter *adapter, 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;
+	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 */
+	_timer		survey_timer;
+	_timer		link_timer;
+	_timer		sa_query_timer;
+	/* _timer		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);
+int 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); */
+
+unsigned char 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);
+void write_cam_from_cache(struct adapter *adapter, u8 id);
+
+/* 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);
+bool rtw_port_switch_chk(struct adapter *adapter);
+void report_wmm_edca_update(struct adapter *padapter);
+
+void beacon_timing_control(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 adapter *padapter);
+void link_timer_hdl (struct adapter *padapter);
+void addba_timer_hdl(struct sta_info *psta);
+void sa_query_timer_hdl(struct adapter *padapter);
+/* void reauth_timer_hdl(struct adapter *padapter); */
+/* void reassoc_timer_hdl(struct adapter *padapter); */
+
+#define set_survey_timer(mlmeext, ms) \
+	do { \
+		/*DBG_871X("%s set_survey_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
+		_set_timer(&(mlmeext)->survey_timer, (ms)); \
+	} while (0)
+
+#define set_link_timer(mlmeext, ms) \
+	do { \
+		/*DBG_871X("%s set_link_timer(%p, %d)\n", __func__, (mlmeext), (ms));*/ \
+		_set_timer(&(mlmeext)->link_timer, (ms)); \
+	} while (0)
+#define set_sa_query_timer(mlmeext, ms) \
+	do { \
+		DBG_871X("%s set_sa_query_timer(%p, %d)\n", __func__, (mlmeext), (ms)); \
+		_set_timer(&(mlmeext)->sa_query_timer, (ms)); \
+	} while (0)
+extern int cckrates_included(unsigned char *rate, int ratelen);
+extern int cckratesonly_included(unsigned char *rate, int ratelen);
+
+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);
+int rtw_get_ch_setting_union(struct adapter *adapter, 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 led_blink_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 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 */
+	GEN_EVT_CODE(_WMM),					/*25*/
+	MAX_C2HEVT
+};
+
+
+#ifdef _RTW_MLME_EXT_C_
+
+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},
+
+};
+
+#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 000000000000..88ace11e42e9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_mp.h
@@ -0,0 +1,512 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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;
+
+	_pkt *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;
+};
+
+typedef void(*wi_act_func)(void* padapter);
+
+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);
+typedef struct _MPT_CONTEXT
+{
+	/*  Indicate if we have started Mass Production Test. */
+	bool			bMassProdTest;
+
+	/*  Indicate if the driver is unloading or unloaded. */
+	bool			bMptDrvUnload;
+
+	_sema			MPh2c_Sema;
+	_timer			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 Regsiter 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];
+
+}MPT_CONTEXT, *PMPT_CONTEXT;
+/* 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,
+#ifdef CONFIG_WOWLAN
+	MP_WOW_ENABLE,
+#endif
+#ifdef CONFIG_AP_WOWLAN
+	MP_AP_WOW_ENABLE,
+#endif
+	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;
+
+	MPT_CONTEXT MptCtx;
+
+	u8 *TXradomBuffer;
+};
+
+typedef struct _IOCMD_STRUCT_ {
+	u8 cmdclass;
+	u16 value;
+	u8 index;
+}IOCMD_STRUCT;
+
+struct rf_reg_param {
+	u32 path;
+	u32 offset;
+	u32 value;
+};
+
+struct bb_reg_param {
+	u32 offset;
+	u32 value;
+};
+
+#define LOWER	true
+#define RAISE	false
+
+/* Hardware Registers */
+#define BB_REG_BASE_ADDR		0x800
+
+/* MP variables */
+enum MP_MODE {
+	MP_OFF,
+	MP_ON,
+	MP_ERR,
+	MP_CONTINUOUS_TX,
+	MP_SINGLE_CARRIER_TX,
+	MP_CARRIER_SUPPRISSION_TX,
+	MP_SINGLE_TONE_TX,
+	MP_PACKET_TX,
+	MP_PACKET_RX
+};
+
+#define MAX_RF_PATH_NUMS	RF_PATH_MAX
+
+extern u8 mpdatarate[NumRates];
+
+/* MP set force data rate base on the definition. */
+enum MPT_RATE_INDEX {
+	/* CCK rate. */
+	MPT_RATE_1M = 0 ,	/* 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 */
+	/* VHT rate. Total: 20*/
+	MPT_RATE_VHT1SS_MCS0 = 100,/*  To reserve MCS16~MCS31, the index starts from #100. */
+	MPT_RATE_VHT1SS_MCS1, /*  #101 */
+	MPT_RATE_VHT1SS_MCS2,
+	MPT_RATE_VHT1SS_MCS3,
+	MPT_RATE_VHT1SS_MCS4,
+	MPT_RATE_VHT1SS_MCS5,
+	MPT_RATE_VHT1SS_MCS6, /*  #106 */
+	MPT_RATE_VHT1SS_MCS7,
+	MPT_RATE_VHT1SS_MCS8,
+	MPT_RATE_VHT1SS_MCS9,
+	MPT_RATE_VHT2SS_MCS0,
+	MPT_RATE_VHT2SS_MCS1, /*  #111 */
+	MPT_RATE_VHT2SS_MCS2,
+	MPT_RATE_VHT2SS_MCS3,
+	MPT_RATE_VHT2SS_MCS4,
+	MPT_RATE_VHT2SS_MCS5,
+	MPT_RATE_VHT2SS_MCS6, /*  #116 */
+	MPT_RATE_VHT2SS_MCS7,
+	MPT_RATE_VHT2SS_MCS8,
+	MPT_RATE_VHT2SS_MCS9,
+	MPT_RATE_LAST
+};
+
+#define MAX_TX_PWR_INDEX_N_MODE 64	/*  0x3F */
+
+enum POWER_MODE {
+	POWER_LOW = 0,
+	POWER_NORMAL
+};
+
+/*  The following enumeration is used to define the value of Reg0xD00[30:28] or JaguarReg0x914[18:16]. */
+enum OFDM_TX_MODE {
+	OFDM_ALL_OFF		= 0,
+	OFDM_ContinuousTx	= 1,
+	OFDM_SingleCarrier	= 2,
+	OFDM_SingleTone		= 4,
+};
+
+#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
+
+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 */
+};
+
+enum MPT_TXPWR_DEF {
+	MPT_CCK,
+	MPT_OFDM, /*  L and HT OFDM */
+	MPT_VHT_OFDM
+};
+
+#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_SetBandwidth(struct adapter *padapter);
+
+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_odm.h b/drivers/staging/rtl8723bs/include/rtw_odm.h
new file mode 100644
index 000000000000..961ae2c218f9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_odm.h
@@ -0,0 +1,36 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_ODM_H__
+#define __RTW_ODM_H__
+
+#include <drv_types.h>
+
+/*
+* This file provides utilities/wrappers for rtw driver to use ODM
+*/
+
+void rtw_odm_dbg_comp_msg(void *sel, struct adapter *adapter);
+void rtw_odm_dbg_comp_set(struct adapter *adapter, u64 comps);
+void rtw_odm_dbg_level_msg(void *sel, struct adapter *adapter);
+void rtw_odm_dbg_level_set(struct adapter *adapter, u32 level);
+
+void rtw_odm_ability_msg(void *sel, struct adapter *adapter);
+void rtw_odm_ability_set(struct adapter *adapter, u32 ability);
+
+void rtw_odm_adaptivity_parm_msg(void *sel, struct adapter *adapter);
+void rtw_odm_adaptivity_parm_set(struct adapter *adapter, s8 TH_L2H_ini, s8 TH_EDCCA_HL_diff,
+	s8 IGI_Base, bool ForceEDCCA, u8 AdapEn_RSSI, u8 IGI_LowerBound);
+void rtw_odm_get_perpkt_rssi(void *sel, struct adapter *adapter);
+#endif /*  __RTW_ODM_H__ */
diff --git a/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h b/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h
new file mode 100644
index 000000000000..cf8e766a27a8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_pwrctrl.h
@@ -0,0 +1,375 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_PWRCTRL_H_
+#define __RTW_PWRCTRL_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 Power_Mgnt
+{
+	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,
+};
+
+#ifdef CONFIG_PNO_SUPPORT
+#define MAX_PNO_LIST_COUNT 16
+#define MAX_SCAN_LIST_COUNT 14 /* 2.4G only */
+#endif
+
+/*
+	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;
+};
+
+
+typedef _sema _pwrlock;
+
+
+#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 transmittd. */
+#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 _PS_BBRegBackup_ {
+	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
+};
+
+#ifdef CONFIG_PNO_SUPPORT
+typedef struct pno_nlo_info
+{
+	u32 fast_scan_period;				/* Fast scan period */
+	u32 ssid_num;				/* number of entry */
+	u32 slow_scan_period;			/* slow scan period */
+	u32 fast_scan_iterations;			/* Fast scan iterations */
+	u8 ssid_length[MAX_PNO_LIST_COUNT];	/* SSID Length Array */
+	u8 ssid_cipher_info[MAX_PNO_LIST_COUNT];	/* Cipher information for security */
+	u8 ssid_channel_info[MAX_PNO_LIST_COUNT];	/* channel information */
+}pno_nlo_info_t;
+
+typedef struct pno_ssid {
+	u32 	SSID_len;
+	u8 SSID[32];
+} pno_ssid_t;
+
+typedef struct pno_ssid_list {
+	pno_ssid_t	node[MAX_PNO_LIST_COUNT];
+}pno_ssid_list_t;
+
+typedef struct pno_scan_channel_info
+{
+	u8 channel;
+	u8 tx_power;
+	u8 timeout;
+	u8 active;				/* set 1 means active scan, or pasivite scan. */
+}pno_scan_channel_info_t;
+
+typedef struct pno_scan_info
+{
+	u8 enableRFE;			/* Enable RFE */
+	u8 period_scan_time;		/* exclusive with fast_scan_period and slow_scan_period */
+	u8 periodScan;			/* exclusive with fast_scan_period and slow_scan_period */
+	u8 orig_80_offset;			/* original channel 80 offset */
+	u8 orig_40_offset;			/* original channel 40 offset */
+	u8 orig_bw;			/* original bandwidth */
+	u8 orig_ch;			/* original channel */
+	u8 channel_num;			/* number of channel */
+	u64	rfe_type;			/* rfe_type && 0x00000000000000ff */
+	pno_scan_channel_info_t ssid_channel_info[MAX_SCAN_LIST_COUNT];
+}pno_scan_info_t;
+#endif /* CONFIG_PNO_SUPPORT */
+
+struct pwrctrl_priv
+{
+	_pwrlock	lock;
+	_pwrlock	check_32k_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;
+	_workitem cpwm_event;
+	u8 brpwmtimeout;
+	_workitem rpwmtimeoutwi;
+	_timer 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 */
+	uint 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 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;
+	u8 bFwCurrentInPSMode;
+	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;
+#ifdef CONFIG_WOWLAN
+	u8 wowlan_pattern;
+	u8 wowlan_magic;
+	u8 wowlan_unicast;
+	u8 wowlan_pattern_idx;
+	u8 wowlan_pno_enable;
+#ifdef CONFIG_PNO_SUPPORT
+	u8 pno_in_resume;
+	u8 pno_inited;
+	pno_nlo_info_t	*pnlo_info;
+	pno_scan_info_t	*pscan_info;
+	pno_ssid_list_t	*pno_ssid_list;
+#endif
+	u32 	wowlan_pattern_context[8][5];
+	u64		wowlan_fw_iv;
+#endif /*  CONFIG_WOWLAN */
+	_timer	pwr_state_check_timer;
+	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_get_ips_mode_req(pwrctl) \
+	(pwrctl)->ips_mode_req
+
+#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 { \
+		/*DBG_871X("%s _rtw_set_pwr_state_check_timer(%p, %d)\n", __func__, (pwrctl), (ms));*/ \
+		_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 000000000000..ce6d9142bc5a
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_qos.h
@@ -0,0 +1,27 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _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 000000000000..570a3c333aa0
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_recv.h
@@ -0,0 +1,553 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _RTW_RECV_H_
+#define _RTW_RECV_H_
+
+	#ifdef CONFIG_SINGLE_RECV_BUF
+		#define NR_RECVBUFF (1)
+	#else
+		#define NR_RECVBUFF (8)
+	#endif /* CONFIG_SINGLE_RECV_BUF */
+
+	#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
+extern u8 rtw_rfc1042_header[];
+extern u8 rtw_bridge_tunnel_header[];
+
+/* 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;
+	_timer 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 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 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 RxPWDBAll;
+
+	u8 SignalQuality;	 /*  in 0-100 index. */
+	s8		RxMIMOSignalQuality[4];	/* per-path's EVM */
+	u8 RxMIMOEVMdbm[4];		/* per-path's EVM dbm */
+
+	u8 RxMIMOSignalStrength[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 BTRxRSSIPercentage;
+	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_singal_strength[4];/*  in 0~100 index */
+	u8 mimo_singal_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 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];
+	u8 src[ETH_ALEN];
+	u8 ta[ETH_ALEN];
+	u8 ra[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+
+	u8 ack_policy;
+
+/* ifdef CONFIG_TCP_CSUM_OFFLOAD_RX */
+	u8 tcpchk_valid; /*  0: invalid, 1: valid */
+	u8 ip_chkrpt; /* 0: incorrect, 1: correct */
+	u8 tcp_chkrpt; /* 0: incorrect, 1: correct */
+/* endif */
+	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 RxPWDBAll;
+	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 {
+	_lock	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;
+#ifdef CONFIG_RX_INDICATE_QUEUE
+	struct task rx_indicate_tasklet;
+	struct ifqueue rx_indicate_queue;
+#endif	/*  CONFIG_RX_INDICATE_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 informatiom */
+	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; */
+
+
+	_timer 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 {
+
+	_lock	lock;
+	sint	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;
+
+	_lock recvbuf_lock;
+
+	u32 ref_cnt;
+
+	struct adapter * adapter;
+
+	u8 *pbuf;
+	u8 *pallocated_buf;
+
+	u32 len;
+	u8 *phead;
+	u8 *pdata;
+	u8 *ptail;
+	u8 *pend;
+
+	_pkt	*pskb;
+	u8 reuse;
+};
+
+
+/*
+	head  ----->
+
+		data  ----->
+
+			payload
+
+		tail  ----->
+
+
+	end   ----->
+
+	len = (unsigned int)(tail - data);
+
+*/
+struct recv_frame_hdr
+{
+	struct list_head	list;
+#ifndef CONFIG_BSD_RX_USE_MBUF
+	struct sk_buff	 *pkt;
+	struct sk_buff	 *pkt_newalloc;
+#else /*  CONFIG_BSD_RX_USE_MBUF */
+	_pkt	*pkt;
+	_pkt *pkt_newalloc;
+#endif /*  CONFIG_BSD_RX_USE_MBUF */
+
+	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 RX_PACKET_TYPE {
+	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);
+
+sint rtw_enqueue_recvbuf_to_head(struct recv_buf *precvbuf, struct __queue *queue);
+sint 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(void *pcontext);
+
+__inline static u8 *get_rxmem(union recv_frame *precvframe)
+{
+	/* always return rx_head... */
+	if (precvframe == NULL)
+		return NULL;
+
+	return precvframe->u.hdr.rx_head;
+}
+
+__inline static u8 *get_recvframe_data(union recv_frame *precvframe)
+{
+
+	/* alwasy return rx_data */
+	if (precvframe == NULL)
+		return NULL;
+
+	return precvframe->u.hdr.rx_data;
+
+}
+
+__inline static u8 *recvframe_pull(union recv_frame *precvframe, sint 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;
+
+}
+
+__inline static u8 *recvframe_put(union recv_frame *precvframe, sint 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;
+
+}
+
+
+
+__inline static 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 == 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;
+
+}
+
+__inline static 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);
+
+}
+
+__inline static sint get_recvframe_len(union recv_frame *precvframe)
+{
+	return precvframe->u.hdr.len;
+}
+
+
+__inline static s32 translate_percentage_to_dbm(u32 SignalStrengthIndex)
+{
+	s32	SignalPower; /*  in dBm. */
+
+#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+	/*  Translate to dBm (x =y-100) */
+	SignalPower = SignalStrengthIndex - 100;
+#else
+	/*  Translate to dBm (x = 0.5y-95). */
+	SignalPower = (s32)((SignalStrengthIndex + 1) >> 1);
+	SignalPower -= 95;
+#endif
+
+	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 000000000000..f9becab7f7ad
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_rf.h
@@ -0,0 +1,159 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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	__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} */
+/*  5G, total 24 chnls */
+/*  {36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120,
+ *   124, 128, 132, 136, 140, 149, 153, 157, 161, 165} */
+#define	MAX_CHANNEL_NUM_2G	14
+#define	MAX_CHANNEL_NUM_5G	24
+#define	MAX_CHANNEL_NUM		38/* 14+24 */
+
+#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 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,
+};
+
+enum _RTL8712_RF_MIMO_CONFIG_ {
+	RTL8712_RFCONFIG_1T = 0x10,
+	RTL8712_RFCONFIG_2T = 0x20,
+	RTL8712_RFCONFIG_1R = 0x01,
+	RTL8712_RFCONFIG_2R = 0x02,
+	RTL8712_RFCONFIG_1T1R = 0x11,
+	RTL8712_RFCONFIG_1T2R = 0x12,
+	RTL8712_RFCONFIG_TURBO = 0x92,
+	RTL8712_RFCONFIG_2T2R = 0x22
+};
+
+enum RF90_RADIO_PATH {
+	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 */
+};
+
+/*  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,
+	CHANNEL_WIDTH_80 = 2,
+	CHANNEL_WIDTH_160 = 3,
+	CHANNEL_WIDTH_80_80 = 4,
+	CHANNEL_WIDTH_MAX = 5,
+};
+
+/*  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 VHT_DATA_SC {
+	VHT_DATA_SC_DONOT_CARE = 0,
+	VHT_DATA_SC_20_UPPER_OF_80MHZ = 1,
+	VHT_DATA_SC_20_LOWER_OF_80MHZ = 2,
+	VHT_DATA_SC_20_UPPERST_OF_80MHZ = 3,
+	VHT_DATA_SC_20_LOWEST_OF_80MHZ = 4,
+	VHT_DATA_SC_20_RECV1 = 5,
+	VHT_DATA_SC_20_RECV2 = 6,
+	VHT_DATA_SC_20_RECV3 = 7,
+	VHT_DATA_SC_20_RECV4 = 8,
+	VHT_DATA_SC_40_UPPER_OF_80MHZ = 9,
+	VHT_DATA_SC_40_LOWER_OF_80MHZ = 10,
+};
+
+enum PROTECTION_MODE {
+	PROTECTION_MODE_AUTO = 0,
+	PROTECTION_MODE_FORCE_ENABLE = 1,
+	PROTECTION_MODE_FORCE_DISABLE = 2,
+};
+
+/* 2007/11/15 MH Define different RF type. */
+enum RT_RF_TYPE_DEFINITION {
+	RF_1T2R = 0,
+	RF_2T4R = 1,
+	RF_2T2R = 2,
+	RF_1T1R = 3,
+	RF_2T2R_GREEN = 4,
+	RF_MAX_TYPE = 5,
+};
+
+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 000000000000..d5af72b2335d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_security.h
@@ -0,0 +1,440 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_SECURITY_H_
+#define __RTW_SECURITY_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 _WPA_IE_ID_	0xdd
+#define _WPA2_IE_ID_	0x30
+
+#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];
+};
+
+
+typedef struct _RT_PMKID_LIST
+{
+	u8 				bUsed;
+	u8 				Bssid[6];
+	u8 				PMKID[16];
+	u8 				SsidBuf[33];
+	u8*					ssid_octet;
+	u16 					ssid_length;
+} RT_PMKID_LIST, *PRT_PMKID_LIST;
+
+
+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];	/*  802.1x Group Key, for inx0 and inx1 */
+	union Keytype	dot118021XGrptxmickey[BIP_MAX_KEYID];
+	union Keytype	dot118021XGrprxmickey[BIP_MAX_KEYID];
+	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[6];		/*  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;
+
+
+	u8 binstallGrpkey;
+#ifdef CONFIG_GTK_OL
+	u8 binstallKCK_KEK;
+#endif /* CONFIG_GTK_OL */
+	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. */
+	RT_PMKID_LIST		PMKIDList[NUM_PMKID_CACHE];	/*  Renamed from PreAuthKey[NUM_PRE_AUTH_KEY]. Annie, 2006-10-13. */
+	u8 		PMKIDIndex;
+
+	u8 bWepDefaultKeyIdxSet;
+
+#define DBG_SW_SEC_CNT
+#ifdef DBG_SW_SEC_CNT
+	u64 wep_sw_enc_cnt_bc;
+	u64 wep_sw_enc_cnt_mc;
+	u64 wep_sw_enc_cnt_uc;
+	u64 wep_sw_dec_cnt_bc;
+	u64 wep_sw_dec_cnt_mc;
+	u64 wep_sw_dec_cnt_uc;
+
+	u64 tkip_sw_enc_cnt_bc;
+	u64 tkip_sw_enc_cnt_mc;
+	u64 tkip_sw_enc_cnt_uc;
+	u64 tkip_sw_dec_cnt_bc;
+	u64 tkip_sw_dec_cnt_mc;
+	u64 tkip_sw_dec_cnt_uc;
+
+	u64 aes_sw_enc_cnt_bc;
+	u64 aes_sw_enc_cnt_mc;
+	u64 aes_sw_enc_cnt_uc;
+	u64 aes_sw_dec_cnt_bc;
+	u64 aes_sw_dec_cnt_mc;
+	u64 aes_sw_dec_cnt_uc;
+#endif /* DBG_SW_SEC_CNT */
+};
+
+struct sha256_state {
+	u64 length;
+	u32 state[8], curlen;
+	u8 buf[64];
+};
+
+#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 _AES_IV_LEN_ 8
+
+#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 */
+};
+
+extern const u32 Te0[256];
+extern const u32 Te1[256];
+extern const u32 Te2[256];
+extern const u32 Te3[256];
+extern const u32 Te4[256];
+extern const u32 Td0[256];
+extern const u32 Td1[256];
+extern const u32 Td2[256];
+extern const u32 Td3[256];
+extern const u32 Td4[256];
+extern const u32 rcon[10];
+extern const u8 Td4s[256];
+extern const u8 rcons[10];
+
+#define RCON(i) (rcons[(i)] << 24)
+
+static inline u32 rotr(u32 val, int bits)
+{
+	return (val >> bits) | (val << (32 - bits));
+}
+
+#define TE0(i) Te0[((i) >> 24) & 0xff]
+#define TE1(i) rotr(Te0[((i) >> 16) & 0xff], 8)
+#define TE2(i) rotr(Te0[((i) >> 8) & 0xff], 16)
+#define TE3(i) rotr(Te0[(i) & 0xff], 24)
+#define TE41(i) ((Te0[((i) >> 24) & 0xff] << 8) & 0xff000000)
+#define TE42(i) (Te0[((i) >> 16) & 0xff] & 0x00ff0000)
+#define TE43(i) (Te0[((i) >> 8) & 0xff] & 0x0000ff00)
+#define TE44(i) ((Te0[(i) & 0xff] >> 8) & 0x000000ff)
+#define TE421(i) ((Te0[((i) >> 16) & 0xff] << 8) & 0xff000000)
+#define TE432(i) (Te0[((i) >> 8) & 0xff] & 0x00ff0000)
+#define TE443(i) (Te0[(i) & 0xff] & 0x0000ff00)
+#define TE414(i) ((Te0[((i) >> 24) & 0xff] >> 8) & 0x000000ff)
+#define TE4(i) ((Te0[(i)] >> 8) & 0x000000ff)
+
+#define TD0(i) Td0[((i) >> 24) & 0xff]
+#define TD1(i) rotr(Td0[((i) >> 16) & 0xff], 8)
+#define TD2(i) rotr(Td0[((i) >> 8) & 0xff], 16)
+#define TD3(i) rotr(Td0[(i) & 0xff], 24)
+#define TD41(i) (Td4s[((i) >> 24) & 0xff] << 24)
+#define TD42(i) (Td4s[((i) >> 16) & 0xff] << 16)
+#define TD43(i) (Td4s[((i) >> 8) & 0xff] << 8)
+#define TD44(i) (Td4s[(i) & 0xff])
+#define TD0_(i) Td0[(i) & 0xff]
+#define TD1_(i) rotr(Td0[(i) & 0xff], 8)
+#define TD2_(i) rotr(Td0[(i) & 0xff], 16)
+#define TD3_(i) rotr(Td0[(i) & 0xff], 24)
+
+#define GETU32(pt) (((u32)(pt)[0] << 24) ^ ((u32)(pt)[1] << 16) ^ \
+			((u32)(pt)[2] <<  8) ^ ((u32)(pt)[3]))
+
+#define PUTU32(ct, st) { \
+(ct)[0] = (u8)((st) >> 24); (ct)[1] = (u8)((st) >> 16); \
+(ct)[2] = (u8)((st) >>  8); (ct)[3] = (u8)(st); }
+
+#define WPA_GET_BE32(a) ((((u32) (a)[0]) << 24) | (((u32) (a)[1]) << 16) | \
+			 (((u32) (a)[2]) << 8) | ((u32) (a)[3]))
+
+#define WPA_PUT_LE16(a, val)			\
+	do {					\
+		(a)[1] = ((u16) (val)) >> 8;	\
+		(a)[0] = ((u16) (val)) & 0xff;	\
+	} while (0)
+
+#define WPA_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)
+
+#define WPA_PUT_BE64(a, val)				\
+	do {						\
+		(a)[0] = (u8) (((u64) (val)) >> 56);	\
+		(a)[1] = (u8) (((u64) (val)) >> 48);	\
+		(a)[2] = (u8) (((u64) (val)) >> 40);	\
+		(a)[3] = (u8) (((u64) (val)) >> 32);	\
+		(a)[4] = (u8) (((u64) (val)) >> 24);	\
+		(a)[5] = (u8) (((u64) (val)) >> 16);	\
+		(a)[6] = (u8) (((u64) (val)) >> 8);	\
+		(a)[7] = (u8) (((u64) (val)) & 0xff);	\
+	} while (0)
+
+/* ===== start - public domain SHA256 implementation ===== */
+
+/* This is based on SHA256 implementation in LibTomCrypt that was released into
+ * public domain by Tom St Denis. */
+
+/* the K array */
+static const unsigned long K[64] = {
+	0x428a2f98UL, 0x71374491UL, 0xb5c0fbcfUL, 0xe9b5dba5UL, 0x3956c25bUL,
+	0x59f111f1UL, 0x923f82a4UL, 0xab1c5ed5UL, 0xd807aa98UL, 0x12835b01UL,
+	0x243185beUL, 0x550c7dc3UL, 0x72be5d74UL, 0x80deb1feUL, 0x9bdc06a7UL,
+	0xc19bf174UL, 0xe49b69c1UL, 0xefbe4786UL, 0x0fc19dc6UL, 0x240ca1ccUL,
+	0x2de92c6fUL, 0x4a7484aaUL, 0x5cb0a9dcUL, 0x76f988daUL, 0x983e5152UL,
+	0xa831c66dUL, 0xb00327c8UL, 0xbf597fc7UL, 0xc6e00bf3UL, 0xd5a79147UL,
+	0x06ca6351UL, 0x14292967UL, 0x27b70a85UL, 0x2e1b2138UL, 0x4d2c6dfcUL,
+	0x53380d13UL, 0x650a7354UL, 0x766a0abbUL, 0x81c2c92eUL, 0x92722c85UL,
+	0xa2bfe8a1UL, 0xa81a664bUL, 0xc24b8b70UL, 0xc76c51a3UL, 0xd192e819UL,
+	0xd6990624UL, 0xf40e3585UL, 0x106aa070UL, 0x19a4c116UL, 0x1e376c08UL,
+	0x2748774cUL, 0x34b0bcb5UL, 0x391c0cb3UL, 0x4ed8aa4aUL, 0x5b9cca4fUL,
+	0x682e6ff3UL, 0x748f82eeUL, 0x78a5636fUL, 0x84c87814UL, 0x8cc70208UL,
+	0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL
+};
+
+
+/* Various logical functions */
+#define RORc(x, y) \
+(((((unsigned long) (x) & 0xFFFFFFFFUL) >> (unsigned long) ((y) & 31)) | \
+   ((unsigned long) (x) << (unsigned long) (32 - ((y) & 31)))) & 0xFFFFFFFFUL)
+#define Ch(x, y, z)       (z ^ (x & (y ^ z)))
+#define Maj(x, y, z)      (((x | y) & z) | (x & y))
+#define S(x, n)         RORc((x), (n))
+#define R(x, n)         (((x)&0xFFFFFFFFUL)>>(n))
+#define Sigma0(x)       (S(x, 2) ^ S(x, 13) ^ S(x, 22))
+#define Sigma1(x)       (S(x, 6) ^ S(x, 11) ^ S(x, 25))
+#define Gamma0(x)       (S(x, 7) ^ S(x, 18) ^ R(x, 3))
+#define Gamma1(x)       (S(x, 17) ^ S(x, 19) ^ R(x, 10))
+#ifndef MIN
+#define MIN(x, y) (((x) < (y)) ? (x) : (y))
+#endif
+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 000000000000..628d987c3e86
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_version.h
@@ -0,0 +1,2 @@
+#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 000000000000..d97ca1630bd4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_wifi_regd.h
@@ -0,0 +1,28 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ *****************************************************************************/
+
+#ifndef __RTW_WIFI_REGD_H__
+#define __RTW_WIFI_REGD_H__
+
+struct country_code_to_enum_rd {
+	u16 countrycode;
+	const char *iso_name;
+};
+
+enum country_code_type_t {
+	COUNTRY_CODE_USER = 0,
+
+	/*add new channel plan above this line */
+	COUNTRY_CODE_MAX
+};
+
+int rtw_regd_init(struct adapter *padapter,
+	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 000000000000..11571649cd2c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/rtw_xmit.h
@@ -0,0 +1,528 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _RTW_XMIT_H_
+#define _RTW_XMIT_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
+
+enum TXDESC_SC{
+	SC_DONT_CARE = 0x00,
+	SC_UPPER = 0x01,
+	SC_LOWER = 0x02,
+	SC_DUPLICATE = 0x03
+};
+
+#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	{
+	/* _lock xmit_lock; */
+	/* struct list_head	pending; */
+	struct __queue *sta_queue;
+	/* struct hw_txqueue *phwtxqueue; */
+	/* sint	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 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];
+	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, const char *msg);
+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;
+
+	_pkt *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
+{
+	_lock	lock;
+	sint	option;
+	sint	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 sint	head;
+	volatile sint	tail;
+	volatile sint	free_sz;	/* in units of 64 bytes */
+	volatile sint      free_cmdsz;
+	volatile sint	 txsz[8];
+	uint	ff_hwaddr;
+	uint	cmd_hwaddr;
+	sint	ac_tag;
+};
+
+struct agg_pkt_info{
+	u16 offset;
+	u16 pkt_len;
+};
+
+enum cmdbuf_type {
+	CMDBUF_BEACON = 0x00,
+	CMDBUF_RSVD,
+	CMDBUF_MAX
+};
+
+struct	xmit_priv {
+
+	_lock	lock;
+
+	_sema	xmit_sema;
+	_sema	terminate_xmitthread_sema;
+
+	/* 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. */
+
+#ifdef CONFIG_SDIO_TX_TASKLET
+	struct tasklet_struct xmit_tasklet;
+#else
+	void *SdioXmitThread;
+	_sema		SdioXmitSema;
+	_sema		SdioXmitTerminateSema;
+#endif /* CONFIG_SDIO_TX_TASKLET */
+
+	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;
+	_mutex ack_tx_mutex;
+	struct submit_ctx ack_tx_ops;
+	u8 seq_no;
+	_lock 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, sint 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, _pkt *pkt, struct xmit_frame *pxmitframe);
+extern s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, _pkt *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, sint entry);
+
+
+s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter);
+void _rtw_free_xmit_priv (struct xmit_priv *pxmitpriv);
+
+
+void rtw_alloc_hwxmits(struct adapter *padapter);
+void rtw_free_hwxmits(struct adapter *padapter);
+
+
+s32 rtw_xmit(struct adapter *padapter, _pkt **pkt);
+bool xmitframe_hiq_filter(struct xmit_frame *xmitframe);
+
+sint 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);
+sint	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 000000000000..8fd8bbeda2d8
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_hal.h
@@ -0,0 +1,28 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 000000000000..8fffc652cf0c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_ops.h
@@ -0,0 +1,49 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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);
+
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+extern u8 RecvOnePkt(struct adapter *padapter, u32 size);
+#endif /*  CONFIG_WOWLAN */
+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 u8 HalQueryTxOQTBufferStatus8723BSdio(struct adapter *padapter);
+#if defined(CONFIG_WOWLAN) || defined(CONFIG_AP_WOWLAN)
+extern void ClearInterrupt8723BSdio(struct adapter *padapter);
+#endif /* CONFIG_WOWLAN */
+
+#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 000000000000..bd62caec409f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_ops_linux.h
@@ -0,0 +1,40 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __SDIO_OPS_LINUX_H__
+#define __SDIO_OPS_LINUX_H__
+
+#define SDIO_ERR_VAL8	0xEA
+#define SDIO_ERR_VAL16	0xEAEA
+#define SDIO_ERR_VAL32	0xEAEAEAEA
+
+u8 sd_f0_read8(struct intf_hdl *pintfhdl, u32 addr, s32 *err);
+
+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 000000000000..86673682fb4f
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sdio_osintf.h
@@ -0,0 +1,24 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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 000000000000..84fa116fc5da
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/sta_info.h
@@ -0,0 +1,392 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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;
+};
+
+typedef struct _RSSI_STA{
+	s32	UndecoratedSmoothedPWDB;
+	s32	UndecoratedSmoothedCCK;
+	s32	UndecoratedSmoothedOFDM;
+	u64	PacketMap;
+	u8 ValidBit;
+}RSSI_STA, *PRSSI_STA;
+
+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 {
+
+	_lock	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 */
+#ifdef CONFIG_GTK_OL
+	u8 kek[RTW_KEK_LEN];
+	u8 kck[RTW_KCK_LEN];
+	u8 replay_ctr[RTW_REPLAY_CTR_LEN];
+#endif /* CONFIG_GTK_OL */
+	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 */
+	_timer 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;
+
+#ifdef CONFIG_AUTO_AP_MODE
+	u8 isrc; /* this device is rc */
+	u16 pid; /*  pairing id */
+#endif
+
+	u8 *passoc_req;
+	u32 assoc_req_len;
+
+	/* for DM */
+	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;
+
+	_lock 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;
+	_lock asoc_list_lock;
+	_lock 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, staion 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;
+};
+
+
+__inline static 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 000000000000..530d698f50d9
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/wifi.h
@@ -0,0 +1,1158 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 _WIFI_H_
+#define _WIFI_H_
+
+
+#ifdef BIT
+/* error	"BIT define occurred earlier elsewhere!\n" */
+#undef BIT
+#endif
+#define BIT(x)	(1 << (x))
+
+
+#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_FRAME_TYPE {
+	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 WIFI_FRAME_SUBTYPE {
+
+    /*  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),
+};
+
+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,
+};
+
+/* Reason codes (IEEE 802.11-2007, 7.3.1.7, Table 7-22) */
+/* IEEE 802.11h */
+#define WLAN_REASON_PWR_CAPABILITY_NOT_VALID 10
+#define WLAN_REASON_SUPPORTED_CHANNEL_NOT_VALID 11
+
+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,
+};
+
+/* Status codes (IEEE 802.11-2007, 7.3.1.9, Table 7-23) */
+/* 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_SPAIN	= 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 ClearToDs(pbuf)	\
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(_TO_DS_))
+
+#define SetFrDs(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_FROM_DS_)
+
+#define GetFrDs(pbuf)	(((*(__le16 *)(pbuf)) & cpu_to_le16(_FROM_DS_)) != 0)
+
+#define ClearFrDs(pbuf)	\
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(_FROM_DS_))
+
+#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 SetRetry(pbuf)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16(_RETRY_)
+
+#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 ClearPrivacy(pbuf)	\
+	*(__le16 *)(pbuf) &= (~cpu_to_le16(_PRIVACY_))
+
+
+#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) &= __constant_cpu_to_le16(~(BIT(3) | BIT(2))); \
+		*(unsigned short *)(pbuf) |= __constant_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 SetAMsdu(pbuf, amsdu)	\
+	*(__le16 *)(pbuf) |= cpu_to_le16((amsdu & 1) << 7)
+
+#define GetAid(pbuf)	(le16_to_cpu(*(__le16 *)((size_t)(pbuf) + 2)) & 0x3fff)
+
+#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))
+
+#define MacAddr_isBcst(addr) \
+	(\
+	((addr[0] == 0xff) && (addr[1] == 0xff) && \
+	(addr[2] == 0xff) && (addr[3] == 0xff) && \
+	(addr[4] == 0xff) && (addr[5] == 0xff))  ? true : false \
+)
+
+__inline static int IS_MCAST(unsigned char *da)
+{
+	if ((*da) & 0x01)
+		return true;
+	else
+		return false;
+}
+
+__inline static unsigned char * get_ra(unsigned char *pframe)
+{
+	unsigned char *ra;
+	ra = GetAddr1Ptr(pframe);
+	return ra;
+}
+__inline static unsigned char * get_ta(unsigned char *pframe)
+{
+	unsigned char *ta;
+	ta = GetAddr2Ptr(pframe);
+	return ta;
+}
+
+__inline static 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;
+}
+
+
+__inline static 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;
+}
+
+__inline static 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;
+}
+
+
+__inline static 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 _SIFSTIME_				((priv->pmib->dot11BssType.net_work_type&WIRELESS_11A)?16: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 _RIC_Descriptor_IE_			75
+#define _MME_IE_					76 /* 802.11w Management MIC element */
+#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
+
+enum ELEMENT_ID {
+	EID_SsId					= 0, /* service set identifier (0:32) */
+	EID_SupRates				= 1, /* supported rates (1:8) */
+	EID_FHParms				= 2, /* FH parameter set (5) */
+	EID_DSParms				= 3, /* DS parameter set (1) */
+	EID_CFParms				= 4, /* CF parameter set (6) */
+	EID_Tim						= 5, /* Traffic Information Map (4:254) */
+	EID_IbssParms				= 6, /* IBSS parameter set (2) */
+	EID_Country					= 7, /* */
+
+	/*  Form 7.3.2: Information elements in 802.11E/D13.0, page 46. */
+	EID_QBSSLoad				= 11,
+	EID_EDCAParms				= 12,
+	EID_TSpec					= 13,
+	EID_TClass					= 14,
+	EID_Schedule				= 15,
+	/*  */
+
+	EID_Ctext					= 16, /* challenge text*/
+	EID_POWER_CONSTRAINT		= 32, /* Power Constraint*/
+
+	/* vivi for WIFITest, 802.11h AP, 20100427 */
+	/*  2010/12/26 MH The definition we can declare always!! */
+	EID_PowerCap				= 33,
+	EID_SupportedChannels		= 36,
+	EID_ChlSwitchAnnounce		= 37,
+
+	EID_MeasureRequest			= 38, /*  Measurement Request */
+	EID_MeasureReport			= 39, /*  Measurement Report */
+
+	EID_ERPInfo				= 42,
+
+	/*  Form 7.3.2: Information elements in 802.11E/D13.0, page 46. */
+	EID_TSDelay				= 43,
+	EID_TCLASProc				= 44,
+	EID_HTCapability			= 45,
+	EID_QoSCap					= 46,
+	/*  */
+
+	EID_WPA2					= 48,
+	EID_ExtSupRates			= 50,
+
+	EID_FTIE					= 55, /*  Defined in 802.11r */
+	EID_Timeout				= 56, /*  Defined in 802.11r */
+
+	EID_SupRegulatory			= 59, /*  Supported Requlatory Classes 802.11y */
+	EID_HTInfo					= 61,
+	EID_SecondaryChnlOffset		= 62,
+
+	EID_BSSCoexistence			= 72, /*  20/40 BSS Coexistence */
+	EID_BSSIntolerantChlReport	= 73,
+	EID_OBSS					= 74, /*  Overlapping BSS Scan Parameters */
+
+	EID_LinkIdentifier			= 101, /*  Defined in 802.11z */
+	EID_WakeupSchedule		= 102, /*  Defined in 802.11z */
+	EID_ChnlSwitchTimeing		= 104, /*  Defined in 802.11z */
+	EID_PTIControl				= 105, /*  Defined in 802.11z */
+	EID_PUBufferStatus			= 106, /*  Defined in 802.11z */
+
+	EID_EXTCapability			= 127, /*  Extended Capabilities */
+	/*  From S19:Aironet IE and S21:AP IP address IE in CCX v1.13, p16 and p18. */
+	EID_Aironet					= 133, /*  0x85: Aironet Element for Cisco CCX */
+	EID_CiscoIP					= 149, /*  0x95: IP Address IE for Cisco CCX */
+
+	EID_CellPwr					= 150, /*  0x96: Cell Power Limit IE. Ref. 0x96. */
+
+	EID_CCKM					= 156,
+
+	EID_Vendor					= 221, /*  0xDD: Vendor Specific */
+
+	EID_WAPI					= 68,
+	EID_VHTCapability			= 191, /*  Based on 802.11ac D2.0 */
+	EID_VHTOperation			= 192, /*  Based on 802.11ac D2.0 */
+	EID_OpModeNotification		= 199, /*  Based on 802.11ac D3.0 */
+};
+
+/* ---------------------------------------------------------------------------
+					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
+
+#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_QoS	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 */
+
+#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_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[6];
+	unsigned char ta[6];
+	__le16 control;
+	__le16 start_seq_num;
+} __attribute__((packed));
+
+/* 802.11 BAR control masks */
+#define IEEE80211_BAR_CTRL_ACK_POLICY_NORMAL     0x0000
+#define IEEE80211_BAR_CTRL_CBMTID_COMPRESSED_BA  0x0004
+
+
+ /**
+ * struct rtw_ieee80211_ht_cap - HT capabilities
+ *
+ * This structure refers to "HT capabilities element" as
+ * described in 802.11n draft section 7.3.2.52
+ */
+
+struct rtw_ieee80211_ht_cap {
+	__le16	cap_info;
+	unsigned char ampdu_params_info;
+	unsigned char supp_mcs_set[16];
+	__le16	extended_ht_cap_info;
+	__le16		tx_BF_cap_info;
+	unsigned char        antenna_selection_info;
+} __attribute__ ((packed));
+
+/**
+ * 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));
+
+enum HT_CAP_AMPDU_FACTOR {
+	MAX_AMPDU_FACTOR_8K		= 0,
+	MAX_AMPDU_FACTOR_16K	= 1,
+	MAX_AMPDU_FACTOR_32K	= 2,
+	MAX_AMPDU_FACTOR_64K	= 3,
+};
+
+/* 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
+
+/* 802.11n HT IE masks */
+#define IEEE80211_HT_IE_CHA_SEC_OFFSET		0x03
+#define IEEE80211_HT_IE_CHA_SEC_NONE		0x00
+#define IEEE80211_HT_IE_CHA_SEC_ABOVE		0x01
+#define IEEE80211_HT_IE_CHA_SEC_BELOW		0x03
+#define IEEE80211_HT_IE_CHA_WIDTH		0x04
+#define IEEE80211_HT_IE_HT_PROTECTION		0x0003
+#define IEEE80211_HT_IE_NON_GF_STA_PRSNT	0x0004
+#define IEEE80211_HT_IE_NON_HT_STA_PRSNT	0x0010
+
+/* block-ack parameters */
+#define IEEE80211_ADDBA_PARAM_POLICY_MASK 0x0002
+#define IEEE80211_ADDBA_PARAM_TID_MASK 0x003C
+#define RTW_IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK 0xFFC0
+#define IEEE80211_DELBA_PARAM_TID_MASK 0xF000
+#define IEEE80211_DELBA_PARAM_INITIATOR_MASK 0x0800
+
+/*
+ * A-PMDU buffer sizes
+ * According to IEEE802.11n spec size varies from 8K to 64K (in powers of 2)
+ */
+#define IEEE80211_MIN_AMPDU_BUF 0x8
+#define IEEE80211_MAX_AMPDU_BUF 0x40
+
+
+/* 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))
+
+#define HT_INFO_STBC_PARAM_DUAL_BEACON			((u16) BIT(6))
+#define HT_INFO_STBC_PARAM_DUAL_STBC_PROTECT		((u16) BIT(7))
+#define HT_INFO_STBC_PARAM_SECONDARY_BCN		((u16) BIT(8))
+#define HT_INFO_STBC_PARAM_LSIG_TXOP_PROTECT_ALLOWED	((u16) BIT(9))
+#define HT_INFO_STBC_PARAM_PCO_ACTIVE			((u16) BIT(10))
+#define HT_INFO_STBC_PARAM_PCO_PHASE			((u16) BIT(11))
+
+
+
+/* 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 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
+
+#define	P2P_FINDPHASE_EX_NONE				0	/*  default value, used when: (1)p2p disabed 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 negotation 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 negoitation handshake */
+	P2P_STATE_GONEGO_OK = 10,						/* 	finish the group negoitation handshake with success */
+	P2P_STATE_GONEGO_FAIL = 11,					/* 	finish the group negoitation handshake with failure */
+	P2P_STATE_RECV_INVITE_REQ_MATCH = 12,		/* 	receiving the P2P Inviation 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 Inviation request and dismatch with the profile. */
+	P2P_STATE_RECV_INVITE_REQ_GO = 18,			/* 	receiving the P2P Inviation request and this wifi is GO. */
+	P2P_STATE_RECV_INVITE_REQ_JOIN = 19,			/* 	receiving the P2P Inviation request to join an existing P2P Group. */
+	P2P_STATE_RX_INVITE_RESP_FAIL = 20,			/* 	recveing the P2P Inviation response with failure */
+	P2P_STATE_RX_INFOR_NOREADY = 21,			/*  receiving p2p negoitation response with information is not available */
+	P2P_STATE_TX_INFOR_NOREADY = 22,			/*  sending p2p negoitation 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
+
+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,
+};
+
+/* 	=====================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 000000000000..af78d97980fa
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/wlan_bssdef.h
@@ -0,0 +1,278 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __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  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_conf_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_conf {
+	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_conf_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_fix_ie {
+	u8  Timestamp[8];
+	u16  BeaconInterval;
+	u16  Capabilities;
+};
+
+struct ndis_80211_var_ie {
+	u8  ElementID;
+	u8  Length;
+	u8  data[1];
+};
+
+/* 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) + IELength
+ *
+ * Except for IELength, 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 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_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
+
+struct ndis_802_11_ai_reqfi {
+	u16 Capabilities;
+	u16 ListenInterval;
+	NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+};
+
+struct ndis_801_11_ai_resfi {
+	u16 Capabilities;
+	u16 StatusCode;
+	u16 AssociationId;
+};
+
+typedef 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_801_11_ai_resfi    ResponseFixedIEs;
+    u32                   ResponseIELength;
+    u32                   OffsetResponseIEs;
+} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
+
+enum NDIS_802_11_RELOAD_DEFAULTS {
+	Ndis802_11ReloadWEPKeys
+};
+
+
+/*  Key mapping keys require a BSSID */
+typedef struct _NDIS_802_11_KEY
+{
+    u32           Length;             /*  Length of this structure */
+    u32           KeyIndex;
+    u32           KeyLength;          /*  length of key in bytes */
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    unsigned long long KeyRSC;
+    u8           KeyMaterial[32];     /*  variable length depending on above field */
+} NDIS_802_11_KEY, *PNDIS_802_11_KEY;
+
+typedef struct _NDIS_802_11_REMOVE_KEY
+{
+    u32                   Length;        /*  Length of this structure */
+    u32                   KeyIndex;
+    NDIS_802_11_MAC_ADDRESS BSSID;
+} NDIS_802_11_REMOVE_KEY, *PNDIS_802_11_REMOVE_KEY;
+
+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_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_wlan_bssid_ex_sz()
+*/
+struct wlan_bssid_ex {
+	u32  Length;
+	NDIS_802_11_MAC_ADDRESS  MacAddress;
+	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  NetworkTypeInUse;
+	struct ndis_802_11_conf  Configuration;
+	enum NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+	NDIS_802_11_RATES_EX  SupportedRates;
+	struct wlan_phy_info PhyInfo;
+	u32  IELength;
+	u8  IEs[MAX_IE_SZ];	/* timestamp, beacon interval, and capability information) */
+} __packed;
+
+__inline  static 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_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	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
+
+#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 000000000000..46909ff7339c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/include/xmit_osdep.h
@@ -0,0 +1,54 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __XMIT_OSDEP_H_
+#define __XMIT_OSDEP_H_
+
+
+struct pkt_file {
+	_pkt *pkt;
+	__kernel_size_t pkt_len;	 /* the remainder length of the open_file */
+	_buffer *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 int _rtw_xmit_entry(_pkt *pkt, _nic_hdl pnetdev);
+extern int rtw_xmit_entry(_pkt *pkt, _nic_hdl 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 void rtw_set_tx_chksum_offload(_pkt *pkt, struct pkt_attrib *pattrib);
+
+extern uint rtw_remainder_len(struct pkt_file *pfile);
+extern void _rtw_open_pktfile(_pkt *pkt, struct pkt_file *pfile);
+extern uint _rtw_pktfile_read (struct pkt_file *pfile, u8 *rmem, uint rlen);
+extern sint rtw_endofpktfile (struct pkt_file *pfile);
+
+extern void rtw_os_pkt_complete(struct adapter *padapter, _pkt *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 000000000000..5e7a61f24f8d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -0,0 +1,3586 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define  _IOCTL_CFG80211_C_
+
+#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
+
+#define RTW_CH_MAX_2G_CHANNEL               14      /* Max channel in 2G band */
+
+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 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_a_rates		(rtw_rates + 4)
+#define RTW_A_RATES_NUM	8
+#define rtw_g_rates		(rtw_rates + 0)
+#define RTW_G_RATES_NUM	12
+
+#define RTW_2G_CHANNELS_NUM 14
+#define RTW_5G_CHANNELS_NUM 37
+
+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 = (struct ieee80211_supported_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 void rtw_spt_band_free(struct ieee80211_supported_band *spt_band)
+{
+	u32 size = 0;
+
+	if (!spt_band)
+		return;
+
+	if (spt_band->band == NL80211_BAND_2GHZ)
+	{
+		size = sizeof(struct ieee80211_supported_band)
+			+ sizeof(struct ieee80211_channel)*RTW_2G_CHANNELS_NUM
+			+ sizeof(struct ieee80211_rate)*RTW_G_RATES_NUM;
+	}
+	kfree((u8 *)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)
+{
+	/* see 802.11 17.3.8.3.2 and Annex J
+	* there are overlapping channel numbers in 5GHz and 2GHz bands */
+	if (band == NL80211_BAND_2GHZ) {
+		if (chan == 14)
+			return 2484;
+             else if (chan < 14)
+			return 2407 + chan * 5;
+	}
+
+	return 0; /* not supported */
+}
+
+static u64 rtw_get_systime_us(void)
+{
+	struct timespec ts;
+	get_monotonic_boottime(&ts);
+	return ((u64)ts.tv_sec*1000000) + ts.tv_nsec / 1000;
+}
+
+#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;
+	u16 notify_capability;
+	u16 notify_interval;
+	u8 *notify_ie;
+	size_t notify_ielen;
+	s32 notify_signal;
+	u8 *buf = NULL, *pbuf;
+	size_t len, bssinf_len = 0;
+	struct ieee80211_hdr *pwlanhdr;
+	__le16 *fctrl;
+	u8 bc_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	struct wireless_dev *wdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = wdev->wiphy;
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+
+
+	/* DBG_8192C("%s\n", __func__); */
+
+	bssinf_len = pnetwork->network.IELength+sizeof (struct ieee80211_hdr_3addr);
+	if (bssinf_len > MAX_BSSINFO_LEN) {
+		DBG_871X("%s IE Length too long > %d byte\n", __func__, MAX_BSSINFO_LEN);
+		goto exit;
+	}
+
+	{
+		u16 wapi_len = 0;
+
+		if (rtw_get_wapi_ie(pnetwork->network.IEs, pnetwork->network.IELength, NULL, &wapi_len)>0)
+		{
+			if (wapi_len > 0)
+			{
+				DBG_871X("%s, no support wapi!\n", __func__);
+				goto exit;
+			}
+		}
+	}
+
+	/* To reduce PBC Overlap rate */
+	/* spin_lock_bh(&pwdev_priv->scan_req_lock); */
+	if (adapter_wdev_data(padapter)->scan_request != NULL)
+	{
+		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.IELength-_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)
+		{
+			if (request->n_ssids == 1 && request->n_channels == 1) /*  it means under processing WPS */
+			{
+				DBG_8192C("ssid =%s, len =%d\n", pssid->Ssid, pssid->SsidLength);
+
+				if (ssids[0].ssid_len == 0) {
+				}
+				else if (pssid->SsidLength == ssids[0].ssid_len &&
+					!memcmp(pssid->Ssid, ssids[0].ssid, ssids[0].ssid_len))
+				{
+					DBG_871X("%s, got sr and ssid match!\n", __func__);
+				}
+				else
+				{
+					if (psr != NULL)
+						*psr = 0; /* clear sr */
+				}
+			}
+		}
+	}
+	/* spin_unlock_bh(&pwdev_priv->scan_req_lock); */
+
+
+	channel = pnetwork->network.Configuration.DSConfig;
+	freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+	notify_channel = ieee80211_get_channel(wiphy, freq);
+
+	notify_timestamp = rtw_get_systime_us();
+
+	notify_interval = le16_to_cpu(*(__le16 *)rtw_get_beacon_interval_from_ie(pnetwork->network.IEs));
+	notify_capability = le16_to_cpu(*(__le16 *)rtw_get_capability_from_ie(pnetwork->network.IEs));
+
+	notify_ie = pnetwork->network.IEs+_FIXED_IE_LENGTH_;
+	notify_ielen = pnetwork->network.IELength-_FIXED_IE_LENGTH_;
+
+	/* 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.PhyInfo.SignalStrength);/* 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 */
+		memcpy(pwlanhdr->addr1, bc_addr, ETH_ALEN);
+		SetFrameSubType(pbuf, WIFI_BEACON);
+	} else {
+		memcpy(pwlanhdr->addr1, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		SetFrameSubType(pbuf, WIFI_PROBERSP);
+	}
+
+	memcpy(pwlanhdr->addr2, pnetwork->network.MacAddress, ETH_ALEN);
+	memcpy(pwlanhdr->addr3, pnetwork->network.MacAddress, ETH_ALEN);
+
+
+	pbuf += sizeof(struct ieee80211_hdr_3addr);
+	len = sizeof (struct ieee80211_hdr_3addr);
+
+	memcpy(pbuf, pnetwork->network.IEs, pnetwork->network.IELength);
+	len += pnetwork->network.IELength;
+
+	*((__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)) {
+		DBG_8192C(FUNC_ADPT_FMT" bss NULL\n", FUNC_ADPT_ARG(padapter));
+		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.DSConfig, NL80211_BAND_2GHZ);
+
+	notify_channel = ieee80211_get_channel(padapter->rtw_wdev->wiphy, freq);
+	bss = cfg80211_get_bss(padapter->rtw_wdev->wiphy, notify_channel,
+			pnetwork->MacAddress, pnetwork->Ssid.Ssid,
+			pnetwork->Ssid.SsidLength,
+			WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
+
+	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.DSConfig;
+	struct ieee80211_channel *chan;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	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));
+			if (!rtw_cfg80211_inform_bss(padapter, cur_network))
+				DBG_871X(FUNC_ADPT_FMT" inform fail !!\n", FUNC_ADPT_ARG(padapter));
+			else
+				DBG_871X(FUNC_ADPT_FMT" inform success !!\n", FUNC_ADPT_ARG(padapter));
+		}
+		else
+		{
+			if (scanned == NULL) {
+				rtw_warn_on(1);
+				return;
+			}
+			if (!memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(struct ndis_802_11_ssid))
+				&& !memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS))
+			) {
+				if (!rtw_cfg80211_inform_bss(padapter, scanned)) {
+					DBG_871X(FUNC_ADPT_FMT" inform fail !!\n", FUNC_ADPT_ARG(padapter));
+				} else {
+					/* DBG_871X(FUNC_ADPT_FMT" inform success !!\n", FUNC_ADPT_ARG(padapter)); */
+				}
+			} else {
+				DBG_871X("scanned & pnetwork compare fail\n");
+				rtw_warn_on(1);
+			}
+		}
+
+		if (!rtw_cfg80211_check_bss(padapter))
+			DBG_871X_LEVEL(_drv_always_, 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.MacAddress, 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;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+	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;
+
+		/* DBG_871X(FUNC_ADPT_FMT" BSS not found\n", FUNC_ADPT_ARG(padapter)); */
+
+		if (scanned == NULL) {
+			rtw_warn_on(1);
+			goto check_bss;
+		}
+
+		if (!memcmp(scanned->network.MacAddress, pnetwork->MacAddress, sizeof(NDIS_802_11_MAC_ADDRESS))
+			&& !memcmp(&(scanned->network.Ssid), &(pnetwork->Ssid), sizeof(struct ndis_802_11_ssid))
+		) {
+			if (!rtw_cfg80211_inform_bss(padapter, scanned)) {
+				DBG_871X(FUNC_ADPT_FMT" inform fail !!\n", FUNC_ADPT_ARG(padapter));
+			} else {
+				/* DBG_871X(FUNC_ADPT_FMT" inform success !!\n", FUNC_ADPT_ARG(padapter)); */
+			}
+		} else {
+			DBG_871X("scanned: %s("MAC_FMT"), cur: %s("MAC_FMT")\n",
+				scanned->network.Ssid.Ssid, MAC_ARG(scanned->network.MacAddress),
+				pnetwork->Ssid.Ssid, MAC_ARG(pnetwork->MacAddress)
+			);
+			rtw_warn_on(1);
+		}
+	}
+
+check_bss:
+	if (!rtw_cfg80211_check_bss(padapter))
+		DBG_871X_LEVEL(_drv_always_, 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.DSConfig;
+		struct cfg80211_roam_info roam_info = {};
+
+		freq = rtw_ieee80211_channel_to_frequency(channel, NL80211_BAND_2GHZ);
+
+		notify_channel = ieee80211_get_channel(wiphy, freq);
+
+		DBG_871X(FUNC_ADPT_FMT" call cfg80211_roamed\n", FUNC_ADPT_ARG(padapter));
+		roam_info.channel = notify_channel;
+		roam_info.bssid = cur_network->network.MacAddress;
+		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.MacAddress
+			, 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;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv* psecuritypriv =&(padapter->securitypriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_8192C("%s\n", __func__);
+
+	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; */
+			DBG_8192C("rtw_set_encryption(), sta has already been removed or never been added\n");
+			goto exit;
+		}
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0 && (psta == NULL))
+	{
+		/* todo:clear default encryption keys */
+
+		DBG_8192C("clear default encryption keys, keyid =%d\n", param->u.crypt.idx);
+
+		goto exit;
+	}
+
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0 && (psta == NULL))
+	{
+		DBG_8192C("r871x_set_encryption, crypt.alg = WEP\n");
+
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		DBG_8192C("r871x_set_encryption, wep_key_idx =%d, len =%d\n", wep_key_idx, wep_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;
+
+	}
+
+
+	if (!psta && check_fwstate(pmlmepriv, WIFI_AP_STATE)) /* group key */ 
+	{
+		if (param->u.crypt.set_tx == 0) /* group key */
+		{
+			if (strcmp(param->u.crypt.alg, "WEP") == 0)
+			{
+				DBG_8192C("%s, set group_key, WEP\n", __func__);
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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)
+			{
+				DBG_8192C("%s, set group_key, TKIP\n", __func__);
+
+				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrptxmickey[param->u.crypt.idx].skey, &(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)
+			{
+				DBG_8192C("%s, set group_key, CCMP\n", __func__);
+
+				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  param->u.crypt.key, (param->u.crypt.key_len>16 ?16:param->u.crypt.key_len));
+			}
+			else
+			{
+				DBG_8192C("%s, set group_key, none\n", __func__);
+
+				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)
+				{
+					DBG_8192C("%s, set pairwise key, WEP\n", __func__);
+
+					psta->dot118021XPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+					{
+						psta->dot118021XPrivacy = _WEP104_;
+					}
+				}
+				else if (strcmp(param->u.crypt.alg, "TKIP") == 0)
+				{
+					DBG_8192C("%s, set pairwise key, TKIP\n", __func__);
+
+					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)
+				{
+
+					DBG_8192C("%s, set pairwise key, CCMP\n", __func__);
+
+					psta->dot118021XPrivacy = _AES_;
+				}
+				else
+				{
+					DBG_8192C("%s, set pairwise key, none\n", __func__);
+
+					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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrptxmickey[param->u.crypt.idx].skey, &(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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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;
+	u32 wep_key_idx, wep_key_len;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	DBG_8192C("%s\n", __func__);
+
+	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
+			|| param->u.crypt.idx >= BIP_MAX_KEYID
+		)
+		{
+			ret = -EINVAL;
+			goto exit;
+		}
+	} else {
+		{
+		ret = -EINVAL;
+		goto exit;
+	}
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("wpa_set_encryption, crypt.alg = WEP\n"));
+		DBG_8192C("wpa_set_encryption, crypt.alg = WEP\n");
+
+		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 (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;
+
+		/* DBG_8192C("%s, : dot11AuthAlgrthm == dot11AuthAlgrthm_8021X\n", __func__); */
+
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE | WIFI_MP_STATE) == true) /* sta mode */
+		{
+			psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
+			if (psta == NULL) {
+				/* DEBUG_ERR(("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
+				DBG_8192C("%s, : Obtain Sta_info fail\n", __func__);
+			}
+			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 */
+				{
+
+					DBG_8192C("%s, : param->u.crypt.set_tx == 1\n", __func__);
+
+					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); */
+					}
+
+					/* DEBUG_ERR((" param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+					DBG_871X(" ~~~~set sta key:unicastkey\n");
+
+					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;
+						/* DEBUG_ERR((" param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+						DBG_871X(" ~~~~set sta key:groupkey\n");
+
+						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)
+					{
+						/* DBG_871X("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));
+						/*DBG_871X("IGTK key below:\n");
+						for (no = 0;no<16;no++)
+							printk(" %02x ", padapter->securitypriv.dot11wBIPKey[param->u.crypt.idx].skey[no]);
+						DBG_871X("\n");*/
+						padapter->securitypriv.dot11wBIPKeyid = param->u.crypt.idx;
+						padapter->securitypriv.binstallBIPkey = true;
+						DBG_871X(" ~~~~set sta key:IGKT\n");
+					}
+				}
+			}
+
+			pbcmc_sta =rtw_get_bcmc_stainfo(padapter);
+			if (pbcmc_sta == NULL)
+			{
+				/* 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:
+
+	DBG_8192C("%s, ret =%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
+				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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	DBG_871X(FUNC_NDEV_FMT" adding key for %pM\n", FUNC_NDEV_ARG(ndev), mac_addr);
+	DBG_871X("cipher = 0x%x\n", params->cipher);
+	DBG_871X("key_len = 0x%x\n", params->key_len);
+	DBG_871X("seq_len = 0x%x\n", params->seq_len);
+	DBG_871X("key_index =%d\n", key_index);
+	DBG_871X("pairwise =%d\n", pairwise);
+
+	param_len = sizeof(struct ieee_param) + params->key_len;
+	param = (struct ieee_param *)rtw_malloc(param_len);
+	if (param == NULL)
+		return -1;
+
+	memset(param, 0, param_len);
+
+	param->cmd = IEEE_CMD_SET_ENCRYPTION;
+	memset(param->sta_addr, 0xff, ETH_ALEN);
+
+	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)
+        {
+                /* DBG_8192C("@@@@@@@@@@ fw_state = 0x%x, iftype =%d\n", pmlmepriv->fw_state, rtw_wdev->iftype); */
+                ret =  rtw_cfg80211_set_encryption(ndev, param, param_len);
+        }
+	else
+	{
+		DBG_8192C("error!\n");
+
+	}
+
+addkey_end:
+	kfree((u8 *)param);
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_get_key(struct wiphy *wiphy, struct net_device *ndev,
+				u8 key_index, bool pairwise, const u8 *mac_addr,
+				void *cookie,
+				void (*callback)(void *cookie,
+						 struct key_params*))
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	return 0;
+}
+
+static int cfg80211_rtw_del_key(struct wiphy *wiphy, struct net_device *ndev,
+				u8 key_index, bool pairwise, const u8 *mac_addr)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	DBG_871X(FUNC_NDEV_FMT" key_index =%d\n", FUNC_NDEV_ARG(ndev), key_index);
+
+	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, u8 key_index
+	, bool unicast, bool multicast
+	)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	DBG_871X(FUNC_NDEV_FMT" key_index =%d, unicast =%d, multicast =%d\n",
+		 FUNC_NDEV_ARG(ndev), key_index, unicast, multicast);
+
+	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 = (struct adapter *)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) {
+		DBG_871X(FUNC_NDEV_FMT" mac ==%p\n", FUNC_NDEV_ARG(ndev), mac);
+		ret = -ENOENT;
+		goto exit;
+	}
+
+	psta = rtw_get_stainfo(pstapriv, (u8 *)mac);
+	if (psta == NULL) {
+		DBG_8192C("%s, sta_info is null\n", __func__);
+		ret = -ENOENT;
+		goto exit;
+	}
+
+#ifdef DEBUG_CFG80211
+	DBG_871X(FUNC_NDEV_FMT" mac ="MAC_FMT"\n", FUNC_NDEV_ARG(ndev), MAC_ARG(mac));
+#endif
+
+	/* 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.MacAddress, ETH_ALEN)) {
+			DBG_871X("%s, mismatch bssid ="MAC_FMT"\n", __func__, MAC_ARG(cur_network->network.MacAddress));
+			ret = -ENOENT;
+			goto exit;
+		}
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
+		sinfo->signal = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_TX_BITRATE);
+		sinfo->txrate.legacy = rtw_get_cur_max_rate(padapter);
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_RX_PACKETS);
+		sinfo->rx_packets = sta_rx_data_pkts(psta);
+
+		sinfo->filled |= BIT(NL80211_STA_INFO_TX_PACKETS);
+		sinfo->tx_packets = psta->sta_stats.tx_pkts;
+
+	}
+
+	/* 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;
+}
+
+extern int netdev_open(struct net_device *pnetdev);
+
+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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct wireless_dev *rtw_wdev = padapter->rtw_wdev;
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	int ret = 0;
+	u8 change = false;
+
+	DBG_871X(FUNC_NDEV_FMT" type =%d\n", FUNC_NDEV_ARG(ndev), type);
+
+	if (adapter_to_dvobj(padapter)->processing_dev_remove == true)
+	{
+		ret = -EPERM;
+		goto exit;
+	}
+
+	{
+		DBG_871X(FUNC_NDEV_FMT" call netdev_open\n", FUNC_NDEV_ARG(ndev));
+		if (netdev_open(ndev) != 0) {
+			DBG_871X(FUNC_NDEV_FMT" call netdev_open fail\n", FUNC_NDEV_ARG(ndev));
+			ret = -EPERM;
+			goto exit;
+		}
+	}
+
+	if (_FAIL == rtw_pwr_wakeup(padapter)) {
+		DBG_871X(FUNC_NDEV_FMT" call rtw_pwr_wakeup fail\n", FUNC_NDEV_ARG(ndev));
+		ret = -EPERM;
+		goto exit;
+	}
+
+	old_type = rtw_wdev->iftype;
+	DBG_871X(FUNC_NDEV_FMT" old_iftype =%d, new_iftype =%d\n",
+		FUNC_NDEV_ARG(ndev), old_type, type);
+
+	if (old_type != type)
+	{
+		change = true;
+		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:
+
+	DBG_871X(FUNC_NDEV_FMT" ret:%d\n", FUNC_NDEV_ARG(ndev), ret);
+	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 != NULL) {
+		#ifdef DEBUG_CFG80211
+		DBG_871X("%s with scan req\n", __func__);
+		#endif
+
+		/* avoid WARN_ON(request != wiphy_to_dev(request->wiphy)->scan_req); */
+		if (pwdev_priv->scan_request->wiphy != pwdev_priv->rtw_wdev->wiphy)
+		{
+			DBG_8192C("error wiphy compare\n");
+		}
+		else
+		{
+			cfg80211_scan_done(pwdev_priv->scan_request, &info);
+		}
+
+		pwdev_priv->scan_request = NULL;
+	} else {
+		#ifdef DEBUG_CFG80211
+		DBG_871X("%s without scan req\n", __func__);
+		#endif
+	}
+	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.MacAddress, select_network.Ssid.Ssid,
+		select_network.Ssid.SsidLength, 0/*WLAN_CAPABILITY_ESS*/,
+		0/*WLAN_CAPABILITY_ESS*/);
+
+	if (bss) {
+		cfg80211_unlink_bss(wiphy, bss);
+		DBG_8192C("%s(): cfg80211_unlink %s!! () ", __func__, select_network.Ssid.Ssid);
+		cfg80211_put_bss(padapter->rtw_wdev->wiphy, bss);
+	}
+	return;
+}
+
+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;
+
+#ifdef DEBUG_CFG80211
+	DBG_8192C("%s\n", __func__);
+#endif
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	while (1)
+	{
+		if (phead == plist)
+			break;
+
+		pnetwork = LIST_CONTAINOR(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
+			&& rtw_mlme_band_check(padapter, pnetwork->network.Configuration.DSConfig) == true
+			&& true == rtw_validate_ssid(&(pnetwork->network.Ssid))
+		)
+		{
+			/* ev =translate_scan(padapter, a, pnetwork, ev, stop); */
+			rtw_cfg80211_inform_bss(padapter, pnetwork);
+		}
+
+		plist = get_next(plist);
+
+	}
+
+	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);
+
+#ifdef DEBUG_CFG80211
+	DBG_8192C("%s, ielen =%d\n", __func__, len);
+#endif
+
+	if (len>0)
+	{
+		if ((wps_ie = rtw_get_wps_ie(buf, len, NULL, &wps_ielen)))
+		{
+			#ifdef DEBUG_CFG80211
+			DBG_8192C("probe_req_wps_ielen =%d\n", wps_ielen);
+			#endif
+
+			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 == NULL) {
+				DBG_8192C("%s()-%d: rtw_malloc() ERROR!\n", __func__, __LINE__);
+				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[RTW_SSID_SCAN_AMOUNT];
+	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 == NULL) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(padapter);
+	pmlmepriv = &padapter->mlmepriv;
+
+/* ifdef DEBUG_CFG80211 */
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+/* 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)
+	{
+#ifdef DEBUG_CFG80211
+		DBG_871X("%s under WIFI_AP_STATE\n", __func__);
+#endif
+
+		if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS|_FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true)
+		{
+			DBG_8192C("%s, fwstate = 0x%x\n", __func__, pmlmepriv->fw_state);
+
+			if (check_fwstate(pmlmepriv, WIFI_UNDER_WPS))
+			{
+				DBG_8192C("AP mode process WPS\n");
+			}
+
+			need_indicate_scan_done = true;
+			goto check_need_indicate_scan_done;
+		}
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_SCAN);
+	if (_FAIL == rtw_pwr_wakeup(padapter)) {
+		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) {
+		DBG_8192C("%s, fwstate = 0x%x\n", __func__, pmlmepriv->fw_state);
+		need_indicate_scan_done = true;
+		goto check_need_indicate_scan_done;
+	} else if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		DBG_8192C("%s, fwstate = 0x%x\n", __func__, pmlmepriv->fw_state);
+		ret = -EBUSY;
+		goto check_need_indicate_scan_done;
+	}
+
+	if (pmlmepriv->LinkDetectInfo.bBusyTraffic == true)
+	{
+		static unsigned long lastscantime = 0;
+		unsigned long passtime;
+
+		passtime = jiffies_to_msecs(jiffies - lastscantime);
+		lastscantime = jiffies;
+		if (passtime > 12000)
+		{
+			DBG_871X("%s: bBusyTraffic == true\n", __func__);
+			need_indicate_scan_done = true;
+			goto check_need_indicate_scan_done;
+		}
+	}
+
+	if (rtw_is_scan_deny(padapter)) {
+		DBG_871X(FUNC_ADPT_FMT  ": scan deny\n", FUNC_ADPT_ARG(padapter));
+		need_indicate_scan_done = true;
+		goto check_need_indicate_scan_done;
+	}
+
+	memset(ssid, 0, sizeof(struct ndis_802_11_ssid)*RTW_SSID_SCAN_AMOUNT);
+	/* parsing request ssids, n_ssids */
+	for (i = 0; i < request->n_ssids && i < RTW_SSID_SCAN_AMOUNT; i++) {
+		#ifdef DEBUG_CFG80211
+		DBG_8192C("ssid =%s, len =%d\n", ssids[i].ssid, ssids[i].ssid_len);
+		#endif
+		memcpy(ssid[i].Ssid, ssids[i].ssid, ssids[i].ssid_len);
+		ssid[i].SsidLength = 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++) {
+		#ifdef DEBUG_CFG80211
+		DBG_871X(FUNC_ADPT_FMT CHAN_FMT"\n", FUNC_ADPT_ARG(padapter), CHAN_ARG(request->channels[i]));
+		#endif
+		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:
+	if (true == 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)
+{
+	DBG_8192C("%s\n", __func__);
+	return 0;
+}
+
+
+
+static int rtw_cfg80211_set_wpa_version(struct security_priv *psecuritypriv, u32 wpa_version)
+{
+	DBG_8192C("%s, wpa_version =%d\n", __func__, 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)
+{
+	DBG_8192C("%s, nl80211_auth_type =%d\n", __func__, 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;
+
+	DBG_8192C("%s, ucast =%d, cipher = 0x%x\n", __func__, ucast, cipher);
+
+
+	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:
+		DBG_8192C("Unsupported cipher: 0x%x\n", cipher);
+		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)
+{
+	DBG_8192C("%s, key_mgt = 0x%x\n", __func__, 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;
+	}
+	else {
+		DBG_8192C("Invalid key mgt: 0x%x\n", key_mgt);
+		/* return -EINVAL; */
+	}
+
+	return 0;
+}
+
+static int rtw_cfg80211_set_wpa_ie(struct adapter *padapter, u8 *pie, size_t ielen)
+{
+	u8 *buf = NULL, *pos = 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 == NULL || !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 == NULL) {
+		ret =  -ENOMEM;
+		goto exit;
+	}
+
+	memcpy(buf, pie , ielen);
+
+	/* dump */
+	{
+		int i;
+		DBG_8192C("set wpa_ie(length:%zu):\n", ielen);
+		for (i = 0;i<ielen;i =i+8)
+			DBG_8192C("0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n", buf[i], buf[i+1], buf[i+2], buf[i+3], buf[i+4], buf[i+5], buf[i+6], buf[i+7]);
+	}
+
+	pos = buf;
+	if (ielen < RSN_HEADER_LEN) {
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("Ie len too short %d\n", ielen));
+		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);
+
+			DBG_8192C("got wpa_ie, wpa_ielen:%u\n", wpa_ielen);
+		}
+	}
+
+	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);
+
+			DBG_8192C("got wpa2_ie, wpa2_ielen:%u\n", wpa2_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;
+	}
+
+	{/* 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) {
+			DBG_8192C("got wps_ie, wps_ielen:%u\n", wps_ielen);
+			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);
+
+	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+		("rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->securitypriv.ndisencryptstatus =%d padapter->securitypriv.ndisauthtype =%d\n",
+		pairwise_cipher, padapter->securitypriv.ndisencryptstatus, padapter->securitypriv.ndisauthtype));
+
+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 = (struct adapter *)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 (_FAIL == rtw_pwr_wakeup(padapter)) {
+		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.SsidLength = params->ssid_len;
+	memcpy(ndis_ssid.Ssid, (u8 *)params->ssid, params->ssid_len);
+
+	/* DBG_8192C("ssid =%s, len =%zu\n", ndis_ssid.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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct wireless_dev *rtw_wdev = padapter->rtw_wdev;
+	enum nl80211_iftype old_type;
+	int ret = 0;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	padapter->mlmepriv.not_indic_disco = true;
+
+	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:
+	padapter->mlmepriv.not_indic_disco = false;
+
+	return 0;
+}
+
+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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	padapter->mlmepriv.not_indic_disco = true;
+
+	DBG_871X("=>"FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	DBG_871X("privacy =%d, key =%p, key_len =%d, key_idx =%d\n",
+		sme->privacy, sme->key, sme->key_len, sme->key_idx);
+
+
+	if (adapter_wdev_data(padapter)->block == true)
+	{
+		ret = -EBUSY;
+		DBG_871X("%s wdev_priv.block is set\n", __func__);
+		goto exit;
+	}
+
+	rtw_ps_deny(padapter, PS_DENY_JOIN);
+	if (_FAIL == rtw_pwr_wakeup(padapter)) {
+		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.SsidLength = sme->ssid_len;
+	memcpy(ndis_ssid.Ssid, (u8 *)sme->ssid, sme->ssid_len);
+
+	DBG_8192C("ssid =%s, len =%zu\n", ndis_ssid.Ssid, sme->ssid_len);
+
+
+	if (sme->bssid)
+		DBG_8192C("bssid ="MAC_FMT"\n", MAC_ARG(sme->bssid));
+
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		ret = -EBUSY;
+		DBG_8192C("%s, fw_state = 0x%x, goto exit\n", __func__, pmlmepriv->fw_state);
+		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;
+
+	DBG_8192C("%s, ie_len =%zu\n", __func__, sme->ie_len);
+
+	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;
+		DBG_871X("%s(): Shared/Auto WEP\n", __func__);
+
+		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, KeyMaterial);
+			pwep =(struct ndis_802_11_wep	 *) rtw_malloc(wep_total_len);
+			if (pwep == NULL) {
+				DBG_871X(" wpa_set_encryption: pwep allocate fail !!!\n");
+				ret = -ENOMEM;
+				goto exit;
+			}
+
+			memset(pwep, 0, wep_total_len);
+
+			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,  (void *)sme->key, pwep->KeyLength);
+
+		if (rtw_set_802_11_add_wep(padapter, pwep) == (u8)_FAIL)
+		{
+			ret = -EOPNOTSUPP ;
+		}
+
+		kfree((u8 *)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;
+	}
+
+	DBG_8192C("set ssid:dot11AuthAlgrthm =%d, dot11PrivacyAlgrthm =%d, dot118021XGrpPrivacy =%d\n", psecuritypriv->dot11AuthAlgrthm, psecuritypriv->dot11PrivacyAlgrthm, psecuritypriv->dot118021XGrpPrivacy);
+
+exit:
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_JOIN);
+
+	DBG_8192C("<=%s, ret %d\n", __func__, ret);
+
+	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 = (struct adapter *)rtw_netdev_priv(ndev);
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	padapter->mlmepriv.not_indic_disco = true;
+
+	rtw_set_to_roam(padapter, 0);
+
+	rtw_scan_abort(padapter);
+	LeaveAllPowerSaveMode(padapter);
+	rtw_disassoc_cmd(padapter, 500, false);
+
+	DBG_871X("%s...call rtw_indicate_disconnect\n", __func__);
+
+	rtw_indicate_disconnect(padapter);
+
+	rtw_free_assoc_resources(padapter, 1);
+	rtw_pwr_wakeup(padapter);
+
+	padapter->mlmepriv.not_indic_disco = false;
+
+	DBG_871X(FUNC_NDEV_FMT" return 0\n", FUNC_NDEV_ARG(ndev));
+	return 0;
+}
+
+static int cfg80211_rtw_set_txpower(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	enum nl80211_tx_power_setting type, int mbm)
+{
+	DBG_8192C("%s\n", __func__);
+	return 0;
+}
+
+static int cfg80211_rtw_get_txpower(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	int *dbm)
+{
+	DBG_8192C("%s\n", __func__);
+
+	*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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct rtw_wdev_priv *rtw_wdev_priv = adapter_wdev_data(padapter);
+
+	DBG_871X(FUNC_NDEV_FMT" enabled:%u, timeout:%d\n", FUNC_NDEV_ARG(ndev),
+		enabled, timeout);
+
+	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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+	u8 strZeroMacAddress[ ETH_ALEN ] = { 0x00 };
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	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))
+		{ /*  BSSID is matched, the same AP => rewrite with new PMKID. */
+			DBG_871X(FUNC_NDEV_FMT" BSSID exists in the PMKList.\n", FUNC_NDEV_ARG(ndev));
+
+			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)
+	{
+		/*  Find a new entry */
+		DBG_871X(FUNC_NDEV_FMT" Use the new entry index = %d for this PMKID.\n",
+			FUNC_NDEV_ARG(ndev), psecuritypriv->PMKIDIndex);
+
+		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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	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. */
+			memset(psecuritypriv->PMKIDList[index].Bssid, 0x00, ETH_ALEN);
+			memset(psecuritypriv->PMKIDList[index].PMKID, 0x00, WLAN_PMKID_LEN);
+			psecuritypriv->PMKIDList[index].bUsed = false;
+			bMatched = true;
+			break;
+		}
+	}
+
+	if (false == bMatched)
+	{
+		DBG_871X(FUNC_NDEV_FMT" do not have matched BSSID\n"
+			, FUNC_NDEV_ARG(ndev));
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cfg80211_rtw_flush_pmksa(struct wiphy *wiphy,
+				    struct net_device *ndev)
+{
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	memset(&psecuritypriv->PMKIDList[ 0 ], 0x00, sizeof(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;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	{
+		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;
+
+	DBG_871X(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	cfg80211_del_sta(ndev, da, GFP_ATOMIC);
+}
+
+static int rtw_cfg80211_monitor_if_open(struct net_device *ndev)
+{
+	int ret = 0;
+
+	DBG_8192C("%s\n", __func__);
+
+	return ret;
+}
+
+static int rtw_cfg80211_monitor_if_close(struct net_device *ndev)
+{
+	int ret = 0;
+
+	DBG_8192C("%s\n", __func__);
+
+	return ret;
+}
+
+static int rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb, struct net_device *ndev)
+{
+	int ret = 0;
+	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 = (struct adapter *)rtw_netdev_priv(ndev);
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	if (!skb)
+		goto fail;
+
+	rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, skb->truesize);
+
+	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)
+	{
+		DBG_8192C("radiotap len (should be 14): %d\n", rtap_len);
+		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 & RTW_IEEE80211_FCTL_FTYPE) == RTW_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
+		 * 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));
+
+		DBG_8192C("should be eapol packet\n");
+
+		/* Use the real net device to transmit the packet */
+		ret = _rtw_xmit_entry(skb, padapter->pnetdev);
+
+		return ret;
+
+	}
+	else if ((frame_control & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE))
+		== (RTW_IEEE80211_FTYPE_MGMT|RTW_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) {
+			DBG_8192C(FUNC_NDEV_FMT" frame_control:0x%x\n", FUNC_NDEV_ARG(ndev),
+				le16_to_cpu(((struct ieee80211_hdr_3addr *)buf)->frame_control));
+			goto fail;
+		}
+
+		DBG_8192C("RTW_Tx:da ="MAC_FMT" via "FUNC_NDEV_FMT"\n",
+			MAC_ARG(GetAddr1Ptr(buf)), FUNC_NDEV_ARG(ndev));
+		if (category == RTW_WLAN_CATEGORY_PUBLIC)
+			DBG_871X("RTW_Tx:%s\n", action_public_str(action));
+		else
+			DBG_871X("RTW_Tx:category(%u), action(%u)\n", category, action);
+
+		/* starting alloc mgmt frame to dump it */
+		if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+		{
+			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);
+
+	}
+	else
+	{
+		DBG_8192C("frame_control = 0x%x\n", frame_control & (RTW_IEEE80211_FCTL_FTYPE|RTW_IEEE80211_FCTL_STYPE));
+	}
+
+
+fail:
+
+	dev_kfree_skb_any(skb);
+
+	return 0;
+
+}
+
+static int rtw_cfg80211_monitor_if_set_mac_address(struct net_device *ndev, void *addr)
+{
+	int ret = 0;
+
+	DBG_8192C("%s\n", __func__);
+
+	return ret;
+}
+
+static const struct net_device_ops rtw_cfg80211_monitor_if_ops = {
+	.ndo_open = rtw_cfg80211_monitor_if_open,
+       .ndo_stop = rtw_cfg80211_monitor_if_close,
+       .ndo_start_xmit = rtw_cfg80211_monitor_if_xmit_entry,
+       .ndo_set_mac_address = rtw_cfg80211_monitor_if_set_mac_address,
+};
+
+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) {
+		DBG_871X(FUNC_ADPT_FMT" without specific name\n", FUNC_ADPT_ARG(padapter));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (pwdev_priv->pmon_ndev) {
+		DBG_871X(FUNC_ADPT_FMT" monitor interface exist: "NDEV_FMT"\n",
+			FUNC_ADPT_ARG(padapter), NDEV_ARG(pwdev_priv->pmon_ndev));
+		ret = -EBUSY;
+		goto out;
+	}
+
+	mon_ndev = alloc_etherdev(sizeof(struct rtw_netdev_priv_indicator));
+	if (!mon_ndev) {
+		DBG_871X(FUNC_ADPT_FMT" allocate ndev fail\n", FUNC_ADPT_ARG(padapter));
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mon_ndev->type = ARPHRD_IEEE80211_RADIOTAP;
+	strncpy(mon_ndev->name, name, IFNAMSIZ);
+	mon_ndev->name[IFNAMSIZ - 1] = 0;
+	mon_ndev->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 = (struct wireless_dev *)rtw_zmalloc(sizeof(struct wireless_dev));
+	if (!mon_wdev) {
+		DBG_871X(FUNC_ADPT_FMT" allocate mon_wdev fail\n", FUNC_ADPT_ARG(padapter));
+		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 = 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((u8 *)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);
+
+	DBG_871X(FUNC_ADPT_FMT " wiphy:%s, name:%s, type:%d\n",
+		FUNC_ADPT_ARG(padapter), wiphy_name(wiphy), name, type);
+
+	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;
+		DBG_871X("Unsupported interface type\n");
+		break;
+	}
+
+	DBG_871X(FUNC_ADPT_FMT" ndev:%p, ret:%d\n", FUNC_ADPT_ARG(padapter), ndev, ret);
+
+	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 = (struct adapter *)rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(adapter);
+
+	unregister_netdevice(ndev);
+
+	if (ndev == pwdev_priv->pmon_ndev) {
+		pwdev_priv->pmon_ndev = NULL;
+		pwdev_priv->ifname_mon[0] = '\0';
+		DBG_871X(FUNC_NDEV_FMT" remove monitor interface\n", FUNC_NDEV_ARG(ndev));
+	}
+
+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);
+
+	DBG_8192C("%s beacon_head_len =%zu, beacon_tail_len =%zu\n", __func__, head_len, tail_len);
+
+	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 */
+	if (rtw_get_wps_ie(pbuf+_FIXED_IE_LENGTH_, len-_FIXED_IE_LENGTH_, NULL, &wps_ielen))
+		DBG_8192C("add bcn, wps_ielen =%d\n", wps_ielen);
+
+	/* pbss_network->IEs will not include p2p_ie, wfd ie */
+	rtw_ies_remove_ie(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, P2P_OUI, 4);
+	rtw_ies_remove_ie(pbuf, &len, _BEACON_IE_OFFSET_, _VENDOR_SPECIFIC_IE_, 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 = (struct adapter *)rtw_netdev_priv(ndev);
+
+	DBG_871X(FUNC_NDEV_FMT" hidden_ssid:%d, auth_type:%d\n", FUNC_NDEV_ARG(ndev),
+		settings->hidden_ssid, settings->auth_type);
+
+	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.SsidLength = settings->ssid_len;
+		memcpy(pbss_network_ext->Ssid.Ssid, (void *)settings->ssid, settings->ssid_len);
+		pbss_network_ext->Ssid.SsidLength = settings->ssid_len;
+	}
+
+	return ret;
+}
+
+static int cfg80211_rtw_change_beacon(struct wiphy *wiphy, struct net_device *ndev,
+                                struct cfg80211_beacon_data *info)
+{
+	int ret = 0;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(ndev);
+
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	ret = rtw_add_beacon(adapter, info->head, info->head_len, info->tail, info->tail_len);
+
+	return ret;
+}
+
+static int cfg80211_rtw_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	return 0;
+}
+
+static int	cfg80211_rtw_add_station(struct wiphy *wiphy, struct net_device *ndev,
+				const u8 *mac,
+			struct station_parameters *params)
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	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;
+	u8 updated = false;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	const u8 *mac = params->mac;
+
+	DBG_871X("+"FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	if (check_fwstate(pmlmepriv, (_FW_LINKED|WIFI_AP_STATE)) != true)
+	{
+		DBG_8192C("%s, fw_state != FW_LINKED|WIFI_AP_STATE\n", __func__);
+		return -EINVAL;
+	}
+
+
+	if (!mac)
+	{
+		DBG_8192C("flush all sta, and cam_entry\n");
+
+		flush_all_cam_entry(padapter);	/* clear CAM */
+
+		ret = rtw_sta_flush(padapter);
+
+		return ret;
+	}
+
+
+	DBG_8192C("free sta macaddr =" MAC_FMT "\n", MAC_ARG(mac));
+
+	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;
+	plist = get_next(phead);
+
+	/* check asoc_queue */
+	while (phead != plist)
+	{
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+
+		plist = get_next(plist);
+
+		if (!memcmp((u8 *)mac, psta->hwaddr, ETH_ALEN))
+		{
+			if (psta->dot8021xalg == 1 && psta->bpairwise_key_installed == false)
+			{
+				DBG_8192C("%s, sta's dot8021xalg = 1 and key_installed = false\n", __func__);
+			}
+			else
+			{
+				DBG_8192C("free psta =%p, aid =%d\n", psta, psta->aid);
+
+				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);
+
+	DBG_871X("-"FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	return ret;
+
+}
+
+static int cfg80211_rtw_change_station(struct wiphy *wiphy, struct net_device *ndev,
+				  const u8 *mac, struct station_parameters *params)
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	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 = LIST_CONTAINOR(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 = (struct adapter *)rtw_netdev_priv(ndev);
+	struct sta_info *psta = NULL;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	psta = rtw_sta_info_get_by_idx(idx, pstapriv);
+	spin_unlock_bh(&pstapriv->asoc_list_lock);
+	if (NULL == psta)
+	{
+		DBG_871X("Station is not found\n");
+		ret = -ENOENT;
+		goto exit;
+	}
+	memcpy(mac, psta->hwaddr, ETH_ALEN);
+	sinfo->filled = BIT(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)
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+	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);
+
+	DBG_8192C("RTW_Rx:cur_ch =%d\n", channel);
+	if (msg)
+		DBG_871X("RTW_Rx:%s\n", msg);
+	else
+		DBG_871X("RTW_Rx:category(%u), action(%u)\n", 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 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 */
+	if ((pmgntframe = alloc_mgtxmitframe(pxmitpriv)) == NULL)
+	{
+		/* 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;
+
+		#ifdef DEBUG_CFG80211
+		DBG_8192C("%s, ack == _FAIL\n", __func__);
+		#endif
+	}
+	else
+	{
+
+		msleep(50);
+
+		#ifdef DEBUG_CFG80211
+		DBG_8192C("%s, ack =%d, ok!\n", __func__, ack);
+		#endif
+		ret = _SUCCESS;
+	}
+
+exit:
+
+	#ifdef DEBUG_CFG80211
+	DBG_8192C("%s, ret =%d\n", __func__, ret);
+	#endif
+
+	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 == NULL) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	padapter = (struct adapter *)rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(padapter);
+
+	/* cookie generation */
+	*cookie = (unsigned long) buf;
+
+#ifdef DEBUG_CFG80211
+	DBG_871X(FUNC_ADPT_FMT" len =%zu, ch =%d"
+		"\n", FUNC_ADPT_ARG(padapter),
+		len, tx_ch
+	);
+#endif /* DEBUG_CFG80211 */
+
+	/* 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) {
+		DBG_8192C(FUNC_ADPT_FMT" frame_control:0x%x\n", FUNC_ADPT_ARG(padapter),
+			le16_to_cpu(((struct ieee80211_hdr_3addr *)buf)->frame_control));
+		goto exit;
+	}
+
+	DBG_8192C("RTW_Tx:tx_ch =%d, da ="MAC_FMT"\n", tx_ch, MAC_ARG(GetAddr1Ptr(buf)));
+	if (category == RTW_WLAN_CATEGORY_PUBLIC)
+		DBG_871X("RTW_Tx:%s\n", action_public_str(action));
+	else
+		DBG_871X("RTW_Tx:category(%u), action(%u)\n", category, action);
+
+	rtw_ps_deny(padapter, PS_DENY_MGNT_TX);
+	if (_FAIL == rtw_pwr_wakeup(padapter)) {
+		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);
+
+	if (tx_ret != _SUCCESS || dump_cnt > 1) {
+		DBG_871X(FUNC_ADPT_FMT" %s (%d/%d)\n", FUNC_ADPT_ARG(padapter),
+			tx_ret == _SUCCESS?"OK":"FAIL", dump_cnt, dump_limit);
+	}
+
+	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)
+		{
+			DBG_871X(FUNC_ADPT_FMT" agree with invitation of persistent group\n",
+				FUNC_ADPT_ARG(padapter));
+			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 cfg80211_rtw_mgmt_frame_register(struct wiphy *wiphy,
+	struct wireless_dev *wdev,
+	u16 frame_type, bool reg)
+{
+	struct net_device *ndev = wdev_to_ndev(wdev);
+	struct adapter *adapter;
+
+	if (ndev == NULL)
+		goto exit;
+
+	adapter = (struct adapter *)rtw_netdev_priv(ndev);
+
+#ifdef DEBUG_CFG80211
+	DBG_871X(FUNC_ADPT_FMT" frame_type:%x, reg:%d\n", FUNC_ADPT_ARG(adapter),
+		frame_type, reg);
+#endif
+
+	if (frame_type != (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ))
+		return;
+exit:
+	return;
+}
+
+#if defined(CONFIG_PNO_SUPPORT)
+static int cfg80211_rtw_sched_scan_start(struct wiphy *wiphy,
+		struct net_device *dev,
+		struct cfg80211_sched_scan_request *request) {
+
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct	mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u8 ret;
+
+	if (padapter->bup == false) {
+		DBG_871X("%s: net device is down.\n", __func__);
+		return -EIO;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true ||
+		check_fwstate(pmlmepriv, _FW_LINKED) == true  ||
+		check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == true) {
+		DBG_871X("%s: device is busy.\n", __func__);
+		rtw_scan_abort(padapter);
+	}
+
+	if (request == NULL) {
+		DBG_871X("%s: invalid cfg80211_requests parameters.\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = rtw_android_cfg80211_pno_setup(dev, request->ssids,
+			request->n_ssids, request->interval);
+
+	if (ret < 0) {
+		DBG_871X("%s ret: %d\n", __func__, ret);
+		goto exit;
+	}
+
+	ret = rtw_android_pno_enable(dev, true);
+	if (ret < 0) {
+		DBG_871X("%s ret: %d\n", __func__, ret);
+		goto exit;
+	}
+exit:
+	return ret;
+}
+
+static int cfg80211_rtw_sched_scan_stop(struct wiphy *wiphy,
+		struct net_device *dev) {
+	return rtw_android_pno_enable(dev, false);
+}
+#endif /* CONFIG_PNO_SUPPORT */
+
+static void rtw_cfg80211_init_ht_capab(struct ieee80211_sta_ht_cap *ht_cap, enum nl80211_band band, u8 rf_type)
+{
+
+#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
+	 */
+	if (rf_type == RF_1T1R)
+	{
+		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);
+	}
+	else if ((rf_type == RF_1T2R) || (rf_type ==RF_2T2R))
+	{
+		ht_cap->mcs.rx_mask[0] = 0xFF;
+		ht_cap->mcs.rx_mask[1] = 0xFF;
+		ht_cap->mcs.rx_mask[4] = 0x01;
+
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
+	}
+	else
+	{
+		DBG_8192C("%s, error rf_type =%d\n", __func__, rf_type);
+	}
+
+}
+
+void rtw_cfg80211_init_wiphy(struct adapter *padapter)
+{
+	u8 rf_type;
+	struct ieee80211_supported_band *bands;
+	struct wireless_dev *pwdev = padapter->rtw_wdev;
+	struct wiphy *wiphy = pwdev->wiphy;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	DBG_8192C("%s:rf_type =%d\n", __func__, rf_type);
+
+	{
+		bands = wiphy->bands[NL80211_BAND_2GHZ];
+		if (bands)
+			rtw_cfg80211_init_ht_capab(&bands->ht_cap, NL80211_BAND_2GHZ, rf_type);
+	}
+
+	/* init regulary domain */
+	rtw_regd_init(padapter, rtw_reg_notifier);
+
+	/* 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;
+#ifdef CONFIG_PNO_SUPPORT
+	wiphy->max_sched_scan_ssids = MAX_PNO_LIST_COUNT;
+#endif
+#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,
+
+	.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,
+	.mgmt_frame_register = cfg80211_rtw_mgmt_frame_register,
+
+#if defined(CONFIG_PNO_SUPPORT)
+	.sched_scan_start = cfg80211_rtw_sched_scan_start,
+	.sched_scan_stop = cfg80211_rtw_sched_scan_stop,
+#endif /* CONFIG_PNO_SUPPORT */
+};
+
+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;
+
+	DBG_8192C("%s(padapter =%p)\n", __func__, padapter);
+
+	/* wiphy */
+	wiphy = wiphy_new(&rtw_cfg80211_ops, sizeof(struct adapter *));
+	if (!wiphy) {
+		DBG_8192C("Couldn't allocate wiphy device\n");
+		ret = -ENOMEM;
+		goto exit;
+	}
+	set_wiphy_dev(wiphy, dev);
+	*((struct adapter **)wiphy_priv(wiphy)) = padapter;
+	rtw_cfg80211_preinit_wiphy(padapter, wiphy);
+
+	ret = wiphy_register(wiphy);
+	if (ret < 0) {
+		DBG_8192C("Couldn't register wiphy device\n");
+		goto free_wiphy;
+	}
+
+	/*  wdev */
+	wdev = (struct wireless_dev *)rtw_zmalloc(sizeof(struct wireless_dev));
+	if (!wdev) {
+		DBG_8192C("Couldn't allocate wireless device\n");
+		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;
+	kfree((u8 *)wdev);
+
+	return ret;
+
+unregister_wiphy:
+	wiphy_unregister(wiphy);
+ free_wiphy:
+	wiphy_free(wiphy);
+exit:
+	return ret;
+
+}
+
+void rtw_wdev_free(struct wireless_dev *wdev)
+{
+	DBG_8192C("%s(wdev =%p)\n", __func__, wdev);
+
+	if (!wdev)
+		return;
+
+	rtw_spt_band_free(wdev->wiphy->bands[NL80211_BAND_2GHZ]);
+
+	wiphy_free(wdev->wiphy);
+
+	kfree((u8 *)wdev);
+}
+
+void rtw_wdev_unregister(struct wireless_dev *wdev)
+{
+	struct net_device *ndev;
+	struct adapter *adapter;
+	struct rtw_wdev_priv *pwdev_priv;
+
+	DBG_8192C("%s(wdev =%p)\n", __func__, wdev);
+
+	if (!wdev)
+		return;
+
+	if (!(ndev = wdev_to_ndev(wdev)))
+		return;
+
+	adapter = (struct adapter *)rtw_netdev_priv(ndev);
+	pwdev_priv = adapter_wdev_data(adapter);
+
+	rtw_cfg80211_indicate_scan_done(adapter, true);
+
+	if (pwdev_priv->pmon_ndev) {
+		DBG_8192C("%s, unregister monitor interface\n", __func__);
+		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 000000000000..916741371bee
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
@@ -0,0 +1,5808 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _IOCTL_LINUX_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <rtw_mp.h>
+#include <linux/jiffies.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'
+
+
+extern u8 key_2char2num(u8 hch, u8 lch);
+
+static u32 rtw_rates[] = {1000000, 2000000, 5500000, 11000000,
+	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
+
+static const char * const iw_operation_mode[] =
+{
+	"Auto", "Ad-Hoc", "Managed",  "Master", "Repeater", "Secondary", "Monitor"
+};
+
+static int hex2num_i(char c)
+{
+	if (c >= '0' && c <= '9')
+		return c - '0';
+	if (c >= 'a' && c <= 'f')
+		return c - 'a' + 10;
+	if (c >= 'A' && c <= 'F')
+		return c - 'A' + 10;
+	return -1;
+}
+
+/**
+ * hwaddr_aton - Convert ASCII string to MAC address
+ * @txt: MAC address as a string (e.g., "00:11:22:33:44:55")
+ * @addr: Buffer for the MAC address (ETH_ALEN = 6 bytes)
+ * Returns: 0 on success, -1 on failure (e.g., string not a MAC address)
+ */
+static int hwaddr_aton_i(const char *txt, u8 *addr)
+{
+	int i;
+
+	for (i = 0; i < 6; i++) {
+		int a, b;
+
+		a = hex2num_i(*txt++);
+		if (a < 0)
+			return -1;
+		b = hex2num_i(*txt++);
+		if (b < 0)
+			return -1;
+		*addr++ = (a << 4) | b;
+		if (i < 5 && *txt++ != ':')
+			return -1;
+	}
+
+	return 0;
+}
+
+void indicate_wx_scan_complete_event(struct adapter *padapter)
+{
+	union iwreq_data wrqu;
+
+	memset(&wrqu, 0, sizeof(union iwreq_data));
+
+	/* DBG_871X("+rtw_indicate_wx_scan_complete_event\n"); */
+}
+
+
+void rtw_indicate_wx_assoc_event(struct adapter *padapter)
+{
+	union iwreq_data wrqu;
+	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));
+
+	memset(&wrqu, 0, sizeof(union iwreq_data));
+
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+
+	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) ==true)
+		memcpy(wrqu.ap_addr.sa_data, pnetwork->MacAddress, ETH_ALEN);
+	else
+		memcpy(wrqu.ap_addr.sa_data, pmlmepriv->cur_network.network.MacAddress, ETH_ALEN);
+
+	DBG_871X_LEVEL(_drv_always_, "assoc success\n");
+}
+
+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);
+}
+
+/*
+uint	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;
+}
+
+uint	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;
+}
+*/
+
+static char *translate_scan(struct adapter *padapter,
+				struct iw_request_info* info, struct wlan_network *pnetwork,
+				char *start, char *stop)
+{
+	struct iw_event iwe;
+	u16 cap;
+	u32 ht_ielen = 0;
+	char *custom = NULL;
+	char *p;
+	u16 max_rate = 0, rate, ht_cap =false, vht_cap = false;
+	u32 i = 0;
+	u8 bw_40MHz = 0, short_GI = 0;
+	u16 mcs_rate = 0, vht_data_rate = 0;
+	u8 ie_offset = (pnetwork->network.Reserved[0] == 2? 0:12);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	u8 ss, sq;
+
+	/*  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((u16)pnetwork->network.Ssid.SsidLength, (u16)32);
+	start = iwe_stream_add_point(info, start, stop, &iwe, pnetwork->network.Ssid.Ssid);
+
+	/* parsing HT_CAP_IE */
+	if (pnetwork->network.Reserved[0] == 2) /*  Probe Request */
+	{
+		p = rtw_get_ie(&pnetwork->network.IEs[0], _HT_CAPABILITY_IE_, &ht_ielen, pnetwork->network.IELength);
+	}
+	else
+	{
+		p = rtw_get_ie(&pnetwork->network.IEs[12], _HT_CAPABILITY_IE_, &ht_ielen, pnetwork->network.IELength-12);
+	}
+	if (p && ht_ielen>0)
+	{
+		struct rtw_ieee80211_ht_cap *pht_capie;
+		ht_cap = true;
+		pht_capie = (struct rtw_ieee80211_ht_cap *)(p+2);
+		memcpy(&mcs_rate , pht_capie->supp_mcs_set, 2);
+		bw_40MHz = (le16_to_cpu(pht_capie->cap_info) & IEEE80211_HT_CAP_SUP_WIDTH) ? 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)) == true)
+	{
+		if (ht_cap == true)
+			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)) == true)
+	{
+		if (ht_cap == true)
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bgn");
+		else
+			snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11bg");
+	}
+	else
+	{
+		if (pnetwork->network.Configuration.DSConfig > 14)
+		{
+			if (vht_cap == true)
+				snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11AC");
+			else if (ht_cap == true)
+				snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11an");
+			else
+				snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11a");
+		}
+		else
+		{
+			if (ht_cap == true)
+				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 */
+	if (pnetwork->network.Reserved[0] == 2) /*  Probe Request */
+	{
+		cap = 0;
+	}
+	else
+	{
+		__le16 le_tmp;
+
+	        iwe.cmd = SIOCGIWMODE;
+		memcpy((u8 *)&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>14*/)
+		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 (vht_cap == true) {
+		max_rate = vht_data_rate;
+	}
+	else if (ht_cap == true)
+	{
+		if (mcs_rate&0x8000)/* MCS15 */
+		{
+			max_rate = (bw_40MHz) ? ((short_GI)?300:270):((short_GI)?144:130);
+
+		}
+		else if (mcs_rate&0x0080)/* MCS7 */
+		{
+			max_rate = (bw_40MHz) ? ((short_GI)?150:135):((short_GI)?72:65);
+		}
+		else/* default MCS7 */
+		{
+			/* DBG_871X("wx_get_scan, mcs_rate_bitmap = 0x%x\n", mcs_rate); */
+			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 = 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 */
+	if (pnetwork->network.Reserved[0] != 2) /*  Probe Request */
+	{
+		u8 *buf;
+		u8 wpa_ie[255], rsn_ie[255];
+		u16 wpa_len = 0, rsn_len = 0;
+		u8 *p;
+		sint out_len = 0;
+		out_len =rtw_get_sec_ie(pnetwork->network.IEs , pnetwork->network.IELength, rsn_ie,&rsn_len, wpa_ie,&wpa_len);
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_scan: ssid =%s\n", pnetwork->network.Ssid.Ssid));
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_scan: wpa_len =%d rsn_len =%d\n", wpa_len, rsn_len));
+
+		buf = kzalloc(MAX_WPA_IE_LEN*2, GFP_KERNEL);
+		if (!buf)
+			return start;
+		if (wpa_len > 0) {
+			p =buf;
+			p += sprintf(p, "wpa_ie =");
+			for (i = 0; i < wpa_len; i++) {
+				p += sprintf(p, "%02x", wpa_ie[i]);
+			}
+
+			if (wpa_len > 100) {
+				printk("-----------------Len %d----------------\n", wpa_len);
+				for (i = 0; i < wpa_len; i++) {
+					printk("%02x ", wpa_ie[i]);
+				}
+				printk("\n");
+				printk("-----------------Len %d----------------\n", wpa_len);
+			}
+
+			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*2);
+			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);
+	}
+
+	{ /* parsing WPS IE */
+		uint cnt = 0, total_ielen;
+		u8 *wpsie_ptr = NULL;
+		uint wps_ielen = 0;
+
+		u8 *ie_ptr = pnetwork->network.IEs + ie_offset;
+		total_ielen = pnetwork->network.IELength - ie_offset;
+
+		if (pnetwork->network.Reserved[0] == 2) /*  Probe Request */
+		{
+			ie_ptr = pnetwork->network.IEs;
+			total_ielen = pnetwork->network.IELength;
+		}
+		else     /*  Beacon or Probe Respones */
+		{
+			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
+	#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		| IW_QUAL_NOISE_UPDATED
+	#else
+		| IW_QUAL_NOISE_INVALID
+	#endif
+	#ifdef CONFIG_SIGNAL_DISPLAY_DBM
+		| IW_QUAL_DBM
+	#endif
+	;
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == true &&
+		is_same_network(&pmlmepriv->cur_network.network, &pnetwork->network, 0)) {
+		ss = padapter->recvpriv.signal_strength;
+		sq = padapter->recvpriv.signal_qual;
+	} else {
+		ss = pnetwork->network.PhyInfo.SignalStrength;
+		sq = pnetwork->network.PhyInfo.SignalQuality;
+	}
+
+
+	#ifdef CONFIG_SIGNAL_DISPLAY_DBM
+	iwe.u.qual.level = (u8) translate_percentage_to_dbm(ss);/* dbm */
+	#else
+	#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+	{
+		/* Do signal scale mapping when using percentage as the unit of signal strength, since the scale mapping is skipped in odm */
+
+		struct hal_com_data *pHal = GET_HAL_DATA(padapter);
+
+		iwe.u.qual.level = (u8)odm_SignalScaleMapping(&pHal->odmpriv, ss);
+	}
+	#else
+	iwe.u.qual.level = (u8)ss;/*  */
+	#endif
+	#endif
+
+	iwe.u.qual.qual = (u8)sq;   /*  signal quality */
+
+	#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+	{
+		s16 tmp_noise = 0;
+		rtw_hal_get_odm_var(padapter, HAL_ODM_NOISE_MONITOR,&(pnetwork->network.Configuration.DSConfig), &(tmp_noise));
+		iwe.u.qual.noise = tmp_noise ;
+	}
+	#else
+	iwe.u.qual.noise = 0; /*  noise level */
+	#endif
+
+	/* DBG_871X("iqual =%d, ilevel =%d, inoise =%d, iupdated =%d\n", iwe.u.qual.qual, iwe.u.qual.level , iwe.u.qual.noise, iwe.u.qual.updated); */
+
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+
+	{
+		u8 *buf;
+		u8 *p, *pos;
+
+		buf = kzalloc(MAX_WPA_IE_LEN, GFP_KERNEL);
+		if (!buf)
+			goto exit;
+		p = buf;
+		pos = pnetwork->network.Reserved;
+		p += sprintf(p, "fm =%02X%02X", pos[1], pos[0]);
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = IWEVCUSTOM;
+		iwe.u.data.length = strlen(buf);
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+		kfree(buf);
+	}
+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))
+	{
+		DBG_871X("wpa_set_auth_algs, AUTH_ALG_SHARED_KEY and  AUTH_ALG_OPEN_SYSTEM [value:0x%x]\n", value);
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeAutoSwitch;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+	}
+	else if (value & AUTH_ALG_SHARED_KEY)
+	{
+		DBG_871X("wpa_set_auth_algs, AUTH_ALG_SHARED_KEY  [value:0x%x]\n", value);
+		padapter->securitypriv.ndisencryptstatus = Ndis802_11Encryption1Enabled;
+
+		padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeShared;
+		padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Shared;
+	}
+	else if (value & AUTH_ALG_OPEN_SYSTEM)
+	{
+		DBG_871X("wpa_set_auth_algs, AUTH_ALG_OPEN_SYSTEM\n");
+		/* padapter->securitypriv.ndisencryptstatus = Ndis802_11EncryptionDisabled; */
+		if (padapter->securitypriv.ndisauthtype < Ndis802_11AuthModeWPAPSK)
+		{
+			padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
+			padapter->securitypriv.dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		}
+
+	}
+	else if (value & AUTH_ALG_LEAP)
+	{
+		DBG_871X("wpa_set_auth_algs, AUTH_ALG_LEAP\n");
+	}
+	else
+	{
+		DBG_871X("wpa_set_auth_algs, error!\n");
+		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;
+
+	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 ||
+		    param->u.crypt.idx >= BIP_MAX_KEYID) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	}
+	else
+	{
+		{
+			ret = -EINVAL;
+			goto exit;
+		}
+	}
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0)
+	{
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("wpa_set_encryption, crypt.alg = WEP\n"));
+		DBG_871X("wpa_set_encryption, crypt.alg = WEP\n");
+
+		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;
+
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("(1)wep_key_idx =%d\n", wep_key_idx));
+		DBG_871X("(1)wep_key_idx =%d\n", wep_key_idx);
+
+		if (wep_key_idx > WEP_KEYS)
+			return -EINVAL;
+
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("(2)wep_key_idx =%d\n", wep_key_idx));
+
+		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, KeyMaterial);
+			pwep =(struct ndis_802_11_wep	 *) rtw_malloc(wep_total_len);
+			if (pwep == NULL) {
+				RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, (" wpa_set_encryption: pwep allocate fail !!!\n"));
+				goto exit;
+			}
+
+			memset(pwep, 0, wep_total_len);
+
+			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)
+		{
+			DBG_871X("wep, set_tx = 1\n");
+
+			if (rtw_set_802_11_add_wep(padapter, pwep) == (u8)_FAIL)
+			{
+				ret = -EOPNOTSUPP ;
+			}
+		}
+		else
+		{
+			DBG_871X("wep, set_tx = 0\n");
+
+			/* 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->KeyMaterial, pwep->KeyLength);
+			psecuritypriv->dot11DefKeylen[wep_key_idx]=pwep->KeyLength;
+			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 == NULL) {
+				/* 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); */
+					}
+
+					/* DEBUG_ERR((" param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+					DBG_871X(" ~~~~set sta key:unicastkey\n");
+
+					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;
+						/* DEBUG_ERR((" param->u.crypt.key_len =%d\n", param->u.crypt.key_len)); */
+						DBG_871X(" ~~~~set sta key:groupkey\n");
+
+						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;
+						DBG_871X(" ~~~~set sta key:IGKT\n");
+					}
+				}
+			}
+
+			pbcmc_sta =rtw_get_bcmc_stainfo(padapter);
+			if (pbcmc_sta == NULL)
+			{
+				/* 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((u8 *)pwep);
+	return ret;
+}
+
+static int rtw_set_wpa_ie(struct adapter *padapter, char *pie, unsigned short ielen)
+{
+	u8 *buf = NULL, *pos = 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 == NULL)) {
+		_clr_fwstate_(&padapter->mlmepriv, WIFI_UNDER_WPS);
+		if (pie == NULL)
+			return ret;
+		else
+			return -EINVAL;
+	}
+
+	if (ielen)
+	{
+		buf = rtw_zmalloc(ielen);
+		if (buf == NULL) {
+			ret =  -ENOMEM;
+			goto exit;
+		}
+
+		memcpy(buf, pie , ielen);
+
+		/* dump */
+		{
+			int i;
+			DBG_871X("\n wpa_ie(length:%d):\n", ielen);
+			for (i = 0;i<ielen;i =i+8)
+				DBG_871X("0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n", buf[i], buf[i+1], buf[i+2], buf[i+3], buf[i+4], buf[i+5], buf[i+6], buf[i+7]);
+		}
+
+		pos = buf;
+		if (ielen < RSN_HEADER_LEN) {
+			RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("Ie len too short %d\n", ielen));
+			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 == _VENDOR_SPECIFIC_IE_) && (!memcmp(&buf[cnt+2], wps_oui, 4)))
+				{
+					DBG_871X("SET WPS_IE\n");
+
+					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);
+
+	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+		 ("rtw_set_wpa_ie: pairwise_cipher = 0x%08x padapter->securitypriv.ndisencryptstatus =%d padapter->securitypriv.ndisauthtype =%d\n",
+		  pairwise_cipher, padapter->securitypriv.ndisencryptstatus, padapter->securitypriv.ndisauthtype));
+
+exit:
+
+	kfree(buf);
+
+	return ret;
+}
+
+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, vht_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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("cmd_code =%x\n", info->cmd));
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED|WIFI_ADHOC_MASTER_STATE) == true) {
+		/* 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) == true)
+		{
+			if (ht_cap == true)
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11b");
+		}
+		else if ((rtw_is_cckrates_included((u8 *)prates)) == true)
+		{
+			if (ht_cap == true)
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bgn");
+			else
+				snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11bg");
+		}
+		else
+		{
+			if (pcur_bss->Configuration.DSConfig > 14)
+			{
+				if (vht_cap == true)
+					snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11AC");
+				else if (ht_cap == true)
+					snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11an");
+				else
+					snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11a");
+			}
+			else
+			{
+				if (ht_cap == true)
+					snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11gn");
+				else
+					snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11g");
+			}
+		}
+	}
+	else
+	{
+		/* prates = &padapter->registrypriv.dev_network.SupportedRates; */
+		/* snprintf(wrqu->name, IFNAMSIZ, "IEEE 802.11g"); */
+		snprintf(wrqu->name, IFNAMSIZ, "unassociated");
+	}
+	return 0;
+}
+
+static int rtw_wx_set_freq(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_, ("+rtw_wx_set_freq\n"));
+
+	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) == true)
+	{
+		/* 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_INFRASTRUCTURE networkType ;
+	int ret = 0;
+
+	if (_FAIL == rtw_pwr_wakeup(padapter)) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	if (padapter->hw_init_completed ==false) {
+		ret = -EPERM;
+		goto exit;
+	}
+
+	switch (wrqu->mode)
+	{
+		case IW_MODE_AUTO:
+			networkType = Ndis802_11AutoUnknown;
+			DBG_871X("set_mode = IW_MODE_AUTO\n");
+			break;
+		case IW_MODE_ADHOC:
+			networkType = Ndis802_11IBSS;
+			DBG_871X("set_mode = IW_MODE_ADHOC\n");
+			break;
+		case IW_MODE_MASTER:
+			networkType = Ndis802_11APMode;
+			DBG_871X("set_mode = IW_MODE_MASTER\n");
+                        /* rtw_setopmode_cmd(padapter, networkType, true); */
+			break;
+		case IW_MODE_INFRA:
+			networkType = Ndis802_11Infrastructure;
+			DBG_871X("set_mode = IW_MODE_INFRA\n");
+			break;
+
+		default :
+			ret = -EINVAL;;
+			RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("\n Mode: %s is not supported \n", iw_operation_mode[wrqu->mode]));
+			goto exit;
+	}
+
+/*
+	if (Ndis802_11APMode == networkType)
+	{
+		rtw_setopmode_cmd(padapter, networkType, true);
+	}
+	else
+	{
+		rtw_setopmode_cmd(padapter, Ndis802_11AutoUnknown, true);
+	}
+*/
+
+	if (rtw_set_802_11_infrastructure_mode(padapter, networkType) ==false) {
+
+		ret = -EPERM;
+		goto exit;
+
+	}
+
+	rtw_setopmode_cmd(padapter, networkType, true);
+
+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);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, (" rtw_wx_get_mode\n"));
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true)
+	{
+		wrqu->mode = IW_MODE_INFRA;
+	}
+	else if  ((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true) ||
+		       (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true))
+
+	{
+		wrqu->mode = IW_MODE_ADHOC;
+	}
+	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+	{
+		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         intReturn = 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 };
+
+	/*
+        There are the BSSID information in the bssid.sa_data array.
+        If cmd is IW_PMKSA_FLUSH, it means the wpa_suppplicant 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.
+        */
+
+	memcpy(strIssueBssid, pPMK->bssid.sa_data, ETH_ALEN);
+        if (pPMK->cmd == IW_PMKSA_ADD)
+        {
+                DBG_871X("[rtw_wx_set_pmkid] IW_PMKSA_ADD!\n");
+                if (!memcmp(strIssueBssid, strZeroMacAddress, ETH_ALEN))
+                {
+                    return(intReturn);
+                }
+                else
+                {
+                    intReturn = 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. */
+
+                                DBG_871X("[rtw_wx_set_pmkid] BSSID exists in the PMKList.\n");
+
+				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 */
+                    DBG_871X("[rtw_wx_set_pmkid] Use the new entry index = %d for this PMKID.\n",
+                            psecuritypriv->PMKIDIndex);
+
+	            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)
+        {
+                DBG_871X("[rtw_wx_set_pmkid] IW_PMKSA_REMOVE!\n");
+                intReturn = 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)
+        {
+            DBG_871X("[rtw_wx_set_pmkid] IW_PMKSA_FLUSH!\n");
+            memset(&psecuritypriv->PMKIDList[ 0 ], 0x00, sizeof(RT_PMKID_LIST) * NUM_PMKID_CACHE);
+            psecuritypriv->PMKIDIndex = 0;
+            intReturn = true;
+        }
+	return intReturn;
+}
+
+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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_range. cmd_code =%x\n", info->cmd));
+
+	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 = 256 - 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] = 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;
+
+/*  Commented by Albert 2009/10/13 */
+/*  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 choosen 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_AUTHENTICATION_MODE	authmode;
+
+	rtw_ps_deny(padapter, PS_DENY_JOIN);
+	if (_FAIL == rtw_pwr_wakeup(padapter))
+	{
+		ret = -1;
+		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 = get_next(phead);
+
+	while (1) {
+		if (phead == pmlmepriv->pscanned)
+			break;
+
+		pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+
+		pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+		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) == false) {
+		ret = -1;
+		goto exit;
+	}
+
+exit:
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_JOIN);
+
+	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);
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_wap\n"));
+
+	if  (((check_fwstate(pmlmepriv, _FW_LINKED)) == true) ||
+			((check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) == true) ||
+			((check_fwstate(pmlmepriv, WIFI_AP_STATE)) == true))
+	{
+
+		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;
+	u16 reason;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct iw_mlme *mlme = (struct iw_mlme *) extra;
+
+
+	if (mlme == NULL)
+		return -1;
+
+	DBG_871X("%s\n", __func__);
+
+	reason = mlme->reason_code;
+
+	DBG_871X("%s, cmd =%d, reason =%d\n", __func__, mlme->cmd, reason);
+
+	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];
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_set_scan\n"));
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d\n", __func__, __LINE__);
+	#endif
+
+	rtw_ps_deny(padapter, PS_DENY_SCAN);
+	if (_FAIL == rtw_pwr_wakeup(padapter))
+	{
+		ret = -1;
+		goto exit;
+	}
+
+	if (padapter->bDriverStopped) {
+		DBG_871X("bDriverStopped =%d\n", padapter->bDriverStopped);
+		ret = -1;
+		goto exit;
+	}
+
+	if (!padapter->bup) {
+		ret = -1;
+		goto exit;
+	}
+
+	if (padapter->hw_init_completed ==false) {
+		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 == true)
+	{
+		indicate_wx_scan_complete_event(padapter);
+		goto exit;
+	}
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY|_FW_UNDER_LINKING) == true)
+	{
+		indicate_wx_scan_complete_event(padapter);
+		goto exit;
+	}
+
+	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;
+
+			DBG_871X("IW_SCAN_THIS_ESSID, ssid =%s, len =%d\n", req->essid, req->essid_len);
+
+			spin_lock_bh(&pmlmepriv->lock);
+
+			_status = rtw_sitesurvey_cmd(padapter, ssid, 1, NULL, 0);
+
+			spin_unlock_bh(&pmlmepriv->lock);
+
+		}
+		else if (req->scan_type == IW_SCAN_TYPE_PASSIVE)
+		{
+			DBG_871X("rtw_wx_set_scan, req->scan_type == IW_SCAN_TYPE_PASSIVE\n");
+		}
+
+	}
+	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;
+
+		/* DBG_871X("%s COMBO_SCAN header is recognized\n", __func__); */
+
+		while (len >= 1) {
+			section = *(pos++); len-= 1;
+
+			switch (section) {
+				case WEXT_CSCAN_SSID_SECTION:
+					/* DBG_871X("WEXT_CSCAN_SSID_SECTION\n"); */
+					if (len < 1) {
+						len = 0;
+						break;
+					}
+
+					sec_len = *(pos++); len-= 1;
+
+					if (sec_len>0 && sec_len<=len) {
+						ssid[ssid_index].SsidLength = sec_len;
+						memcpy(ssid[ssid_index].Ssid, pos, ssid[ssid_index].SsidLength);
+						/* DBG_871X("%s COMBO_SCAN with specific ssid:%s, %d\n", __func__ */
+						/* 	, ssid[ssid_index].Ssid, ssid[ssid_index].SsidLength); */
+						ssid_index++;
+					}
+
+					pos+=sec_len; len-=sec_len;
+					break;
+
+
+				case WEXT_CSCAN_CHANNEL_SECTION:
+					/* DBG_871X("WEXT_CSCAN_CHANNEL_SECTION\n"); */
+					pos+= 1; len-= 1;
+					break;
+				case WEXT_CSCAN_ACTV_DWELL_SECTION:
+					/* DBG_871X("WEXT_CSCAN_ACTV_DWELL_SECTION\n"); */
+					pos+=2; len-=2;
+					break;
+				case WEXT_CSCAN_PASV_DWELL_SECTION:
+					/* DBG_871X("WEXT_CSCAN_PASV_DWELL_SECTION\n"); */
+					pos+=2; len-=2;
+					break;
+				case WEXT_CSCAN_HOME_DWELL_SECTION:
+					/* DBG_871X("WEXT_CSCAN_HOME_DWELL_SECTION\n"); */
+					pos+=2; len-=2;
+					break;
+				case WEXT_CSCAN_TYPE_SECTION:
+					/* DBG_871X("WEXT_CSCAN_TYPE_SECTION\n"); */
+					pos+= 1; len-= 1;
+					break;
+				default:
+					/* DBG_871X("Unknown CSCAN section %c\n", section); */
+					len = 0; /*  stop parsing */
+			}
+			/* DBG_871X("len:%d\n", len); */
+
+		}
+
+		/* jeff: it has still some scan paramater 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 == false)
+		ret = -1;
+
+exit:
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_SCAN);
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d return %d\n", __func__, __LINE__, ret);
+	#endif
+
+	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;
+	sint wait_status;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_scan\n"));
+	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, (" Start of Query SIOCGIWSCAN .\n"));
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d\n", __func__, __LINE__);
+	#endif
+
+	if (adapter_to_pwrctl(padapter)->brfoffbyhw && padapter->bDriverStopped)
+	{
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	wait_status = _FW_UNDER_SURVEY | _FW_UNDER_LINKING;
+
+	if (check_fwstate(pmlmepriv, wait_status))
+		return -EAGAIN;
+
+	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	phead = get_list_head(queue);
+	plist = get_next(phead);
+
+	while (1)
+	{
+		if (phead == plist)
+			break;
+
+		if ((stop - ev) < SCAN_ITEM_SIZE) {
+			ret = -E2BIG;
+			break;
+		}
+
+		pnetwork = LIST_CONTAINOR(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
+			&& rtw_mlme_band_check(padapter, pnetwork->network.Configuration.DSConfig) == true
+			&& true == rtw_validate_ssid(&(pnetwork->network.Ssid))
+		)
+		{
+			ev =translate_scan(padapter, a, pnetwork, ev, stop);
+		}
+
+		plist = get_next(plist);
+
+	}
+
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	wrqu->data.length = ev-extra;
+	wrqu->data.flags = 0;
+
+exit:
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d return %d\n", __func__, __LINE__, ret);
+	#endif
+
+	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_AUTHENTICATION_MODE authmode;
+	struct ndis_802_11_ssid ndis_ssid;
+	u8 *dst_ssid, *src_ssid;
+
+	uint ret = 0, len;
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d\n", __func__, __LINE__);
+	#endif
+
+	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+		 ("+rtw_wx_set_essid: fw_state = 0x%08x\n", get_fwstate(pmlmepriv)));
+
+	rtw_ps_deny(padapter, PS_DENY_JOIN);
+	if (_FAIL == rtw_pwr_wakeup(padapter))
+	{
+		ret = -1;
+		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;
+	DBG_871X("=>%s\n", __func__);
+	if (wrqu->essid.flags && wrqu->essid.length)
+	{
+		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? wrqu->essid.length : IW_ESSID_MAX_SIZE;
+
+		if (wrqu->essid.length != 33)
+			DBG_871X("ssid =%s, len =%d\n", extra, wrqu->essid.length);
+
+		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;
+
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("rtw_wx_set_essid: ssid =[%s]\n", src_ssid));
+		spin_lock_bh(&queue->lock);
+		phead = get_list_head(queue);
+		pmlmepriv->pscanned = get_next(phead);
+
+		while (1) {
+			if (phead == pmlmepriv->pscanned)
+			{
+			        RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_warning_,
+					 ("rtw_wx_set_essid: scan_q is empty, set ssid to check if scanning again!\n"));
+
+				break;
+			}
+
+			pnetwork = LIST_CONTAINOR(pmlmepriv->pscanned, struct wlan_network, list);
+
+			pmlmepriv->pscanned = get_next(pmlmepriv->pscanned);
+
+			dst_ssid = pnetwork->network.Ssid.Ssid;
+
+			RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+				 ("rtw_wx_set_essid: dst_ssid =%s\n",
+				  pnetwork->network.Ssid.Ssid));
+
+			if ((!memcmp(dst_ssid, src_ssid, ndis_ssid.SsidLength)) &&
+				(pnetwork->network.Ssid.SsidLength ==ndis_ssid.SsidLength))
+			{
+				RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+					 ("rtw_wx_set_essid: find match, set infra mode\n"));
+
+				if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)
+				{
+					if (pnetwork->network.InfrastructureMode != pmlmepriv->cur_network.network.InfrastructureMode)
+						continue;
+				}
+
+				if (rtw_set_802_11_infrastructure_mode(padapter, pnetwork->network.InfrastructureMode) == false)
+				{
+					ret = -1;
+					spin_unlock_bh(&queue->lock);
+					goto exit;
+				}
+
+				break;
+			}
+		}
+		spin_unlock_bh(&queue->lock);
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_,
+			 ("set ssid: set_802_11_auth. mode =%d\n", authmode));
+		rtw_set_802_11_authentication_mode(padapter, authmode);
+		/* set_802_11_encryption_mode(padapter, padapter->securitypriv.ndisencryptstatus); */
+		if (rtw_set_802_11_ssid(padapter, &ndis_ssid) == false) {
+			ret = -1;
+			goto exit;
+		}
+	}
+
+exit:
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_JOIN);
+
+	DBG_871X("<=%s, ret %d\n", __func__, ret);
+
+	#ifdef DBG_IOCTL
+	DBG_871X("DBG_IOCTL %s:%d return %d\n", __func__, __LINE__, ret);
+	#endif
+
+	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, ret = 0;
+	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;
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("rtw_wx_get_essid\n"));
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == true) ||
+	      (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true))
+	{
+		len = pcur_bss->Ssid.SsidLength;
+
+		wrqu->essid.length = len;
+
+		memcpy(extra, pcur_bss->Ssid.Ssid, len);
+
+		wrqu->essid.flags = 1;
+	}
+	else
+	{
+		ret = -1;
+		goto exit;
+	}
+
+exit:
+	return ret;
+}
+
+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};
+
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, (" rtw_wx_set_rate\n"));
+	RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("target_rate = %d, fixed = %d\n", target_rate, fixed));
+
+	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;
+		}
+
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("datarate_inx =%d\n", datarates[i]));
+	}
+
+	if (rtw_setdatarate_cmd(padapter, datarates) != _SUCCESS) {
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_err_, ("rtw_wx_set_rate Fail!!!\n"));
+		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;
+	}
+
+	DBG_871X("%s, rts_thresh =%d\n", __func__, padapter->registrypriv.rts_thresh);
+
+	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);
+
+	DBG_871X("%s, rts_thresh =%d\n", __func__, padapter->registrypriv.rts_thresh);
+
+	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;
+	}
+
+	DBG_871X("%s, frag_len =%d\n", __func__, padapter->xmitpriv.frag_len);
+
+	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);
+
+	DBG_871X("%s, frag_len =%d\n", __func__, padapter->xmitpriv.frag_len);
+
+	wrqu->frag.value = padapter->xmitpriv.frag_len;
+	wrqu->frag.fixed = 0;	/* no auto select */
+	/* wrqu->frag.disabled = (wrqu->frag.value == DEFAULT_FRAG_THRESHOLD); */
+
+	return 0;
+}
+
+static int rtw_wx_get_retry(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->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_AUTHENTICATION_MODE authmode;
+
+	struct iw_point *erq = &(wrqu->encoding);
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	DBG_871X("+rtw_wx_set_enc, flags = 0x%x\n", erq->flags);
+
+	memset(&wep, 0, sizeof(struct ndis_802_11_wep));
+
+	key = erq->flags & IW_ENCODE_INDEX;
+
+	if (erq->flags & IW_ENCODE_DISABLED)
+	{
+		DBG_871X("EncryptionDisabled\n");
+		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;
+		DBG_871X("rtw_wx_set_enc, key =%d\n", key);
+	}
+
+	/* set authentication mode */
+	if (erq->flags & IW_ENCODE_OPEN)
+	{
+		DBG_871X("rtw_wx_set_enc():IW_ENCODE_OPEN\n");
+		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)
+	{
+		DBG_871X("rtw_wx_set_enc():IW_ENCODE_RESTRICTED\n");
+		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
+	{
+		DBG_871X("rtw_wx_set_enc():erq->flags = 0x%x\n", erq->flags);
+
+		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 + FIELD_OFFSET(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;
+
+			DBG_871X("(keyindex_provided == 1), keyid =%d, key_len =%d\n", key, padapter->securitypriv.dot11DefKeylen[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) == false) {
+		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, ret = 0;
+	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) != true)
+		 {
+		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;
+
+	/* if (padapter->securitypriv.ndisauthtype == Ndis802_11AuthModeOpen) */
+	/*  */
+	/*       erq->flags |= IW_ENCODE_OPEN; */
+	/*  */
+
+	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 ret;
+}
+
+static int rtw_wx_get_power(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->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);
+			DBG_871X("%s...call rtw_indicate_disconnect\n ", __func__);
+			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 = 0;
+
+	param_len = sizeof(struct ieee_param) + pext->key_len;
+	param = (struct ieee_param *)rtw_malloc(param_len);
+	if (param == NULL)
+		return -1;
+
+	memset(param, 0, param_len);
+
+	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;
+	case IW_ENCODE_ALG_AES_CMAC:
+		alg_name = "BIP";
+		break;
+	default:
+		ret = -1;
+		goto exit;
+	}
+
+	strncpy((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)
+		|| (pext->ext_flags & IW_ENCODE_ALG_AES_CMAC)
+	))
+	{
+		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 + 1, pext + 1, pext->key_len); */
+		memcpy(param->u.crypt.key, pext + 1, pext->key_len);
+	}
+
+	if (pencoding->flags & IW_ENCODE_DISABLED)
+	{
+		/* todo: remove key */
+		/* remove = 1; */
+	}
+
+	ret =  wpa_set_encryption(dev, param, param_len);
+
+exit:
+	kfree((u8 *)param);
+
+	return ret;
+}
+
+
+static int rtw_wx_get_nick(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 security_priv *psecuritypriv = &padapter->securitypriv; */
+
+	if (extra)
+	{
+		wrqu->data.length = 14;
+		wrqu->data.flags = 1;
+		memcpy(extra, "<WIFI@REALTEK>", 14);
+	}
+	return 0;
+}
+
+static int rtw_wx_read32(struct net_device *dev,
+                            struct iw_request_info *info,
+                            union iwreq_data *wrqu, char *extra)
+{
+	struct adapter *padapter;
+	struct iw_point *p;
+	u16 len;
+	u32 addr;
+	u32 data32;
+	u32 bytes;
+	u8 *ptmp;
+	int ret;
+
+
+	ret = 0;
+	padapter = (struct adapter *)rtw_netdev_priv(dev);
+	p = &wrqu->data;
+	len = p->length;
+	if (0 == len)
+		return -EINVAL;
+
+	ptmp = (u8 *)rtw_malloc(len);
+	if (NULL == ptmp)
+		return -ENOMEM;
+
+	if (copy_from_user(ptmp, p->pointer, len)) {
+		ret = -EFAULT;
+		goto exit;
+	}
+
+	bytes = 0;
+	addr = 0;
+	sscanf(ptmp, "%d,%x", &bytes, &addr);
+
+	switch (bytes) {
+		case 1:
+			data32 = rtw_read8(padapter, addr);
+			sprintf(extra, "0x%02X", data32);
+			break;
+		case 2:
+			data32 = rtw_read16(padapter, addr);
+			sprintf(extra, "0x%04X", data32);
+			break;
+		case 4:
+			data32 = rtw_read32(padapter, addr);
+			sprintf(extra, "0x%08X", data32);
+			break;
+		default:
+			DBG_871X(KERN_INFO "%s: usage> read [bytes],[address(hex)]\n", __func__);
+			ret = -EINVAL;
+			goto exit;
+	}
+	DBG_871X(KERN_INFO "%s: addr = 0x%08X data =%s\n", __func__, addr, extra);
+
+exit:
+	kfree(ptmp);
+
+	return 0;
+}
+
+static int rtw_wx_write32(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 addr;
+	u32 data32;
+	u32 bytes;
+
+
+	bytes = 0;
+	addr = 0;
+	data32 = 0;
+	sscanf(extra, "%d,%x,%x", &bytes, &addr, &data32);
+
+	switch (bytes) {
+		case 1:
+			rtw_write8(padapter, addr, (u8)data32);
+			DBG_871X(KERN_INFO "%s: addr = 0x%08X data = 0x%02X\n", __func__, addr, (u8)data32);
+			break;
+		case 2:
+			rtw_write16(padapter, addr, (u16)data32);
+			DBG_871X(KERN_INFO "%s: addr = 0x%08X data = 0x%04X\n", __func__, addr, (u16)data32);
+			break;
+		case 4:
+			rtw_write32(padapter, addr, data32);
+			DBG_871X(KERN_INFO "%s: addr = 0x%08X data = 0x%08X\n", __func__, addr, data32);
+			break;
+		default:
+			DBG_871X(KERN_INFO "%s: usage> write [bytes],[address(hex)],[data(hex)]\n", __func__);
+			return -EINVAL;
+	}
+
+	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;
+	addr = *((u32*)extra + 1);
+	data32 = rtw_hal_read_rfreg(padapter, path, 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;
+	addr = *((u32*)extra + 1);
+	data32 = *((u32*)extra + 2);
+/* 	DBG_871X("%s: path =%d addr = 0x%02x data = 0x%05x\n", __func__, path, addr, data32); */
+	rtw_hal_write_rfreg(padapter, path, addr, 0xFFFFF, data32);
+
+	return 0;
+}
+
+static int rtw_wx_priv_null(struct net_device *dev, struct iw_request_info *a,
+		 union iwreq_data *wrqu, char *b)
+{
+	return -1;
+}
+
+static int dummy(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); */
+
+	/* DBG_871X("cmd_code =%x, fwstate = 0x%x\n", a->cmd, get_fwstate(pmlmepriv)); */
+
+	return -1;
+
+}
+
+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 (_SUCCESS == rtw_set_chplan_cmd(padapter, channel_plan_req, 1, 1)) {
+		DBG_871X("%s set channel_plan = 0x%02X\n", __func__, channel_plan_req);
+	} else
+		return -EPERM;
+
+	return 0;
+}
+
+static int rtw_wx_set_mtk_wps_probe_ie(struct net_device *dev,
+		struct iw_request_info *a,
+		union iwreq_data *wrqu, char *b)
+{
+	return 0;
+}
+
+static int rtw_wx_get_sensitivity(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *buf)
+{
+	return 0;
+}
+
+static int rtw_wx_set_mtk_wps_ie(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	return 0;
+}
+
+/*
+typedef int (*iw_handler)(struct net_device *dev, struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra);
+*/
+/*
+ *For all data larger than 16 octets, we need to use a
+ *pointer to memory allocated in user space.
+ */
+static  int rtw_drvext_hdl(struct net_device *dev, struct iw_request_info *info,
+						union iwreq_data *wrqu, char *extra)
+{
+	return 0;
+}
+
+static int rtw_mp_ioctl_hdl(struct net_device *dev, struct iw_request_info *info,
+						union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	return ret;
+}
+
+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;
+
+	DBG_871X("+rtw_get_aplist_info\n");
+
+	if ((padapter->bDriverStopped) || (pdata == NULL))
+	{
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	while ((check_fwstate(pmlmepriv, (_FW_UNDER_SURVEY|_FW_UNDER_LINKING))) == true)
+	{
+		msleep(30);
+		cnt++;
+		if (cnt > 100)
+			break;
+	}
+
+
+	/* pdata->length = 0;? */
+	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 = get_next(phead);
+
+	while (1)
+	{
+		if (phead == plist)
+			break;
+
+
+		pnetwork = LIST_CONTAINOR(plist, struct wlan_network, list);
+
+		/* if (hwaddr_aton_i(pdata->pointer, bssid)) */
+		if (hwaddr_aton_i(data, bssid))
+		{
+			DBG_871X("Invalid BSSID '%s'.\n", (u8 *)data);
+			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 */
+		{
+			DBG_871X("BSSID:" MAC_FMT "\n", MAC_ARG(bssid));
+
+			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 = get_next(plist);
+
+	}
+
+	spin_unlock_bh(&(pmlmepriv->scanned_queue.lock));
+
+	if (pdata->length>=34)
+	{
+		if (copy_to_user((u8 __force __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 == NULL))
+	{
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	selector = *pdata;
+	if (selector < 3 && selector >= 0) {
+		padapter->pid[selector] = *(pdata+1);
+		DBG_871X("%s set pid[%d]=%d\n", __func__, selector , padapter->pid[selector]);
+	}
+	else
+		DBG_871X("%s selector %d error\n", __func__, selector);
+
+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;
+        unsigned int uintRet = 0;
+
+	if ((true == padapter->bDriverStopped) ||(true ==padapter->bSurpriseRemoved) || (NULL == pdata))
+	{
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	uintRet = copy_from_user((void*) &u32wps_start, pdata->pointer, 4);
+	if (u32wps_start == 0)
+	{
+		u32wps_start = *extra;
+	}
+
+	DBG_871X("[%s] wps_start = %d\n", __func__, u32wps_start);
+
+#ifdef CONFIG_INTEL_WIDI
+	process_intel_widi_wps_status(padapter, u32wps_start);
+#endif /* CONFIG_INTEL_WIDI */
+
+exit:
+
+	return ret;
+
+}
+
+static int rtw_p2p_set(struct net_device *dev,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra)
+{
+
+	int ret = 0;
+
+	return ret;
+
+}
+
+static int rtw_p2p_get(struct net_device *dev,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra)
+{
+
+	int ret = 0;
+
+	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;
+
+	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.ifname;
+
+		strncpy(rereg_priv->old_ifname, reg_ifname, IFNAMSIZ);
+		rereg_priv->old_ifname[IFNAMSIZ-1] = 0;
+	}
+
+	/* DBG_871X("%s wrqu->data.length:%d\n", __func__, wrqu->data.length); */
+	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;
+	}
+
+	DBG_871X("%s new_ifname:%s\n", __func__, new_ifname);
+	if (0 != (ret = rtw_change_ifname(padapter, new_ifname))) {
+		goto exit;
+	}
+
+	strncpy(rereg_priv->old_ifname, new_ifname, IFNAMSIZ);
+	rereg_priv->old_ifname[IFNAMSIZ-1] = 0;
+
+	if (!memcmp(new_ifname, "disable%d", 9)) {
+
+		DBG_871X("%s disable\n", __func__);
+		/*  free network queue for Android's timming issue */
+		rtw_free_network_queue(padapter, true);
+
+		/*  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 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;
+	u32 extra_arg, *pdata, val32;
+	struct sta_info *psta;
+	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:
+					DBG_871X("rtw_read8(0x%x) = 0x%02x\n", arg, rtw_read8(padapter, arg));
+					break;
+				case 2:
+					DBG_871X("rtw_read16(0x%x) = 0x%04x\n", arg, rtw_read16(padapter, arg));
+					break;
+				case 4:
+					DBG_871X("rtw_read32(0x%x) = 0x%08x\n", arg, rtw_read32(padapter, arg));
+					break;
+			}
+			break;
+		case 0x71:/* write_reg */
+			switch (minor_cmd)
+			{
+				case 1:
+					rtw_write8(padapter, arg, extra_arg);
+					DBG_871X("rtw_write8(0x%x) = 0x%02x\n", arg, rtw_read8(padapter, arg));
+					break;
+				case 2:
+					rtw_write16(padapter, arg, extra_arg);
+					DBG_871X("rtw_write16(0x%x) = 0x%04x\n", arg, rtw_read16(padapter, arg));
+					break;
+				case 4:
+					rtw_write32(padapter, arg, extra_arg);
+					DBG_871X("rtw_write32(0x%x) = 0x%08x\n", arg, rtw_read32(padapter, arg));
+					break;
+			}
+			break;
+		case 0x72:/* read_bb */
+			DBG_871X("read_bbreg(0x%x) = 0x%x\n", arg, rtw_hal_read_bbreg(padapter, arg, 0xffffffff));
+			break;
+		case 0x73:/* write_bb */
+			rtw_hal_write_bbreg(padapter, arg, 0xffffffff, extra_arg);
+			DBG_871X("write_bbreg(0x%x) = 0x%x\n", arg, rtw_hal_read_bbreg(padapter, arg, 0xffffffff));
+			break;
+		case 0x74:/* read_rf */
+			DBG_871X("read RF_reg path(0x%02x), offset(0x%x), value(0x%08x)\n", minor_cmd, arg, rtw_hal_read_rfreg(padapter, minor_cmd, arg, 0xffffffff));
+			break;
+		case 0x75:/* write_rf */
+			rtw_hal_write_rfreg(padapter, minor_cmd, arg, 0xffffffff, extra_arg);
+			DBG_871X("write RF_reg path(0x%02x), offset(0x%x), value(0x%08x)\n", minor_cmd, arg, rtw_hal_read_rfreg(padapter, minor_cmd, arg, 0xffffffff));
+			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;
+					padapter->recvpriv.signal_strength_dbg =extra_arg;
+					break;
+			}
+			break;
+		case 0x78: /* IOL test */
+			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;
+
+				DBG_871X("%s set RESP_TXAGC to %s %u\n", __func__, sign?"minus":"plus", value);
+
+				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:
+					DBG_871X("fwstate = 0x%x\n", get_fwstate(pmlmepriv));
+					break;
+				case 0x01:
+					DBG_871X("minor_cmd 0x%x\n", minor_cmd);
+					break;
+				case 0x02:
+					DBG_871X("pmlmeinfo->state = 0x%x\n", pmlmeinfo->state);
+					DBG_871X("DrvBcnEarly =%d\n", pmlmeext->DrvBcnEarly);
+					DBG_871X("DrvBcnTimeOut =%d\n", pmlmeext->DrvBcnTimeOut);
+					break;
+				case 0x03:
+					DBG_871X("qos_option =%d\n", pmlmepriv->qospriv.qos_option);
+					DBG_871X("ht_option =%d\n", pmlmepriv->htpriv.ht_option);
+					break;
+				case 0x04:
+					DBG_871X("cur_ch =%d\n", pmlmeext->cur_channel);
+					DBG_871X("cur_bw =%d\n", pmlmeext->cur_bwmode);
+					DBG_871X("cur_ch_off =%d\n", pmlmeext->cur_ch_offset);
+
+					DBG_871X("oper_ch =%d\n", rtw_get_oper_ch(padapter));
+					DBG_871X("oper_bw =%d\n", rtw_get_oper_bw(padapter));
+					DBG_871X("oper_ch_offet =%d\n", rtw_get_oper_choffset(padapter));
+
+					break;
+				case 0x05:
+					psta = rtw_get_stainfo(pstapriv, cur_network->network.MacAddress);
+					if (psta)
+					{
+						int i;
+						struct recv_reorder_ctrl *preorder_ctrl;
+
+						DBG_871X("SSID =%s\n", cur_network->network.Ssid.Ssid);
+						DBG_871X("sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+						DBG_871X("cur_channel =%d, cur_bwmode =%d, cur_ch_offset =%d\n", pmlmeext->cur_channel, pmlmeext->cur_bwmode, pmlmeext->cur_ch_offset);
+						DBG_871X("rtsen =%d, cts2slef =%d\n", psta->rtsen, psta->cts2self);
+						DBG_871X("state = 0x%x, aid =%d, macid =%d, raid =%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+						DBG_871X("qos_en =%d, ht_en =%d, init_rate =%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+						DBG_871X("bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m);
+						DBG_871X("ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+						DBG_871X("agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+
+						for (i = 0;i<16;i++)
+						{
+							preorder_ctrl = &psta->recvreorder_ctrl[i];
+							if (preorder_ctrl->enable)
+							{
+								DBG_871X("tid =%d, indicate_seq =%d\n", i, preorder_ctrl->indicate_seq);
+							}
+						}
+
+					}
+					else
+					{
+						DBG_871X("can't get sta's macaddr, cur_network's macaddr:" MAC_FMT "\n", MAC_ARG(cur_network->network.MacAddress));
+					}
+					break;
+				case 0x06:
+					{
+						u32 ODMFlag;
+						rtw_hal_get_hwreg(padapter, HW_VAR_DM_FLAG, (u8 *)(&ODMFlag));
+						DBG_871X("(B)DMFlag = 0x%x, arg = 0x%x\n", ODMFlag, arg);
+						ODMFlag = (u32)(0x0f&arg);
+						DBG_871X("(A)DMFlag = 0x%x\n", ODMFlag);
+						rtw_hal_set_hwreg(padapter, HW_VAR_DM_FLAG, (u8 *)(&ODMFlag));
+					}
+					break;
+				case 0x07:
+					DBG_871X("bSurpriseRemoved =%d, bDriverStopped =%d\n",
+						padapter->bSurpriseRemoved, padapter->bDriverStopped);
+					break;
+				case 0x08:
+					{
+						DBG_871X("minor_cmd 0x%x\n", minor_cmd);
+					}
+					break;
+				case 0x09:
+					{
+						int i, j;
+						struct list_head	*plist, *phead;
+						struct recv_reorder_ctrl *preorder_ctrl;
+
+						DBG_871X("sta_dz_bitmap = 0x%x, tim_bitmap = 0x%x\n", pstapriv->sta_dz_bitmap, pstapriv->tim_bitmap);
+
+						spin_lock_bh(&pstapriv->sta_hash_lock);
+
+						for (i = 0; i< NUM_STA; i++)
+						{
+							phead = &(pstapriv->sta_hash[i]);
+							plist = get_next(phead);
+
+							while (phead != plist)
+							{
+								psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+								plist = get_next(plist);
+
+								if (extra_arg == psta->aid)
+								{
+									DBG_871X("sta's macaddr:" MAC_FMT "\n", MAC_ARG(psta->hwaddr));
+									DBG_871X("rtsen =%d, cts2slef =%d\n", psta->rtsen, psta->cts2self);
+									DBG_871X("state = 0x%x, aid =%d, macid =%d, raid =%d\n", psta->state, psta->aid, psta->mac_id, psta->raid);
+									DBG_871X("qos_en =%d, ht_en =%d, init_rate =%d\n", psta->qos_option, psta->htpriv.ht_option, psta->init_rate);
+									DBG_871X("bwmode =%d, ch_offset =%d, sgi_20m =%d, sgi_40m =%d\n", psta->bw_mode, psta->htpriv.ch_offset, psta->htpriv.sgi_20m, psta->htpriv.sgi_40m);
+									DBG_871X("ampdu_enable = %d\n", psta->htpriv.ampdu_enable);
+									DBG_871X("agg_enable_bitmap =%x, candidate_tid_bitmap =%x\n", psta->htpriv.agg_enable_bitmap, psta->htpriv.candidate_tid_bitmap);
+									DBG_871X("capability = 0x%x\n", psta->capability);
+									DBG_871X("flags = 0x%x\n", psta->flags);
+									DBG_871X("wpa_psk = 0x%x\n", psta->wpa_psk);
+									DBG_871X("wpa2_group_cipher = 0x%x\n", psta->wpa2_group_cipher);
+									DBG_871X("wpa2_pairwise_cipher = 0x%x\n", psta->wpa2_pairwise_cipher);
+									DBG_871X("qos_info = 0x%x\n", psta->qos_info);
+									DBG_871X("dot118021XPrivacy = 0x%x\n", psta->dot118021XPrivacy);
+
+
+
+									for (j = 0;j<16;j++)
+									{
+										preorder_ctrl = &psta->recvreorder_ctrl[j];
+										if (preorder_ctrl->enable)
+										{
+											DBG_871X("tid =%d, indicate_seq =%d\n", j, preorder_ctrl->indicate_seq);
+										}
+									}
+
+								}
+
+							}
+						}
+
+						spin_unlock_bh(&pstapriv->sta_hash_lock);
+
+					}
+					break;
+				case 0x0a:
+					{
+						int max_mac_id = 0;
+						max_mac_id = rtw_search_max_mac_id(padapter);
+						printk("%s ==> max_mac_id = %d\n", __func__, max_mac_id);
+					}
+					break;
+				case 0x0b: /* Enable = 1, Disable = 0 driver control vrtl_carrier_sense. */
+					if (arg == 0) {
+						DBG_871X("disable driver ctrl vcs\n");
+						padapter->driver_vcs_en = 0;
+					}
+					else if (arg == 1) {
+						DBG_871X("enable driver ctrl vcs = %d\n", extra_arg);
+						padapter->driver_vcs_en = 1;
+
+						if (extra_arg>2)
+							padapter->driver_vcs_type = 1;
+						else
+							padapter->driver_vcs_type = extra_arg;
+					}
+					break;
+				case 0x0c:/* dump rx/tx packet */
+					{
+						if (arg == 0) {
+							DBG_871X("dump rx packet (%d)\n", extra_arg);
+							/* pHalData->bDumpRxPkt =extra_arg; */
+							rtw_hal_set_def_var(padapter, HAL_DEF_DBG_DUMP_RXPKT, &(extra_arg));
+						}
+						else if (arg == 1) {
+							DBG_871X("dump tx packet (%d)\n", extra_arg);
+							rtw_hal_set_def_var(padapter, HAL_DEF_DBG_DUMP_TXPKT, &(extra_arg));
+						}
+					}
+					break;
+				case 0x0e:
+					{
+						if (arg == 0) {
+							DBG_871X("disable driver ctrl rx_ampdu_factor\n");
+							padapter->driver_rx_ampdu_factor = 0xFF;
+						}
+						else if (arg == 1) {
+
+							DBG_871X("enable driver ctrl rx_ampdu_factor = %d\n", extra_arg);
+
+							if ((extra_arg & 0x03) > 0x03)
+								padapter->driver_rx_ampdu_factor = 0xFF;
+							else
+								padapter->driver_rx_ampdu_factor = extra_arg;
+						}
+					}
+					break;
+
+				case 0x10:/*  driver version display */
+					dump_drv_version(RTW_DBGDUMP);
+					break;
+				case 0x11:/* dump linked status */
+					{
+						 linked_info_dump(padapter, 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 (pregpriv && (extra_arg == 0 || extra_arg == 1|| extra_arg == 2 || extra_arg == 3))
+					{
+						pregpriv->rx_stbc = extra_arg;
+						DBG_871X("set rx_stbc =%d\n", pregpriv->rx_stbc);
+					}
+					else
+						DBG_871X("get rx_stbc =%d\n", pregpriv->rx_stbc);
+
+				}
+				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 (pregpriv && extra_arg < 3)
+					{
+						pregpriv->ampdu_enable = extra_arg;
+						DBG_871X("set ampdu_enable =%d\n", pregpriv->ampdu_enable);
+					}
+					else
+						DBG_871X("get ampdu_enable =%d\n", pregpriv->ampdu_enable);
+
+				}
+				break;
+				case 0x14:
+				{
+					DBG_871X("minor_cmd 0x%x\n", minor_cmd);
+				}
+				break;
+				case 0x16:
+				{
+					if (arg == 0xff) {
+						rtw_odm_dbg_comp_msg(RTW_DBGDUMP, padapter);
+					}
+					else {
+						u64 dbg_comp = (u64)extra_arg;
+						rtw_odm_dbg_comp_set(padapter, dbg_comp);
+					}
+				}
+					break;
+#ifdef DBG_FIXED_CHAN
+				case 0x17:
+					{
+						struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+						printk("===>  Fixed channel to %d\n", extra_arg);
+						pmlmeext->fixed_chan = extra_arg;
+
+					}
+					break;
+#endif
+				case 0x18:
+					{
+						printk("===>  Switch USB Mode %d\n", extra_arg);
+						rtw_hal_set_hwreg(padapter, HW_VAR_USB_MODE, (u8 *)&extra_arg);
+					}
+					break;
+				case 0x19:
+					{
+						struct registry_priv *pregistrypriv = &padapter->registrypriv;
+						/*  extra_arg : */
+						/*  BIT0: Enable VHT LDPC Rx, BIT1: Enable VHT LDPC Tx, */
+						/*  BIT4: Enable HT LDPC Rx, BIT5: Enable HT LDPC Tx */
+						if (arg == 0) {
+							DBG_871X("driver disable LDPC\n");
+							pregistrypriv->ldpc_cap = 0x00;
+						}
+						else if (arg == 1) {
+							DBG_871X("driver set LDPC cap = 0x%x\n", extra_arg);
+							pregistrypriv->ldpc_cap = (u8)(extra_arg&0x33);
+						}
+					}
+                                        break;
+				case 0x1a:
+					{
+						struct registry_priv *pregistrypriv = &padapter->registrypriv;
+						/*  extra_arg : */
+						/*  BIT0: Enable VHT STBC Rx, BIT1: Enable VHT STBC Tx, */
+						/*  BIT4: Enable HT STBC Rx, BIT5: Enable HT STBC Tx */
+						if (arg == 0) {
+							DBG_871X("driver disable STBC\n");
+							pregistrypriv->stbc_cap = 0x00;
+						}
+						else if (arg == 1) {
+							DBG_871X("driver set STBC cap = 0x%x\n", extra_arg);
+							pregistrypriv->stbc_cap = (u8)(extra_arg&0x33);
+						}
+					}
+                                        break;
+				case 0x1b:
+					{
+						struct registry_priv *pregistrypriv = &padapter->registrypriv;
+
+						if (arg == 0) {
+							DBG_871X("disable driver ctrl max_rx_rate, reset to default_rate_set\n");
+							init_mlme_default_rate_set(padapter);
+							pregistrypriv->ht_enable = (u8)rtw_ht_enable;
+						}
+						else if (arg == 1) {
+
+							int i;
+							u8 max_rx_rate;
+
+							DBG_871X("enable driver ctrl max_rx_rate = 0x%x\n", extra_arg);
+
+							max_rx_rate = (u8)extra_arg;
+
+							if (max_rx_rate < 0xc) /*  max_rx_rate < MSC0 -> B or G -> disable HT */
+							{
+								pregistrypriv->ht_enable = 0;
+								for (i = 0; i<NumRates; i++)
+								{
+									if (pmlmeext->datarate[i] > max_rx_rate)
+										pmlmeext->datarate[i] = 0xff;
+								}
+
+							}
+							else if (max_rx_rate < 0x1c) /*  mcs0~mcs15 */
+							{
+								u32 mcs_bitmap = 0x0;
+
+								for (i = 0; i<((max_rx_rate+1)-0xc); i++)
+									mcs_bitmap |= BIT(i);
+
+								set_mcs_rate_by_mask(pmlmeext->default_supported_mcs_set, mcs_bitmap);
+							}
+						}
+					}
+                                        break;
+				case 0x1c: /* enable/disable driver control AMPDU Density for peer sta's rx */
+					{
+						if (arg == 0) {
+							DBG_871X("disable driver ctrl ampdu density\n");
+							padapter->driver_ampdu_spacing = 0xFF;
+						}
+						else if (arg == 1) {
+
+							DBG_871X("enable driver ctrl ampdu density = %d\n", extra_arg);
+
+							if ((extra_arg & 0x07) > 0x07)
+								padapter->driver_ampdu_spacing = 0xFF;
+							else
+								padapter->driver_ampdu_spacing = extra_arg;
+						}
+					}
+					break;
+#ifdef CONFIG_BACKGROUND_NOISE_MONITOR
+				case 0x1e:
+					{
+						struct hal_com_data	*pHalData = GET_HAL_DATA(padapter);
+						PDM_ODM_T pDM_Odm = &pHalData->odmpriv;
+						u8 chan = rtw_get_oper_ch(padapter);
+						DBG_871X("===========================================\n");
+						ODM_InbandNoise_Monitor(pDM_Odm, true, 0x1e, 100);
+						DBG_871X("channel(%d), noise_a = %d, noise_b = %d , noise_all:%d\n",
+							chan, pDM_Odm->noise_level.noise[ODM_RF_PATH_A],
+							pDM_Odm->noise_level.noise[ODM_RF_PATH_B],
+							pDM_Odm->noise_level.noise_all);
+						DBG_871X("===========================================\n");
+
+					}
+					break;
+#endif
+				case 0x23:
+					{
+						DBG_871X("turn %s the bNotifyChannelChange Variable\n", (extra_arg == 1)?"on":"off");
+						padapter->bNotifyChannelChange = extra_arg;
+						break;
+					}
+				case 0x24:
+					{
+						break;
+					}
+#ifdef CONFIG_GPIO_API
+		            case 0x25: /* Get GPIO register */
+		                    {
+			                    /*
+			                    * dbg 0x7f250000 [gpio_num], Get gpio value, gpio_num:0~7
+			                    */
+
+			                    int value;
+			                    DBG_871X("Read GPIO Value  extra_arg = %d\n", extra_arg);
+			                    value = rtw_get_gpio(dev, extra_arg);
+			                    DBG_871X("Read GPIO Value = %d\n", value);
+			                    break;
+		                    }
+		            case 0x26: /* Set GPIO direction */
+		                    {
+
+			                    /* dbg 0x7f26000x [y], Set gpio direction,
+			                    * x: gpio_num, 4~7  y: indicate direction, 0~1
+			                    */
+
+			                    int value;
+			                    DBG_871X("Set GPIO Direction! arg = %d , extra_arg =%d\n", arg , extra_arg);
+			                    value = rtw_config_gpio(dev, arg, extra_arg);
+			                    DBG_871X("Set GPIO Direction %s\n", (value ==-1)?"Fail!!!":"Success");
+			                    break;
+					}
+				case 0x27: /* Set GPIO output direction value */
+					{
+						/*
+						* dbg 0x7f27000x [y], Set gpio output direction value,
+						* x: gpio_num, 4~7  y: indicate direction, 0~1
+						*/
+
+						int value;
+						DBG_871X("Set GPIO Value! arg = %d , extra_arg =%d\n", arg , extra_arg);
+						value = rtw_set_gpio_output_value(dev, arg, extra_arg);
+						DBG_871X("Set GPIO Value %s\n", (value ==-1)?"Fail!!!":"Success");
+						break;
+					}
+#endif
+				case 0xaa:
+					{
+						if ((extra_arg & 0x7F)> 0x3F) extra_arg = 0xFF;
+						DBG_871X("chang data rate to :0x%02x\n", extra_arg);
+						padapter->fix_rate = 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(RTW_DBGDUMP, padapter);
+						}
+						else if (extra_arg == 1) {
+							bb_reg_dump(RTW_DBGDUMP, padapter);
+						}
+						else if (extra_arg ==2) {
+							rf_reg_dump(RTW_DBGDUMP, padapter);
+						}
+					}
+					break;
+
+				case 0xee:/* turn on/off dynamic funcs */
+					{
+						u32 odm_flag;
+
+						if (0xf ==extra_arg) {
+							rtw_hal_get_def_var(padapter, HAL_DEF_DBG_DM_FUNC,&odm_flag);
+							DBG_871X(" === DMFlag(0x%08x) ===\n", odm_flag);
+							DBG_871X("extra_arg = 0  - disable all dynamic func\n");
+							DBG_871X("extra_arg = 1  - disable DIG- BIT(0)\n");
+							DBG_871X("extra_arg = 2  - disable High power - BIT(1)\n");
+							DBG_871X("extra_arg = 3  - disable tx power tracking - BIT(2)\n");
+							DBG_871X("extra_arg = 4  - disable BT coexistence - BIT(3)\n");
+							DBG_871X("extra_arg = 5  - disable antenna diversity - BIT(4)\n");
+							DBG_871X("extra_arg = 6  - enable all dynamic func\n");
+						}
+						else {
+							/*extra_arg = 0  - disable all dynamic func
+								extra_arg = 1  - disable DIG
+								extra_arg = 2  - disable tx power tracking
+								extra_arg = 3  - turn on all dynamic func
+							*/
+							rtw_hal_set_def_var(padapter, HAL_DEF_DBG_DM_FUNC, &(extra_arg));
+							rtw_hal_get_def_var(padapter, HAL_DEF_DBG_DM_FUNC,&odm_flag);
+							DBG_871X(" === DMFlag(0x%08x) ===\n", odm_flag);
+						}
+					}
+					break;
+
+				case 0xfd:
+					rtw_write8(padapter, 0xc50, arg);
+					DBG_871X("wr(0xc50) = 0x%x\n", rtw_read8(padapter, 0xc50));
+					rtw_write8(padapter, 0xc58, arg);
+					DBG_871X("wr(0xc58) = 0x%x\n", rtw_read8(padapter, 0xc58));
+					break;
+				case 0xfe:
+					DBG_871X("rd(0xc50) = 0x%x\n", rtw_read8(padapter, 0xc50));
+					DBG_871X("rd(0xc58) = 0x%x\n", rtw_read8(padapter, 0xc58));
+					break;
+				case 0xff:
+					{
+						DBG_871X("dbg(0x210) = 0x%x\n", rtw_read32(padapter, 0x210));
+						DBG_871X("dbg(0x608) = 0x%x\n", rtw_read32(padapter, 0x608));
+						DBG_871X("dbg(0x280) = 0x%x\n", rtw_read32(padapter, 0x280));
+						DBG_871X("dbg(0x284) = 0x%x\n", rtw_read32(padapter, 0x284));
+						DBG_871X("dbg(0x288) = 0x%x\n", rtw_read32(padapter, 0x288));
+
+						DBG_871X("dbg(0x664) = 0x%x\n", rtw_read32(padapter, 0x664));
+
+
+						DBG_871X("\n");
+
+						DBG_871X("dbg(0x430) = 0x%x\n", rtw_read32(padapter, 0x430));
+						DBG_871X("dbg(0x438) = 0x%x\n", rtw_read32(padapter, 0x438));
+
+						DBG_871X("dbg(0x440) = 0x%x\n", rtw_read32(padapter, 0x440));
+
+						DBG_871X("dbg(0x458) = 0x%x\n", rtw_read32(padapter, 0x458));
+
+						DBG_871X("dbg(0x484) = 0x%x\n", rtw_read32(padapter, 0x484));
+						DBG_871X("dbg(0x488) = 0x%x\n", rtw_read32(padapter, 0x488));
+
+						DBG_871X("dbg(0x444) = 0x%x\n", rtw_read32(padapter, 0x444));
+						DBG_871X("dbg(0x448) = 0x%x\n", rtw_read32(padapter, 0x448));
+						DBG_871X("dbg(0x44c) = 0x%x\n", rtw_read32(padapter, 0x44c));
+						DBG_871X("dbg(0x450) = 0x%x\n", rtw_read32(padapter, 0x450));
+					}
+					break;
+			}
+			break;
+		default:
+			DBG_871X("error dbg cmd!\n");
+			break;
+	}
+
+
+	return ret;
+
+}
+
+static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
+{
+	uint ret = 0;
+	struct adapter *padapter = (struct adapter *)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;
+		}
+
+		RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_info_, ("wpa_set_param:padapter->securitypriv.ndisauthtype =%d\n", padapter->securitypriv.ndisauthtype));
+
+		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 */
+		/* DBG_871X(KERN_WARNING "------------------------>wpax value = %x\n", value); */
+		/*
+		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 = (struct adapter *)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->length < sizeof(struct ieee_param) || !p->pointer) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	param = (struct ieee_param *)rtw_malloc(p->length);
+	if (param == NULL)
+	{
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	if (copy_from_user(param, p->pointer, p->length))
+	{
+		kfree((u8 *)param);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	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((struct adapter *)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:
+		DBG_871X("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((u8 *)param);
+
+out:
+
+	/* 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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct security_priv* psecuritypriv =&(padapter->securitypriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("%s\n", __func__);
+
+	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; */
+			DBG_871X("rtw_set_encryption(), sta has already been removed or never been added\n");
+			goto exit;
+		}
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0 && (psta == NULL))
+	{
+		/* todo:clear default encryption keys */
+
+		psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Open;
+		psecuritypriv->ndisencryptstatus = Ndis802_11EncryptionDisabled;
+		psecuritypriv->dot11PrivacyAlgrthm = _NO_PRIVACY_;
+		psecuritypriv->dot118021XGrpPrivacy = _NO_PRIVACY_;
+
+		DBG_871X("clear default encryption keys, keyid =%d\n", param->u.crypt.idx);
+
+		goto exit;
+	}
+
+
+	if (strcmp(param->u.crypt.alg, "WEP") == 0 && (psta == NULL))
+	{
+		DBG_871X("r871x_set_encryption, crypt.alg = WEP\n");
+
+		wep_key_idx = param->u.crypt.idx;
+		wep_key_len = param->u.crypt.key_len;
+
+		DBG_871X("r871x_set_encryption, wep_key_idx =%d, len =%d\n", wep_key_idx, wep_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, KeyMaterial);
+			pwep =(struct ndis_802_11_wep *)rtw_malloc(wep_total_len);
+			if (pwep == NULL) {
+				DBG_871X(" r871x_set_encryption: pwep allocate fail !!!\n");
+				goto exit;
+			}
+
+			memset(pwep, 0, wep_total_len);
+
+			pwep->KeyLength = wep_key_len;
+			pwep->Length = wep_total_len;
+
+		}
+
+		pwep->KeyIndex = wep_key_idx;
+
+		memcpy(pwep->KeyMaterial,  param->u.crypt.key, pwep->KeyLength);
+
+		if (param->u.crypt.set_tx)
+		{
+			DBG_871X("wep, set_tx = 1\n");
+
+			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_Auto;
+			psecuritypriv->ndisencryptstatus = Ndis802_11Encryption1Enabled;
+			psecuritypriv->dot11PrivacyAlgrthm = _WEP40_;
+			psecuritypriv->dot118021XGrpPrivacy = _WEP40_;
+
+			if (pwep->KeyLength == 13)
+			{
+				psecuritypriv->dot11PrivacyAlgrthm = _WEP104_;
+				psecuritypriv->dot118021XGrpPrivacy = _WEP104_;
+			}
+
+
+			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
+
+			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->KeyMaterial, pwep->KeyLength);
+
+			psecuritypriv->dot11DefKeylen[wep_key_idx]=pwep->KeyLength;
+
+			rtw_ap_set_wep_key(padapter, pwep->KeyMaterial, pwep->KeyLength, wep_key_idx, 1);
+		}
+		else
+		{
+			DBG_871X("wep, set_tx = 0\n");
+
+			/* 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->KeyMaterial, pwep->KeyLength);
+
+			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->KeyLength;
+
+			rtw_ap_set_wep_key(padapter, pwep->KeyMaterial, pwep->KeyLength, 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)
+			{
+				DBG_871X("%s, set group_key, WEP\n", __func__);
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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)
+			{
+				DBG_871X("%s, set group_key, TKIP\n", __func__);
+
+				psecuritypriv->dot118021XGrpPrivacy = _TKIP_;
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrptxmickey[param->u.crypt.idx].skey, &(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)
+			{
+				DBG_871X("%s, set group_key, CCMP\n", __func__);
+
+				psecuritypriv->dot118021XGrpPrivacy = _AES_;
+
+				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  param->u.crypt.key, (param->u.crypt.key_len>16 ?16:param->u.crypt.key_len));
+			}
+			else
+			{
+				DBG_871X("%s, set group_key, none\n", __func__);
+
+				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)
+				{
+					DBG_871X("%s, set pairwise key, WEP\n", __func__);
+
+					psta->dot118021XPrivacy = _WEP40_;
+					if (param->u.crypt.key_len == 13)
+					{
+						psta->dot118021XPrivacy = _WEP104_;
+					}
+				}
+				else if (strcmp(param->u.crypt.alg, "TKIP") == 0)
+				{
+					DBG_871X("%s, set pairwise key, TKIP\n", __func__);
+
+					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)
+				{
+
+					DBG_871X("%s, set pairwise key, CCMP\n", __func__);
+
+					psta->dot118021XPrivacy = _AES_;
+				}
+				else
+				{
+					DBG_871X("%s, set pairwise key, none\n", __func__);
+
+					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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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(psecuritypriv->dot118021XGrptxmickey[param->u.crypt.idx].skey, &(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(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,  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((u8 *)pwep);
+
+	return ret;
+
+}
+
+static int rtw_set_beacon(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	unsigned char *pbuf = param->u.bcn_ie.buf;
+
+
+	DBG_871X("%s, len =%d\n", __func__, len);
+
+	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 int rtw_hostapd_sta_flush(struct net_device *dev)
+{
+	/* _irqL irqL; */
+	/* struct list_head	*phead, *plist; */
+	int ret = 0;
+	/* struct sta_info *psta = NULL; */
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	/* struct sta_priv *pstapriv = &padapter->stapriv; */
+
+	DBG_871X("%s\n", __func__);
+
+	flush_all_cam_entry(padapter);	/* clear CAM */
+
+	ret = rtw_sta_flush(padapter);
+
+	return ret;
+
+}
+
+static int rtw_add_sta(struct net_device *dev, struct ieee_param *param)
+{
+	int ret = 0;
+	struct sta_info *psta = NULL;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("rtw_add_sta(aid =%d) =" MAC_FMT "\n", param->u.add_sta.aid, MAC_ARG(param->sta_addr));
+
+	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)
+	{
+		DBG_871X("rtw_add_sta(), free has been added psta =%p\n", psta);
+		spin_lock_bh(&(pstapriv->sta_hash_lock));
+		rtw_free_stainfo(padapter,  psta);
+		spin_unlock_bh(&(pstapriv->sta_hash_lock));
+
+		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;
+
+		/* DBG_871X("rtw_add_sta(), init sta's variables, psta =%p\n", psta); */
+
+		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 rtw_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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("rtw_del_sta =" MAC_FMT "\n", MAC_ARG(param->sta_addr));
+
+	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;
+
+		/* DBG_871X("free psta =%p, aid =%d\n", psta, psta->aid); */
+
+		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;
+
+	}
+	else
+	{
+		DBG_871X("rtw_del_sta(), sta has already been removed or never been added\n");
+
+		/* ret = -1; */
+	}
+
+
+	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 = (struct adapter *)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;
+
+	DBG_871X("rtw_ioctl_get_sta_info, sta_addr: " MAC_FMT "\n", MAC_ARG(param_ex->sta_addr));
+
+	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 rtw_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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct sta_priv *pstapriv = &padapter->stapriv;
+
+	DBG_871X("rtw_get_sta_wpaie, sta_addr: " MAC_FMT "\n", MAC_ARG(param->sta_addr));
+
+	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_GENERIC))
+		{
+			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; */
+			DBG_871X("sta's wpa_ie is NONE\n");
+		}
+	}
+	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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
+	int ie_len;
+
+	DBG_871X("%s, len =%d\n", __func__, len);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	if (pmlmepriv->wps_beacon_ie)
+	{
+		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 == NULL) {
+			DBG_871X("%s()-%d: rtw_malloc() ERROR!\n", __func__, __LINE__);
+			return -EINVAL;
+		}
+
+		memcpy(pmlmepriv->wps_beacon_ie, param->u.bcn_ie.buf, ie_len);
+
+		update_beacon(padapter, _VENDOR_SPECIFIC_IE_, 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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	int ie_len;
+
+	DBG_871X("%s, len =%d\n", __func__, len);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	if (pmlmepriv->wps_probe_resp_ie)
+	{
+		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 == NULL) {
+			DBG_871X("%s()-%d: rtw_malloc() ERROR!\n", __func__, __LINE__);
+			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 = (struct adapter *)rtw_netdev_priv(dev);
+	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	int ie_len;
+
+	DBG_871X("%s, len =%d\n", __func__, len);
+
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE) != true)
+		return -EINVAL;
+
+	ie_len = len-12-2;/*  12 = param header, 2:no packed */
+
+
+	if (pmlmepriv->wps_assoc_resp_ie)
+	{
+		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 == NULL) {
+			DBG_871X("%s()-%d: rtw_malloc() ERROR!\n", __func__, __LINE__);
+			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 = (struct 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];
+	sint 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;
+
+		if (0)
+			DBG_871X(FUNC_ADPT_FMT" ssid:(%s,%d), from ie:(%s,%d), (%s,%d)\n", FUNC_ADPT_ARG(adapter),
+				 ssid, ssid_len,
+				 pbss_network->Ssid.Ssid, pbss_network->Ssid.SsidLength,
+				 pbss_network_ext->Ssid.Ssid, pbss_network_ext->Ssid.SsidLength);
+
+		memcpy(pbss_network->Ssid.Ssid, (void *)ssid, ssid_len);
+		pbss_network->Ssid.SsidLength = ssid_len;
+		memcpy(pbss_network_ext->Ssid.Ssid, (void *)ssid, ssid_len);
+		pbss_network_ext->Ssid.SsidLength = ssid_len;
+
+		if (0)
+			DBG_871X(FUNC_ADPT_FMT" after ssid:(%s,%d), (%s,%d)\n", FUNC_ADPT_ARG(adapter),
+				 pbss_network->Ssid.Ssid, pbss_network->Ssid.SsidLength,
+				 pbss_network_ext->Ssid.Ssid, pbss_network_ext->Ssid.SsidLength);
+	}
+
+	DBG_871X(FUNC_ADPT_FMT" ignore_broadcast_ssid:%d, %s,%d\n", FUNC_ADPT_ARG(adapter),
+		ignore_broadcast_ssid, ssid, ssid_len);
+
+	return ret;
+}
+
+static int rtw_ioctl_acl_remove_sta(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)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;
+	}
+
+	ret = rtw_acl_remove_sta(padapter, param->sta_addr);
+
+	return ret;
+
+}
+
+static int rtw_ioctl_acl_add_sta(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)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;
+	}
+
+	ret = rtw_acl_add_sta(padapter, param->sta_addr);
+
+	return ret;
+
+}
+
+static int rtw_ioctl_set_macaddr_acl(struct net_device *dev, struct ieee_param *param, int len)
+{
+	int ret = 0;
+	struct adapter *padapter = (struct adapter *)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 = (struct adapter *)rtw_netdev_priv(dev);
+
+	/* DBG_871X("%s\n", __func__); */
+
+	/*
+	* 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 ==false) {
+		ret = -EPERM;
+		goto out;
+	}
+
+
+	/* if (p->length < sizeof(struct ieee_param) || !p->pointer) { */
+	if (!p->pointer) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	param = (struct ieee_param *)rtw_malloc(p->length);
+	if (param == NULL)
+	{
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	if (copy_from_user(param, p->pointer, p->length))
+	{
+		kfree((u8 *)param);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	/* DBG_871X("%s, cmd =%d\n", __func__, param->cmd); */
+
+	switch (param->cmd)
+	{
+		case RTL871X_HOSTAPD_FLUSH:
+
+			ret = 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:
+			DBG_871X("Unknown hostapd request: %d\n", param->cmd);
+			ret = -EOPNOTSUPP;
+			break;
+
+	}
+
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+
+
+	kfree((u8 *)param);
+
+out:
+
+	return ret;
+
+}
+
+static int rtw_wx_set_priv(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *awrq,
+				char *extra)
+{
+
+#ifdef DEBUG_RTW_WX_SET_PRIV
+	char *ext_dbg;
+#endif
+
+	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;
+
+	/* RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_notice_, ("+rtw_wx_set_priv\n")); */
+	if (dwrq->length == 0)
+		return -EFAULT;
+
+	len = dwrq->length;
+	if (!(ext = vmalloc(len)))
+		return -ENOMEM;
+
+	if (copy_from_user(ext, dwrq->pointer, len)) {
+		vfree(ext);
+		return -EFAULT;
+	}
+
+
+	/* RT_TRACE(_module_rtl871x_ioctl_os_c, _drv_notice_, */
+	/* 	 ("rtw_wx_set_priv: %s req =%s\n", */
+	/* 	  dev->name, ext)); */
+
+	#ifdef DEBUG_RTW_WX_SET_PRIV
+	if (!(ext_dbg = vmalloc(len)))
+	{
+		vfree(ext, len);
+		return -ENOMEM;
+	}
+
+	memcpy(ext_dbg, ext, len);
+	#endif
+
+	/* 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;
+
+			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(cp_sz);
+			if (pmlmepriv->wps_probe_req_ie == NULL) {
+				printk("%s()-%d: rtw_malloc() ERROR!\n", __func__, __LINE__);
+				ret =  -EINVAL;
+				goto FREE_EXT;
+
+			}
+
+			memcpy(pmlmepriv->wps_probe_req_ie, probereq_wpsie, cp_sz);
+			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);
+	#ifdef DEBUG_RTW_WX_SET_PRIV
+	vfree(ext_dbg);
+	#endif
+
+	/* DBG_871X("rtw_wx_set_priv: (SIOCSIWPRIV) %s ret =%d\n", */
+	/* 		dev->name, ret); */
+
+	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);
+
+	DBG_871X("[%s] extra = %s\n", __func__, extra);
+
+	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 int rtw_mp_efuse_get(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wdata, char *extra)
+{
+	int err = 0;
+	return err;
+}
+
+static int rtw_mp_efuse_set(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wdata, char *extra)
+{
+	int err = 0;
+	return err;
+}
+
+static int rtw_tdls(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	return ret;
+}
+
+
+static int rtw_tdls_get(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+	return ret;
+}
+
+
+
+
+
+#ifdef CONFIG_INTEL_WIDI
+static int rtw_widi_set(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);
+
+	process_intel_widi_cmd(padapter, extra);
+
+	return ret;
+}
+
+static int rtw_widi_set_probe_request(struct net_device *dev,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra)
+{
+	int	ret = 0;
+	u8 *pbuf = NULL;
+	struct adapter	*padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	pbuf = rtw_malloc(sizeof(l2_msg_t));
+	if (pbuf)
+	{
+		if (copy_from_user(pbuf, wrqu->data.pointer, wrqu->data.length))
+			ret = -EFAULT;
+		/* memcpy(pbuf, wrqu->data.pointer, wrqu->data.length); */
+
+		if (wrqu->data.flags == 0)
+			intel_widi_wk_cmd(padapter, INTEL_WIDI_ISSUE_PROB_WK, pbuf, sizeof(l2_msg_t));
+		else if (wrqu->data.flags == 1)
+			rtw_set_wfd_rds_sink_info(padapter, (l2_msg_t *)pbuf);
+	}
+	return ret;
+}
+#endif /*  CONFIG_INTEL_WIDI */
+
+static int rtw_test(
+	struct net_device *dev,
+	struct iw_request_info *info,
+	union iwreq_data *wrqu, char *extra)
+{
+	u32 len;
+	u8 *pbuf, *pch;
+	char *ptmp;
+	u8 *delim = ",";
+	struct adapter *padapter = rtw_netdev_priv(dev);
+
+
+	DBG_871X("+%s\n", __func__);
+	len = wrqu->data.length;
+
+	pbuf = (u8 *)rtw_zmalloc(len);
+	if (pbuf == NULL) {
+		DBG_871X("%s: no memory!\n", __func__);
+		return -ENOMEM;
+	}
+
+	if (copy_from_user(pbuf, wrqu->data.pointer, len)) {
+		kfree(pbuf);
+		DBG_871X("%s: copy from user fail!\n", __func__);
+		return -EFAULT;
+	}
+	DBG_871X("%s: string =\"%s\"\n", __func__, pbuf);
+
+	ptmp = (char*)pbuf;
+	pch = strsep(&ptmp, delim);
+	if ((pch == NULL) || (strlen(pch) == 0)) {
+		kfree(pbuf);
+		DBG_871X("%s: parameter error(level 1)!\n", __func__);
+		return -EFAULT;
+	}
+
+	if (strcmp(pch, "bton") == 0)
+	{
+		rtw_btcoex_SetManualControl(padapter, false);
+	}
+
+	if (strcmp(pch, "btoff") == 0)
+	{
+		rtw_btcoex_SetManualControl(padapter, true);
+	}
+
+	if (strcmp(pch, "h2c") == 0)
+	{
+		u8 param[8];
+		u8 count = 0;
+		u32 tmp;
+		u8 i;
+		u32 pos;
+		s32 ret;
+
+
+		do {
+			pch = strsep(&ptmp, delim);
+			if ((pch == NULL) || (strlen(pch) == 0))
+				break;
+
+			sscanf(pch, "%x", &tmp);
+			param[count++] = (u8)tmp;
+		} while (count < 8);
+
+		if (count == 0) {
+			kfree(pbuf);
+			DBG_871X("%s: parameter error(level 2)!\n", __func__);
+			return -EFAULT;
+		}
+
+		ret = rtw_hal_fill_h2c_cmd(padapter, param[0], count-1, &param[1]);
+
+		pos = sprintf(extra, "H2C ID = 0x%02x content =", param[0]);
+		for (i = 1; i<count; i++) {
+			pos += sprintf(extra+pos, "%02x,", param[i]);
+		}
+		extra[pos] = 0;
+		pos--;
+		pos += sprintf(extra+pos, " %s", ret == _FAIL?"FAIL":"OK");
+
+		wrqu->data.length = strlen(extra) + 1;
+	}
+
+	kfree(pbuf);
+	return 0;
+}
+
+static iw_handler rtw_handlers[] =
+{
+	NULL,					/* SIOCSIWCOMMIT */
+	rtw_wx_get_name,		/* SIOCGIWNAME */
+	dummy,					/* SIOCSIWNWID */
+	dummy,					/* SIOCGIWNWID */
+	rtw_wx_set_freq,		/* SIOCSIWFREQ */
+	rtw_wx_get_freq,		/* SIOCGIWFREQ */
+	rtw_wx_set_mode,		/* SIOCSIWMODE */
+	rtw_wx_get_mode,		/* SIOCGIWMODE */
+	dummy,					/* SIOCSIWSENS */
+	rtw_wx_get_sens,		/* SIOCGIWSENS */
+	NULL,					/* SIOCSIWRANGE */
+	rtw_wx_get_range,		/* SIOCGIWRANGE */
+	rtw_wx_set_priv,		/* SIOCSIWPRIV */
+	NULL,					/* SIOCGIWPRIV */
+	NULL,					/* SIOCSIWSTATS */
+	NULL,					/* SIOCGIWSTATS */
+	dummy,					/* SIOCSIWSPY */
+	dummy,					/* SIOCGIWSPY */
+	NULL,					/* SIOCGIWTHRSPY */
+	NULL,					/* SIOCWIWTHRSPY */
+	rtw_wx_set_wap,		/* SIOCSIWAP */
+	rtw_wx_get_wap,		/* SIOCGIWAP */
+	rtw_wx_set_mlme,		/* request MLME operation; uses struct iw_mlme */
+	dummy,					/* SIOCGIWAPLIST -- depricated */
+	rtw_wx_set_scan,		/* SIOCSIWSCAN */
+	rtw_wx_get_scan,		/* SIOCGIWSCAN */
+	rtw_wx_set_essid,		/* SIOCSIWESSID */
+	rtw_wx_get_essid,		/* SIOCGIWESSID */
+	dummy,					/* SIOCSIWNICKN */
+	rtw_wx_get_nick,		/* SIOCGIWNICKN */
+	NULL,					/* -- hole -- */
+	NULL,					/* -- hole -- */
+	rtw_wx_set_rate,		/* SIOCSIWRATE */
+	rtw_wx_get_rate,		/* SIOCGIWRATE */
+	rtw_wx_set_rts,			/* SIOCSIWRTS */
+	rtw_wx_get_rts,			/* SIOCGIWRTS */
+	rtw_wx_set_frag,		/* SIOCSIWFRAG */
+	rtw_wx_get_frag,		/* SIOCGIWFRAG */
+	dummy,					/* SIOCSIWTXPOW */
+	dummy,					/* SIOCGIWTXPOW */
+	dummy,					/* SIOCSIWRETRY */
+	rtw_wx_get_retry,		/* SIOCGIWRETRY */
+	rtw_wx_set_enc,			/* SIOCSIWENCODE */
+	rtw_wx_get_enc,			/* SIOCGIWENCODE */
+	dummy,					/* SIOCSIWPOWER */
+	rtw_wx_get_power,		/* SIOCGIWPOWER */
+	NULL,					/*---hole---*/
+	NULL,					/*---hole---*/
+	rtw_wx_set_gen_ie,		/* SIOCSIWGENIE */
+	NULL,					/* SIOCGWGENIE */
+	rtw_wx_set_auth,		/* SIOCSIWAUTH */
+	NULL,					/* SIOCGIWAUTH */
+	rtw_wx_set_enc_ext,		/* SIOCSIWENCODEEXT */
+	NULL,					/* SIOCGIWENCODEEXT */
+	rtw_wx_set_pmkid,		/* SIOCSIWPMKSA */
+	NULL,					/*---hole---*/
+};
+
+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 + 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 | 2, 0, "setpid"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x6,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wps_start"
+	},
+/* for PLATFORM_MT53XX */
+	{
+		SIOCIWFIRSTPRIV + 0x7,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "get_sensitivity"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x8,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wps_prob_req_ie"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x9,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wps_assoc_req_ie"
+	},
+
+/* for RTK_DMP_PLATFORM */
+	{
+		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 | 1024, 0, "p2p_set"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x11,
+		IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK , "p2p_get"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x12, 0, 0, "NULL"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x13,
+		IW_PRIV_TYPE_CHAR | 64, IW_PRIV_TYPE_CHAR | 64 , "p2p_get2"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x14,
+		IW_PRIV_TYPE_CHAR  | 64, 0, "tdls"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x15,
+		IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024 , "tdls_get"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x16,
+		IW_PRIV_TYPE_CHAR | 64, 0, "pm_set"
+	},
+
+	{SIOCIWFIRSTPRIV + 0x18, IW_PRIV_TYPE_CHAR | IFNAMSIZ , 0 , "rereg_nd_name"},
+	{SIOCIWFIRSTPRIV + 0x1A, IW_PRIV_TYPE_CHAR | 1024, 0, "efuse_set"},
+	{SIOCIWFIRSTPRIV + 0x1B, IW_PRIV_TYPE_CHAR | 128, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "efuse_get"},
+	{
+		SIOCIWFIRSTPRIV + 0x1D,
+		IW_PRIV_TYPE_CHAR | 40, IW_PRIV_TYPE_CHAR | 0x7FF, "test"
+	},
+
+#ifdef CONFIG_INTEL_WIDI
+	{
+		SIOCIWFIRSTPRIV + 0x1E,
+		IW_PRIV_TYPE_CHAR | 1024, 0, "widi_set"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x1F,
+		IW_PRIV_TYPE_CHAR | 128, 0, "widi_prob_req"
+	},
+#endif /*  CONFIG_INTEL_WIDI */
+
+#ifdef CONFIG_WOWLAN
+		{ MP_WOW_ENABLE , IW_PRIV_TYPE_CHAR | 1024, 0, "wow_mode" }, /* set */
+#endif
+#ifdef CONFIG_AP_WOWLAN
+		{ MP_AP_WOW_ENABLE , IW_PRIV_TYPE_CHAR | 1024, 0, "ap_wow_mode" }, /* set */
+#endif
+};
+
+static iw_handler rtw_private_handler[] =
+{
+	rtw_wx_write32,					/* 0x00 */
+	rtw_wx_read32,					/* 0x01 */
+	rtw_drvext_hdl,					/* 0x02 */
+	rtw_mp_ioctl_hdl,				/* 0x03 */
+
+/*  for MM DTV platform */
+	rtw_get_ap_info,					/* 0x04 */
+
+	rtw_set_pid,						/* 0x05 */
+	rtw_wps_start,					/* 0x06 */
+
+/*  for PLATFORM_MT53XX */
+	rtw_wx_get_sensitivity,			/* 0x07 */
+	rtw_wx_set_mtk_wps_probe_ie,	/* 0x08 */
+	rtw_wx_set_mtk_wps_ie,			/* 0x09 */
+
+/*  for RTK_DMP_PLATFORM */
+/*  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 */
+	rtw_wx_priv_null,				/* 0x0E */
+	rtw_wx_priv_null,				/* 0x0F */
+	rtw_p2p_set,					/* 0x10 */
+	rtw_p2p_get,					/* 0x11 */
+	NULL,							/* 0x12 */
+	rtw_p2p_get2,					/* 0x13 */
+
+	rtw_tdls,						/* 0x14 */
+	rtw_tdls_get,					/* 0x15 */
+
+	rtw_pm_set,						/* 0x16 */
+	rtw_wx_priv_null,				/* 0x17 */
+	rtw_rereg_nd_name,				/* 0x18 */
+	rtw_wx_priv_null,				/* 0x19 */
+	rtw_mp_efuse_set,				/* 0x1A */
+	rtw_mp_efuse_get,				/* 0x1B */
+	NULL,							/*  0x1C is reserved for hostapd */
+	rtw_test,						/*  0x1D */
+#ifdef CONFIG_INTEL_WIDI
+	rtw_widi_set,					/* 0x1E */
+	rtw_widi_set_probe_request,		/* 0x1F */
+#endif /*  CONFIG_INTEL_WIDI */
+};
+
+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_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;
+		/* DBG_871X("No link  level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise); */
+	}
+	else {
+		#ifdef CONFIG_SIGNAL_DISPLAY_DBM
+		tmp_level = translate_percentage_to_dbm(padapter->recvpriv.signal_strength);
+		#else
+		#ifdef CONFIG_SKIP_SIGNAL_SCALE_MAPPING
+		{
+			/* Do signal scale mapping when using percentage as the unit of signal strength, since the scale mapping is skipped in odm */
+
+			struct hal_com_data *pHal = GET_HAL_DATA(padapter);
+
+			tmp_level = (u8)odm_SignalScaleMapping(&pHal->odmpriv, padapter->recvpriv.signal_strength);
+		}
+		#else
+		tmp_level = padapter->recvpriv.signal_strength;
+		#endif
+		#endif
+
+		tmp_qual = padapter->recvpriv.signal_qual;
+#if defined(CONFIG_SIGNAL_DISPLAY_DBM) && defined(CONFIG_BACKGROUND_NOISE_MONITOR)
+		if (rtw_linked_check(padapter)) {
+			struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+			struct noise_info info;
+			info.bPauseDIG = true;
+			info.IGIValue = 0x1e;
+			info.max_time = 100;/* ms */
+			info.chan = pmlmeext->cur_channel ;/* rtw_get_oper_ch(padapter); */
+			rtw_ps_deny(padapter, PS_DENY_IOCTL);
+			LeaveAllPowerSaveModeDirect(padapter);
+
+			rtw_hal_set_odm_var(padapter, HAL_ODM_NOISE_MONITOR,&info, false);
+			/* ODM_InbandNoise_Monitor(podmpriv, true, 0x20, 100); */
+			rtw_ps_deny_cancel(padapter, PS_DENY_IOCTL);
+			rtw_hal_get_odm_var(padapter, HAL_ODM_NOISE_MONITOR,&(info.chan), &(padapter->recvpriv.noise));
+			DBG_871X("chan:%d, noise_level:%d\n", info.chan, padapter->recvpriv.noise);
+		}
+#endif
+		tmp_noise = padapter->recvpriv.noise;
+		DBG_871X("level:%d, qual:%d, noise:%d, rssi (%d)\n", tmp_level, tmp_qual, tmp_noise, padapter->recvpriv.rssi);
+
+		piwstats->qual.level = tmp_level;
+		piwstats->qual.qual = tmp_qual;
+		piwstats->qual.noise = tmp_noise;
+	}
+	piwstats->qual.updated = IW_QUAL_ALL_UPDATED ;/* IW_QUAL_DBM; */
+
+	#ifdef CONFIG_SIGNAL_DISPLAY_DBM
+	piwstats->qual.updated = piwstats->qual.updated | IW_QUAL_DBM;
+	#endif
+
+	return &padapter->iwstats;
+}
+
+struct iw_handler_def rtw_handlers_def =
+{
+	.standard = rtw_handlers,
+	.num_standard = sizeof(rtw_handlers) / sizeof(iw_handler),
+#if defined(CONFIG_WEXT_PRIV)
+	.private = rtw_private_handler,
+	.private_args = (struct iw_priv_args *)rtw_private_args,
+	.num_private = sizeof(rtw_private_handler) / sizeof(iw_handler),
+	.num_private_args = sizeof(rtw_private_args) / sizeof(struct iw_priv_args),
+#endif
+	.get_wireless_stats = rtw_get_wireless_stats,
+};
+
+/*  copy from net/wireless/wext.c start */
+/* ---------------------------------------------------------------- */
+/*
+ * Calculate size of private arguments
+ */
+static const char iw_priv_type_size[] = {
+	0,                              /* IW_PRIV_TYPE_NONE */
+	1,                              /* IW_PRIV_TYPE_BYTE */
+	1,                              /* IW_PRIV_TYPE_CHAR */
+	0,                              /* Not defined */
+	sizeof(__u32),                  /* IW_PRIV_TYPE_INT */
+	sizeof(struct iw_freq),         /* IW_PRIV_TYPE_FLOAT */
+	sizeof(struct sockaddr),        /* IW_PRIV_TYPE_ADDR */
+	0,                              /* Not defined */
+};
+
+static int get_priv_size(__u16 args)
+{
+	int num = args & IW_PRIV_SIZE_MASK;
+	int type = (args & IW_PRIV_TYPE_MASK) >> 12;
+
+	return num * iw_priv_type_size[type];
+}
+/*  copy from net/wireless/wext.c end */
+
+static int rtw_ioctl_wext_private(struct net_device *dev, union iwreq_data *wrq_data)
+{
+	int err = 0;
+	u8 *input = NULL;
+	u32 input_len = 0;
+	const char delim[] = " ";
+	u8 *output = NULL;
+	u32 output_len = 0;
+	u32 count = 0;
+	u8 *buffer = NULL;
+	u32 buffer_len = 0;
+	char *ptr = NULL;
+	u8 cmdname[17] = {0}; /*  IFNAMSIZ+1 */
+	u32 cmdlen;
+	s32 len;
+	u8 *extra = NULL;
+	u32 extra_size = 0;
+
+	s32 k;
+	const iw_handler *priv;		/* Private ioctl */
+	const struct iw_priv_args *priv_args;	/* Private ioctl description */
+	u32 num_priv;				/* Number of ioctl */
+	u32 num_priv_args;			/* Number of descriptions */
+	iw_handler handler;
+	int temp;
+	int subcmd = 0;				/* sub-ioctl index */
+	int offset = 0;				/* Space for sub-ioctl index */
+
+	union iwreq_data wdata;
+
+
+	memcpy(&wdata, wrq_data, sizeof(wdata));
+
+	input_len = 2048;
+	input = rtw_zmalloc(input_len);
+	if (NULL == input)
+		return -ENOMEM;
+	if (copy_from_user(input, wdata.data.pointer, input_len)) {
+		err = -EFAULT;
+		goto exit;
+	}
+	ptr = input;
+	len = strlen(input);
+
+	sscanf(ptr, "%16s", cmdname);
+	cmdlen = strlen(cmdname);
+	DBG_8192C("%s: cmd =%s\n", __func__, cmdname);
+
+	/*  skip command string */
+	if (cmdlen > 0)
+		cmdlen += 1; /*  skip one space */
+	ptr += cmdlen;
+	len -= cmdlen;
+	DBG_8192C("%s: parameters =%s\n", __func__, ptr);
+
+	priv = rtw_private_handler;
+	priv_args = rtw_private_args;
+	num_priv = sizeof(rtw_private_handler) / sizeof(iw_handler);
+	num_priv_args = sizeof(rtw_private_args) / sizeof(struct iw_priv_args);
+
+	if (num_priv_args == 0) {
+		err = -EOPNOTSUPP;
+		goto exit;
+	}
+
+	/* Search the correct ioctl */
+	k = -1;
+	while ((++k < num_priv_args) && strcmp(priv_args[k].name, cmdname));
+
+	/* If not found... */
+	if (k == num_priv_args) {
+		err = -EOPNOTSUPP;
+		goto exit;
+	}
+
+	/* Watch out for sub-ioctls ! */
+	if (priv_args[k].cmd < SIOCDEVPRIVATE)
+	{
+		int j = -1;
+
+		/* Find the matching *real* ioctl */
+		while ((++j < num_priv_args) && ((priv_args[j].name[0] != '\0') ||
+			(priv_args[j].set_args != priv_args[k].set_args) ||
+			(priv_args[j].get_args != priv_args[k].get_args)));
+
+		/* If not found... */
+		if (j == num_priv_args) {
+			err = -EINVAL;
+			goto exit;
+		}
+
+		/* Save sub-ioctl number */
+		subcmd = priv_args[k].cmd;
+		/* Reserve one int (simplify alignment issues) */
+		offset = sizeof(__u32);
+		/* Use real ioctl definition from now on */
+		k = j;
+	}
+
+	buffer = rtw_zmalloc(4096);
+	if (NULL == buffer) {
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	/* If we have to set some data */
+	if ((priv_args[k].set_args & IW_PRIV_TYPE_MASK) &&
+		(priv_args[k].set_args & IW_PRIV_SIZE_MASK))
+	{
+		u8 *str;
+
+		switch (priv_args[k].set_args & IW_PRIV_TYPE_MASK)
+		{
+			case IW_PRIV_TYPE_BYTE:
+				/* Fetch args */
+				count = 0;
+				do {
+					str = strsep(&ptr, delim);
+					if (NULL == str) break;
+					sscanf(str, "%i", &temp);
+					buffer[count++] = (u8)temp;
+				} while (1);
+				buffer_len = count;
+
+				/* Number of args to fetch */
+				wdata.data.length = count;
+				if (wdata.data.length > (priv_args[k].set_args & IW_PRIV_SIZE_MASK))
+					wdata.data.length = priv_args[k].set_args & IW_PRIV_SIZE_MASK;
+
+				break;
+
+			case IW_PRIV_TYPE_INT:
+				/* Fetch args */
+				count = 0;
+				do {
+					str = strsep(&ptr, delim);
+					if (NULL == str) break;
+					sscanf(str, "%i", &temp);
+					((s32*)buffer)[count++] = (s32)temp;
+				} while (1);
+				buffer_len = count * sizeof(s32);
+
+				/* Number of args to fetch */
+				wdata.data.length = count;
+				if (wdata.data.length > (priv_args[k].set_args & IW_PRIV_SIZE_MASK))
+					wdata.data.length = priv_args[k].set_args & IW_PRIV_SIZE_MASK;
+
+				break;
+
+			case IW_PRIV_TYPE_CHAR:
+				if (len > 0)
+				{
+					/* Size of the string to fetch */
+					wdata.data.length = len;
+					if (wdata.data.length > (priv_args[k].set_args & IW_PRIV_SIZE_MASK))
+						wdata.data.length = priv_args[k].set_args & IW_PRIV_SIZE_MASK;
+
+					/* Fetch string */
+					memcpy(buffer, ptr, wdata.data.length);
+				}
+				else
+				{
+					wdata.data.length = 1;
+					buffer[0] = '\0';
+				}
+				buffer_len = wdata.data.length;
+				break;
+
+			default:
+				DBG_8192C("%s: Not yet implemented...\n", __func__);
+				err = -1;
+				goto exit;
+		}
+
+		if ((priv_args[k].set_args & IW_PRIV_SIZE_FIXED) &&
+			(wdata.data.length != (priv_args[k].set_args & IW_PRIV_SIZE_MASK)))
+		{
+			DBG_8192C("%s: The command %s needs exactly %d argument(s)...\n",
+					__func__, cmdname, priv_args[k].set_args & IW_PRIV_SIZE_MASK);
+			err = -EINVAL;
+			goto exit;
+		}
+	}   /* if args to set */
+	else
+	{
+		wdata.data.length = 0L;
+	}
+
+	/* Those two tests are important. They define how the driver
+	* will have to handle the data */
+	if ((priv_args[k].set_args & IW_PRIV_SIZE_FIXED) &&
+		((get_priv_size(priv_args[k].set_args) + offset) <= IFNAMSIZ))
+	{
+		/* First case : all SET args fit within wrq */
+		if (offset)
+			wdata.mode = subcmd;
+		memcpy(wdata.name + offset, buffer, IFNAMSIZ - offset);
+	}
+	else
+	{
+		if ((priv_args[k].set_args == 0) &&
+			(priv_args[k].get_args & IW_PRIV_SIZE_FIXED) &&
+			(get_priv_size(priv_args[k].get_args) <= IFNAMSIZ))
+		{
+			/* Second case : no SET args, GET args fit within wrq */
+			if (offset)
+				wdata.mode = subcmd;
+		}
+		else
+		{
+			/* Third case : args won't fit in wrq, or variable number of args */
+			if (copy_to_user(wdata.data.pointer, buffer, buffer_len)) {
+				err = -EFAULT;
+				goto exit;
+			}
+			wdata.data.flags = subcmd;
+		}
+	}
+
+	kfree(input);
+	input = NULL;
+
+	extra_size = 0;
+	if (IW_IS_SET(priv_args[k].cmd))
+	{
+		/* Size of set arguments */
+		extra_size = get_priv_size(priv_args[k].set_args);
+
+		/* Does it fits in iwr ? */
+		if ((priv_args[k].set_args & IW_PRIV_SIZE_FIXED) &&
+			((extra_size + offset) <= IFNAMSIZ))
+			extra_size = 0;
+	} else {
+		/* Size of get arguments */
+		extra_size = get_priv_size(priv_args[k].get_args);
+
+		/* Does it fits in iwr ? */
+		if ((priv_args[k].get_args & IW_PRIV_SIZE_FIXED) &&
+			(extra_size <= IFNAMSIZ))
+			extra_size = 0;
+	}
+
+	if (extra_size == 0) {
+		extra = (u8 *)&wdata;
+		kfree(buffer);
+		buffer = NULL;
+	} else
+		extra = buffer;
+
+	handler = priv[priv_args[k].cmd - SIOCIWFIRSTPRIV];
+	err = handler(dev, NULL, &wdata, extra);
+
+	/* If we have to get some data */
+	if ((priv_args[k].get_args & IW_PRIV_TYPE_MASK) &&
+		(priv_args[k].get_args & IW_PRIV_SIZE_MASK))
+	{
+		int j;
+		int n = 0;	/* number of args */
+		u8 str[20] = {0};
+
+		/* Check where is the returned data */
+		if ((priv_args[k].get_args & IW_PRIV_SIZE_FIXED) &&
+			(get_priv_size(priv_args[k].get_args) <= IFNAMSIZ))
+			n = priv_args[k].get_args & IW_PRIV_SIZE_MASK;
+		else
+			n = wdata.data.length;
+
+		output = rtw_zmalloc(4096);
+		if (NULL == output) {
+			err =  -ENOMEM;
+			goto exit;
+		}
+
+		switch (priv_args[k].get_args & IW_PRIV_TYPE_MASK)
+		{
+			case IW_PRIV_TYPE_BYTE:
+				/* Display args */
+				for (j = 0; j < n; j++)
+				{
+					sprintf(str, "%d  ", extra[j]);
+					len = strlen(str);
+					output_len = strlen(output);
+					if ((output_len + len + 1) > 4096) {
+						err = -E2BIG;
+						goto exit;
+					}
+					memcpy(output+output_len, str, len);
+				}
+				break;
+
+			case IW_PRIV_TYPE_INT:
+				/* Display args */
+				for (j = 0; j < n; j++)
+				{
+					sprintf(str, "%d  ", ((__s32*)extra)[j]);
+					len = strlen(str);
+					output_len = strlen(output);
+					if ((output_len + len + 1) > 4096) {
+						err = -E2BIG;
+						goto exit;
+					}
+					memcpy(output+output_len, str, len);
+				}
+				break;
+
+			case IW_PRIV_TYPE_CHAR:
+				/* Display args */
+				memcpy(output, extra, n);
+				break;
+
+			default:
+				DBG_8192C("%s: Not yet implemented...\n", __func__);
+				err = -1;
+				goto exit;
+		}
+
+		output_len = strlen(output) + 1;
+		wrq_data->data.length = output_len;
+		if (copy_to_user(wrq_data->data.pointer, output, output_len)) {
+			err = -EFAULT;
+			goto exit;
+		}
+	}   /* if args to set */
+	else
+	{
+		wrq_data->data.length = 0;
+	}
+
+exit:
+	kfree(input);
+	kfree(buffer);
+	kfree(output);
+
+	return err;
+}
+
+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;
+		case SIOCDEVPRIVATE:
+			ret = rtw_ioctl_wext_private(dev, &wrq->u);
+			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 000000000000..46315d1a82f7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/mlme_linux.c
@@ -0,0 +1,206 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+
+
+#define _MLME_OSDEP_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+
+static void _dynamic_check_timer_handlder (void *FunctionContext)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+
+	rtw_dynamic_check_timer_handlder(adapter);
+
+	_set_timer(&adapter->mlmepriv.dynamic_chk_timer, 2000);
+}
+
+static void _rtw_set_scan_deny_timer_hdl(void *FunctionContext)
+{
+	struct adapter *adapter = (struct adapter *)FunctionContext;
+	rtw_set_scan_deny_timer_hdl(adapter);
+}
+
+void rtw_init_mlme_timer(struct adapter *padapter)
+{
+	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	_init_timer(&(pmlmepriv->assoc_timer), padapter->pnetdev, _rtw_join_timeout_handler, padapter);
+	/* _init_timer(&(pmlmepriv->sitesurveyctrl.sitesurvey_ctrl_timer), padapter->pnetdev, sitesurvey_ctrl_handler, padapter); */
+	_init_timer(&(pmlmepriv->scan_to_timer), padapter->pnetdev, rtw_scan_timeout_handler, padapter);
+
+	_init_timer(&(pmlmepriv->dynamic_chk_timer), padapter->pnetdev, _dynamic_check_timer_handlder, padapter);
+
+	_init_timer(&(pmlmepriv->set_scan_deny_timer), padapter->pnetdev, _rtw_set_scan_deny_timer_hdl, padapter);
+}
+
+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);
+
+	rtw_indicate_wx_assoc_event(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);
+	indicate_wx_scan_complete_event(padapter);
+}
+
+static 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. */
+
+		memset(&backupPMKIDList[ 0 ], 0x00, sizeof(RT_PMKID_LIST) * NUM_PMKID_CACHE);
+
+		memcpy(&backupPMKIDList[ 0 ], &adapter->securitypriv.PMKIDList[ 0 ], sizeof(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(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)
+{
+	/* 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);
+
+	rtw_indicate_wx_disassoc_event(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;
+
+	RT_TRACE(_module_mlme_osdep_c_, _drv_info_, ("+rtw_report_sec_ie, authmode =%d\n", authmode));
+
+	buff = NULL;
+	if (authmode == _WPA_IE_ID_)
+	{
+		RT_TRACE(_module_mlme_osdep_c_, _drv_info_, ("rtw_report_sec_ie, authmode =%d\n", authmode));
+
+		buff = rtw_zmalloc(IW_CUSTOM_MAX);
+		if (NULL == buff) {
+			DBG_871X(FUNC_ADPT_FMT ": alloc memory FAIL!!\n",
+				FUNC_ADPT_ARG(adapter));
+			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;
+
+		kfree(buff);
+	}
+}
+
+void init_addba_retry_timer(struct adapter *padapter, struct sta_info *psta)
+{
+	_init_timer(&psta->addba_retry_timer, padapter->pnetdev, addba_timer_hdl, psta);
+}
+
+void init_mlme_ext_timer(struct adapter *padapter)
+{
+	struct	mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+
+	_init_timer(&pmlmeext->survey_timer, padapter->pnetdev, survey_timer_hdl, padapter);
+	_init_timer(&pmlmeext->link_timer, padapter->pnetdev, link_timer_hdl, padapter);
+	_init_timer(&pmlmeext->sa_query_timer, padapter->pnetdev, sa_query_timer_hdl, padapter);
+}
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 000000000000..f83cfc76505c
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
@@ -0,0 +1,1916 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _OS_INTFS_C_
+
+#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 = 0x00;
+static int rtw_rfintfs = HWPI;
+static int rtw_lbkmode = 0;/* 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_MODE_MAX;
+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 = 0;
+/* 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 = 0;
+static int rtw_software_decrypt = 0;
+
+static int rtw_acm_method = 0;/*  0:By SW 1:By HW. */
+
+static int rtw_wmm_enable = 1;/*  default is set to enable the wmm. */
+static int rtw_uapsd_enable = 0;
+static int rtw_uapsd_max_sp = NO_LIMIT;
+static int rtw_uapsd_acbk_en = 0;
+static int rtw_uapsd_acbe_en = 0;
+static int rtw_uapsd_acvi_en = 0;
+static int rtw_uapsd_acvo_en = 0;
+
+int rtw_ht_enable = 1;
+/*  0: 20 MHz, 1: 40 MHz, 2: 80 MHz, 3: 160MHz, 4: 80+80MHz */
+/*  2.4G use bit 0 ~ 3, 5G use bit 4 ~ 7 */
+/*  0x21 means enable 2.4G 40MHz & 5G 80MHz */
+static int rtw_bw_mode = 0x21;
+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;/*  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 */
+
+/* int rf_config = RF_1T2R;  1T2R */
+static int rtw_rf_config = RF_MAX_TYPE;  /* auto */
+static int rtw_low_power = 0;
+static int rtw_wifi_spec = 0;
+static int rtw_channel_plan = RT_CHANNEL_DOMAIN_MAX;
+
+static int rtw_btcoex_enable = 1;
+module_param(rtw_btcoex_enable, int, 0644);
+MODULE_PARM_DESC(rtw_btcoex_enable, "Enable BT co-existence mechanism");
+static int rtw_bt_iso = 2;/*  0:Low, 1:High, 2:From Efuse */
+static int rtw_bt_sco = 3;/*  0:Idle, 1:None-SCO, 2:SCO, 3:From Counter, 4.Busy, 5.OtherBusy */
+static int rtw_bt_ampdu = 1 ;/*  0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+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_AcceptAddbaReq = true;/*  0:Reject AP's Add BA req, 1:Accept AP's Add BA req. */
+
+static int rtw_antdiv_cfg = 1; /*  0:OFF , 1:ON, 2:decide by Efuse config */
+static int rtw_antdiv_type = 0 ; /* 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_enusbss = 0;/* 0:disable, 1:enable */
+
+static int rtw_hwpdn_mode =2;/* 0:disable, 1:enable, 2: by EFUSE config */
+
+#ifdef CONFIG_HW_PWRP_DETECTION
+static int rtw_hwpwrp_detect = 1;
+#else
+static int rtw_hwpwrp_detect = 0; /* HW power  ping detect 0:disable , 1:enable */
+#endif
+
+static int rtw_hw_wps_pbc = 0;
+
+int rtw_mc2u_disable = 0;
+
+static int rtw_80211d = 0;
+
+#ifdef CONFIG_QOS_OPTIMIZATION
+static int rtw_qos_opt_enable = 1;/* 0: disable, 1:enable */
+#else
+static int rtw_qos_opt_enable = 0;/* 0: disable, 1:enable */
+#endif
+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 = NULL;  /*  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_rf_config, 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_enusbss, 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");
+
+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 = 0;
+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 = 0;
+static int rtw_tx_pwr_by_rate = 0;
+
+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");
+
+char *rtw_phy_file_path = "";
+module_param(rtw_phy_file_path, charp, 0644);
+MODULE_PARM_DESC(rtw_phy_file_path, "The path of phy parameter");
+/*  PHY FILE Bit Map */
+/*  BIT0 - MAC,				0: non-support, 1: support */
+/*  BIT1 - BB,					0: non-support, 1: support */
+/*  BIT2 - BB_PG,				0: non-support, 1: support */
+/*  BIT3 - BB_MP,				0: non-support, 1: support */
+/*  BIT4 - RF,					0: non-support, 1: support */
+/*  BIT5 - RF_TXPWR_TRACK,	0: non-support, 1: support */
+/*  BIT6 - RF_TXPWR_LMT,		0: non-support, 1: support */
+static int rtw_load_phy_file = (BIT2|BIT6);
+module_param(rtw_load_phy_file, int, 0644);
+MODULE_PARM_DESC(rtw_load_phy_file,"PHY File Bit Map");
+static int rtw_decrypt_phy_file = 0;
+module_param(rtw_decrypt_phy_file, int, 0644);
+MODULE_PARM_DESC(rtw_decrypt_phy_file,"Enable Decrypt PHY File");
+
+int _netdev_open(struct net_device *pnetdev);
+int netdev_open (struct net_device *pnetdev);
+static int netdev_close (struct net_device *pnetdev);
+
+static uint loadparam(struct adapter *padapter, _nic_hdl pnetdev)
+{
+	uint status = _SUCCESS;
+	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.SsidLength = 3;
+
+	registry_par->channel = (u8)rtw_channel;
+	registry_par->wireless_mode = (u8)rtw_wireless_mode;
+
+	if (registry_par->channel > 14)
+		registry_par->channel = 1;
+
+	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->rf_config = (u8)rtw_rf_config;
+	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->btcoex = (u8)rtw_btcoex_enable;
+	registry_par->bt_iso = (u8)rtw_bt_iso;
+	registry_par->bt_sco = (u8)rtw_bt_sco;
+	registry_par->bt_ampdu = (u8)rtw_bt_ampdu;
+	registry_par->ant_num = (s8)rtw_ant_num;
+
+	registry_par->bAcceptAddbaReq = (u8)rtw_AcceptAddbaReq;
+
+	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;
+#ifdef CONFIG_INTEL_WIDI
+	registry_par->max_roaming_times = (u8)rtw_max_roaming_times + 2;
+#endif /*  CONFIG_INTEL_WIDI */
+
+	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->TxBBSwing_5G = 0xFF;
+	registry_par->bEn_RFE = 1;
+	registry_par->RFE_Type = 64;
+
+	registry_par->load_phy_file = (u8)rtw_load_phy_file;
+	registry_par->RegDecryptCustomFile = (u8)rtw_decrypt_phy_file;
+	registry_par->qos_opt_enable = (u8)rtw_qos_opt_enable;
+
+	registry_par->hiq_filter = (u8)rtw_hiq_filter;
+	return status;
+}
+
+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 == false)
+	{
+		/* DBG_871X("r8711_net_set_mac_address(), MAC =%x:%x:%x:%x:%x:%x\n", addr->sa_data[0], addr->sa_data[1], addr->sa_data[2], addr->sa_data[3], */
+		/* addr->sa_data[4], addr->sa_data[5]); */
+		memcpy(padapter->eeprompriv.mac_addr, addr->sa_data, ETH_ALEN);
+		/* memcpy(pnetdev->dev_addr, addr->sa_data, ETH_ALEN); */
+		/* padapter->bset_hwaddr = true; */
+	}
+
+	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
+				, void *accel_priv
+				, select_queue_fallback_t fallback
+)
+{
+	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;
+
+	DBG_871X_LEVEL(_drv_info_, FUNC_NDEV_FMT" state:%lu\n", FUNC_NDEV_ARG(dev), state);
+
+	switch (state) {
+	case NETDEV_CHANGENAME:
+		rtw_adapter_proc_replace(dev);
+		break;
+	}
+
+	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);
+
+	DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	strncpy(adapter->old_ifname, dev->name, IFNAMSIZ);
+	rtw_adapter_proc_init(dev);
+
+	return 0;
+}
+
+static void rtw_ndev_uninit(struct net_device *dev)
+{
+	struct adapter *adapter = rtw_netdev_priv(dev);
+
+	DBG_871X_LEVEL(_drv_always_, FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(adapter));
+	rtw_adapter_proc_deinit(dev);
+}
+
+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;
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+init_net_dev\n"));
+
+	if (old_padapter != NULL)
+		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; */
+
+	DBG_871X("register rtw_netdev_ops to netdev_ops\n");
+	pnetdev->netdev_ops = &rtw_netdev_ops;
+
+	/* pnetdev->tx_timeout = NULL; */
+	pnetdev->watchdog_timeo = HZ*3; /* 3 second timeout */
+	pnetdev->wireless_handlers = (struct iw_handler_def *)&rtw_handlers_def;
+
+	/* 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 == NULL)
+		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;
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_start_drv_threads\n"));
+	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
+		down(&padapter->cmdpriv.terminate_cmdthread_sema); /* wait for cmd_thread to run */
+
+	rtw_hal_start_thread(padapter);
+	return _status;
+}
+
+void rtw_stop_drv_threads (struct adapter *padapter)
+{
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_stop_drv_threads\n"));
+
+	rtw_stop_cmd_thread(padapter);
+
+	/*  Below is to termindate tx_thread... */
+	up(&padapter->xmitpriv.xmit_sema);
+	down(&padapter->xmitpriv.terminate_xmitthread_sema);
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("\n drv_halt: rtw_xmit_thread can be terminated !\n"));
+
+	rtw_hal_stop_thread(padapter);
+}
+
+static u8 rtw_init_default_value(struct adapter *padapter)
+{
+	u8 ret  = _SUCCESS;
+	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;
+#ifdef CONFIG_GTK_OL
+	psecuritypriv->binstallKCK_KEK = _FAIL;
+#endif /* CONFIG_GTK_OL */
+	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;
+
+	return ret;
+}
+
+struct dvobj_priv *devobj_init(void)
+{
+	struct dvobj_priv *pdvobj = NULL;
+
+	if ((pdvobj = (struct dvobj_priv*)rtw_zmalloc(sizeof(*pdvobj))) == NULL)
+		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((u8 *)pdvobj);
+}
+
+u8 rtw_reset_drv_sw(struct adapter *padapter)
+{
+	u8 ret8 = _SUCCESS;
+	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);
+
+	return ret8;
+}
+
+
+u8 rtw_init_drv_sw(struct adapter *padapter)
+{
+	u8 ret8 = _SUCCESS;
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_init_drv_sw\n"));
+
+	ret8 = rtw_init_default_value(padapter);
+
+	rtw_init_hal_com_default_value(padapter);
+
+	if ((rtw_init_cmd_priv(&padapter->cmdpriv)) == _FAIL) {
+		RT_TRACE(_module_os_intfs_c_, _drv_err_, ("\n Can't init cmd_priv\n"));
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+	padapter->cmdpriv.padapter =padapter;
+
+	if ((rtw_init_evt_priv(&padapter->evtpriv)) == _FAIL) {
+		RT_TRACE(_module_os_intfs_c_, _drv_err_, ("\n Can't init evt_priv\n"));
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+
+	if (rtw_init_mlme_priv(padapter) == _FAIL) {
+		RT_TRACE(_module_os_intfs_c_, _drv_err_, ("\n Can't init mlme_priv\n"));
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+	if (init_mlme_ext_priv(padapter) == _FAIL) {
+		RT_TRACE(_module_os_intfs_c_, _drv_err_, ("\n Can't init mlme_ext_priv\n"));
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+	if (_rtw_init_xmit_priv(&padapter->xmitpriv, padapter) == _FAIL) {
+		DBG_871X("Can't _rtw_init_xmit_priv\n");
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+	if (_rtw_init_recv_priv(&padapter->recvpriv, padapter) == _FAIL) {
+		DBG_871X("Can't _rtw_init_recv_priv\n");
+		ret8 = _FAIL;
+		goto exit;
+	}
+	/*  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) {
+		DBG_871X("Can't _rtw_init_sta_priv\n");
+		ret8 = _FAIL;
+		goto exit;
+	}
+
+	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);
+
+#ifdef CONFIG_INTEL_WIDI
+	if (rtw_init_intel_widi(padapter) == _FAIL) {
+		DBG_871X("Can't rtw_init_intel_widi\n");
+		ret8 = _FAIL;
+		goto exit;
+	}
+#endif /* CONFIG_INTEL_WIDI */
+
+exit:
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-rtw_init_drv_sw\n"));
+
+	return ret8;
+}
+
+void rtw_cancel_all_timer(struct adapter *padapter)
+{
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+rtw_cancel_all_timer\n"));
+
+	del_timer_sync(&padapter->mlmepriv.assoc_timer);
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("rtw_cancel_all_timer:cancel association timer complete!\n"));
+
+	del_timer_sync(&padapter->mlmepriv.scan_to_timer);
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("rtw_cancel_all_timer:cancel scan_to_timer!\n"));
+
+	del_timer_sync(&padapter->mlmepriv.dynamic_chk_timer);
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("rtw_cancel_all_timer:cancel dynamic_chk_timer!\n"));
+
+	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);
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("rtw_cancel_all_timer:cancel set_scan_deny_timer!\n"));
+
+	del_timer_sync(&padapter->recvpriv.signal_stat_timer);
+
+	/* cancel dm timer */
+	rtw_hal_dm_deinit(padapter);
+}
+
+u8 rtw_free_drv_sw(struct adapter *padapter)
+{
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("==>rtw_free_drv_sw"));
+
+#ifdef CONFIG_INTEL_WIDI
+	rtw_free_intel_widi(padapter);
+#endif /* CONFIG_INTEL_WIDI */
+
+	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);
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("<==rtw_free_drv_sw\n"));
+
+	/* 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 != NULL)
+		padapter->pbuddy_adapter->pbuddy_adapter = NULL;
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-rtw_free_drv_sw\n"));
+
+	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;
+
+	memcpy(pnetdev->dev_addr, padapter->eeprompriv.mac_addr, ETH_ALEN);
+
+	/* Tell the network stack we exist */
+	if (register_netdev(pnetdev) != 0) {
+		DBG_871X(FUNC_NDEV_FMT "Failed!\n", FUNC_NDEV_ARG(pnetdev));
+		ret = _FAIL;
+		goto error_register_netdev;
+	}
+
+	DBG_871X("%s, MAC Address (if%d) = " MAC_FMT "\n", __func__, (padapter->iface_id+1), MAC_ARG(pnetdev->dev_addr));
+
+	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);
+}
+
+int _netdev_open(struct net_device *pnetdev)
+{
+	uint status;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - dev_open\n"));
+	DBG_871X("+871x_drv - drv_open, bup =%d\n", padapter->bup);
+
+	padapter->netif_up = true;
+
+	if (pwrctrlpriv->ps_flag == true) {
+		padapter->net_closed = false;
+		goto netdev_open_normal_process;
+	}
+
+	if (padapter->bup == false) {
+		padapter->bDriverStopped = false;
+		padapter->bSurpriseRemoved = false;
+		padapter->bCardDisableWOHSM = false;
+
+		status = rtw_hal_init(padapter);
+		if (status == _FAIL) {
+			RT_TRACE(_module_os_intfs_c_, _drv_err_, ("rtl871x_hal_init(): Can't init h/w!\n"));
+			goto netdev_open_error;
+		}
+
+		DBG_871X("MAC Address = "MAC_FMT"\n", MAC_ARG(pnetdev->dev_addr));
+
+		status =rtw_start_drv_threads(padapter);
+		if (status == _FAIL) {
+			DBG_871X("Initialize driver software resource Failed!\n");
+			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:
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - dev_open\n"));
+	DBG_871X("-871x_drv - drv_open, bup =%d\n", padapter->bup);
+
+	return 0;
+
+netdev_open_error:
+
+	padapter->bup = false;
+
+	netif_carrier_off(pnetdev);
+	rtw_netif_stop_queue(pnetdev);
+
+	RT_TRACE(_module_os_intfs_c_, _drv_err_, ("-871x_drv - dev_open, fail!\n"));
+	DBG_871X("-871x_drv - drv_open fail, bup =%d\n", padapter->bup);
+
+	return (-1);
+
+}
+
+int netdev_open(struct net_device *pnetdev)
+{
+	int ret;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctrlpriv = adapter_to_pwrctl(padapter);
+
+	if (pwrctrlpriv->bInSuspend == true)
+	{
+		DBG_871X("+871x_drv - drv_open, bInSuspend =%d\n", 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;
+
+	DBG_871X("===> %s.........\n", __func__);
+
+
+	padapter->bDriverStopped = false;
+	padapter->bCardDisableWOHSM = false;
+	/* padapter->bup = true; */
+
+	status = rtw_hal_init(padapter);
+	if (status == _FAIL)
+	{
+		RT_TRACE(_module_os_intfs_c_, _drv_err_, ("ips_netdrv_open(): Can't init h/w!\n"));
+		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:
+	/* padapter->bup = false; */
+	DBG_871X("-ips_netdrv_open - drv_open failure, bup =%d\n", padapter->bup);
+
+	return _FAIL;
+}
+
+
+int rtw_ips_pwr_up(struct adapter *padapter)
+{
+	int result;
+	DBG_871X("===>  rtw_ips_pwr_up..............\n");
+
+	result = ips_netdrv_open(padapter);
+
+	DBG_871X("<===  rtw_ips_pwr_up..............\n");
+	return result;
+
+}
+
+void rtw_ips_pwr_down(struct adapter *padapter)
+{
+	DBG_871X("===> rtw_ips_pwr_down...................\n");
+
+	padapter->bCardDisableWOHSM = true;
+	padapter->net_closed = true;
+
+	rtw_ips_dev_unload(padapter);
+	padapter->bCardDisableWOHSM = false;
+	DBG_871X("<=== rtw_ips_pwr_down.....................\n");
+}
+
+void rtw_ips_dev_unload(struct adapter *padapter)
+{
+	DBG_871X("====> %s...\n", __func__);
+
+
+	if (padapter->bSurpriseRemoved == false)
+	{
+		rtw_hal_deinit(padapter);
+	}
+
+}
+
+
+static int pm_netdev_open(struct net_device *pnetdev, u8 bnormal)
+{
+	int status = -1;
+
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
+
+	if (true == 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 = (struct adapter *)rtw_netdev_priv(pnetdev);
+	struct pwrctrl_priv *pwrctl = adapter_to_pwrctl(padapter);
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+871x_drv - drv_close\n"));
+
+	if (pwrctl->bInternalAutoSuspend == true)
+	{
+		/* 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)
+	{
+		DBG_871X("(1)871x_drv - drv_close, bup =%d, hw_init_completed =%d\n", padapter->bup, padapter->hw_init_completed);
+
+		padapter->bDriverStopped = true;
+
+		rtw_dev_unload(padapter);
+	}
+	else*/
+	if (pwrctl->rf_pwrstate == rf_on) {
+		DBG_871X("(2)871x_drv - drv_close, bup =%d, hw_init_completed =%d\n", padapter->bup, padapter->hw_init_completed);
+
+		/* 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;
+
+	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-871x_drv - drv_close\n"));
+	DBG_871X("-871x_drv - drv_close, bup =%d\n", padapter->bup);
+
+	return 0;
+
+}
+
+void rtw_ndev_destructor(struct net_device *ndev)
+{
+	DBG_871X(FUNC_NDEV_FMT"\n", FUNC_NDEV_ARG(ndev));
+
+	if (ndev->ieee80211_ptr)
+		kfree((u8 *)ndev->ieee80211_ptr);
+
+	free_netdev(ndev);
+}
+
+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;
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("+%s\n", __func__));
+
+	if (padapter->bup == true)
+	{
+		DBG_871X("===> %s\n", __func__);
+
+		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);
+
+		RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("@ rtw_dev_unload: stop intf complete!\n"));
+
+		if (!pwrctl->bInternalAutoSuspend)
+			rtw_stop_drv_threads(padapter);
+
+		while (atomic_read(&(pcmdpriv->cmdthd_running)) == true) {
+			if (cnt > 5) {
+				DBG_871X("stop cmdthd timeout\n");
+				break;
+			} else {
+				cnt ++;
+				DBG_871X("cmdthd is running(%d)\n", cnt);
+				msleep(10);
+			}
+		}
+
+		RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("@ %s: stop thread complete!\n", __func__));
+
+		/* check the status of IPS */
+		if (rtw_hal_check_ips_status(padapter) == true || pwrctl->rf_pwrstate == rf_off) { /* check HW status and SW state */
+			DBG_871X_LEVEL(_drv_always_, "%s: driver in IPS-FWLPS\n", __func__);
+			pdbgpriv->dbg_dev_unload_inIPS_cnt++;
+			LeaveAllPowerSaveMode(padapter);
+		} else {
+			DBG_871X_LEVEL(_drv_always_, "%s: driver not in IPS\n", __func__);
+		}
+
+		if (padapter->bSurpriseRemoved == false)
+		{
+			rtw_btcoex_IpsNotify(padapter, pwrctl->ips_mode_req);
+#ifdef CONFIG_WOWLAN
+			if (pwrctl->bSupportRemoteWakeup == true &&
+				pwrctl->wowlan_mode ==true) {
+				DBG_871X_LEVEL(_drv_always_, "%s bSupportRemoteWakeup ==true  do not run rtw_hal_deinit()\n", __func__);
+			}
+			else
+#endif
+			{
+				/* amy modify 20120221 for power seq is different between driver open and ips */
+				rtw_hal_deinit(padapter);
+			}
+			padapter->bSurpriseRemoved = true;
+		}
+		RT_TRACE(_module_hci_intfs_c_, _drv_notice_,
+			 ("@ %s: deinit hal complete!\n", __func__));
+
+		padapter->bup = false;
+
+		DBG_871X("<=== %s\n", __func__);
+	}
+	else {
+		RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("%s: bup ==false\n", __func__));
+		DBG_871X("%s: bup ==false\n", __func__);
+	}
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("-%s\n", __func__));
+}
+
+static int rtw_suspend_free_assoc_resource(struct adapter *padapter)
+{
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+
+	if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME)) {
+		if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+			&& check_fwstate(pmlmepriv, _FW_LINKED))
+		{
+			DBG_871X("%s %s(" MAC_FMT "), length:%d assoc_ssid.length:%d\n", __func__,
+					pmlmepriv->cur_network.network.Ssid.Ssid,
+					MAC_ARG(pmlmepriv->cur_network.network.MacAddress),
+					pmlmepriv->cur_network.network.Ssid.SsidLength,
+					pmlmepriv->assoc_ssid.SsidLength);
+			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) == true)
+	{
+		DBG_871X_LEVEL(_drv_always_, "%s: fw_under_linking\n", __func__);
+		rtw_indicate_disconnect(padapter);
+	}
+
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return _SUCCESS;
+}
+
+#ifdef CONFIG_WOWLAN
+int rtw_suspend_wow(struct adapter *padapter)
+{
+	u8 ch, bw, offset;
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct net_device *pnetdev = padapter->pnetdev;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct wowlan_ioctl_param poidparam;
+	int ret = _SUCCESS;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+
+
+	DBG_871X("wowlan_mode: %d\n", pwrpriv->wowlan_mode);
+	DBG_871X("wowlan_pno_enable: %d\n", pwrpriv->wowlan_pno_enable);
+
+	if (pwrpriv->wowlan_mode == true) {
+		if (pnetdev)
+			rtw_netif_stop_queue(pnetdev);
+		/*  1. stop thread */
+		padapter->bDriverStopped = true;	/* for stop thread */
+		rtw_stop_drv_threads(padapter);
+		padapter->bDriverStopped = false;	/* for 32k command */
+
+		/*  2. disable interrupt */
+		if (padapter->intf_stop) {
+			padapter->intf_stop(padapter);
+		}
+
+		/*  2.1 clean interupt */
+		if (padapter->HalFunc.clear_interrupt)
+			padapter->HalFunc.clear_interrupt(padapter);
+
+		/*  2.2 free irq */
+		/* sdio_free_irq(adapter_to_dvobj(padapter)); */
+		if (padapter->intf_free_irq)
+			padapter->intf_free_irq(adapter_to_dvobj(padapter));
+
+		poidparam.subcode = WOWLAN_ENABLE;
+		padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WOWLAN, (u8 *)&poidparam);
+		if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME)) {
+			if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)
+				&& check_fwstate(pmlmepriv, _FW_LINKED))
+			{
+				DBG_871X("%s %s(" MAC_FMT "), length:%d assoc_ssid.length:%d\n", __func__,
+						pmlmepriv->cur_network.network.Ssid.Ssid,
+						MAC_ARG(pmlmepriv->cur_network.network.MacAddress),
+						pmlmepriv->cur_network.network.Ssid.SsidLength,
+						pmlmepriv->assoc_ssid.SsidLength);
+
+				rtw_set_to_roam(padapter, 0);
+			}
+		}
+
+		DBG_871X_LEVEL(_drv_always_, "%s: wowmode suspending\n", __func__);
+
+		if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) == true)
+		{
+			DBG_871X_LEVEL(_drv_always_, "%s: fw_under_survey\n", __func__);
+			rtw_indicate_scan_done(padapter, 1);
+			clr_fwstate(pmlmepriv, _FW_UNDER_SURVEY);
+		}
+
+		if (rtw_get_ch_setting_union(padapter, &ch, &bw, &offset) != 0) {
+			DBG_871X(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
+				FUNC_ADPT_ARG(padapter), ch, bw, offset);
+			set_channel_bwmode(padapter, ch, offset, bw);
+		}
+
+		if (pwrpriv->wowlan_pno_enable)
+			DBG_871X_LEVEL(_drv_always_, "%s: pno: %d\n", __func__, pwrpriv->wowlan_pno_enable);
+		else
+			rtw_set_ps_mode(padapter, PS_MODE_DTIM, 0, 0, "WOWLAN");
+
+	}
+	else
+	{
+		DBG_871X_LEVEL(_drv_always_, "%s: ### ERROR ### wowlan_mode =%d\n", __func__, pwrpriv->wowlan_mode);
+	}
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return ret;
+}
+#endif /* ifdef CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+int rtw_suspend_ap_wow(struct adapter *padapter)
+{
+	u8 ch, bw, offset;
+	struct net_device *pnetdev = padapter->pnetdev;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct wowlan_ioctl_param poidparam;
+	int ret = _SUCCESS;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+
+	pwrpriv->wowlan_ap_mode = true;
+
+	DBG_871X("wowlan_ap_mode: %d\n", pwrpriv->wowlan_ap_mode);
+
+	if (pnetdev)
+		rtw_netif_stop_queue(pnetdev);
+	/*  1. stop thread */
+	padapter->bDriverStopped = true;	/* for stop thread */
+	rtw_stop_drv_threads(padapter);
+	padapter->bDriverStopped = false;	/* for 32k command */
+
+	/*  2. disable interrupt */
+	rtw_hal_disable_interrupt(padapter); /*  It need wait for leaving 32K. */
+
+	/*  2.1 clean interupt */
+	if (padapter->HalFunc.clear_interrupt)
+		padapter->HalFunc.clear_interrupt(padapter);
+
+	/*  2.2 free irq */
+	/* sdio_free_irq(adapter_to_dvobj(padapter)); */
+	if (padapter->intf_free_irq)
+		padapter->intf_free_irq(adapter_to_dvobj(padapter));
+
+	poidparam.subcode = WOWLAN_AP_ENABLE;
+	padapter->HalFunc.SetHwRegHandler(padapter,
+					HW_VAR_AP_WOWLAN, (u8 *)&poidparam);
+
+	DBG_871X_LEVEL(_drv_always_, "%s: wowmode suspending\n", __func__);
+
+	if (rtw_get_ch_setting_union(padapter, &ch, &bw, &offset) != 0) {
+		DBG_871X(FUNC_ADPT_FMT" back to linked/linking union - ch:%u, bw:%u, offset:%u\n",
+			 FUNC_ADPT_ARG(padapter), ch, bw, offset);
+		set_channel_bwmode(padapter, ch, offset, bw);
+	}
+
+	rtw_set_ps_mode(padapter, PS_MODE_MIN, 0, 0, "AP-WOWLAN");
+
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return ret;
+}
+#endif /* ifdef CONFIG_AP_WOWLAN */
+
+
+static int rtw_suspend_normal(struct adapter *padapter)
+{
+	struct net_device *pnetdev = padapter->pnetdev;
+	int ret = _SUCCESS;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+	if (pnetdev) {
+		netif_carrier_off(pnetdev);
+		rtw_netif_stop_queue(pnetdev);
+	}
+
+	rtw_suspend_free_assoc_resource(padapter);
+
+	if ((rtw_hal_check_ips_status(padapter) == true)
+		|| (adapter_to_pwrctl(padapter)->rf_pwrstate == rf_off))
+	{
+		DBG_871X_LEVEL(_drv_always_, "%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));
+
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return ret;
+}
+
+int 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;
+
+	int ret = 0;
+	unsigned long start_time = jiffies;
+
+	DBG_871X_LEVEL(_drv_always_, " suspend start\n");
+	DBG_871X("==> %s (%s:%d)\n", __func__, current->comm, current->pid);
+	pdbgpriv->dbg_suspend_cnt++;
+
+	pwrpriv->bInSuspend = true;
+
+	while (pwrpriv->bips_processing == true)
+		msleep(1);
+
+	if ((!padapter->bup) || (padapter->bDriverStopped)||(padapter->bSurpriseRemoved))
+	{
+		DBG_871X("%s bup =%d bDriverStopped =%d bSurpriseRemoved = %d\n", __func__
+			, 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) == true) {
+		rtw_btcoex_SuspendNotify(padapter, 0);
+		DBG_871X("WIFI_AP_STATE\n");
+	} else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+		rtw_btcoex_SuspendNotify(padapter, 1);
+		DBG_871X("STATION\n");
+	}
+
+	rtw_ps_deny_cancel(padapter, PS_DENY_SUSPEND);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+	#ifdef CONFIG_WOWLAN
+		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+			pwrpriv->wowlan_mode = true;
+		} else if (pwrpriv->wowlan_pno_enable == true) {
+			pwrpriv->wowlan_mode |= pwrpriv->wowlan_pno_enable;
+		}
+
+		if (pwrpriv->wowlan_mode == true)
+		rtw_suspend_wow(padapter);
+		else
+			rtw_suspend_normal(padapter);
+
+	#else /* CONFIG_WOWLAN */
+		rtw_suspend_normal(padapter);
+	#endif /* CONFIG_WOWLAN */
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	#ifdef CONFIG_AP_WOWLAN
+		rtw_suspend_ap_wow(padapter);
+	#else
+		rtw_suspend_normal(padapter);
+	#endif /* CONFIG_AP_WOWLAN */
+	} else {
+		rtw_suspend_normal(padapter);
+	}
+
+	DBG_871X_LEVEL(_drv_always_, "rtw suspend success in %d ms\n",
+		jiffies_to_msecs(jiffies - start_time));
+
+exit:
+	DBG_871X("<===  %s return %d.............. in %dms\n", __func__
+		, ret, jiffies_to_msecs(jiffies - start_time));
+
+	return ret;
+}
+
+#ifdef CONFIG_WOWLAN
+int rtw_resume_process_wow(struct adapter *padapter)
+{
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct net_device *pnetdev = padapter->pnetdev;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	struct wowlan_ioctl_param poidparam;
+	struct sta_info *psta = NULL;
+	int ret = _SUCCESS;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+
+	if (padapter) {
+		pnetdev = padapter->pnetdev;
+		pwrpriv = adapter_to_pwrctl(padapter);
+	} else {
+		pdbgpriv->dbg_resume_error_cnt++;
+		ret = -1;
+		goto exit;
+	}
+
+	if (padapter->bDriverStopped || padapter->bSurpriseRemoved) {
+		DBG_871X("%s pdapter %p bDriverStopped %d bSurpriseRemoved %d\n",
+				__func__, padapter, padapter->bDriverStopped,
+				padapter->bSurpriseRemoved);
+		goto exit;
+	}
+
+#ifdef CONFIG_PNO_SUPPORT
+	pwrpriv->pno_in_resume = true;
+#endif
+
+	if (pwrpriv->wowlan_mode == true) {
+		rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "WOWLAN");
+
+		pwrpriv->bFwCurrentInPSMode = false;
+
+		if (padapter->intf_stop) {
+			padapter->intf_stop(padapter);
+		}
+
+		if (padapter->HalFunc.clear_interrupt)
+			padapter->HalFunc.clear_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;
+			RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("%s: sdio_alloc_irq Failed!!\n", __func__));
+			goto exit;
+		}
+
+		/* Disable WOW, set H2C command */
+		poidparam.subcode =WOWLAN_DISABLE;
+		padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WOWLAN, (u8 *)&poidparam);
+
+		psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
+		if (psta) {
+			set_sta_rate(padapter, psta);
+		}
+
+
+		padapter->bDriverStopped = false;
+		DBG_871X("%s: wowmode resuming, DriverStopped:%d\n", __func__, padapter->bDriverStopped);
+		rtw_start_drv_threads(padapter);
+
+		if (padapter->intf_start) {
+			padapter->intf_start(padapter);
+		}
+
+		/*  start netif queue */
+		if (pnetdev) {
+			if (!rtw_netif_queue_stopped(pnetdev))
+				rtw_netif_start_queue(pnetdev);
+			else
+				rtw_netif_wake_queue(pnetdev);
+		}
+	}
+	else {
+
+		DBG_871X_LEVEL(_drv_always_, "%s: ### ERROR ### wowlan_mode =%d\n", __func__, pwrpriv->wowlan_mode);
+	}
+
+	if (padapter->pid[1]!= 0) {
+		DBG_871X("pid[1]:%d\n", padapter->pid[1]);
+		rtw_signal_process(padapter->pid[1], SIGUSR2);
+	}
+
+	if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME)) {
+		if (pwrpriv->wowlan_wake_reason == FWDecisionDisconnect ||
+			pwrpriv->wowlan_wake_reason == Rx_DisAssoc ||
+			pwrpriv->wowlan_wake_reason == Rx_DeAuth) {
+
+			DBG_871X("%s: disconnect reason: %02x\n", __func__,
+						pwrpriv->wowlan_wake_reason);
+			rtw_indicate_disconnect(padapter);
+
+			rtw_sta_media_status_rpt(padapter,
+				rtw_get_stainfo(&padapter->stapriv,
+					get_bssid(&padapter->mlmepriv)), 0);
+
+			rtw_free_assoc_resources(padapter, 1);
+			pmlmeinfo->state = WIFI_FW_NULL_STATE;
+
+		} else {
+			DBG_871X("%s: do roaming\n", __func__);
+			rtw_roaming(padapter, NULL);
+		}
+	}
+
+	if (pwrpriv->wowlan_mode == true) {
+		pwrpriv->bips_processing = false;
+		_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+	} else {
+		DBG_871X_LEVEL(_drv_always_, "do not reset timer\n");
+	}
+
+	pwrpriv->wowlan_mode =false;
+
+	/* clean driver side wake up reason. */
+	pwrpriv->wowlan_wake_reason = 0;
+exit:
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return ret;
+}
+#endif /* ifdef CONFIG_WOWLAN */
+
+#ifdef CONFIG_AP_WOWLAN
+int rtw_resume_process_ap_wow(struct adapter *padapter)
+{
+	struct net_device *pnetdev = padapter->pnetdev;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	struct wowlan_ioctl_param poidparam;
+	int ret = _SUCCESS;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+
+	if (padapter) {
+		pnetdev = padapter->pnetdev;
+		pwrpriv = adapter_to_pwrctl(padapter);
+	} else {
+		pdbgpriv->dbg_resume_error_cnt++;
+		ret = -1;
+		goto exit;
+	}
+
+	rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "AP-WOWLAN");
+
+	pwrpriv->bFwCurrentInPSMode = false;
+
+	rtw_hal_disable_interrupt(padapter);
+
+	if (padapter->HalFunc.clear_interrupt)
+		padapter->HalFunc.clear_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;
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("%s: sdio_alloc_irq Failed!!\n", __func__));
+		goto exit;
+	}
+
+	/* Disable WOW, set H2C command */
+	poidparam.subcode = WOWLAN_AP_DISABLE;
+	padapter->HalFunc.SetHwRegHandler(padapter,
+		HW_VAR_AP_WOWLAN, (u8 *)&poidparam);
+	pwrpriv->wowlan_ap_mode = false;
+
+	padapter->bDriverStopped = false;
+	DBG_871X("%s: wowmode resuming, DriverStopped:%d\n", __func__, padapter->bDriverStopped);
+	rtw_start_drv_threads(padapter);
+
+	if (padapter->intf_start) {
+		padapter->intf_start(padapter);
+	}
+
+	/*  start netif queue */
+	if (pnetdev) {
+		if (!rtw_netif_queue_stopped(pnetdev))
+			rtw_netif_start_queue(pnetdev);
+		else
+			rtw_netif_wake_queue(pnetdev);
+	}
+
+	if (padapter->pid[1]!= 0) {
+		DBG_871X("pid[1]:%d\n", padapter->pid[1]);
+		rtw_signal_process(padapter->pid[1], SIGUSR2);
+	}
+
+	pwrpriv->bips_processing = false;
+	_set_timer(&padapter->mlmepriv.dynamic_chk_timer, 2000);
+
+	/* clean driver side wake up reason. */
+	pwrpriv->wowlan_wake_reason = 0;
+exit:
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(padapter));
+	return ret;
+}
+#endif /* ifdef CONFIG_APWOWLAN */
+
+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;
+
+	DBG_871X("==> "FUNC_ADPT_FMT" entry....\n", FUNC_ADPT_ARG(padapter));
+	/*  interface init */
+	/* if (sdio_init(adapter_to_dvobj(padapter)) != _SUCCESS) */
+	if ((padapter->intf_init) && (padapter->intf_init(adapter_to_dvobj(padapter)) != _SUCCESS))
+	{
+		ret = -1;
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("%s: initialize SDIO Failed!!\n", __func__));
+		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;
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("%s: sdio_alloc_irq Failed!!\n", __func__));
+		goto exit;
+	}
+
+	rtw_reset_drv_sw(padapter);
+	pwrpriv->bkeepfwalive = false;
+
+	DBG_871X("bkeepfwalive(%x)\n", pwrpriv->bkeepfwalive);
+	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) {
+		DBG_871X("pid[1]:%d\n", padapter->pid[1]);
+		rtw_signal_process(padapter->pid[1], SIGUSR2);
+	}
+
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+
+		if (rtw_chk_roam_flags(padapter, RTW_ROAM_ON_RESUME))
+			rtw_roaming(padapter, NULL);
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+		rtw_ap_restore_network(padapter);
+	} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE)) {
+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+	} else {
+		DBG_871X(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(pmlmepriv));
+	}
+
+	DBG_871X("<== "FUNC_ADPT_FMT" exit....\n", FUNC_ADPT_ARG(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);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
+	DBG_871X_LEVEL(_drv_always_, "resume start\n");
+	DBG_871X("==> %s (%s:%d)\n", __func__, current->comm, current->pid);
+
+	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
+	#ifdef CONFIG_WOWLAN
+		if (pwrpriv->wowlan_mode == true)
+			rtw_resume_process_wow(padapter);
+		else
+			rtw_resume_process_normal(padapter);
+	#else
+		rtw_resume_process_normal(padapter);
+	#endif
+
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
+	#ifdef CONFIG_AP_WOWLAN
+		rtw_resume_process_ap_wow(padapter);
+	#else
+		rtw_resume_process_normal(padapter);
+	#endif /* CONFIG_AP_WOWLAN */
+	} else {
+		rtw_resume_process_normal(padapter);
+	}
+
+	rtw_btcoex_SuspendNotify(padapter, 0);
+
+	if (pwrpriv) {
+		pwrpriv->bInSuspend = false;
+	#ifdef CONFIG_PNO_SUPPORT
+		pwrpriv->pno_in_resume = false;
+	#endif
+	}
+	DBG_871X_LEVEL(_drv_always_, "%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 000000000000..02db59e8b593
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/osdep_service.c
@@ -0,0 +1,481 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+
+
+#define _OSDEP_SERVICE_C_
+
+#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;
+}
+
+u8 *_rtw_malloc(u32 sz)
+{
+	u8 *pbuf = NULL;
+
+	pbuf = kmalloc(sz, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+
+	return pbuf;
+}
+
+u8 *_rtw_zmalloc(u32 sz)
+{
+	u8 *pbuf = _rtw_malloc(sz);
+
+	if (pbuf != NULL) {
+		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 struct sk_buff *_rtw_skb_clone(struct sk_buff *skb)
+{
+	return skb_clone(skb, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+}
+
+inline int _rtw_netif_rx(_nic_hdl ndev, struct sk_buff *skb)
+{
+	skb->dev = ndev;
+	return netif_rx(skb);
+}
+
+void rtw_init_timer(_timer *ptimer, void *padapter, void *pfunc)
+{
+	struct adapter *adapter = (struct adapter *)padapter;
+
+	_init_timer(ptimer, adapter->pnetdev, pfunc, adapter);
+}
+
+void _rtw_init_queue(struct __queue *pqueue)
+{
+	INIT_LIST_HEAD(&(pqueue->queue));
+
+	spin_lock_init(&(pqueue->lock));
+}
+
+/*
+* Open a file with the specific @param path, @param flag, @param mode
+* @param fpp the pointer of struct file pointer to get struct file pointer while file opening is success
+* @param path the path of the file to open
+* @param flag file operation flags, please refer to linux document
+* @param mode please refer to linux document
+* @return Linux specific error code
+*/
+static int openFile(struct file **fpp, char *path, int flag, int mode)
+{
+	struct file *fp;
+
+	fp =filp_open(path, flag, mode);
+	if (IS_ERR(fp)) {
+		*fpp = NULL;
+		return PTR_ERR(fp);
+	}
+	else {
+		*fpp =fp;
+		return 0;
+	}
+}
+
+/*
+* Close the file with the specific @param fp
+* @param fp the pointer of struct file to close
+* @return always 0
+*/
+static int closeFile(struct file *fp)
+{
+	filp_close(fp, NULL);
+	return 0;
+}
+
+static int readFile(struct file *fp, char *buf, int len)
+{
+	int rlen = 0, sum = 0;
+
+	if (!fp->f_op || !fp->f_op->read)
+		return -EPERM;
+
+	while (sum<len) {
+		rlen =fp->f_op->read(fp, (char __force __user *)buf+sum, len-sum, &fp->f_pos);
+		if (rlen>0)
+			sum+=rlen;
+		else if (0 != rlen)
+			return rlen;
+		else
+			break;
+	}
+
+	return  sum;
+
+}
+
+/*
+* Test if the specifi @param path is a file and readable
+* @param path the path of the file to test
+* @return Linux specific error code
+*/
+static int isFileReadable(char *path)
+{
+	struct file *fp;
+	int ret = 0;
+	mm_segment_t oldfs;
+	char buf;
+
+	fp =filp_open(path, O_RDONLY, 0);
+	if (IS_ERR(fp)) {
+		ret = PTR_ERR(fp);
+	}
+	else {
+		oldfs = get_fs(); set_fs(get_ds());
+
+		if (1!=readFile(fp, &buf, 1))
+			ret = PTR_ERR(fp);
+
+		set_fs(oldfs);
+		filp_close(fp, NULL);
+	}
+	return ret;
+}
+
+/*
+* Open the file with @param path and retrive the file content into memory starting from @param buf for @param sz at most
+* @param path the path of the file to open and read
+* @param buf the starting address of the buffer to store file content
+* @param sz how many bytes to read at most
+* @return the byte we've read, or Linux specific error code
+*/
+static int retriveFromFile(char *path, u8 *buf, u32 sz)
+{
+	int ret =-1;
+	mm_segment_t oldfs;
+	struct file *fp;
+
+	if (path && buf) {
+		if (0 == (ret =openFile(&fp, path, O_RDONLY, 0))) {
+			DBG_871X("%s openFile path:%s fp =%p\n", __func__, path , fp);
+
+			oldfs = get_fs(); set_fs(get_ds());
+			ret =readFile(fp, buf, sz);
+			set_fs(oldfs);
+			closeFile(fp);
+
+			DBG_871X("%s readFile, ret:%d\n", __func__, ret);
+
+		} else {
+			DBG_871X("%s openFile path:%s Fail, ret:%d\n", __func__, path, ret);
+		}
+	} else {
+		DBG_871X("%s NULL pointer\n", __func__);
+		ret =  -EINVAL;
+	}
+	return ret;
+}
+
+/*
+* Test if the specifi @param path is a file and readable
+* @param path the path of the file to test
+* @return true or false
+*/
+int rtw_is_file_readable(char *path)
+{
+	if (isFileReadable(path) == 0)
+		return true;
+	else
+		return false;
+}
+
+/*
+* Open the file with @param path and retrive the file content into memory starting from @param buf for @param sz at most
+* @param path the path of the file to open and read
+* @param buf the starting address of the buffer to store file content
+* @param sz how many bytes to read at most
+* @return the byte we've read
+*/
+int rtw_retrive_from_file(char *path, u8 *buf, u32 sz)
+{
+	int ret =retriveFromFile(path, buf, sz);
+	return ret>= 0?ret:0;
+}
+
+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;
+}
+
+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);
+
+	memcpy(pnetdev->dev_addr, padapter->eeprompriv.mac_addr, ETH_ALEN);
+
+	if (!rtnl_is_locked())
+		ret = register_netdev(pnetdev);
+	else
+		ret = register_netdevice(pnetdev);
+
+	if (ret != 0) {
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("register_netdev() failed\n"));
+		goto error;
+	}
+
+	return 0;
+
+error:
+
+	return -1;
+
+}
+
+u64 rtw_modular64(u64 x, u64 y)
+{
+	return do_div(x, y);
+}
+
+void rtw_buf_free(u8 **buf, u32 *buf_len)
+{
+	u32 ori_len;
+
+	if (!buf || !buf_len)
+		return;
+
+	ori_len = *buf_len;
+
+	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;
+
+	DBG_871X("%s on %u\n", __func__, cbuf->write);
+	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;
+
+        DBG_871X("%s on %u\n", __func__, cbuf->read);
+	buf = cbuf->bufs[cbuf->read];
+	cbuf->read = (cbuf->read+1)%cbuf->size;
+
+	return buf;
+}
+
+/**
+ * rtw_cbuf_alloc - allocte 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 = (struct rtw_cbuf *)rtw_malloc(sizeof(*cbuf) + sizeof(void*)*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 000000000000..e731ab4e2bd7
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
@@ -0,0 +1,365 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _RECV_OSDEP_C_
+
+#include <drv_types.h>
+#include <rtw_debug.h>
+#include <linux/jiffies.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 */
+int rtw_os_recv_resource_alloc(struct adapter *padapter, union recv_frame *precvframe)
+{
+	int	res = _SUCCESS;
+
+	precvframe->u.hdr.pkt_newalloc = precvframe->u.hdr.pkt = NULL;
+
+	return res;
+}
+
+/* free os related resource in union recv_frame */
+void rtw_os_recv_resource_free(struct recv_priv *precvpriv)
+{
+	sint 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)
+		{
+			dev_kfree_skb_any(precvframe->u.hdr.pkt);/* free skb by driver */
+			precvframe->u.hdr.pkt = NULL;
+		}
+		precvframe++;
+	}
+}
+
+/* free os related resource in struct recv_buf */
+int rtw_os_recvbuf_resource_free(struct adapter *padapter, struct recv_buf *precvbuf)
+{
+	int ret = _SUCCESS;
+
+	if (precvbuf->pskb)
+	{
+		dev_kfree_skb_any(precvbuf->pskb);
+	}
+	return ret;
+
+}
+
+_pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8 *pdata)
+{
+	u16 eth_type;
+	u8 *data_ptr;
+	_pkt *sub_skb;
+	struct rx_pkt_attrib *pattrib;
+
+	pattrib = &prframe->u.hdr.attrib;
+
+	sub_skb = rtw_skb_alloc(nSubframe_Length + 12);
+	if (sub_skb)
+	{
+		skb_reserve(sub_skb, 12);
+		data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
+		memcpy(data_ptr, (pdata + ETH_HLEN), nSubframe_Length);
+	}
+	else
+	{
+		sub_skb = rtw_skb_clone(prframe->u.hdr.pkt);
+		if (sub_skb)
+		{
+			sub_skb->data = pdata + ETH_HLEN;
+			sub_skb->len = nSubframe_Length;
+			skb_set_tail_pointer(sub_skb, nSubframe_Length);
+		}
+		else
+		{
+			DBG_871X("%s(): rtw_skb_clone() Fail!!!\n", __func__);
+			return NULL;
+		}
+	}
+
+	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);
+	}
+
+	return sub_skb;
+}
+
+void rtw_os_recv_indicate_pkt(struct adapter *padapter, _pkt *pkt, struct rx_pkt_attrib *pattrib)
+{
+	struct mlme_priv*pmlmepriv = &padapter->mlmepriv;
+	int ret;
+
+	/* Indicat the packets to upper layer */
+	if (pkt) {
+		if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
+		{
+			_pkt *pskb2 = NULL;
+			struct sta_info *psta = NULL;
+			struct sta_priv *pstapriv = &padapter->stapriv;
+			int bmcast = IS_MCAST(pattrib->dst);
+
+			/* DBG_871X("bmcast =%d\n", bmcast); */
+
+			if (memcmp(pattrib->dst, myid(&padapter->eeprompriv), ETH_ALEN))
+			{
+				/* DBG_871X("not ap psta =%p, addr =%pM\n", psta, pattrib->dst); */
+
+				if (bmcast)
+				{
+					psta = rtw_get_bcmc_stainfo(padapter);
+					pskb2 = rtw_skb_clone(pkt);
+				} else {
+					psta = rtw_get_stainfo(pstapriv, pattrib->dst);
+				}
+
+				if (psta)
+				{
+					struct net_device *pnetdev = (struct net_device*)padapter->pnetdev;
+
+					/* DBG_871X("directly forwarding to the rtw_xmit_entry\n"); */
+
+					/* 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 != NULL)) {
+						pkt = pskb2;
+						DBG_COUNTER(padapter->rx_logs.os_indicate_ap_mcast);
+					} else {
+						DBG_COUNTER(padapter->rx_logs.os_indicate_ap_forward);
+						return;
+					}
+				}
+			}
+			else/*  to APself */
+			{
+				/* DBG_871X("to APSelf\n"); */
+				DBG_COUNTER(padapter->rx_logs.os_indicate_ap_self);
+			}
+		}
+
+		pkt->protocol = eth_type_trans(pkt, padapter->pnetdev);
+		pkt->dev = padapter->pnetdev;
+
+#ifdef CONFIG_TCP_CSUM_OFFLOAD_RX
+		if ((pattrib->tcpchk_valid == 1) && (pattrib->tcp_chkrpt == 1)) {
+			pkt->ip_summed = CHECKSUM_UNNECESSARY;
+		} else {
+			pkt->ip_summed = CHECKSUM_NONE;
+		}
+#else /* !CONFIG_TCP_CSUM_OFFLOAD_RX */
+		pkt->ip_summed = CHECKSUM_NONE;
+#endif /* CONFIG_TCP_CSUM_OFFLOAD_RX */
+
+		ret = rtw_netif_rx(padapter->pnetdev, pkt);
+		if (ret == NET_RX_SUCCESS)
+			DBG_COUNTER(padapter->rx_logs.os_netif_ok);
+		else
+			DBG_COUNTER(padapter->rx_logs.os_netif_err);
+	}
+}
+
+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);
+}
+
+#ifdef CONFIG_AUTO_AP_MODE
+static void rtw_os_ksocket_send(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	_pkt *skb = precv_frame->u.hdr.pkt;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+	struct sta_info *psta = precv_frame->u.hdr.psta;
+
+	DBG_871X("eth rx: got eth_type = 0x%x\n", pattrib->eth_type);
+
+	if (psta && psta->isrc && psta->pid>0)
+	{
+		u16 rx_pid;
+
+		rx_pid = *(u16*)(skb->data+ETH_HLEN);
+
+		DBG_871X("eth rx(pid = 0x%x): sta("MAC_FMT") pid = 0x%x\n",
+			rx_pid, MAC_ARG(psta->hwaddr), psta->pid);
+
+		if (rx_pid == psta->pid)
+		{
+			int i;
+			u16 len = *(u16*)(skb->data+ETH_HLEN+2);
+			/* u16 ctrl_type = *(u16*)(skb->data+ETH_HLEN+4); */
+
+			/* DBG_871X("eth, RC: len = 0x%x, ctrl_type = 0x%x\n", len, ctrl_type); */
+			DBG_871X("eth, RC: len = 0x%x\n", len);
+
+			for (i = 0;i<len;i++)
+				DBG_871X("0x%x\n", *(skb->data+ETH_HLEN+4+i));
+				/* DBG_871X("0x%x\n", *(skb->data+ETH_HLEN+6+i)); */
+
+			DBG_871X("eth, RC-end\n");
+		}
+
+	}
+
+}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+int rtw_recv_indicatepkt(struct adapter *padapter, union recv_frame *precv_frame)
+{
+	struct recv_priv *precvpriv;
+	struct __queue	*pfree_recv_queue;
+	_pkt *skb;
+	struct rx_pkt_attrib *pattrib = &precv_frame->u.hdr.attrib;
+
+	DBG_COUNTER(padapter->rx_logs.os_indicate);
+
+	precvpriv = &(padapter->recvpriv);
+	pfree_recv_queue = &(precvpriv->free_recv_queue);
+
+	skb = precv_frame->u.hdr.pkt;
+	if (skb == NULL)
+	{
+		RT_TRACE(_module_recv_osdep_c_, _drv_err_, ("rtw_recv_indicatepkt():skb == NULL something wrong!!!!\n"));
+		goto _recv_indicatepkt_drop;
+	}
+
+	RT_TRACE(_module_recv_osdep_c_, _drv_info_, ("rtw_recv_indicatepkt():skb != NULL !!!\n"));
+	RT_TRACE(_module_recv_osdep_c_, _drv_info_, ("rtw_recv_indicatepkt():precv_frame->u.hdr.rx_head =%p  precv_frame->hdr.rx_data =%p\n", precv_frame->u.hdr.rx_head, precv_frame->u.hdr.rx_data));
+	RT_TRACE(_module_recv_osdep_c_, _drv_info_, ("precv_frame->hdr.rx_tail =%p precv_frame->u.hdr.rx_end =%p precv_frame->hdr.len =%d\n", precv_frame->u.hdr.rx_tail, precv_frame->u.hdr.rx_end, precv_frame->u.hdr.len));
+
+	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;
+
+	RT_TRACE(_module_recv_osdep_c_, _drv_info_, ("\n skb->head =%p skb->data =%p skb->tail =%p skb->end =%p skb->len =%d\n", skb->head, skb->data, skb_tail_pointer(skb), skb_end_pointer(skb), skb->len));
+
+#ifdef CONFIG_AUTO_AP_MODE
+	if (0x8899 == pattrib->eth_type)
+	{
+		rtw_os_ksocket_send(padapter, precv_frame);
+
+		/* goto _recv_indicatepkt_drop; */
+	}
+#endif /* CONFIG_AUTO_AP_MODE */
+
+	rtw_os_recv_indicate_pkt(padapter, skb, pattrib);
+
+	precv_frame->u.hdr.pkt = NULL; /*  pointers to NULL before rtw_free_recvframe() */
+
+	rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	RT_TRACE(_module_recv_osdep_c_, _drv_info_, ("\n rtw_recv_indicatepkt :after rtw_os_recv_indicate_pkt!!!!\n"));
+
+        return _SUCCESS;
+
+_recv_indicatepkt_drop:
+
+	 /* enqueue back to free_recv_queue */
+	 rtw_free_recvframe(precv_frame, pfree_recv_queue);
+
+	 DBG_COUNTER(padapter->rx_logs.os_indicate_err);
+	 return _FAIL;
+}
+
+void rtw_init_recv_timer(struct recv_reorder_ctrl *preorder_ctrl)
+{
+	struct adapter *padapter = preorder_ctrl->padapter;
+
+	_init_timer(&(preorder_ctrl->reordering_ctrl_timer), padapter->pnetdev, rtw_reordering_ctrl_timeout_handler, preorder_ctrl);
+
+}
diff --git a/drivers/staging/rtl8723bs/os_dep/rtw_proc.c b/drivers/staging/rtl8723bs/os_dep/rtw_proc.c
new file mode 100644
index 000000000000..92277457aba4
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/rtw_proc.c
@@ -0,0 +1,787 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 <drv_types.h>
+#include <rtw_debug.h>
+#include "rtw_proc.h"
+
+#ifdef PROC_DEBUG
+
+static struct proc_dir_entry *rtw_proc = NULL;
+
+#define RTW_PROC_NAME "rtl8723bs"
+
+#define get_proc_net init_net.proc_net
+
+inline struct proc_dir_entry *rtw_proc_create_dir(const char *name, struct proc_dir_entry *parent, void *data)
+{
+	struct proc_dir_entry *entry;
+
+	entry = proc_mkdir_data(name, S_IRUGO|S_IXUGO, parent, data);
+
+	return entry;
+}
+
+inline struct proc_dir_entry *rtw_proc_create_entry(const char *name, struct proc_dir_entry *parent,
+	const struct file_operations *fops, void *data)
+{
+	struct proc_dir_entry *entry;
+
+	entry = proc_create_data(name,  S_IFREG|S_IRUGO, parent, fops, data);
+
+	return entry;
+}
+
+static int proc_get_dummy(struct seq_file *m, void *v)
+{
+	return 0;
+}
+
+static int proc_get_drv_version(struct seq_file *m, void *v)
+{
+	dump_drv_version(m);
+	return 0;
+}
+
+static int proc_get_log_level(struct seq_file *m, void *v)
+{
+	dump_log_level(m);
+	return 0;
+}
+
+static ssize_t proc_set_log_level(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	char tmp[32];
+	int log_level;
+
+	if (count < 1)
+		return -EINVAL;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		sscanf(tmp, "%d ", &log_level);
+		if (log_level >= _drv_always_ && log_level <= _drv_debug_)
+		{
+			GlobalDebugLevel = log_level;
+			printk("%d\n", GlobalDebugLevel);
+		}
+	} else {
+		return -EFAULT;
+	}
+
+	return count;
+}
+
+/*
+* rtw_drv_proc:
+* init/deinit when register/unregister driver
+*/
+static const struct rtw_proc_hdl drv_proc_hdls [] = {
+	{"ver_info", proc_get_drv_version, NULL},
+	{"log_level", proc_get_log_level, proc_set_log_level},
+};
+
+static const int drv_proc_hdls_num = sizeof(drv_proc_hdls) / sizeof(struct rtw_proc_hdl);
+
+static int rtw_drv_proc_open(struct inode *inode, struct file *file)
+{
+	/* struct net_device *dev = proc_get_parent_data(inode); */
+	ssize_t index = (ssize_t)PDE_DATA(inode);
+	const struct rtw_proc_hdl *hdl = drv_proc_hdls+index;
+
+	return single_open(file, hdl->show, NULL);
+}
+
+static ssize_t rtw_drv_proc_write(struct file *file, const char __user *buffer, size_t count, loff_t *pos)
+{
+	ssize_t index = (ssize_t)PDE_DATA(file_inode(file));
+	const struct rtw_proc_hdl *hdl = drv_proc_hdls+index;
+	ssize_t (*write)(struct file *, const char __user *, size_t, loff_t *, void *) = hdl->write;
+
+	if (write)
+		return write(file, buffer, count, pos, NULL);
+
+	return -EROFS;
+}
+
+static const struct file_operations rtw_drv_proc_fops = {
+	.owner = THIS_MODULE,
+	.open = rtw_drv_proc_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.write = rtw_drv_proc_write,
+};
+
+int rtw_drv_proc_init(void)
+{
+	int ret = _FAIL;
+	ssize_t i;
+	struct proc_dir_entry *entry = NULL;
+
+	if (rtw_proc != NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	rtw_proc = rtw_proc_create_dir(RTW_PROC_NAME, get_proc_net, NULL);
+
+	if (rtw_proc == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	for (i = 0;i<drv_proc_hdls_num;i++) {
+		entry = rtw_proc_create_entry(drv_proc_hdls[i].name, rtw_proc, &rtw_drv_proc_fops, (void *)i);
+		if (!entry) {
+			rtw_warn_on(1);
+			goto exit;
+		}
+	}
+
+	ret = _SUCCESS;
+
+exit:
+	return ret;
+}
+
+void rtw_drv_proc_deinit(void)
+{
+	int i;
+
+	if (rtw_proc == NULL)
+		return;
+
+	for (i = 0;i<drv_proc_hdls_num;i++)
+		remove_proc_entry(drv_proc_hdls[i].name, rtw_proc);
+
+	remove_proc_entry(RTW_PROC_NAME, get_proc_net);
+	rtw_proc = NULL;
+}
+
+static int proc_get_sd_f0_reg_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	sd_f0_reg_dump(m, adapter);
+
+	return 0;
+}
+
+static int proc_get_mac_reg_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	mac_reg_dump(m, adapter);
+
+	return 0;
+}
+
+static int proc_get_bb_reg_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	bb_reg_dump(m, adapter);
+
+	return 0;
+}
+
+static int proc_get_rf_reg_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rf_reg_dump(m, adapter);
+
+	return 0;
+}
+static int proc_get_linked_info_dump(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	if (padapter)
+		DBG_871X_SEL_NL(m, "linked_info_dump :%s\n", (padapter->bLinkInfoDump)?"enable":"disable");
+
+	return 0;
+}
+
+static ssize_t proc_set_linked_info_dump(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	char tmp[2];
+	int mode = 0;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+		if (padapter)
+		{
+			/* padapter->bLinkInfoDump = mode; */
+			/* DBG_871X("linked_info_dump =%s\n", (padapter->bLinkInfoDump)?"enable":"disable"); */
+			 linked_info_dump(padapter, mode);
+		}
+
+	}
+
+	return count;
+
+}
+
+static int proc_get_rx_info(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	/* Counts of packets whose seq_num is less than preorder_ctrl->indicate_seq, Ex delay, retransmission, redundant packets and so on */
+	DBG_871X_SEL_NL(m,"Counts of Packets Whose Seq_Num Less Than Reorder Control Seq_Num: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_drop_count);
+	/* How many times the Rx Reorder Timer is triggered. */
+	DBG_871X_SEL_NL(m,"Rx Reorder Time-out Trigger Counts: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_forced_indicate_count);
+	/* Total counts of packets loss */
+	DBG_871X_SEL_NL(m,"Rx Packet Loss Counts: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_loss_count);
+	DBG_871X_SEL_NL(m,"Duplicate Management Frame Drop Count: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_dup_mgt_frame_drop_count);
+	DBG_871X_SEL_NL(m,"AMPDU BA window shift Count: %llu\n", (unsigned long long)pdbgpriv->dbg_rx_ampdu_window_shift_cnt);
+	return 0;
+}
+
+
+static ssize_t proc_reset_rx_info(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+	char cmd[32];
+	if (buffer && !copy_from_user(cmd, buffer, sizeof(cmd))) {
+		if ('0' == cmd[0]) {
+			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;
+		}
+	}
+
+	return count;
+}
+
+static int proc_get_cam(struct seq_file *m, void *v)
+{
+	return 0;
+}
+
+static ssize_t proc_set_cam(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter;
+
+	char tmp[32];
+	char cmd[5];
+	u8 id;
+
+	adapter = (struct adapter *)rtw_netdev_priv(dev);
+	if (!adapter)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		/* c <id>: clear specific cam entry */
+		/* wfc <id>: write specific cam entry from cam cache */
+
+		int num = sscanf(tmp, "%4s %hhu", cmd, &id);
+
+		if (num < 2)
+			return count;
+
+		if (strcmp("c", cmd) == 0) {
+			_clear_cam_entry(adapter, id);
+			adapter->securitypriv.hw_decrypted = false; /* temporarily set this for TX path to use SW enc */
+		} else if (strcmp("wfc", cmd) == 0) {
+			write_cam_from_cache(adapter, id);
+		}
+	}
+
+	return count;
+}
+
+static int proc_get_cam_cache(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapter);
+	u8 i;
+
+	DBG_871X_SEL_NL(m, "cam bitmap:0x%016llx\n", dvobj->cam_ctl.bitmap);
+
+	DBG_871X_SEL_NL(m, "%-2s %-6s %-17s %-32s %-3s %-7s"
+		/*  %-2s %-2s %-4s %-5s" */
+		"\n"
+		, "id", "ctrl", "addr", "key", "kid", "type"
+		/*  "MK", "GK", "MFB", "valid" */
+	);
+
+	for (i = 0;i<32;i++) {
+		if (dvobj->cam_cache[i].ctrl != 0)
+			DBG_871X_SEL_NL(m, "%2u 0x%04x "MAC_FMT" "KEY_FMT" %3u %-7s"
+				/*  %2u %2u 0x%02x %5u" */
+				"\n", i
+				, dvobj->cam_cache[i].ctrl
+				, MAC_ARG(dvobj->cam_cache[i].mac)
+				, KEY_ARG(dvobj->cam_cache[i].key)
+				, (dvobj->cam_cache[i].ctrl)&0x03
+				, security_type_str(((dvobj->cam_cache[i].ctrl)>>2)&0x07)
+				/*  ((dvobj->cam_cache[i].ctrl)>>5)&0x01 */
+				/*  ((dvobj->cam_cache[i].ctrl)>>6)&0x01 */
+				/*  ((dvobj->cam_cache[i].ctrl)>>8)&0x7f */
+				/*  ((dvobj->cam_cache[i].ctrl)>>15)&0x01 */
+			);
+	}
+
+	return 0;
+}
+
+/*
+* rtw_adapter_proc:
+* init/deinit when register/unregister net_device
+*/
+static const struct rtw_proc_hdl adapter_proc_hdls [] = {
+	{"write_reg", proc_get_dummy, proc_set_write_reg},
+	{"read_reg", proc_get_read_reg, proc_set_read_reg},
+	{"fwstate", proc_get_fwstate, NULL},
+	{"sec_info", proc_get_sec_info, NULL},
+	{"mlmext_state", proc_get_mlmext_state, NULL},
+	{"qos_option", proc_get_qos_option, NULL},
+	{"ht_option", proc_get_ht_option, NULL},
+	{"rf_info", proc_get_rf_info, NULL},
+	{"survey_info", proc_get_survey_info, NULL},
+	{"ap_info", proc_get_ap_info, NULL},
+	{"adapter_state", proc_get_adapter_state, NULL},
+	{"trx_info", proc_get_trx_info, NULL},
+	{"rate_ctl", proc_get_rate_ctl, proc_set_rate_ctl},
+	{"cam", proc_get_cam, proc_set_cam},
+	{"cam_cache", proc_get_cam_cache, NULL},
+	{"suspend_info", proc_get_suspend_resume_info, NULL},
+	{"rx_info", proc_get_rx_info, proc_reset_rx_info},
+
+	{"roam_flags", proc_get_roam_flags, proc_set_roam_flags},
+	{"roam_param", proc_get_roam_param, proc_set_roam_param},
+	{"roam_tgt_addr", proc_get_dummy, proc_set_roam_tgt_addr},
+
+	{"sd_f0_reg_dump", proc_get_sd_f0_reg_dump, NULL},
+
+	{"fwdl_test_case", proc_get_dummy, proc_set_fwdl_test_case},
+	{"wait_hiq_empty", proc_get_dummy, proc_set_wait_hiq_empty},
+
+	{"mac_reg_dump", proc_get_mac_reg_dump, NULL},
+	{"bb_reg_dump", proc_get_bb_reg_dump, NULL},
+	{"rf_reg_dump", proc_get_rf_reg_dump, NULL},
+
+	{"all_sta_info", proc_get_all_sta_info, NULL},
+
+	{"rx_signal", proc_get_rx_signal, proc_set_rx_signal},
+	{"hw_info", proc_get_hw_status, NULL},
+
+	{"ht_enable", proc_get_ht_enable, proc_set_ht_enable},
+	{"bw_mode", proc_get_bw_mode, proc_set_bw_mode},
+	{"ampdu_enable", proc_get_ampdu_enable, proc_set_ampdu_enable},
+	{"rx_stbc", proc_get_rx_stbc, proc_set_rx_stbc},
+	{"rx_ampdu", proc_get_rx_ampdu, proc_set_rx_ampdu},
+
+	{"en_fwps", proc_get_en_fwps, proc_set_en_fwps},
+
+	/* path_rssi", proc_get_two_path_rssi, NULL}, */
+	{"rssi_disp", proc_get_rssi_disp, proc_set_rssi_disp},
+
+	{"btcoex_dbg", proc_get_btcoex_dbg, proc_set_btcoex_dbg},
+	{"btcoex", proc_get_btcoex_info, NULL},
+
+	{"linked_info_dump", proc_get_linked_info_dump, proc_set_linked_info_dump},
+#ifdef CONFIG_DBG_COUNTER
+	{"rx_logs", proc_get_rx_logs, NULL},
+	{"tx_logs", proc_get_tx_logs, NULL},
+	{"int_logs", proc_get_int_logs, NULL},
+#endif
+};
+
+static const int adapter_proc_hdls_num = sizeof(adapter_proc_hdls) / sizeof(struct rtw_proc_hdl);
+
+static int rtw_adapter_proc_open(struct inode *inode, struct file *file)
+{
+	ssize_t index = (ssize_t)PDE_DATA(inode);
+	const struct rtw_proc_hdl *hdl = adapter_proc_hdls+index;
+
+	return single_open(file, hdl->show, proc_get_parent_data(inode));
+}
+
+static ssize_t rtw_adapter_proc_write(struct file *file, const char __user *buffer, size_t count, loff_t *pos)
+{
+	ssize_t index = (ssize_t)PDE_DATA(file_inode(file));
+	const struct rtw_proc_hdl *hdl = adapter_proc_hdls+index;
+	ssize_t (*write)(struct file *, const char __user *, size_t, loff_t *, void *) = hdl->write;
+
+	if (write)
+		return write(file, buffer, count, pos, ((struct seq_file *)file->private_data)->private);
+
+	return -EROFS;
+}
+
+static const struct file_operations rtw_adapter_proc_fops = {
+	.owner = THIS_MODULE,
+	.open = rtw_adapter_proc_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.write = rtw_adapter_proc_write,
+};
+
+int proc_get_odm_dbg_comp(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_odm_dbg_comp_msg(m, adapter);
+
+	return 0;
+}
+
+ssize_t proc_set_odm_dbg_comp(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+
+	u64 dbg_comp;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%llx", &dbg_comp);
+
+		if (num != 1)
+			return count;
+
+		rtw_odm_dbg_comp_set(adapter, dbg_comp);
+	}
+
+	return count;
+}
+
+int proc_get_odm_dbg_level(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_odm_dbg_level_msg(m, adapter);
+
+	return 0;
+}
+
+ssize_t proc_set_odm_dbg_level(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+
+	u32 dbg_level;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%u", &dbg_level);
+
+		if (num != 1)
+			return count;
+
+		rtw_odm_dbg_level_set(adapter, dbg_level);
+	}
+
+	return count;
+}
+
+static int proc_get_odm_ability(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_odm_ability_msg(m, adapter);
+
+	return 0;
+}
+
+static ssize_t proc_set_odm_ability(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *adapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+
+	u32 ability;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x", &ability);
+
+		if (num != 1)
+			return count;
+
+		rtw_odm_ability_set(adapter, ability);
+	}
+
+	return count;
+}
+
+int proc_get_odm_adaptivity(struct seq_file *m, void *v)
+{
+	struct net_device *dev = m->private;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+
+	rtw_odm_adaptivity_parm_msg(m, padapter);
+
+	return 0;
+}
+
+ssize_t proc_set_odm_adaptivity(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data)
+{
+	struct net_device *dev = data;
+	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
+	char tmp[32];
+	u32 TH_L2H_ini;
+	s8 TH_EDCCA_HL_diff;
+	u32 IGI_Base;
+	int ForceEDCCA;
+	u8 AdapEn_RSSI;
+	u8 IGI_LowerBound;
+
+	if (count < 1)
+		return -EFAULT;
+
+	if (buffer && !copy_from_user(tmp, buffer, sizeof(tmp))) {
+
+		int num = sscanf(tmp, "%x %hhd %x %d %hhu %hhu",
+			&TH_L2H_ini, &TH_EDCCA_HL_diff, &IGI_Base, &ForceEDCCA, &AdapEn_RSSI, &IGI_LowerBound);
+
+		if (num != 6)
+			return count;
+
+		rtw_odm_adaptivity_parm_set(padapter, (s8)TH_L2H_ini, TH_EDCCA_HL_diff, (s8)IGI_Base, (bool)ForceEDCCA, AdapEn_RSSI, IGI_LowerBound);
+	}
+
+	return count;
+}
+
+/*
+* rtw_odm_proc:
+* init/deinit when register/unregister net_device, along with rtw_adapter_proc
+*/
+static const struct rtw_proc_hdl odm_proc_hdls [] = {
+	{"dbg_comp", proc_get_odm_dbg_comp, proc_set_odm_dbg_comp},
+	{"dbg_level", proc_get_odm_dbg_level, proc_set_odm_dbg_level},
+	{"ability", proc_get_odm_ability, proc_set_odm_ability},
+	{"adaptivity", proc_get_odm_adaptivity, proc_set_odm_adaptivity},
+};
+
+static const int odm_proc_hdls_num = sizeof(odm_proc_hdls) / sizeof(struct rtw_proc_hdl);
+
+static int rtw_odm_proc_open(struct inode *inode, struct file *file)
+{
+	ssize_t index = (ssize_t)PDE_DATA(inode);
+	const struct rtw_proc_hdl *hdl = odm_proc_hdls+index;
+
+	return single_open(file, hdl->show, proc_get_parent_data(inode));
+}
+
+static ssize_t rtw_odm_proc_write(struct file *file, const char __user *buffer, size_t count, loff_t *pos)
+{
+	ssize_t index = (ssize_t)PDE_DATA(file_inode(file));
+	const struct rtw_proc_hdl *hdl = odm_proc_hdls+index;
+	ssize_t (*write)(struct file *, const char __user *, size_t, loff_t *, void *) = hdl->write;
+
+	if (write)
+		return write(file, buffer, count, pos, ((struct seq_file *)file->private_data)->private);
+
+	return -EROFS;
+}
+
+static const struct file_operations rtw_odm_proc_fops = {
+	.owner = THIS_MODULE,
+	.open = rtw_odm_proc_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.write = rtw_odm_proc_write,
+};
+
+static struct proc_dir_entry *rtw_odm_proc_init(struct net_device *dev)
+{
+	struct proc_dir_entry *dir_odm = NULL;
+	struct proc_dir_entry *entry = NULL;
+	struct adapter	*adapter = rtw_netdev_priv(dev);
+	ssize_t i;
+
+	if (adapter->dir_dev == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (adapter->dir_odm != NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	dir_odm = rtw_proc_create_dir("odm", adapter->dir_dev, dev);
+	if (dir_odm == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	adapter->dir_odm = dir_odm;
+
+	for (i = 0;i<odm_proc_hdls_num;i++) {
+		entry = rtw_proc_create_entry(odm_proc_hdls[i].name, dir_odm, &rtw_odm_proc_fops, (void *)i);
+		if (!entry) {
+			rtw_warn_on(1);
+			goto exit;
+		}
+	}
+
+exit:
+	return dir_odm;
+}
+
+static void rtw_odm_proc_deinit(struct adapter	*adapter)
+{
+	struct proc_dir_entry *dir_odm = NULL;
+	int i;
+
+	dir_odm = adapter->dir_odm;
+
+	if (dir_odm == NULL) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	for (i = 0;i<odm_proc_hdls_num;i++)
+		remove_proc_entry(odm_proc_hdls[i].name, dir_odm);
+
+	remove_proc_entry("odm", adapter->dir_dev);
+
+	adapter->dir_odm = NULL;
+}
+
+struct proc_dir_entry *rtw_adapter_proc_init(struct net_device *dev)
+{
+	struct proc_dir_entry *drv_proc = rtw_proc;
+	struct proc_dir_entry *dir_dev = NULL;
+	struct proc_dir_entry *entry = NULL;
+	struct adapter *adapter = rtw_netdev_priv(dev);
+	ssize_t i;
+
+	if (drv_proc == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	if (adapter->dir_dev != NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	dir_dev = rtw_proc_create_dir(dev->name, drv_proc, dev);
+	if (dir_dev == NULL) {
+		rtw_warn_on(1);
+		goto exit;
+	}
+
+	adapter->dir_dev = dir_dev;
+
+	for (i = 0;i<adapter_proc_hdls_num;i++) {
+		entry = rtw_proc_create_entry(adapter_proc_hdls[i].name, dir_dev, &rtw_adapter_proc_fops, (void *)i);
+		if (!entry) {
+			rtw_warn_on(1);
+			goto exit;
+		}
+	}
+
+	rtw_odm_proc_init(dev);
+
+exit:
+	return dir_dev;
+}
+
+void rtw_adapter_proc_deinit(struct net_device *dev)
+{
+	struct proc_dir_entry *drv_proc = rtw_proc;
+	struct proc_dir_entry *dir_dev = NULL;
+	struct adapter *adapter = rtw_netdev_priv(dev);
+	int i;
+
+	dir_dev = adapter->dir_dev;
+
+	if (dir_dev == NULL) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	for (i = 0;i<adapter_proc_hdls_num;i++)
+		remove_proc_entry(adapter_proc_hdls[i].name, dir_dev);
+
+	rtw_odm_proc_deinit(adapter);
+
+	remove_proc_entry(dev->name, drv_proc);
+
+	adapter->dir_dev = NULL;
+}
+
+void rtw_adapter_proc_replace(struct net_device *dev)
+{
+	struct proc_dir_entry *drv_proc = rtw_proc;
+	struct proc_dir_entry *dir_dev = NULL;
+	struct adapter *adapter = rtw_netdev_priv(dev);
+	int i;
+
+	dir_dev = adapter->dir_dev;
+
+	if (dir_dev == NULL) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	for (i = 0;i<adapter_proc_hdls_num;i++)
+		remove_proc_entry(adapter_proc_hdls[i].name, dir_dev);
+
+	rtw_odm_proc_deinit(adapter);
+
+	remove_proc_entry(adapter->old_ifname, drv_proc);
+
+	adapter->dir_dev = NULL;
+
+	rtw_adapter_proc_init(dev);
+
+}
+
+#endif /* PROC_DEBUG */
diff --git a/drivers/staging/rtl8723bs/os_dep/rtw_proc.h b/drivers/staging/rtl8723bs/os_dep/rtw_proc.h
new file mode 100644
index 000000000000..f633663fa790
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/rtw_proc.h
@@ -0,0 +1,45 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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 __RTW_PROC_H__
+#define __RTW_PROC_H__
+
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+
+struct rtw_proc_hdl {
+	char *name;
+	int (*show)(struct seq_file *, void *);
+	ssize_t (*write)(struct file *file, const char __user *buffer, size_t count, loff_t *pos, void *data);
+};
+
+#ifdef PROC_DEBUG
+
+int rtw_drv_proc_init(void);
+void rtw_drv_proc_deinit(void);
+struct proc_dir_entry *rtw_adapter_proc_init(struct net_device *dev);
+void rtw_adapter_proc_deinit(struct net_device *dev);
+void rtw_adapter_proc_replace(struct net_device *dev);
+
+#else //!PROC_DEBUG
+
+static inline int rtw_drv_proc_init(void) {return 0;}
+static inline void rtw_drv_proc_deinit(void) {}
+static inline struct proc_dir_entry *rtw_adapter_proc_init(struct net_device *dev){return NULL;}
+static inline void rtw_adapter_proc_deinit(struct net_device *dev){}
+static inline void rtw_adapter_proc_replace(struct net_device *dev){}
+
+#endif //!PROC_DEBUG
+
+#endif //__RTW_PROC_H__
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 000000000000..d2fb489d2e83
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
@@ -0,0 +1,695 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _HCI_INTF_C_
+
+#include <drv_types.h>
+#include <rtw_debug.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, 0x0623), },
+	{ SDIO_DEVICE(0x024c, 0x0626), },
+	{ SDIO_DEVICE(0x024c, 0xb723), },
+	{ /* end: all zeroes */				},
+};
+static const struct acpi_device_id acpi_ids[] = {
+	{"OBDA8723", 0x0000},
+	{}
+};
+
+MODULE_DEVICE_TABLE(sdio, sdio_ids);
+MODULE_DEVICE_TABLE(acpi, acpi_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,
+};
+
+struct sdio_drv_priv {
+	struct sdio_driver r871xs_drv;
+	int drv_registered;
+};
+
+static struct sdio_drv_priv sdio_drvpriv = {
+	.r871xs_drv.probe = rtw_drv_init,
+	.r871xs_drv.remove = rtw_dev_remove,
+	.r871xs_drv.name = "rtl8723bs",
+	.r871xs_drv.id_table = sdio_ids,
+	.r871xs_drv.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) {
+		DBG_871X("%s if1 == NULL\n", __func__);
+		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)
+{
+	PSDIO_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)
+{
+    PSDIO_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++;
+				DBG_871X_LEVEL(_drv_err_,"%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;
+    }
+}
+
+#ifdef CONFIG_GPIO_WAKEUP
+extern unsigned int oob_irq;
+static irqreturn_t gpio_hostwakeup_irq_thread(int irq, void *data)
+{
+	struct adapter *padapter = (struct adapter *)data;
+	DBG_871X_LEVEL(_drv_always_, "gpio_hostwakeup_irq_thread\n");
+	/* Disable interrupt before calling handler */
+	/* disable_irq_nosync(oob_irq); */
+	rtw_lock_suspend_timeout(HZ/2);
+	return IRQ_HANDLED;
+}
+
+static u8 gpio_hostwakeup_alloc_irq(struct adapter *padapter)
+{
+	int err;
+	if (oob_irq == 0) {
+		DBG_871X("oob_irq ZERO!\n");
+		return _FAIL;
+	}
+	/* dont set it IRQF_TRIGGER_LOW, or wowlan */
+	/* power is high after suspend */
+	/* and failing can prevent can not sleep issue if */
+	/* wifi gpio12 pin is not linked with CPU */
+	err = request_threaded_irq(oob_irq, gpio_hostwakeup_irq_thread, NULL,
+		/* IRQF_TRIGGER_LOW | IRQF_ONESHOT, */
+		IRQF_TRIGGER_FALLING,
+		"rtw_wifi_gpio_wakeup", padapter);
+	if (err < 0) {
+		DBG_871X("Oops: can't allocate gpio irq %d err:%d\n", oob_irq, err);
+		return false;
+	} else {
+		DBG_871X("allocate gpio irq %d ok\n", oob_irq);
+	}
+
+	enable_irq_wake(oob_irq);
+	return _SUCCESS;
+}
+
+static void gpio_hostwakeup_free_irq(struct adapter *padapter)
+{
+	if (oob_irq == 0)
+		return;
+
+	disable_irq_wake(oob_irq);
+	free_irq(oob_irq, padapter);
+}
+#endif
+
+static u32 sdio_init(struct dvobj_priv *dvobj)
+{
+	PSDIO_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++;
+		DBG_8192C(KERN_CRIT "%s: sdio_enable_func FAIL(%d)!\n", __func__, err);
+		goto release;
+	}
+
+	err = sdio_set_block_size(func, 512);
+	if (err) {
+		dvobj->drv_dbg.dbg_sdio_init_error_cnt++;
+		DBG_8192C(KERN_CRIT "%s: sdio_set_block_size FAIL(%d)!\n", __func__, err);
+		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;
+
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("+sdio_deinit\n"));
+
+	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++;
+			DBG_8192C(KERN_ERR "%s: sdio_disable_func(%d)\n", __func__, err);
+		}
+
+		if (dvobj->irq_alloc) {
+			err = sdio_release_irq(func);
+			if (err)
+			{
+				dvobj->drv_dbg.dbg_sdio_free_irq_error_cnt++;
+				DBG_8192C(KERN_ERR "%s: sdio_release_irq(%d)\n", __func__, err);
+			}
+			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;
+	PSDIO_DATA psdio;
+
+	dvobj = devobj_init();
+	if (dvobj == NULL) {
+		goto exit;
+	}
+
+	sdio_set_drvdata(func, dvobj);
+
+	psdio = &dvobj->intf_data;
+	psdio->func = func;
+
+	if (sdio_init(dvobj) != _SUCCESS) {
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("%s: initialize SDIO Failed!\n", __func__));
+		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);
+	}
+	return;
+}
+
+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 == NULL) {
+		DBG_8192C(KERN_ERR "%s: padapter is NULL!\n", __func__);
+		return;
+	}
+
+	/*  hal dep */
+	rtw_hal_enable_interrupt(padapter);
+}
+
+static void sd_intf_stop(struct adapter *padapter)
+{
+	if (padapter == NULL) {
+		DBG_8192C(KERN_ERR "%s: padapter is NULL!\n", __func__);
+		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;
+	PSDIO_DATA psdio = &dvobj->intf_data;
+
+	padapter = (struct adapter *)vzalloc(sizeof(*padapter));
+	if (padapter == NULL) {
+		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);
+
+	rtw_wdev_alloc(padapter, dvobj_to_dev(dvobj));
+
+	/* 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)
+	{
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
+			("rtw_drv_init: Can't init io_priv\n"));
+		goto free_hal_data;
+	}
+
+	rtw_hal_read_chip_version(padapter);
+
+	rtw_hal_chip_configure(padapter);
+
+	rtw_btcoex_Initialize(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) {
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_,
+			 ("rtw_drv_init: Initialize driver software resource Failed!\n"));
+		goto free_hal_data;
+	}
+
+	/* 3 8. get WLan MAC address */
+	/*  set mac addr */
+	rtw_macaddr_cfg(&psdio->func->dev, padapter->eeprompriv.mac_addr);
+
+	rtw_hal_disable_interrupt(padapter);
+
+	DBG_871X("bDriverStopped:%d, bSurpriseRemoved:%d, bup:%d, hw_init_completed:%d\n"
+		, padapter->bDriverStopped
+		, padapter->bSurpriseRemoved
+		, padapter->bup
+		, padapter->hw_init_completed
+	);
+
+	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);
+
+#ifdef CONFIG_GPIO_WAKEUP
+	gpio_hostwakeup_free_irq(if1);
+#endif
+
+	rtw_cancel_all_timer(if1);
+
+#ifdef CONFIG_WOWLAN
+	adapter_to_pwrctl(if1)->wowlan_mode =false;
+	DBG_871X_LEVEL(_drv_always_, "%s wowlan_mode:%d\n", __func__, adapter_to_pwrctl(if1)->wowlan_mode);
+#endif /* CONFIG_WOWLAN */
+
+	rtw_dev_unload(if1);
+	DBG_871X("+r871xu_dev_remove, hw_init_completed =%d\n", if1->hw_init_completed);
+
+	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, *if2 = NULL;
+	struct dvobj_priv *dvobj;
+
+	dvobj = sdio_dvobj_init(func);
+	if (dvobj == NULL) {
+		RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("initialize device object priv Failed!\n"));
+		goto exit;
+	}
+
+	if1 = rtw_sdio_if1_init(dvobj, id);
+	if (if1 == NULL) {
+		DBG_871X("rtw_init_primarystruct adapter Failed!\n");
+		goto free_dvobj;
+	}
+
+	/* dev_alloc_name && register_netdev */
+	status = rtw_drv_register_netdev(if1);
+	if (status != _SUCCESS) {
+		goto free_if2;
+	}
+
+	if (sdio_alloc_irq(dvobj) != _SUCCESS)
+		goto free_if2;
+
+#ifdef	CONFIG_GPIO_WAKEUP
+	gpio_hostwakeup_alloc_irq(if1);
+#endif
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_err_, ("-871x_drv - drv_init, success!\n"));
+
+	rtw_ndev_notifier_register();
+	status = _SUCCESS;
+
+free_if2:
+	if (status != _SUCCESS && if2) {
+	}
+	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;
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("+rtw_dev_remove\n"));
+
+	dvobj->processing_dev_remove = true;
+
+	rtw_unregister_netdevs(dvobj);
+
+	if (padapter->bSurpriseRemoved == false) {
+		int err;
+
+		/* test surprise remove */
+		sdio_claim_host(func);
+		sdio_readb(func, 0, &err);
+		sdio_release_host(func);
+		if (err == -ENOMEDIUM) {
+			padapter->bSurpriseRemoved = true;
+			DBG_871X(KERN_NOTICE "%s: device had been removed!\n", __func__);
+		}
+	}
+
+	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);
+
+	RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("-rtw_dev_remove\n"));
+}
+
+extern int pm_netdev_open(struct net_device *pnetdev, u8 bnormal);
+extern int pm_netdev_close(struct net_device *pnetdev, u8 bnormal);
+
+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 == true)
+	{
+		DBG_871X("%s bDriverStopped = %d\n", __func__, padapter->bDriverStopped);
+		return 0;
+	}
+
+	if (pwrpriv->bInSuspend == true)
+	{
+		DBG_871X("%s bInSuspend = %d\n", __func__, pwrpriv->bInSuspend);
+		pdbgpriv->dbg_suspend_error_cnt++;
+		return 0;
+	}
+
+	return rtw_suspend_common(padapter);
+}
+
+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 == false)
+	{
+		pdbgpriv->dbg_resume_error_cnt++;
+		DBG_871X("%s bInSuspend = %d\n", __func__, pwrpriv->bInSuspend);
+		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 pwrctrl_priv *pwrpriv = dvobj_to_pwrctl(psdpriv);
+	struct adapter *padapter = psdpriv->if1;
+	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
+	int ret = 0;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	DBG_871X("==> %s (%s:%d)\n", __func__, current->comm, current->pid);
+
+	pdbgpriv->dbg_resume_cnt++;
+
+	if (pwrpriv->bInternalAutoSuspend)
+	{
+		ret = rtw_resume_process(padapter);
+	}
+	else
+	{
+		if (pwrpriv->wowlan_mode || pwrpriv->wowlan_ap_mode)
+		{
+			ret = rtw_resume_process(padapter);
+		}
+		else
+		{
+			ret = rtw_resume_process(padapter);
+		}
+	}
+	pmlmeext->last_scan_time = jiffies;
+	DBG_871X("<========  %s return %d\n", __func__, ret);
+	return ret;
+
+}
+
+static int __init rtw_drv_entry(void)
+{
+	int ret = 0;
+
+	DBG_871X_LEVEL(_drv_always_, "module init start\n");
+	dump_drv_version(RTW_DBGDUMP);
+#ifdef BTCOEXVERSION
+	DBG_871X_LEVEL(_drv_always_, "rtl8723bs BT-Coex version = %s\n", BTCOEXVERSION);
+#endif /*  BTCOEXVERSION */
+
+	sdio_drvpriv.drv_registered = true;
+	rtw_drv_proc_init();
+
+	ret = sdio_register_driver(&sdio_drvpriv.r871xs_drv);
+	if (ret != 0)
+	{
+		sdio_drvpriv.drv_registered = false;
+		rtw_drv_proc_deinit();
+		rtw_ndev_notifier_unregister();
+		DBG_871X("%s: register driver failed!!(%d)\n", __func__, ret);
+		goto exit;
+	}
+
+	goto exit;
+
+exit:
+	DBG_871X_LEVEL(_drv_always_, "module init ret =%d\n", ret);
+	return ret;
+}
+
+static void __exit rtw_drv_halt(void)
+{
+	DBG_871X_LEVEL(_drv_always_, "module exit start\n");
+
+	sdio_drvpriv.drv_registered = false;
+
+	sdio_unregister_driver(&sdio_drvpriv.r871xs_drv);
+
+	rtw_drv_proc_deinit();
+	rtw_ndev_notifier_unregister();
+
+	DBG_871X_LEVEL(_drv_always_, "module exit success\n");
+
+	rtw_mstat_dump(RTW_DBGDUMP);
+}
+
+
+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 000000000000..33f0f83b002d
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_ops_linux.c
@@ -0,0 +1,599 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ *******************************************************************************/
+#define _SDIO_OPS_LINUX_C_
+
+#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);
+	PSDIO_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)
+{
+	PSDIO_DATA sdio_data = &dvobj->intf_data;
+
+	sdio_data->sys_sdio_irq_thd = thd_hdl;
+}
+
+u8 sd_f0_read8(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		return v;
+	}
+
+	func = psdio->func;
+	claim_needed = rtw_sdio_claim_host_needed(func);
+
+	if (claim_needed)
+		sdio_claim_host(func);
+	v = sdio_f0_readb(func, addr, err);
+	if (claim_needed)
+		sdio_release_host(func);
+	if (err && *err)
+		DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x\n", __func__, *err, addr);
+	return v;
+}
+
+/*
+ * 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;
+	PSDIO_DATA psdio;
+
+	int err = 0, i;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		return err;
+	}
+
+	func = psdio->func;
+
+	for (i = 0; i < cnt; i++) {
+		pdata[i] = sdio_readb(func, addr+i, &err);
+		if (err) {
+			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x\n", __func__, err, addr+i);
+			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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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;
+	PSDIO_DATA psdio;
+
+	int err = 0, i;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		return err;
+	}
+
+	func = psdio->func;
+
+	for (i = 0; i < cnt; i++) {
+		sdio_writeb(func, pdata[i], addr+i, &err);
+		if (err) {
+			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x val = 0x%02x\n", __func__, err, addr+i, pdata[i]);
+			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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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);
+	if (err && *err)
+		DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x\n", __func__, *err, addr);
+	return v;
+}
+
+u32 sd_read32(struct intf_hdl *pintfhdl, u32 addr, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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;
+
+		DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x\n", __func__, *err, addr, v);
+
+		*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 {
+				DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
+				if ((-ESHUTDOWN == *err) || (-ENODEV == *err)) {
+					padapter->bSurpriseRemoved = true;
+				}
+
+				if (rtw_inc_and_chk_continual_io_error(psdiodev) == true) {
+					padapter->bSurpriseRemoved = true;
+					break;
+				}
+			}
+		}
+
+		if (i ==SD_IO_TRY_CNT)
+			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
+		else
+			DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
+
+	}
+	return  v;
+}
+
+void sd_write8(struct intf_hdl *pintfhdl, u32 addr, u8 v, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	PSDIO_DATA psdio;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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);
+	if (err && *err)
+		DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x val = 0x%02x\n", __func__, *err, addr, v);
+}
+
+void sd_write32(struct intf_hdl *pintfhdl, u32 addr, u32 v, s32 *err)
+{
+	struct adapter *padapter;
+	struct dvobj_priv *psdiodev;
+	PSDIO_DATA psdio;
+	struct sdio_func *func;
+	bool claim_needed;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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;
+
+		DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x val = 0x%08x\n", __func__, *err, addr, v);
+
+		*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 {
+				DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x, val = 0x%x, try_cnt =%d\n", __func__, *err, addr, v, i);
+				if ((-ESHUTDOWN == *err) || (-ENODEV == *err)) {
+					padapter->bSurpriseRemoved = true;
+				}
+
+				if (rtw_inc_and_chk_continual_io_error(psdiodev) == true) {
+					padapter->bSurpriseRemoved = true;
+					break;
+				}
+			}
+		}
+
+		if (i ==SD_IO_TRY_CNT)
+			DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x val = 0x%08x, try_cnt =%d\n", __func__, *err, addr, v, i);
+		else
+			DBG_871X(KERN_ERR "%s: (%d) addr = 0x%05x val = 0x%08x, try_cnt =%d\n", __func__, *err, addr, v, i);
+	}
+}
+
+/*
+ * 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;
+	PSDIO_DATA psdio;
+
+	int err = -EPERM;
+	struct sdio_func *func;
+
+	padapter = pintfhdl->padapter;
+	psdiodev = pintfhdl->pintf_dev;
+	psdio = &psdiodev->intf_data;
+
+	if (padapter->bSurpriseRemoved) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		return err;
+	}
+
+	func = psdio->func;
+
+	if (unlikely((cnt == 1) || (cnt ==2)))
+	{
+		int i;
+		u8 *pbuf = (u8 *)pdata;
+
+		for (i = 0; i < cnt; i++)
+		{
+			*(pbuf+i) = sdio_readb(func, addr+i, &err);
+
+			if (err) {
+				DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x\n", __func__, err, addr);
+				break;
+			}
+		}
+		return err;
+	}
+
+	err = sdio_memcpy_fromio(func, pdata, addr, cnt);
+	if (err) {
+		DBG_871X(KERN_ERR "%s: FAIL(%d)! ADDR =%#x Size =%d\n", __func__, err, 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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		return err;
+	}
+
+	func = psdio->func;
+/* 	size = sdio_align_size(func, cnt); */
+
+	if (unlikely((cnt == 1) || (cnt ==2)))
+	{
+		int i;
+		u8 *pbuf = (u8 *)pdata;
+
+		for (i = 0; i < cnt; i++)
+		{
+			sdio_writeb(func, *(pbuf+i), addr+i, &err);
+			if (err) {
+				DBG_871X(KERN_ERR "%s: FAIL!(%d) addr = 0x%05x val = 0x%02x\n", __func__, err, addr, *(pbuf+i));
+				break;
+			}
+		}
+
+		return err;
+	}
+
+	size = cnt;
+	err = sdio_memcpy_toio(func, addr, pdata, size);
+	if (err) {
+		DBG_871X(KERN_ERR "%s: FAIL(%d)! ADDR =%#x Size =%d(%d)\n", __func__, err, addr, cnt, 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;
+	PSDIO_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) {
+		/* DBG_871X(" %s (padapter->bSurpriseRemoved ||adapter->pwrctrlpriv.pnp_bstop_trx)!!!\n", __func__); */
+		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 000000000000..9c61125f5910
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/wifi_regd.c
@@ -0,0 +1,164 @@
+/******************************************************************************
+ *
+ * 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)
+
+/* 2G chan 14, PASSIVS SCAN, NO OFDM (B only) */
+#define RTW_2GHZ_CH14	\
+	REG_RULE(2484-10, 2484+10, 40, 0, 20,	\
+	NL80211_RRF_PASSIVE_SCAN | NL80211_RRF_NO_OFDM)
+
+static const struct ieee80211_regdomain rtw_regdom_rd = {
+	.n_reg_rules = 3,
+	.alpha2 = "99",
+	.reg_rules = {
+		      RTW_2GHZ_CH01_11,
+		      RTW_2GHZ_CH12_13,
+		      }
+};
+
+static int rtw_ieee80211_channel_to_frequency(int chan, int band)
+{
+	/* see 802.11 17.3.8.3.2 and Annex J
+	 * there are overlapping channel numbers in 5GHz and 2GHz bands */
+
+	/* 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;
+	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);
+}
+
+int rtw_regd_init(struct adapter *padapter,
+		  void (*reg_notifier) (struct wiphy * wiphy,
+				       struct regulatory_request *request))
+{
+	/* struct registry_priv  *registrypriv = &padapter->registrypriv; */
+	struct wiphy *wiphy = padapter->rtw_wdev->wiphy;
+	_rtw_regd_init_wiphy(NULL, wiphy, reg_notifier);
+
+	return 0;
+}
+
+void rtw_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
+{
+	struct rtw_regulatory *reg = NULL;
+
+	DBG_8192C("%s\n", __func__);
+
+	_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 000000000000..76968161f936
--- /dev/null
+++ b/drivers/staging/rtl8723bs/os_dep/xmit_linux.c
@@ -0,0 +1,296 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License 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.
+ *
+ ******************************************************************************/
+#define _XMIT_OSDEP_C_
+
+#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 (_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 _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;
+}
+
+sint rtw_endofpktfile(struct pkt_file *pfile)
+{
+	if (pfile->pkt_len == 0)
+		return true;
+	return false;
+}
+
+void rtw_set_tx_chksum_offload(_pkt *pkt, struct pkt_attrib *pattrib)
+{
+
+}
+
+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 == NULL)
+		{
+			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, _pkt *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))
+		up(&pri_adapter->xmitpriv.xmit_sema);
+}
+
+static void rtw_check_xmit_resource(struct adapter *padapter, _pkt *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) {
+			/* DBG_871X("%s(): stop netif_subqueue[%d]\n", __func__, queue); */
+			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;
+
+	DBG_COUNTER(padapter->tx_logs.os_tx_m2u);
+
+	spin_lock_bh(&pstapriv->asoc_list_lock);
+	phead = &pstapriv->asoc_list;
+	plist = get_next(phead);
+
+	/* free sta asoc_queue */
+	while (phead != plist) {
+		int stainfo_offset;
+		psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
+		plist = get_next(plist);
+
+		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))
+		{
+			DBG_COUNTER(padapter->tx_logs.os_tx_m2u_ignore_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)
+		)
+		{
+			DBG_COUNTER(padapter->tx_logs.os_tx_m2u_ignore_self);
+			continue;
+		}
+
+		DBG_COUNTER(padapter->tx_logs.os_tx_m2u_entry);
+
+		newskb = rtw_skb_copy(skb);
+
+		if (newskb) {
+			memcpy(newskb->data, psta->hwaddr, 6);
+			res = rtw_xmit(padapter, &newskb);
+			if (res < 0) {
+				DBG_COUNTER(padapter->tx_logs.os_tx_m2u_entry_err_xmit);
+				DBG_871X("%s()-%d: rtw_xmit() return error!\n", __func__, __LINE__);
+				pxmitpriv->tx_drop++;
+				dev_kfree_skb_any(newskb);
+			}
+		} else {
+			DBG_COUNTER(padapter->tx_logs.os_tx_m2u_entry_err_skb);
+			DBG_871X("%s-%d: rtw_skb_copy() failed!\n", __func__, __LINE__);
+			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;
+}
+
+int _rtw_xmit_entry(_pkt *pkt, _nic_hdl 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;
+
+	DBG_COUNTER(padapter->tx_logs.os_tx);
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, ("+xmit_enry\n"));
+
+	if (rtw_if_up(padapter) == false) {
+		DBG_COUNTER(padapter->tx_logs.os_tx_err_up);
+		RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit_entry: rtw_if_up fail\n"));
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s if_up fail\n", __func__);
+		#endif
+		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)
+			#ifdef CONFIG_TX_BCAST2UNI
+			|| is_broadcast_mac_addr(pkt->data)
+			#endif
+			)
+		&& (padapter->registrypriv.wifi_spec == 0)
+		)
+	{
+		if (pxmitpriv->free_xmitframe_cnt > (NR_XMITFRAME/4)) {
+			res = rtw_mlcst2unicst(padapter, pkt);
+			if (res == true) {
+				goto exit;
+			}
+		} else {
+			/* DBG_871X("Stop M2U(%d, %d)! ", pxmitpriv->free_xmitframe_cnt, pxmitpriv->free_xmitbuf_cnt); */
+			/* DBG_871X("!m2u); */
+			DBG_COUNTER(padapter->tx_logs.os_tx_m2u_stop);
+		}
+	}
+
+	res = rtw_xmit(padapter, &pkt);
+	if (res < 0) {
+		#ifdef DBG_TX_DROP_FRAME
+		DBG_871X("DBG_TX_DROP_FRAME %s rtw_xmit fail\n", __func__);
+		#endif
+		goto drop_packet;
+	}
+
+	RT_TRACE(_module_xmit_osdep_c_, _drv_info_, ("rtw_xmit_entry: tx_pkts =%d\n", (u32)pxmitpriv->tx_pkts));
+	goto exit;
+
+drop_packet:
+	pxmitpriv->tx_drop++;
+	dev_kfree_skb_any(pkt);
+	RT_TRACE(_module_xmit_osdep_c_, _drv_notice_, ("rtw_xmit_entry: drop, tx_drop =%d\n", (u32)pxmitpriv->tx_drop));
+
+exit:
+	return 0;
+}
+
+int rtw_xmit_entry(_pkt *pkt, _nic_hdl pnetdev)
+{
+	int ret = 0;
+
+	if (pkt) {
+		rtw_mstat_update(MSTAT_TYPE_SKB, MSTAT_ALLOC_SUCCESS, pkt->truesize);
+		ret = _rtw_xmit_entry(pkt, pnetdev);
+	}
+
+	return ret;
+}
diff --git a/drivers/staging/rts5208/ms.c b/drivers/staging/rts5208/ms.c
index 806c12180714..482a29dd06f8 100644
--- a/drivers/staging/rts5208/ms.c
+++ b/drivers/staging/rts5208/ms.c
@@ -2594,7 +2594,7 @@ static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
 	u16 start, end, phy_blk, log_blk, tmp_blk, idx;
 	u8 extra[MS_EXTRA_SIZE], us1, us2;
 
-	dev_dbg(rtsx_dev(chip), "ms_build_l2p_tbl: %d\n", seg_no);
+	dev_dbg(rtsx_dev(chip), "%s: %d\n", __func__, seg_no);
 
 	if (!ms_card->segment) {
 		retval = ms_init_l2p_tbl(chip);
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 3511157a2c78..7f4107bffe31 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -1490,7 +1490,7 @@ int rtsx_write_register(struct rtsx_chip *chip, u16 addr, u8 mask, u8 data)
 
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
 		val = rtsx_readl(chip, RTSX_HAIMR);
-		if ((val & (1 << 31)) == 0) {
+		if ((val & BIT(31)) == 0) {
 			if (data != (u8)val) {
 				rtsx_trace(chip);
 				return STATUS_FAIL;
@@ -1518,7 +1518,7 @@ int rtsx_read_register(struct rtsx_chip *chip, u16 addr, u8 *data)
 
 	for (i = 0; i < MAX_RW_REG_CNT; i++) {
 		val = rtsx_readl(chip, RTSX_HAIMR);
-		if ((val & (1 << 31)) == 0)
+		if ((val & BIT(31)) == 0)
 			break;
 	}
 
diff --git a/drivers/staging/rts5208/rtsx_transport.c b/drivers/staging/rts5208/rtsx_transport.c
index 23799013c432..8b57e17ee6d3 100644
--- a/drivers/staging/rts5208/rtsx_transport.c
+++ b/drivers/staging/rts5208/rtsx_transport.c
@@ -766,8 +766,7 @@ int rtsx_transfer_data(struct rtsx_chip *chip, u8 card, void *buf, size_t len,
 		return -EIO;
 
 	if (use_sg) {
-		err = rtsx_transfer_sglist_adma(chip, card,
-						(struct scatterlist *)buf,
+		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);
diff --git a/drivers/staging/sm750fb/ddk750_chip.c b/drivers/staging/sm750fb/ddk750_chip.c
index 10cf7295dc6c..5e4bfb601cea 100644
--- a/drivers/staging/sm750fb/ddk750_chip.c
+++ b/drivers/staging/sm750fb/ddk750_chip.c
@@ -16,9 +16,9 @@ logical_chip_type_t sm750_get_chip_type(void)
 
 void sm750_set_chip_type(unsigned short devId, u8 revId)
 {
-	if (devId == 0x718)
+	if (devId == 0x718) {
 		chip = SM718;
-	else if (devId == 0x750) {
+	} else if (devId == 0x750) {
 		chip = SM750;
 		/* SM750 and SM750LE are different in their revision ID only. */
 		if (revId == SM750LE_REVISION_ID) {
@@ -69,11 +69,11 @@ static void set_chip_clock(unsigned int frequency)
 		pll.clockType = 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.
+		 * 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.
 		 */
 		ulActualMxClk = sm750_calc_pll_value(frequency, &pll);
 
@@ -352,7 +352,7 @@ unsigned int sm750_calc_pll_value(unsigned int request_orig, struct pll_value *p
 		RN = N * request;
 		quo = RN / input;
 		rem = RN % input;/* rem always small than 14318181 */
-		fl_quo = (rem * 10000 / input);
+		fl_quo = rem * 10000 / input;
 
 		for (d = max_d; d >= 0; d--) {
 			X = BIT(d);
diff --git a/drivers/staging/sm750fb/ddk750_chip.h b/drivers/staging/sm750fb/ddk750_chip.h
index fbeb615aa432..2c7a9b9a7c8a 100644
--- a/drivers/staging/sm750fb/ddk750_chip.h
+++ b/drivers/staging/sm750fb/ddk750_chip.h
@@ -98,6 +98,6 @@ void sm750_set_chip_type(unsigned short devId, u8 revId);
 unsigned int sm750_calc_pll_value(unsigned int request, struct  pll_value *pll);
 unsigned int sm750_format_pll_reg(struct pll_value *pPLL);
 unsigned int ddk750_get_vm_size(void);
-int ddk750_init_hw(struct initchip_param *);
+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
index e4724a660d07..9b116ed6ecc7 100644
--- a/drivers/staging/sm750fb/ddk750_display.c
+++ b/drivers/staging/sm750fb/ddk750_display.c
@@ -146,7 +146,8 @@ void ddk750_setLogicalDispOut(disp_output_t output)
 
 	if (output & PNL_SEQ_USAGE) {
 		/* set  panel sequence */
-		swPanelPowerSequence((output & PNL_SEQ_MASK) >> PNL_SEQ_OFFSET, 4);
+		swPanelPowerSequence((output & PNL_SEQ_MASK) >> PNL_SEQ_OFFSET,
+				     4);
 	}
 
 	if (output & DAC_USAGE)
diff --git a/drivers/staging/sm750fb/ddk750_display.h b/drivers/staging/sm750fb/ddk750_display.h
index 8abca88f089e..609bf742efff 100644
--- a/drivers/staging/sm750fb/ddk750_display.h
+++ b/drivers/staging/sm750fb/ddk750_display.h
@@ -12,7 +12,6 @@
 #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
@@ -24,7 +23,6 @@
 #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]
@@ -66,7 +64,6 @@
 #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]
@@ -87,8 +84,6 @@
 #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
@@ -107,6 +102,6 @@ typedef enum _disp_output_t {
 }
 disp_output_t;
 
-void ddk750_setLogicalDispOut(disp_output_t);
+void ddk750_setLogicalDispOut(disp_output_t output);
 
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_dvi.c b/drivers/staging/sm750fb/ddk750_dvi.c
index 250c2f478778..171ae063f06f 100644
--- a/drivers/staging/sm750fb/ddk750_dvi.c
+++ b/drivers/staging/sm750fb/ddk750_dvi.c
@@ -5,7 +5,6 @@
 #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
@@ -30,7 +29,6 @@ static dvi_ctrl_device_t g_dcftSupportedDviController[] = {
 #endif
 };
 
-
 int dviInit(
 	unsigned char edgeSelect,
 	unsigned char busSelect,
@@ -47,7 +45,7 @@ int dviInit(
 	dvi_ctrl_device_t *pCurrentDviCtrl;
 
 	pCurrentDviCtrl = g_dcftSupportedDviController;
-	if (pCurrentDviCtrl->pfnInit != NULL) {
+	if (pCurrentDviCtrl->pfnInit) {
 		return pCurrentDviCtrl->pfnInit(edgeSelect, busSelect, dualEdgeClkSelect, hsyncEnable,
 						vsyncEnable, deskewEnable, deskewSetting, continuousSyncEnable,
 						pllFilterEnable, pllFilterValue);
@@ -56,5 +54,3 @@ int dviInit(
 }
 
 #endif
-
-
diff --git a/drivers/staging/sm750fb/ddk750_hwi2c.c b/drivers/staging/sm750fb/ddk750_hwi2c.c
index 68716ef7cb06..fe814e4881f9 100644
--- a/drivers/staging/sm750fb/ddk750_hwi2c.c
+++ b/drivers/staging/sm750fb/ddk750_hwi2c.c
@@ -217,7 +217,7 @@ unsigned char sm750_hw_i2c_read_reg(
 	unsigned char reg
 )
 {
-	unsigned char value = (0xFF);
+	unsigned char value = 0xFF;
 
 	if (hw_i2c_write_data(addr, 1, &reg) == 1)
 		hw_i2c_read_data(addr, 1, &value);
diff --git a/drivers/staging/sm750fb/ddk750_mode.c b/drivers/staging/sm750fb/ddk750_mode.c
index 1df7d57dea6d..bb673e18999b 100644
--- a/drivers/staging/sm750fb/ddk750_mode.c
+++ b/drivers/staging/sm750fb/ddk750_mode.c
@@ -12,7 +12,8 @@
  * HW only supports 7 predefined pixel clocks, and clock select is
  * in bit 29:27 of Display Control register.
  */
-static unsigned long displayControlAdjust_SM750LE(mode_parameter_t *pModeParam, unsigned long dispControl)
+static unsigned long displayControlAdjust_SM750LE(struct mode_parameter *pModeParam,
+						  unsigned long dispControl)
 {
 	unsigned long x, y;
 
@@ -28,9 +29,9 @@ static unsigned long displayControlAdjust_SM750LE(mode_parameter_t *pModeParam,
 	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));
+	       (((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
@@ -71,11 +72,9 @@ static unsigned long displayControlAdjust_SM750LE(mode_parameter_t *pModeParam,
 	return dispControl;
 }
 
-
-
 /* only timing related registers will be  programed */
-static int programModeRegisters(mode_parameter_t *pModeParam,
-						struct pll_value *pll)
+static int programModeRegisters(struct mode_parameter *pModeParam,
+				struct pll_value *pll)
 {
 	int ret = 0;
 	int cnt = 0;
@@ -84,34 +83,38 @@ static int programModeRegisters(mode_parameter_t *pModeParam,
 	if (pll->clockType == SECONDARY_PLL) {
 		/* programe secondary pixel clock */
 		poke32(CRT_PLL_CTRL, sm750_format_pll_reg(pll));
-		poke32(CRT_HORIZONTAL_TOTAL,
-			(((pModeParam->horizontal_total - 1) <<
-				CRT_HORIZONTAL_TOTAL_TOTAL_SHIFT) &
-				CRT_HORIZONTAL_TOTAL_TOTAL_MASK) |
-			((pModeParam->horizontal_display_end - 1) &
-				CRT_HORIZONTAL_TOTAL_DISPLAY_END_MASK));
-
-		poke32(CRT_HORIZONTAL_SYNC,
-			((pModeParam->horizontal_sync_width <<
-				CRT_HORIZONTAL_SYNC_WIDTH_SHIFT) &
-				CRT_HORIZONTAL_SYNC_WIDTH_MASK) |
-			((pModeParam->horizontal_sync_start - 1) &
-				CRT_HORIZONTAL_SYNC_START_MASK));
-
-		poke32(CRT_VERTICAL_TOTAL,
-			(((pModeParam->vertical_total - 1) <<
-				CRT_VERTICAL_TOTAL_TOTAL_SHIFT) &
-				CRT_VERTICAL_TOTAL_TOTAL_MASK) |
-			((pModeParam->vertical_display_end - 1) &
-				CRT_VERTICAL_TOTAL_DISPLAY_END_MASK));
-
-		poke32(CRT_VERTICAL_SYNC,
-			((pModeParam->vertical_sync_height <<
-				CRT_VERTICAL_SYNC_HEIGHT_SHIFT) &
-				CRT_VERTICAL_SYNC_HEIGHT_MASK) |
-			((pModeParam->vertical_sync_start - 1) &
-				CRT_VERTICAL_SYNC_START_MASK));
 
+		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)
@@ -143,25 +146,25 @@ static int programModeRegisters(mode_parameter_t *pModeParam,
 		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));
+		       ((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));
+		       (((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));
+		       ((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)
@@ -202,7 +205,7 @@ static int programModeRegisters(mode_parameter_t *pModeParam,
 	return ret;
 }
 
-int ddk750_setModeTiming(mode_parameter_t *parm, clock_type_t clock)
+int ddk750_setModeTiming(struct mode_parameter *parm, clock_type_t clock)
 {
 	struct pll_value pll;
 	unsigned int uiActualPixelClk;
@@ -219,5 +222,3 @@ int ddk750_setModeTiming(mode_parameter_t *parm, clock_type_t clock)
 	programModeRegisters(parm, &pll);
 	return 0;
 }
-
-
diff --git a/drivers/staging/sm750fb/ddk750_mode.h b/drivers/staging/sm750fb/ddk750_mode.h
index e846dc2c3d5c..d5eae36d85cb 100644
--- a/drivers/staging/sm750fb/ddk750_mode.h
+++ b/drivers/staging/sm750fb/ddk750_mode.h
@@ -3,27 +3,25 @@
 
 #include "ddk750_chip.h"
 
-typedef enum _spolarity_t {
+enum spolarity {
 	POS = 0, /* positive */
 	NEG, /* negative */
-}
-spolarity_t;
+};
 
-
-typedef struct _mode_parameter_t {
+struct mode_parameter {
 	/* Horizontal timing. */
 	unsigned long horizontal_total;
 	unsigned long horizontal_display_end;
 	unsigned long horizontal_sync_start;
 	unsigned long horizontal_sync_width;
-	spolarity_t horizontal_sync_polarity;
+	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;
-	spolarity_t vertical_sync_polarity;
+	enum spolarity vertical_sync_polarity;
 
 	/* Refresh timing. */
 	unsigned long pixel_clock;
@@ -31,11 +29,8 @@ typedef struct _mode_parameter_t {
 	unsigned long vertical_frequency;
 
 	/* Clock Phase. This clock phase only applies to Panel. */
-	spolarity_t clock_phase_polarity;
-}
-mode_parameter_t;
-
-int ddk750_setModeTiming(mode_parameter_t *, clock_type_t);
-
+	enum spolarity clock_phase_polarity;
+};
 
+int ddk750_setModeTiming(struct mode_parameter *parm, clock_type_t clock);
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_power.c b/drivers/staging/sm750fb/ddk750_power.c
index 02ff6204ee1e..222ae1a06feb 100644
--- a/drivers/staging/sm750fb/ddk750_power.c
+++ b/drivers/staging/sm750fb/ddk750_power.c
@@ -24,7 +24,6 @@ static unsigned int get_power_mode(void)
 	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.
@@ -80,8 +79,6 @@ void sm750_set_current_gate(unsigned int gate)
 		poke32(MODE0_GATE, gate);
 }
 
-
-
 /*
  * This function enable/disable the 2D engine.
  */
@@ -145,5 +142,3 @@ void sm750_enable_i2c(unsigned int enable)
 
 	sm750_set_current_gate(gate);
 }
-
-
diff --git a/drivers/staging/sm750fb/ddk750_power.h b/drivers/staging/sm750fb/ddk750_power.h
index 4274d74d47c1..44c4fc587e96 100644
--- a/drivers/staging/sm750fb/ddk750_power.h
+++ b/drivers/staging/sm750fb/ddk750_power.h
@@ -14,7 +14,7 @@ DPMS_t;
 	       (peek32(MISC_CTRL) & ~MISC_CTRL_DAC_POWER_OFF) | (off)); \
 }
 
-void ddk750_set_dpms(DPMS_t);
+void ddk750_set_dpms(DPMS_t state);
 void sm750_set_power_mode(unsigned int powerMode);
 void sm750_set_current_gate(unsigned int gate);
 
@@ -38,5 +38,4 @@ void sm750_enable_gpio(unsigned int enable);
  */
 void sm750_enable_i2c(unsigned int enable);
 
-
 #endif
diff --git a/drivers/staging/sm750fb/ddk750_reg.h b/drivers/staging/sm750fb/ddk750_reg.h
index 4ed6d8d7712a..f9b989b7a152 100644
--- a/drivers/staging/sm750fb/ddk750_reg.h
+++ b/drivers/staging/sm750fb/ddk750_reg.h
@@ -532,7 +532,6 @@
 #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
@@ -1279,7 +1278,6 @@
 #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)
@@ -1445,7 +1443,6 @@
 #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)
@@ -1454,5 +1451,4 @@
 #define GPIO_DATA_DIRECTION_SM750LE_1                   BIT(1)
 #define GPIO_DATA_DIRECTION_SM750LE_0                   BIT(0)
 
-
 #endif
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index e49f8845f923..386d4adcd91d 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -419,7 +419,7 @@ static int lynxfb_suspend(struct pci_dev *pdev, pm_message_t mesg)
 		if (ret) {
 			dev_err(&pdev->dev,
 				"error:%d occurred in pci_save_state\n", ret);
-			return ret;
+			goto lynxfb_suspend_err;
 		}
 
 		ret = pci_set_power_state(pdev, pci_choose_state(pdev, mesg));
@@ -427,11 +427,13 @@ static int lynxfb_suspend(struct pci_dev *pdev, pm_message_t mesg)
 			dev_err(&pdev->dev,
 				"error:%d occurred in pci_set_power_state\n",
 				ret);
-			return ret;
+			goto lynxfb_suspend_err;
 		}
 	}
 
 	pdev->dev.power.power_state = mesg;
+
+lynxfb_suspend_err:
 	console_unlock();
 	return ret;
 }
@@ -456,7 +458,7 @@ static int lynxfb_resume(struct pci_dev *pdev)
 	if (ret) {
 		dev_err(&pdev->dev,
 			"error:%d occurred in pci_set_power_state\n", ret);
-		return ret;
+		goto lynxfb_resume_err;
 	}
 
 	if (pdev->dev.power.power_state.event != PM_EVENT_FREEZE) {
@@ -466,7 +468,7 @@ static int lynxfb_resume(struct pci_dev *pdev)
 			dev_err(&pdev->dev,
 				"error:%d occurred in pci_enable_device\n",
 				ret);
-			return ret;
+			goto lynxfb_resume_err;
 		}
 		pci_set_master(pdev);
 	}
@@ -498,6 +500,8 @@ static int lynxfb_resume(struct pci_dev *pdev)
 	}
 
 	pdev->dev.power.power_state.event = PM_EVENT_RESUME;
+
+lynxfb_resume_err:
 	console_unlock();
 	return ret;
 }
@@ -806,7 +810,6 @@ static int lynxfb_set_fbinfo(struct fb_info *info, int index)
 	}
 
 	for (i = 0; i < 3; i++) {
-
 		ret = fb_find_mode(var, info, g_fbmode[index],
 				   pdb[i], cdb[i], NULL, 8);
 
@@ -834,15 +837,15 @@ static int lynxfb_set_fbinfo(struct fb_info *info, int index)
 
 	/* 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",
+	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,
@@ -954,23 +957,23 @@ static void sm750fb_setup(struct sm750_dev *sm750_dev, char *src)
 		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")))
+		if (!strncmp(opt, "swap", strlen("swap"))) {
 			swap = 1;
-		else if (!strncmp(opt, "nocrt", strlen("nocrt")))
+		} else if (!strncmp(opt, "nocrt", strlen("nocrt"))) {
 			sm750_dev->nocrt = 1;
-		else if (!strncmp(opt, "36bit", strlen("36bit")))
+		} else if (!strncmp(opt, "36bit", strlen("36bit"))) {
 			sm750_dev->pnltype = sm750_doubleTFT;
-		else if (!strncmp(opt, "18bit", strlen("18bit")))
+		} else if (!strncmp(opt, "18bit", strlen("18bit"))) {
 			sm750_dev->pnltype = sm750_dualTFT;
-		else if (!strncmp(opt, "24bit", strlen("24bit")))
+		} else if (!strncmp(opt, "24bit", strlen("24bit"))) {
 			sm750_dev->pnltype = sm750_24TFT;
-		else if (!strncmp(opt, "nohwc0", strlen("nohwc0")))
+		} else if (!strncmp(opt, "nohwc0", strlen("nohwc0"))) {
 			g_hwcursor &= ~0x1;
-		else if (!strncmp(opt, "nohwc1", strlen("nohwc1")))
+		} else if (!strncmp(opt, "nohwc1", strlen("nohwc1"))) {
 			g_hwcursor &= ~0x2;
-		else if (!strncmp(opt, "nohwc", strlen("nohwc")))
+		} else if (!strncmp(opt, "nohwc", strlen("nohwc"))) {
 			g_hwcursor = 0;
-		else {
+		} else {
 			if (!g_fbmode[0]) {
 				g_fbmode[0] = opt;
 				dev_info(&sm750_dev->pdev->dev,
@@ -1168,13 +1171,13 @@ static int __init lynxfb_setup(char *options)
 	 */
 	while ((opt = strsep(&options, ":")) != NULL) {
 		/* options that mean for any lynx chips are configured here */
-		if (!strncmp(opt, "noaccel", strlen("noaccel")))
+		if (!strncmp(opt, "noaccel", strlen("noaccel"))) {
 			g_noaccel = 1;
-		else if (!strncmp(opt, "nomtrr", strlen("nomtrr")))
+		} else if (!strncmp(opt, "nomtrr", strlen("nomtrr"))) {
 			g_nomtrr = 1;
-		else if (!strncmp(opt, "dual", strlen("dual")))
+		} else if (!strncmp(opt, "dual", strlen("dual"))) {
 			g_dualview = 1;
-		else {
+		} else {
 			strcat(tmp, opt);
 			tmp += strlen(opt);
 			if (options)
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
index 28f4b9b4f95f..5b186dafedec 100644
--- a/drivers/staging/sm750fb/sm750.h
+++ b/drivers/staging/sm750fb/sm750.h
@@ -177,15 +177,15 @@ struct lynxfb_par {
 
 static inline unsigned long ps_to_hz(unsigned int psvalue)
 {
-	unsigned long long numerator = 1000*1000*1000*1000ULL;
+	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*, struct pci_dev *);
-void hw_sm750_initAccel(struct sm750_dev *);
+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);
 
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
index af0db5789c53..6be86e4963be 100644
--- a/drivers/staging/sm750fb/sm750_accel.c
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -144,11 +144,9 @@ unsigned int height, /* width and height of rectangle in pixel value */
 unsigned int rop2)   /* ROP value */
 {
 	unsigned int nDirection, de_ctrl;
-	int opSign;
 
 	nDirection = LEFT_TO_RIGHT;
 	/* Direction of ROP2 operation: 1 = Left to Right, (-1) = Right to Left */
-	opSign = 1;
 	de_ctrl = 0;
 
 	/* If source and destination are the same surface, need to check for overlay cases */
@@ -212,7 +210,6 @@ unsigned int rop2)   /* ROP value */
 		sy += height - 1;
 		dx += width - 1;
 		dy += height - 1;
-		opSign = (-1);
 	}
 
 	/*
@@ -259,8 +256,6 @@ unsigned int rop2)   /* ROP value */
 	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 */
@@ -276,8 +271,6 @@ unsigned int rop2)   /* ROP value */
 		DE_CONTROL_COMMAND_BITBLT | DE_CONTROL_STATUS;
 	write_dpr(accel, DE_CONTROL, de_ctrl); /* dpr0c */
 
-	}
-
 	return 0;
 }
 
@@ -384,7 +377,7 @@ int sm750_hw_imageblit(struct lynx_accel *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++)
+		for (j = 0; j < (ul4BytesPerScan / 4); j++)
 			write_dpPort(accel, *(unsigned int *)(pSrcbuf + (j * 4)));
 
 		if (ulBytesRemain) {
diff --git a/drivers/staging/sm750fb/sm750_cursor.c b/drivers/staging/sm750fb/sm750_cursor.c
index b1651b0d2034..b64dc8a4a8fb 100644
--- a/drivers/staging/sm750fb/sm750_cursor.c
+++ b/drivers/staging/sm750fb/sm750_cursor.c
@@ -54,6 +54,7 @@ void sm750_hw_cursor_enable(struct lynx_cursor *cursor)
 	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);
@@ -65,15 +66,17 @@ void sm750_hw_cursor_setSize(struct lynx_cursor *cursor,
 	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));
+	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)
 {
@@ -111,14 +114,14 @@ void sm750_hw_cursor_setData(struct lynx_cursor *cursor,
 		data = 0;
 
 		for (j = 0; j < 8; j++) {
-			if (mask & (0x80>>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);
+				data |= ((opr & (0x80 >> j)) ? 2 : 1) << (j * 2);
 			}
 		}
 		iowrite16(data, pbuffer);
@@ -131,10 +134,7 @@ void sm750_hw_cursor_setData(struct lynx_cursor *cursor,
 		} else {
 			pbuffer += sizeof(u16);
 		}
-
 	}
-
-
 }
 
 
@@ -165,19 +165,18 @@ void sm750_hw_cursor_setData2(struct lynx_cursor *cursor,
 		data = 0;
 
 		for (j = 0; j < 8; j++) {
-			if (mask & (1<<j))
-				data |= ((color & (1<<j))?1:2)<<(j*2);
+			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))) {
+		if (!(i & (pitch - 1))) {
 			/* need a return */
 			pstart += offset;
 			pbuffer = pstart;
 		} else {
 			pbuffer += sizeof(u16);
 		}
-
 	}
 }
diff --git a/drivers/staging/sm750fb/sm750_hw.c b/drivers/staging/sm750fb/sm750_hw.c
index fab3fc9c8330..baf1bbdc92ff 100644
--- a/drivers/staging/sm750fb/sm750_hw.c
+++ b/drivers/staging/sm750fb/sm750_hw.c
@@ -252,7 +252,7 @@ int hw_sm750_crtc_setMode(struct lynxfb_crtc *crtc,
 {
 	int ret, fmt;
 	u32 reg;
-	mode_parameter_t modparm;
+	struct mode_parameter modparm;
 	clock_type_t clock;
 	struct sm750_dev *sm750_dev;
 	struct lynxfb_par *par;
diff --git a/drivers/staging/speakup/buffers.c b/drivers/staging/speakup/buffers.c
index 723d5df44221..f459e4004bfa 100644
--- a/drivers/staging/speakup/buffers.c
+++ b/drivers/staging/speakup/buffers.c
@@ -7,10 +7,10 @@
 
 #define SYNTH_BUF_SIZE 8192	/* currently 8K bytes */
 
-static u_char synth_buffer[SYNTH_BUF_SIZE];	/* guess what this is for! */
-static u_char *buff_in = synth_buffer;
-static u_char *buff_out = synth_buffer;
-static u_char *buffer_end = synth_buffer + SYNTH_BUF_SIZE - 1;
+static u16 synth_buffer[SYNTH_BUF_SIZE];	/* guess what this is for! */
+static u16 *buff_in = synth_buffer;
+static u16 *buff_out = synth_buffer;
+static u16 *buffer_end = synth_buffer + SYNTH_BUF_SIZE - 1;
 
 /* These try to throttle applications by stopping the TTYs
  * Note: we need to make sure that we will restart them eventually, which is
@@ -44,13 +44,13 @@ static void speakup_stop_ttys(void)
 
 static int synth_buffer_free(void)
 {
-	int bytes_free;
+	int chars_free;
 
 	if (buff_in >= buff_out)
-		bytes_free = SYNTH_BUF_SIZE - (buff_in - buff_out);
+		chars_free = SYNTH_BUF_SIZE - (buff_in - buff_out);
 	else
-		bytes_free = buff_out - buff_in;
-	return bytes_free;
+		chars_free = buff_out - buff_in;
+	return chars_free;
 }
 
 int synth_buffer_empty(void)
@@ -59,7 +59,7 @@ int synth_buffer_empty(void)
 }
 EXPORT_SYMBOL_GPL(synth_buffer_empty);
 
-void synth_buffer_add(char ch)
+void synth_buffer_add(u16 ch)
 {
 	if (!synth->alive) {
 		/* This makes sure that we won't stop TTYs if there is no synth
@@ -78,9 +78,9 @@ void synth_buffer_add(char ch)
 		buff_in = synth_buffer;
 }
 
-char synth_buffer_getc(void)
+u16 synth_buffer_getc(void)
 {
-	char ch;
+	u16 ch;
 
 	if (buff_out == buff_in)
 		return 0;
@@ -91,7 +91,7 @@ char synth_buffer_getc(void)
 }
 EXPORT_SYMBOL_GPL(synth_buffer_getc);
 
-char synth_buffer_peek(void)
+u16 synth_buffer_peek(void)
 {
 	if (buff_out == buff_in)
 		return 0;
@@ -99,6 +99,18 @@ char synth_buffer_peek(void)
 }
 EXPORT_SYMBOL_GPL(synth_buffer_peek);
 
+void synth_buffer_skip_nonlatin1(void)
+{
+	while (buff_out != buff_in) {
+		if (*buff_out < 0x100)
+			return;
+		buff_out++;
+		if (buff_out > buffer_end)
+			buff_out = synth_buffer;
+	}
+}
+EXPORT_SYMBOL_GPL(synth_buffer_skip_nonlatin1);
+
 void synth_buffer_clear(void)
 {
 	buff_in = synth_buffer;
diff --git a/drivers/staging/speakup/fakekey.c b/drivers/staging/speakup/fakekey.c
index d76da0a1382c..294c74b47224 100644
--- a/drivers/staging/speakup/fakekey.c
+++ b/drivers/staging/speakup/fakekey.c
@@ -56,7 +56,7 @@ int speakup_add_virtual_keyboard(void)
 
 void speakup_remove_virtual_keyboard(void)
 {
-	if (virt_keyboard != NULL) {
+	if (virt_keyboard) {
 		input_unregister_device(virt_keyboard);
 		virt_keyboard = NULL;
 	}
diff --git a/drivers/staging/speakup/i18n.c b/drivers/staging/speakup/i18n.c
index 2f9b3df7f78d..7809867f5d28 100644
--- a/drivers/staging/speakup/i18n.c
+++ b/drivers/staging/speakup/i18n.c
@@ -407,12 +407,12 @@ static char *next_specifier(char *input)
 	int found = 0;
 	char *next_percent = input;
 
-	while ((next_percent != NULL) && !found) {
+	while (next_percent && !found) {
 		next_percent = strchr(next_percent, '%');
-		if (next_percent != NULL) {
+		if (next_percent) {
 			/* skip over doubled percent signs */
-			while ((next_percent[0] == '%')
-			       && (next_percent[1] == '%'))
+			while (next_percent[0] == '%' &&
+			       next_percent[1] == '%')
 				next_percent += 2;
 			if (*next_percent == '%')
 				found = 1;
@@ -476,19 +476,20 @@ static char *find_specifier_end(char *input)
 /*
  * Function: compare_specifiers
  * Compare the format specifiers pointed to by *input1 and *input2.
- * Return 1 if they are the same, 0 otherwise.  Advance *input1 and *input2
- * so that they point to the character following the end of the specifier.
+ * Return true if they are the same, false otherwise.
+ * Advance *input1 and *input2 so that they point to the character following
+ * the end of the specifier.
  */
-static int compare_specifiers(char **input1, char **input2)
+static bool compare_specifiers(char **input1, char **input2)
 {
-	int same = 0;
+	bool same = false;
 	char *end1 = find_specifier_end(*input1);
 	char *end2 = find_specifier_end(*input2);
 	size_t length1 = end1 - *input1;
 	size_t length2 = end2 - *input2;
 
 	if ((length1 == length2) && !memcmp(*input1, *input2, length1))
-		same = 1;
+		same = true;
 
 	*input1 = end1;
 	*input2 = end2;
@@ -499,12 +500,12 @@ static int compare_specifiers(char **input1, char **input2)
  * Function: fmt_validate
  * Check that two format strings contain the same number of format specifiers,
  * and that the order of specifiers is the same in both strings.
- * Return 1 if the condition holds, 0 if it doesn't.
+ * Return true if the condition holds, false if it doesn't.
  */
-static int fmt_validate(char *template, char *user)
+static bool fmt_validate(char *template, char *user)
 {
-	int valid = 1;
-	int still_comparing = 1;
+	bool valid = true;
+	bool still_comparing = true;
 	char *template_ptr = template;
 	char *user_ptr = user;
 
@@ -516,10 +517,10 @@ static int fmt_validate(char *template, char *user)
 			valid = compare_specifiers(&template_ptr, &user_ptr);
 		} else {
 			/* No more format specifiers in one or both strings. */
-			still_comparing = 0;
+			still_comparing = false;
 			/* See if one has more specifiers than the other. */
 			if (template_ptr || user_ptr)
-				valid = 0;
+				valid = false;
 		}
 	}
 	return valid;
@@ -540,34 +541,30 @@ static int fmt_validate(char *template, char *user)
  */
 ssize_t spk_msg_set(enum msg_index_t index, char *text, size_t length)
 {
-	int rc = 0;
 	char *newstr = NULL;
 	unsigned long flags;
 
-	if ((index >= MSG_FIRST_INDEX) && (index < MSG_LAST_INDEX)) {
-		newstr = kmalloc(length + 1, GFP_KERNEL);
-		if (newstr) {
-			memcpy(newstr, text, length);
-			newstr[length] = '\0';
-			if ((index >= MSG_FORMATTED_START
-			&& index <= MSG_FORMATTED_END)
-				&& !fmt_validate(speakup_default_msgs[index],
-				newstr)) {
-				kfree(newstr);
-				return -EINVAL;
-			}
-			spin_lock_irqsave(&speakup_info.spinlock, flags);
-			if (speakup_msgs[index] != speakup_default_msgs[index])
-				kfree(speakup_msgs[index]);
-			speakup_msgs[index] = newstr;
-			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		} else {
-			rc = -ENOMEM;
-		}
-	} else {
-		rc = -EINVAL;
+	if ((index < MSG_FIRST_INDEX) || (index >= MSG_LAST_INDEX))
+		return -EINVAL;
+
+	newstr = kmalloc(length + 1, GFP_KERNEL);
+	if (!newstr)
+		return -ENOMEM;
+
+	memcpy(newstr, text, length);
+	newstr[length] = '\0';
+	if (index >= MSG_FORMATTED_START &&
+	    index <= MSG_FORMATTED_END &&
+	    !fmt_validate(speakup_default_msgs[index], newstr)) {
+		kfree(newstr);
+		return -EINVAL;
 	}
-	return rc;
+	spin_lock_irqsave(&speakup_info.spinlock, flags);
+	if (speakup_msgs[index] != speakup_default_msgs[index])
+		kfree(speakup_msgs[index]);
+	speakup_msgs[index] = newstr;
+	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
+	return 0;
 }
 
 /*
@@ -607,7 +604,7 @@ void spk_reset_msg_group(struct msg_group_t *group)
 void spk_initialize_msgs(void)
 {
 	memcpy(speakup_msgs, speakup_default_msgs,
-		sizeof(speakup_default_msgs));
+	       sizeof(speakup_default_msgs));
 }
 
 /* Free user-supplied strings when module is unloaded: */
diff --git a/drivers/staging/speakup/keyhelp.c b/drivers/staging/speakup/keyhelp.c
index ce94cb13e256..4e6e5daba50c 100644
--- a/drivers/staging/speakup/keyhelp.c
+++ b/drivers/staging/speakup/keyhelp.c
@@ -117,7 +117,7 @@ static void say_key(int key)
 	}
 	if ((key > 0) && (key <= num_key_names))
 		synth_printf(" %s\n",
-				spk_msg_get(MSG_KEYNAMES_START + (key - 1)));
+			     spk_msg_get(MSG_KEYNAMES_START + (key - 1)));
 }
 
 static int help_init(void)
@@ -163,17 +163,15 @@ int spk_handle_help(struct vc_data *vc, u_char type, u_char ch, u_short key)
 		}
 		cur_item = letter_offsets[ch - 'a'];
 	} else if (type == KT_CUR) {
-		if (ch == 0
-		    && (MSG_FUNCNAMES_START + cur_item + 1) <=
-		    MSG_FUNCNAMES_END)
+		if (ch == 0 &&
+		    (MSG_FUNCNAMES_START + cur_item + 1) <= MSG_FUNCNAMES_END)
 			cur_item++;
 		else if (ch == 3 && cur_item > 0)
 			cur_item--;
 		else
 			return -1;
-	} else if (type == KT_SPKUP
-			&& ch == SPEAKUP_HELP
-			&& !spk_special_handler) {
+	} else if (type == KT_SPKUP && ch == SPEAKUP_HELP &&
+		   !spk_special_handler) {
 		spk_special_handler = spk_handle_help;
 		synth_printf("%s\n", spk_msg_get(MSG_HELP_INFO));
 		build_key_data(); /* rebuild each time in case new mapping */
@@ -182,7 +180,7 @@ int spk_handle_help(struct vc_data *vc, u_char type, u_char ch, u_short key)
 		name = NULL;
 		if ((type != KT_SPKUP) && (key > 0) && (key <= num_key_names)) {
 			synth_printf("%s\n",
-				spk_msg_get(MSG_KEYNAMES_START + key - 1));
+				     spk_msg_get(MSG_KEYNAMES_START + key - 1));
 			return 1;
 		}
 		for (i = 0; funcvals[i] != 0 && !name; i++) {
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 4e7ebc306488..ca85476e3ff7 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -26,7 +26,7 @@
  * This is called when a user reads the characters or chartab sys file.
  */
 static ssize_t chars_chartab_show(struct kobject *kobj,
-	struct kobj_attribute *attr, char *buf)
+				  struct kobj_attribute *attr, char *buf)
 {
 	int i;
 	int len = 0;
@@ -79,7 +79,7 @@ static ssize_t chars_chartab_show(struct kobject *kobj,
  * character descriptions or chartab entries.
  */
 static void report_char_chartab_status(int reset, int received, int used,
-	int rejected, int do_characters)
+				       int rejected, int do_characters)
 {
 	static char const *object_type[] = {
 		"character class entries",
@@ -92,8 +92,8 @@ static void report_char_chartab_status(int reset, int received, int used,
 		pr_info("%s reset to defaults\n", object_type[do_characters]);
 	} else if (received) {
 		len = snprintf(buf, sizeof(buf),
-				" updated %d of %d %s\n",
-				used, received, object_type[do_characters]);
+			       " updated %d of %d %s\n",
+			       used, received, object_type[do_characters]);
 		if (rejected)
 			snprintf(buf + (len - 1), sizeof(buf) - (len - 1),
 				 " with %d reject%s\n",
@@ -106,9 +106,10 @@ static void report_char_chartab_status(int reset, int received, int used,
  * This is called when a user changes the characters or chartab parameters.
  */
 static ssize_t chars_chartab_store(struct kobject *kobj,
-	struct kobj_attribute *attr, const char *buf, size_t count)
+				   struct kobj_attribute *attr,
+				   const char *buf, size_t count)
 {
-	char *cp = (char *) buf;
+	char *cp = (char *)buf;
 	char *end = cp + count; /* the null at the end of the buffer */
 	char *linefeed = NULL;
 	char keyword[MAX_DESC_LEN + 1];
@@ -129,7 +130,6 @@ static ssize_t chars_chartab_store(struct kobject *kobj,
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	while (cp < end) {
-
 		while ((cp < end) && (*cp == ' ' || *cp == '\t'))
 			cp++;
 
@@ -214,7 +214,7 @@ static ssize_t chars_chartab_store(struct kobject *kobj,
 
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 	report_char_chartab_status(reset, received, used, rejected,
-		do_characters);
+				   do_characters);
 	return retval;
 }
 
@@ -222,7 +222,7 @@ static ssize_t chars_chartab_store(struct kobject *kobj,
  * This is called when a user reads the keymap parameter.
  */
 static ssize_t keymap_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf)
+			   char *buf)
 {
 	char *cp = buf;
 	int i;
@@ -258,7 +258,7 @@ static ssize_t keymap_show(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when a user changes the keymap parameter.
  */
 static ssize_t keymap_store(struct kobject *kobj, struct kobj_attribute *attr,
-	const char *buf, size_t count)
+			    const char *buf, size_t count)
 {
 	int i;
 	ssize_t ret = count;
@@ -292,9 +292,9 @@ static ssize_t keymap_store(struct kobject *kobj, struct kobj_attribute *attr,
 	i *= (int)cp1[-1] + 1;
 	i += 2; /* 0 and last map ver */
 	if (cp1[-3] != KEY_MAP_VER || cp1[-1] > 10 ||
-			i+SHIFT_TBL_SIZE+4 >= sizeof(spk_key_buf)) {
+	    i + SHIFT_TBL_SIZE + 4 >= sizeof(spk_key_buf)) {
 		pr_warn("i %d %d %d %d\n", i,
-				(int)cp1[-3], (int)cp1[-2], (int)cp1[-1]);
+			(int)cp1[-3], (int)cp1[-2], (int)cp1[-1]);
 		kfree(in_buff);
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		return -EINVAL;
@@ -308,7 +308,7 @@ static ssize_t keymap_store(struct kobject *kobj, struct kobj_attribute *attr,
 	if (i != 0 || cp1[-1] != KEY_MAP_VER || cp1[-2] != 0) {
 		ret = -EINVAL;
 		pr_warn("end %d %d %d %d\n", i,
-				(int)cp1[-3], (int)cp1[-2], (int)cp1[-1]);
+			(int)cp1[-3], (int)cp1[-2], (int)cp1[-1]);
 	} else {
 		if (spk_set_key_info(in_buff, spk_key_buf)) {
 			spk_set_key_info(spk_key_defaults, spk_key_buf);
@@ -325,7 +325,7 @@ static ssize_t keymap_store(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when a user changes the value of the silent parameter.
  */
 static ssize_t silent_store(struct kobject *kobj, struct kobj_attribute *attr,
-	const char *buf, size_t count)
+			    const char *buf, size_t count)
 {
 	int len;
 	struct vc_data *vc = vc_cons[fg_console].d;
@@ -344,7 +344,7 @@ static ssize_t silent_store(struct kobject *kobj, struct kobj_attribute *attr,
 		return -EINVAL;
 	}
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
-	if (ch&2) {
+	if (ch & 2) {
 		shut = 1;
 		spk_do_flush();
 	} else {
@@ -364,7 +364,7 @@ static ssize_t silent_store(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when a user reads the synth setting.
  */
 static ssize_t synth_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf)
+			  char *buf)
 {
 	int rv;
 
@@ -379,7 +379,7 @@ static ssize_t synth_show(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when a user requests to change synthesizers.
  */
 static ssize_t synth_store(struct kobject *kobj, struct kobj_attribute *attr,
-	const char *buf, size_t count)
+			   const char *buf, size_t count)
 {
 	int len;
 	char new_synth_name[10];
@@ -392,7 +392,7 @@ static ssize_t synth_store(struct kobject *kobj, struct kobj_attribute *attr,
 		len--;
 	new_synth_name[len] = '\0';
 	spk_strlwr(new_synth_name);
-	if ((synth != NULL) && (!strcmp(new_synth_name, synth->name))) {
+	if (synth && !strcmp(new_synth_name, synth->name)) {
 		pr_warn("%s already in use\n", new_synth_name);
 	} else if (synth_init(new_synth_name) != 0) {
 		pr_warn("failed to init synth %s\n", new_synth_name);
@@ -405,7 +405,8 @@ static ssize_t synth_store(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when text is sent to the synth via the synth_direct file.
  */
 static ssize_t synth_direct_store(struct kobject *kobj,
-	struct kobj_attribute *attr, const char *buf, size_t count)
+				  struct kobj_attribute *attr,
+				  const char *buf, size_t count)
 {
 	u_char tmp[256];
 	int len;
@@ -435,7 +436,7 @@ static ssize_t synth_direct_store(struct kobject *kobj,
  * This function is called when a user reads the version.
  */
 static ssize_t version_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf)
+			    char *buf)
 {
 	char *cp;
 
@@ -451,7 +452,7 @@ static ssize_t version_show(struct kobject *kobj, struct kobj_attribute *attr,
  * This is called when a user reads the punctuation settings.
  */
 static ssize_t punc_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf)
+			 char *buf)
 {
 	int i;
 	char *cp = buf;
@@ -471,27 +472,27 @@ static ssize_t punc_show(struct kobject *kobj, struct kobj_attribute *attr,
 	var = spk_get_punc_var(p_header->var_id);
 	if (!var) {
 		pr_warn("var is null, p_header->var_id is %i\n",
-				p_header->var_id);
+			p_header->var_id);
 		return -EINVAL;
 	}
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
-	pb = (struct st_bits_data *) &spk_punc_info[var->value];
+	pb = (struct st_bits_data *)&spk_punc_info[var->value];
 	mask = pb->mask;
 	for (i = 33; i < 128; i++) {
-		if (!(spk_chartab[i]&mask))
+		if (!(spk_chartab[i] & mask))
 			continue;
 		*cp++ = (char)i;
 	}
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-	return cp-buf;
+	return cp - buf;
 }
 
 /*
  * This is called when a user changes the punctuation settings.
  */
 static ssize_t punc_store(struct kobject *kobj, struct kobj_attribute *attr,
-			 const char *buf, size_t count)
+			  const char *buf, size_t count)
 {
 	int x;
 	struct st_var_header *p_header;
@@ -513,7 +514,7 @@ static ssize_t punc_store(struct kobject *kobj, struct kobj_attribute *attr,
 	var = spk_get_punc_var(p_header->var_id);
 	if (!var) {
 		pr_warn("var is null, p_header->var_id is %i\n",
-				p_header->var_id);
+			p_header->var_id);
 		return -EINVAL;
 	}
 
@@ -538,7 +539,7 @@ static ssize_t punc_store(struct kobject *kobj, struct kobj_attribute *attr,
  * This function is called when a user reads one of the variable parameters.
  */
 ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
-	char *buf)
+		     char *buf)
 {
 	int rv = 0;
 	struct st_var_header *param;
@@ -553,7 +554,7 @@ ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
 		return -EINVAL;
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
-	var = (struct var_t *) param->data;
+	var = (struct var_t *)param->data;
 	switch (param->var_type) {
 	case VAR_NUM:
 	case VAR_TIME:
@@ -575,14 +576,14 @@ ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
 			*cp1++ = '"';
 			*cp1++ = '\n';
 			*cp1 = '\0';
-			rv = cp1-buf;
+			rv = cp1 - buf;
 		} else {
 			rv = sprintf(buf, "\"\"\n");
 		}
 		break;
 	default:
 		rv = sprintf(buf, "Bad parameter  %s, type %i\n",
-			param->name, param->var_type);
+			     param->name, param->var_type);
 		break;
 	}
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
@@ -594,7 +595,7 @@ EXPORT_SYMBOL_GPL(spk_var_show);
  * Used to reset either default_pitch or default_vol.
  */
 static inline void spk_reset_default_value(char *header_name,
-					int *synth_default_value, int idx)
+					   int *synth_default_value, int idx)
 {
 	struct st_var_header *param;
 
@@ -614,7 +615,7 @@ static inline void spk_reset_default_value(char *header_name,
  * variable parameters.
  */
 ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
-			 const char *buf, size_t count)
+		      const char *buf, size_t count)
 {
 	struct st_var_header *param;
 	int ret;
@@ -663,9 +664,9 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 			var_data = param->data;
 			value = var_data->u.n.value;
 			spk_reset_default_value("pitch", synth->default_pitch,
-				value);
+						value);
 			spk_reset_default_value("vol", synth->default_vol,
-				value);
+						value);
 		}
 		break;
 	case VAR_STRING:
@@ -680,7 +681,7 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		ret = spk_set_string_var(cp, param, len);
 		if (ret == -E2BIG)
 			pr_warn("value too long for %s\n",
-					param->name);
+				param->name);
 		break;
 	default:
 		pr_warn("%s unknown type %d\n",
@@ -700,7 +701,7 @@ EXPORT_SYMBOL_GPL(spk_var_store);
  */
 
 static ssize_t message_show_helper(char *buf, enum msg_index_t first,
-	enum msg_index_t last)
+				   enum msg_index_t last)
 {
 	size_t bufsize = PAGE_SIZE;
 	char *buf_pointer = buf;
@@ -713,7 +714,7 @@ static ssize_t message_show_helper(char *buf, enum msg_index_t first,
 		if (bufsize <= 1)
 			break;
 		printed = scnprintf(buf_pointer, bufsize, "%d\t%s\n",
-			index, spk_msg_get(cursor));
+				    index, spk_msg_get(cursor));
 		buf_pointer += printed;
 		bufsize -= printed;
 	}
@@ -722,7 +723,7 @@ static ssize_t message_show_helper(char *buf, enum msg_index_t first,
 }
 
 static void report_msg_status(int reset, int received, int used,
-	int rejected, char *groupname)
+			      int rejected, char *groupname)
 {
 	int len;
 	char buf[160];
@@ -743,9 +744,9 @@ static void report_msg_status(int reset, int received, int used,
 }
 
 static ssize_t message_store_helper(const char *buf, size_t count,
-	struct msg_group_t *group)
+				    struct msg_group_t *group)
 {
-	char *cp = (char *) buf;
+	char *cp = (char *)buf;
 	char *end = cp + count;
 	char *linefeed = NULL;
 	char *temp = NULL;
@@ -762,7 +763,6 @@ static ssize_t message_store_helper(const char *buf, size_t count,
 	enum msg_index_t curmessage;
 
 	while (cp < end) {
-
 		while ((cp < end) && (*cp == ' ' || *cp == '\t'))
 			cp++;
 
@@ -828,13 +828,15 @@ static ssize_t message_store_helper(const char *buf, size_t count,
 }
 
 static ssize_t message_show(struct kobject *kobj,
-	struct kobj_attribute *attr, char *buf)
+			    struct kobj_attribute *attr, char *buf)
 {
 	ssize_t retval = 0;
 	struct msg_group_t *group = spk_find_msg_group(attr->attr.name);
 	unsigned long flags;
 
-	BUG_ON(!group);
+	if (WARN_ON(!group))
+		return -EINVAL;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	retval = message_show_helper(buf, group->start, group->end);
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
@@ -842,11 +844,13 @@ static ssize_t message_show(struct kobject *kobj,
 }
 
 static ssize_t message_store(struct kobject *kobj, struct kobj_attribute *attr,
-	const char *buf, size_t count)
+			     const char *buf, size_t count)
 {
 	struct msg_group_t *group = spk_find_msg_group(attr->attr.name);
 
-	BUG_ON(!group);
+	if (WARN_ON(!group))
+		return -EINVAL;
+
 	return message_store_helper(buf, count, group);
 }
 
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index c2f70ef5b9b3..d2ad596850f3 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -67,7 +67,7 @@ MODULE_PARM_DESC(quiet, "Do not announce when the synthesizer is found.");
 special_func spk_special_handler;
 
 short spk_pitch_shift, synth_flags;
-static char buf[256];
+static u16 buf[256];
 int spk_attrib_bleep, spk_bleeps, spk_bleep_time = 10;
 int spk_no_intr, spk_spell_delay;
 int spk_key_echo, spk_say_word_ctl;
@@ -108,11 +108,12 @@ enum {
 	CT_Window,
 	CT_Max
 };
+
 #define read_all_mode CT_Max
 
 static struct tty_struct *tty;
 
-static void spkup_write(const char *in_buf, int count);
+static void spkup_write(const u16 *in_buf, int count);
 
 static char *phonetic[] = {
 	"alfa", "bravo", "charlie", "delta", "echo", "foxtrot", "golf", "hotel",
@@ -238,7 +239,8 @@ static u_short default_chartab[256] = {
 struct task_struct *speakup_task;
 struct bleep spk_unprocessed_sound;
 static int spk_keydown;
-static u_char spk_lastkey, spk_close_press, keymap_flags;
+static u16 spk_lastkey;
+static u_char spk_close_press, keymap_flags;
 static u_char last_keycode, this_speakup_key;
 static u_long last_spk_jiffy;
 
@@ -299,7 +301,7 @@ static void speakup_shut_up(struct vc_data *vc)
 	spk_shut_up |= 0x01;
 	spk_parked &= 0xfe;
 	speakup_date(vc);
-	if (synth != NULL)
+	if (synth)
 		spk_do_flush();
 }
 
@@ -404,8 +406,9 @@ static void say_attributes(struct vc_data *vc)
 	if (bg > 7) {
 		synth_printf(" %s ", spk_msg_get(MSG_ON_BLINKING));
 		bg -= 8;
-	} else
+	} else {
 		synth_printf(" %s ", spk_msg_get(MSG_ON));
+	}
 	synth_printf("%s\n", spk_msg_get(MSG_COLORS_START + bg));
 }
 
@@ -426,39 +429,38 @@ static void announce_edge(struct vc_data *vc, int msg_id)
 			spk_msg_get(MSG_EDGE_MSGS_START + msg_id - 1));
 }
 
-static void speak_char(u_char ch)
+static void speak_char(u16 ch)
 {
-	char *cp = spk_characters[ch];
+	char *cp;
 	struct var_t *direct = spk_get_var(DIRECT);
 
-	if (direct && direct->u.n.value) {
-		if (IS_CHAR(ch, B_CAP)) {
+	if (ch >= 0x100 || (direct && direct->u.n.value)) {
+		if (ch < 0x100 && IS_CHAR(ch, B_CAP)) {
 			spk_pitch_shift++;
 			synth_printf("%s", spk_str_caps_start);
 		}
-		synth_printf("%c", ch);
-		if (IS_CHAR(ch, B_CAP))
+		synth_putwc_s(ch);
+		if (ch < 0x100 && IS_CHAR(ch, B_CAP))
 			synth_printf("%s", spk_str_caps_stop);
 		return;
 	}
-	if (cp == NULL) {
+
+	cp = spk_characters[ch];
+	if (!cp) {
 		pr_info("speak_char: cp == NULL!\n");
 		return;
 	}
-	synth_buffer_add(SPACE);
 	if (IS_CHAR(ch, B_CAP)) {
 		spk_pitch_shift++;
-		synth_printf("%s", spk_str_caps_start);
-		synth_printf("%s", cp);
-		synth_printf("%s", spk_str_caps_stop);
+		synth_printf("%s %s %s",
+			     spk_str_caps_start, cp, spk_str_caps_stop);
 	} else {
 		if (*cp == '^') {
-			synth_printf("%s", spk_msg_get(MSG_CTRL));
 			cp++;
-		}
-		synth_printf("%s", cp);
+			synth_printf(" %s%s ", spk_msg_get(MSG_CTRL), cp);
+		} else
+			synth_printf(" %s ", cp);
 	}
-	synth_buffer_add(SPACE);
 }
 
 static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
@@ -478,7 +480,7 @@ static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
 			c |= 0x100;
 		}
 
-		ch = inverse_translate(vc, c, 0);
+		ch = inverse_translate(vc, c, 1);
 		*attribs = (w & 0xff00) >> 8;
 	}
 	return ch;
@@ -486,7 +488,7 @@ static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
 
 static void say_char(struct vc_data *vc)
 {
-	u_short ch;
+	u16 ch;
 
 	spk_old_attr = spk_attr;
 	ch = get_char(vc, (u_short *)spk_pos, &spk_attr);
@@ -496,20 +498,20 @@ static void say_char(struct vc_data *vc)
 		if (spk_attrib_bleep & 2)
 			say_attributes(vc);
 	}
-	speak_char(ch & 0xff);
+	speak_char(ch);
 }
 
 static void say_phonetic_char(struct vc_data *vc)
 {
-	u_short ch;
+	u16 ch;
 
 	spk_old_attr = spk_attr;
 	ch = get_char(vc, (u_short *)spk_pos, &spk_attr);
-	if (isascii(ch) && isalpha(ch)) {
+	if (ch <= 0x7f && isalpha(ch)) {
 		ch &= 0x1f;
 		synth_printf("%s\n", phonetic[--ch]);
 	} else {
-		if (IS_CHAR(ch, B_NUM))
+		if (ch < 0x100 && IS_CHAR(ch, B_NUM))
 			synth_printf("%s ", spk_msg_get(MSG_NUMBER));
 		speak_char(ch);
 	}
@@ -551,42 +553,42 @@ static void say_next_char(struct vc_data *vc)
 static u_long get_word(struct vc_data *vc)
 {
 	u_long cnt = 0, tmpx = spk_x, tmp_pos = spk_pos;
-	char ch;
-	u_short attr_ch;
+	u16 ch;
+	u16 attr_ch;
 	u_char temp;
 
 	spk_old_attr = spk_attr;
-	ch = (char)get_char(vc, (u_short *)tmp_pos, &temp);
+	ch = get_char(vc, (u_short *)tmp_pos, &temp);
 
 /* decided to take out the sayword if on a space (mis-information */
 	if (spk_say_word_ctl && ch == SPACE) {
 		*buf = '\0';
 		synth_printf("%s\n", spk_msg_get(MSG_SPACE));
 		return 0;
-	} else if ((tmpx < vc->vc_cols - 2)
-		   && (ch == SPACE || ch == 0 || IS_WDLM(ch))
-		   && ((char)get_char(vc, (u_short *)&tmp_pos + 1, &temp) >
-		       SPACE)) {
+	} else if (tmpx < vc->vc_cols - 2 &&
+		   (ch == SPACE || ch == 0 || (ch < 0x100 && IS_WDLM(ch))) &&
+		   get_char(vc, (u_short *)&tmp_pos + 1, &temp) > SPACE) {
 		tmp_pos += 2;
 		tmpx++;
 	} else
 		while (tmpx > 0) {
-			ch = (char)get_char(vc, (u_short *)tmp_pos - 1, &temp);
-			if ((ch == SPACE || ch == 0 || IS_WDLM(ch))
-			    && ((char)get_char(vc, (u_short *)tmp_pos, &temp) >
-				SPACE))
+			ch = get_char(vc, (u_short *)tmp_pos - 1, &temp);
+			if ((ch == SPACE || ch == 0 ||
+			     (ch < 0x100 && IS_WDLM(ch))) &&
+			    get_char(vc, (u_short *)tmp_pos, &temp) > SPACE)
 				break;
 			tmp_pos -= 2;
 			tmpx--;
 		}
 	attr_ch = get_char(vc, (u_short *)tmp_pos, &spk_attr);
-	buf[cnt++] = attr_ch & 0xff;
+	buf[cnt++] = attr_ch;
 	while (tmpx < vc->vc_cols - 1) {
 		tmp_pos += 2;
 		tmpx++;
-		ch = (char)get_char(vc, (u_short *)tmp_pos, &temp);
-		if ((ch == SPACE) || ch == 0
-		    || (IS_WDLM(buf[cnt - 1]) && (ch > SPACE)))
+		ch = get_char(vc, (u_short *)tmp_pos, &temp);
+		if (ch == SPACE || ch == 0 ||
+		    (buf[cnt - 1] < 0x100 && IS_WDLM(buf[cnt - 1]) &&
+		     ch > SPACE))
 			break;
 		buf[cnt++] = ch;
 	}
@@ -610,7 +612,7 @@ static void say_word(struct vc_data *vc)
 static void say_prev_word(struct vc_data *vc)
 {
 	u_char temp;
-	char ch;
+	u16 ch;
 	u_short edge_said = 0, last_state = 0, state = 0;
 
 	spk_parked |= 0x01;
@@ -636,13 +638,14 @@ static void say_prev_word(struct vc_data *vc)
 				break;
 			spk_y--;
 			spk_x = vc->vc_cols - 1;
-		} else
+		} else {
 			spk_x--;
+		}
 		spk_pos -= 2;
-		ch = (char)get_char(vc, (u_short *)spk_pos, &temp);
+		ch = get_char(vc, (u_short *)spk_pos, &temp);
 		if (ch == SPACE || ch == 0)
 			state = 0;
-		else if (IS_WDLM(ch))
+		else if (ch < 0x100 && IS_WDLM(ch))
 			state = 1;
 		else
 			state = 2;
@@ -663,7 +666,7 @@ static void say_prev_word(struct vc_data *vc)
 static void say_next_word(struct vc_data *vc)
 {
 	u_char temp;
-	char ch;
+	u16 ch;
 	u_short edge_said = 0, last_state = 2, state = 0;
 
 	spk_parked |= 0x01;
@@ -672,10 +675,10 @@ static void say_next_word(struct vc_data *vc)
 		return;
 	}
 	while (1) {
-		ch = (char)get_char(vc, (u_short *)spk_pos, &temp);
+		ch = get_char(vc, (u_short *)spk_pos, &temp);
 		if (ch == SPACE || ch == 0)
 			state = 0;
-		else if (IS_WDLM(ch))
+		else if (ch < 0x100 && IS_WDLM(ch))
 			state = 1;
 		else
 			state = 2;
@@ -690,8 +693,9 @@ static void say_next_word(struct vc_data *vc)
 			spk_y++;
 			spk_x = 0;
 			edge_said = edge_right;
-		} else
+		} else {
 			spk_x++;
+		}
 		spk_pos += 2;
 		last_state = state;
 	}
@@ -703,39 +707,47 @@ static void say_next_word(struct vc_data *vc)
 static void spell_word(struct vc_data *vc)
 {
 	static char const *delay_str[] = { "", ",", ".", ". .", ". . ." };
-	char *cp = buf, *str_cap = spk_str_caps_stop;
-	char *cp1, *last_cap = spk_str_caps_stop;
-	u_char ch;
+	u16 *cp = buf;
+	char *cp1;
+	char *str_cap = spk_str_caps_stop;
+	char *last_cap = spk_str_caps_stop;
+	struct var_t *direct = spk_get_var(DIRECT);
+	u16 ch;
 
 	if (!get_word(vc))
 		return;
-	while ((ch = (u_char)*cp)) {
+	while ((ch = *cp)) {
 		if (cp != buf)
 			synth_printf(" %s ", delay_str[spk_spell_delay]);
-		if (IS_CHAR(ch, B_CAP)) {
+		/* FIXME: Non-latin1 considered as lower case */
+		if (ch < 0x100 && IS_CHAR(ch, B_CAP)) {
 			str_cap = spk_str_caps_start;
 			if (*spk_str_caps_stop)
 				spk_pitch_shift++;
 			else	/* synth has no pitch */
 				last_cap = spk_str_caps_stop;
-		} else
+		} else {
 			str_cap = spk_str_caps_stop;
+		}
 		if (str_cap != last_cap) {
 			synth_printf("%s", str_cap);
 			last_cap = str_cap;
 		}
-		if (this_speakup_key == SPELL_PHONETIC
-		    && (isascii(ch) && isalpha(ch))) {
-			ch &= 31;
+		if (ch >= 0x100 || (direct && direct->u.n.value)) {
+			synth_putwc_s(ch);
+		} else if (this_speakup_key == SPELL_PHONETIC &&
+		    ch <= 0x7f && isalpha(ch)) {
+			ch &= 0x1f;
 			cp1 = phonetic[--ch];
+			synth_printf("%s", cp1);
 		} else {
 			cp1 = spk_characters[ch];
 			if (*cp1 == '^') {
 				synth_printf("%s", spk_msg_get(MSG_CTRL));
 				cp1++;
 			}
+			synth_printf("%s", cp1);
 		}
-		synth_printf("%s", cp1);
 		cp++;
 	}
 	if (str_cap != spk_str_caps_stop)
@@ -751,7 +763,7 @@ static int get_line(struct vc_data *vc)
 	spk_old_attr = spk_attr;
 	spk_attr = get_attributes(vc, (u_short *)spk_pos);
 	for (i = 0; i < vc->vc_cols; i++) {
-		buf[i] = (u_char)get_char(vc, (u_short *)tmp, &tmp2);
+		buf[i] = get_char(vc, (u_short *)tmp, &tmp2);
 		tmp += 2;
 	}
 	for (--i; i >= 0; i--)
@@ -763,7 +775,7 @@ static int get_line(struct vc_data *vc)
 static void say_line(struct vc_data *vc)
 {
 	int i = get_line(vc);
-	char *cp;
+	u16 *cp;
 	u_short saved_punc_mask = spk_punc_mask;
 
 	if (i == 0) {
@@ -775,7 +787,7 @@ static void say_line(struct vc_data *vc)
 		cp = buf;
 		while (*cp == SPACE)
 			cp++;
-		synth_printf("%d, ", (cp - buf) + 1);
+		synth_printf("%zd, ", (cp - buf) + 1);
 	}
 	spk_punc_mask = spk_punc_masks[spk_reading_punc];
 	spkup_write(buf, i);
@@ -816,7 +828,7 @@ static int say_from_to(struct vc_data *vc, u_long from, u_long to,
 	spk_old_attr = spk_attr;
 	spk_attr = get_attributes(vc, (u_short *)from);
 	while (from < to) {
-		buf[i++] = (char)get_char(vc, (u_short *)from, &tmp);
+		buf[i++] = get_char(vc, (u_short *)from, &tmp);
 		from += 2;
 		if (i >= vc->vc_size_row)
 			break;
@@ -852,11 +864,11 @@ static void say_line_from_to(struct vc_data *vc, u_long from, u_long to,
 
 static int currsentence;
 static int numsentences[2];
-static char *sentbufend[2];
-static char *sentmarks[2][10];
+static u16 *sentbufend[2];
+static u16 *sentmarks[2][10];
 static int currbuf;
 static int bn;
-static char sentbuf[2][256];
+static u16 sentbuf[2][256];
 
 static int say_sentence_num(int num, int prev)
 {
@@ -892,10 +904,10 @@ static int get_sentence_buf(struct vc_data *vc, int read_punc)
 	spk_attr = get_attributes(vc, (u_short *)start);
 
 	while (start < end) {
-		sentbuf[bn][i] = (char)get_char(vc, (u_short *)start, &tmp);
+		sentbuf[bn][i] = get_char(vc, (u_short *)start, &tmp);
 		if (i > 0) {
-			if (sentbuf[bn][i] == SPACE && sentbuf[bn][i - 1] == '.'
-			    && numsentences[bn] < 9) {
+			if (sentbuf[bn][i] == SPACE && sentbuf[bn][i - 1] == '.' &&
+			    numsentences[bn] < 9) {
 				/* Sentence Marker */
 				numsentences[bn]++;
 				sentmarks[bn][numsentences[bn]] =
@@ -995,7 +1007,7 @@ static void right_edge(struct vc_data *vc)
 static void say_first_char(struct vc_data *vc)
 {
 	int i, len = get_line(vc);
-	u_char ch;
+	u16 ch;
 
 	spk_parked |= 0x01;
 	if (len == 0) {
@@ -1015,7 +1027,7 @@ static void say_first_char(struct vc_data *vc)
 static void say_last_char(struct vc_data *vc)
 {
 	int len = get_line(vc);
-	u_char ch;
+	u16 ch;
 
 	spk_parked |= 0x01;
 	if (len == 0) {
@@ -1040,9 +1052,8 @@ static void say_position(struct vc_data *vc)
 static void say_char_num(struct vc_data *vc)
 {
 	u_char tmp;
-	u_short ch = get_char(vc, (u_short *)spk_pos, &tmp);
+	u16 ch = get_char(vc, (u_short *)spk_pos, &tmp);
 
-	ch &= 0xff;
 	synth_printf(spk_msg_get(MSG_CHAR_INFO), ch, ch);
 }
 
@@ -1070,10 +1081,10 @@ static void say_to_right(struct vc_data *vc)
 
 /* end of stub functions. */
 
-static void spkup_write(const char *in_buf, int count)
+static void spkup_write(const u16 *in_buf, int count)
 {
 	static int rep_count;
-	static u_char ch = '\0', old_ch = '\0';
+	static u16 ch = '\0', old_ch = '\0';
 	static u_short char_type, last_type;
 	int in_count = count;
 
@@ -1085,8 +1096,11 @@ static void spkup_write(const char *in_buf, int count)
 			    (currsentence <= numsentences[bn]))
 				synth_insert_next_index(currsentence++);
 		}
-		ch = (u_char)*in_buf++;
-		char_type = spk_chartab[ch];
+		ch = *in_buf++;
+		if (ch < 0x100)
+			char_type = spk_chartab[ch];
+		else
+			char_type = ALPHA;
 		if (ch == old_ch && !(char_type & B_NUM)) {
 			if (++rep_count > 2)
 				continue;
@@ -1106,10 +1120,10 @@ static void spkup_write(const char *in_buf, int count)
 		} else if (char_type & B_ALPHA) {
 			if ((synth_flags & SF_DEC) && (last_type & PUNC))
 				synth_buffer_add(SPACE);
-			synth_printf("%c", ch);
+			synth_putwc_s(ch);
 		} else if (char_type & B_NUM) {
 			rep_count = 0;
-			synth_printf("%c", ch);
+			synth_putwc_s(ch);
 		} else if (char_type & spk_punc_mask) {
 			speak_char(ch);
 			char_type &= ~PUNC;	/* for dec nospell processing */
@@ -1122,7 +1136,7 @@ static void spkup_write(const char *in_buf, int count)
 			 * repeats on you don't get nothing repeated count
 			 */
 			if (ch != old_ch)
-				synth_printf("%c", ch);
+				synth_putwc_s(ch);
 			else
 				rep_count = 0;
 		} else {
@@ -1140,7 +1154,7 @@ static void spkup_write(const char *in_buf, int count)
 		if (last_type & CH_RPT) {
 			synth_printf(" ");
 			synth_printf(spk_msg_get(MSG_REPEAT_DESC2),
-					++rep_count);
+				     ++rep_count);
 			synth_printf(" ");
 		}
 		rep_count = 0;
@@ -1157,7 +1171,7 @@ static void do_handle_shift(struct vc_data *vc, u_char value, char up_flag)
 {
 	unsigned long flags;
 
-	if (synth == NULL || up_flag || spk_killed)
+	if (!synth || up_flag || spk_killed)
 		return;
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	if (cursor_track == read_all_mode) {
@@ -1195,7 +1209,7 @@ static void do_handle_latin(struct vc_data *vc, u_char value, char up_flag)
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		return;
 	}
-	if (synth == NULL || spk_killed) {
+	if (!synth || spk_killed) {
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		return;
 	}
@@ -1216,13 +1230,19 @@ int spk_set_key_info(const u_char *key_info, u_char *k_buffer)
 	u_char ch, version, num_keys;
 
 	version = *cp++;
-	if (version != KEY_MAP_VER)
-		return -1;
+	if (version != KEY_MAP_VER) {
+		pr_debug("version found %d should be %d\n",
+			 version, KEY_MAP_VER);
+		return -EINVAL;
+	}
 	num_keys = *cp;
 	states = (int)cp[1];
 	key_data_len = (states + 1) * (num_keys + 1);
-	if (key_data_len + SHIFT_TBL_SIZE + 4 >= sizeof(spk_key_buf))
-		return -2;
+	if (key_data_len + SHIFT_TBL_SIZE + 4 >= sizeof(spk_key_buf)) {
+		pr_debug("too many key_infos (%d over %u)\n",
+			 key_data_len + SHIFT_TBL_SIZE + 4, (unsigned int)(sizeof(spk_key_buf)));
+		return -EINVAL;
+	}
 	memset(k_buffer, 0, SHIFT_TBL_SIZE);
 	memset(spk_our_keys, 0, sizeof(spk_our_keys));
 	spk_shift_table = k_buffer;
@@ -1233,14 +1253,19 @@ int spk_set_key_info(const u_char *key_info, u_char *k_buffer)
 	cp1 += 2;		/* now pointing at shift states */
 	for (i = 1; i <= states; i++) {
 		ch = *cp1++;
-		if (ch >= SHIFT_TBL_SIZE)
-			return -3;
+		if (ch >= SHIFT_TBL_SIZE) {
+			pr_debug("(%d) not valid shift state (max_allowed = %d)\n", ch,
+				 SHIFT_TBL_SIZE);
+			return -EINVAL;
+		}
 		spk_shift_table[ch] = i;
 	}
 	keymap_flags = *cp1++;
 	while ((ch = *cp1)) {
-		if (ch >= MAX_KEY)
-			return -4;
+		if (ch >= MAX_KEY) {
+			pr_debug("(%d), not valid key, (max_allowed = %d)\n", ch, MAX_KEY);
+			return -EINVAL;
+		}
 		spk_our_keys[ch] = cp1;
 		cp1 += states + 1;
 	}
@@ -1279,8 +1304,8 @@ void spk_reset_default_chars(void)
 
 	/* First, free any non-default */
 	for (i = 0; i < 256; i++) {
-		if ((spk_characters[i] != NULL)
-		    && (spk_characters[i] != spk_default_chars[i]))
+		if (spk_characters[i] &&
+		    (spk_characters[i] != spk_default_chars[i]))
 			kfree(spk_characters[i]);
 	}
 
@@ -1316,19 +1341,20 @@ static int edit_bits(struct vc_data *vc, u_char type, u_char ch, u_short key)
 }
 
 /* Allocation concurrency is protected by the console semaphore */
-static int speakup_allocate(struct vc_data *vc)
+static int speakup_allocate(struct vc_data *vc, gfp_t gfp_flags)
 {
 	int vc_num;
 
 	vc_num = vc->vc_num;
-	if (speakup_console[vc_num] == NULL) {
+	if (!speakup_console[vc_num]) {
 		speakup_console[vc_num] = kzalloc(sizeof(*speakup_console[0]),
-						  GFP_ATOMIC);
-		if (speakup_console[vc_num] == NULL)
+						  gfp_flags);
+		if (!speakup_console[vc_num])
 			return -ENOMEM;
 		speakup_date(vc);
-	} else if (!spk_parked)
+	} else if (!spk_parked) {
 		speakup_date(vc);
+	}
 
 	return 0;
 }
@@ -1373,7 +1399,7 @@ static void kbd_fakekey2(struct vc_data *vc, int command)
 
 static void read_all_doc(struct vc_data *vc)
 {
-	if ((vc->vc_num != fg_console) || synth == NULL || spk_shut_up)
+	if ((vc->vc_num != fg_console) || !synth || spk_shut_up)
 		return;
 	if (!synth_supports_indexing())
 		return;
@@ -1381,9 +1407,9 @@ static void read_all_doc(struct vc_data *vc)
 		prev_cursor_track = cursor_track;
 	cursor_track = read_all_mode;
 	spk_reset_index_count(0);
-	if (get_sentence_buf(vc, 0) == -1)
+	if (get_sentence_buf(vc, 0) == -1) {
 		kbd_fakekey2(vc, RA_DOWN_ARROW);
-	else {
+	} else {
 		say_sentence_num(0, 0);
 		synth_insert_next_index(0);
 		start_read_all_timer(vc, RA_TIMER);
@@ -1430,8 +1456,9 @@ static void handle_cursor_read_all(struct vc_data *vc, int command)
 			if (!say_sentence_num(sentcount + 1, 1)) {
 				sn = 1;
 				spk_reset_index_count(sn);
-			} else
+			} else {
 				synth_insert_next_index(0);
+			}
 			if (!say_sentence_num(sn, 0)) {
 				kbd_fakekey2(vc, RA_FIND_NEXT_SENT);
 				return;
@@ -1460,9 +1487,9 @@ static void handle_cursor_read_all(struct vc_data *vc, int command)
 		rv = get_sentence_buf(vc, 0);
 		if (rv == -1)
 			read_all_doc(vc);
-		if (rv == 0)
+		if (rv == 0) {
 			kbd_fakekey2(vc, RA_FIND_NEXT_SENT);
-		else {
+		} else {
 			say_sentence_num(1, 0);
 			synth_insert_next_index(0);
 			start_read_all_timer(vc, RA_TIMER);
@@ -1487,7 +1514,7 @@ static int pre_handle_cursor(struct vc_data *vc, u_char value, char up_flag)
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	if (cursor_track == read_all_mode) {
 		spk_parked &= 0xfe;
-		if (synth == NULL || up_flag || spk_shut_up) {
+		if (!synth || up_flag || spk_shut_up) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			return NOTIFY_STOP;
 		}
@@ -1509,7 +1536,7 @@ static void do_handle_cursor(struct vc_data *vc, u_char value, char up_flag)
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	spk_parked &= 0xfe;
-	if (synth == NULL || up_flag || spk_shut_up || cursor_track == CT_Off) {
+	if (!synth || up_flag || spk_shut_up || cursor_track == CT_Off) {
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		return;
 	}
@@ -1533,7 +1560,7 @@ static void do_handle_cursor(struct vc_data *vc, u_char value, char up_flag)
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 }
 
-static void update_color_buffer(struct vc_data *vc, const char *ic, int len)
+static void update_color_buffer(struct vc_data *vc, const u16 *ic, int len)
 {
 	int i, bi, hi;
 	int vc_num = vc->vc_num;
@@ -1548,7 +1575,7 @@ static void update_color_buffer(struct vc_data *vc, const char *ic, int len)
 		speakup_console[vc_num]->ht.ry[bi] = vc->vc_y;
 	}
 	while ((hi < COLOR_BUFFER_SIZE) && (i < len)) {
-		if ((ic[i] > 32) && (ic[i] < 127)) {
+		if (ic[i] > 32) {
 			speakup_console[vc_num]->ht.highbuf[bi][hi] = ic[i];
 			hi++;
 		} else if ((ic[i] == 32) && (hi != 0)) {
@@ -1705,7 +1732,7 @@ static void speakup_bs(struct vc_data *vc)
 		return;
 	if (!spk_parked)
 		speakup_date(vc);
-	if (spk_shut_up || synth == NULL) {
+	if (spk_shut_up || !synth) {
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		return;
 	}
@@ -1718,11 +1745,11 @@ static void speakup_bs(struct vc_data *vc)
 }
 
 /* called by: vt_notifier_call() */
-static void speakup_con_write(struct vc_data *vc, const char *str, int len)
+static void speakup_con_write(struct vc_data *vc, u16 *str, int len)
 {
 	unsigned long flags;
 
-	if ((vc->vc_num != fg_console) || spk_shut_up || synth == NULL)
+	if ((vc->vc_num != fg_console) || spk_shut_up || !synth)
 		return;
 	if (!spin_trylock_irqsave(&speakup_info.spinlock, flags))
 		/* Speakup output, discard */
@@ -1751,7 +1778,7 @@ static void speakup_con_update(struct vc_data *vc)
 {
 	unsigned long flags;
 
-	if (speakup_console[vc->vc_num] == NULL || spk_parked)
+	if (!speakup_console[vc->vc_num] || spk_parked)
 		return;
 	if (!spin_trylock_irqsave(&speakup_info.spinlock, flags))
 		/* Speakup output, discard */
@@ -1766,7 +1793,7 @@ static void do_handle_spec(struct vc_data *vc, u_char value, char up_flag)
 	int on_off = 2;
 	char *label;
 
-	if (synth == NULL || up_flag || spk_killed)
+	if (!synth || up_flag || spk_killed)
 		return;
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	spk_shut_up &= 0xfe;
@@ -1810,7 +1837,7 @@ static int inc_dec_var(u_char value)
 
 	var_id = var_id / 2 + FIRST_SET_VAR;
 	p_header = spk_get_var_header(var_id);
-	if (p_header == NULL)
+	if (!p_header)
 		return -1;
 	if (p_header->var_type != VAR_NUM)
 		return -1;
@@ -1893,7 +1920,7 @@ static void speakup_bits(struct vc_data *vc)
 {
 	int val = this_speakup_key - (FIRST_EDIT_BITS - 1);
 
-	if (spk_special_handler != NULL || val < 1 || val > 6) {
+	if (spk_special_handler || val < 1 || val > 6) {
 		synth_printf("%s\n", spk_msg_get(MSG_ERROR));
 		return;
 	}
@@ -1908,6 +1935,7 @@ static int handle_goto(struct vc_data *vc, u_char type, u_char ch, u_short key)
 	static int num;
 	int maxlen;
 	char *cp;
+	u16 wch;
 
 	if (type == KT_SPKUP && ch == SPEAKUP_GOTO)
 		goto do_goto;
@@ -1916,18 +1944,20 @@ static int handle_goto(struct vc_data *vc, u_char type, u_char ch, u_short key)
 	if (type != 0)
 		goto oops;
 	if (ch == 8) {
+		u16 wch;
 		if (num == 0)
 			return -1;
-		ch = goto_buf[--num];
+		wch = goto_buf[--num];
 		goto_buf[num] = '\0';
-		spkup_write(&ch, 1);
+		spkup_write(&wch, 1);
 		return 1;
 	}
 	if (ch < '+' || ch > 'y')
 		goto oops;
+	wch = ch;
 	goto_buf[num++] = ch;
 	goto_buf[num] = '\0';
-	spkup_write(&ch, 1);
+	spkup_write(&wch, 1);
 	maxlen = (*goto_buf >= '0') ? 3 : 4;
 	if ((ch == '+' || ch == '-') && num == 1)
 		return 1;
@@ -1984,7 +2014,7 @@ do_goto:
 
 static void speakup_goto(struct vc_data *vc)
 {
-	if (spk_special_handler != NULL) {
+	if (spk_special_handler) {
 		synth_printf("%s\n", spk_msg_get(MSG_ERROR));
 		return;
 	}
@@ -2072,8 +2102,8 @@ speakup_key(struct vc_data *vc, int shift_state, int keycode, u_short keysym,
 	tty = vc->port.tty;
 	if (type >= 0xf0)
 		type -= 0xf0;
-	if (type == KT_PAD
-		&& (vt_get_leds(fg_console, VC_NUMLOCK))) {
+	if (type == KT_PAD &&
+	    (vt_get_leds(fg_console, VC_NUMLOCK))) {
 		if (up_flag) {
 			spk_keydown = 0;
 			goto out;
@@ -2135,7 +2165,7 @@ speakup_key(struct vc_data *vc, int shift_state, int keycode, u_short keysym,
 		}
 	}
 no_map:
-	if (type == KT_SPKUP && spk_special_handler == NULL) {
+	if (type == KT_SPKUP && !spk_special_handler) {
 		do_spkup(vc, new_key);
 		spk_close_press = 0;
 		ret = 1;
@@ -2144,10 +2174,10 @@ no_map:
 	if (up_flag || spk_killed || type == KT_SHIFT)
 		goto out;
 	spk_shut_up &= 0xfe;
-	kh = (value == KVAL(K_DOWN))
-	    || (value == KVAL(K_UP))
-	    || (value == KVAL(K_LEFT))
-	    || (value == KVAL(K_RIGHT));
+	kh = (value == KVAL(K_DOWN)) ||
+	    (value == KVAL(K_UP)) ||
+	    (value == KVAL(K_LEFT)) ||
+	    (value == KVAL(K_RIGHT));
 	if ((cursor_track != read_all_mode) || !kh)
 		if (!spk_no_intr)
 			spk_do_flush();
@@ -2155,10 +2185,11 @@ no_map:
 		if (type == KT_SPEC && value == 1) {
 			value = '\n';
 			type = KT_LATIN;
-		} else if (type == KT_LETTER)
+		} else if (type == KT_LETTER) {
 			type = KT_LATIN;
-		else if (value == 0x7f)
+		} else if (value == 0x7f) {
 			value = 8;	/* make del = backspace */
+		}
 		ret = (*spk_special_handler) (vc, type, value, keycode);
 		spk_close_press = 0;
 		if (ret < 0)
@@ -2246,17 +2277,16 @@ static int vt_notifier_call(struct notifier_block *nb,
 	switch (code) {
 	case VT_ALLOCATE:
 		if (vc->vc_mode == KD_TEXT)
-			speakup_allocate(vc);
+			speakup_allocate(vc, GFP_ATOMIC);
 		break;
 	case VT_DEALLOCATE:
 		speakup_deallocate(vc);
 		break;
 	case VT_WRITE:
-		if (param->c == '\b')
+		if (param->c == '\b') {
 			speakup_bs(vc);
-		else if (param->c < 0x100) {
-			char d = param->c;
-
+		} else {
+			u16 d = param->c;
 			speakup_con_write(vc, &d, 1);
 		}
 		break;
@@ -2306,7 +2336,6 @@ static int __init speakup_init(void)
 {
 	int i;
 	long err = 0;
-	struct st_spk_t *first_console;
 	struct vc_data *vc = vc_cons[fg_console].d;
 	struct var_t *var;
 
@@ -2331,18 +2360,9 @@ static int __init speakup_init(void)
 	if (err)
 		goto error_virtkeyboard;
 
-	first_console = kzalloc(sizeof(*first_console), GFP_KERNEL);
-	if (!first_console) {
-		err = -ENOMEM;
-		goto error_alloc;
-	}
-
-	speakup_console[vc->vc_num] = first_console;
-	speakup_date(vc);
-
 	for (i = 0; i < MAX_NR_CONSOLES; i++)
 		if (vc_cons[i].d) {
-			err = speakup_allocate(vc_cons[i].d);
+			err = speakup_allocate(vc_cons[i].d, GFP_KERNEL);
 			if (err)
 				goto error_kobjects;
 		}
@@ -2401,7 +2421,6 @@ error_kobjects:
 	for (i = 0; i < MAX_NR_CONSOLES; i++)
 		kfree(speakup_console[i]);
 
-error_alloc:
 	speakup_remove_virtual_keyboard();
 
 error_virtkeyboard:
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index aeb2b865615a..08f68fc2864e 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -75,7 +75,7 @@ int speakup_set_selection(struct tty_struct *tty)
 		speakup_clear_selection();
 		spk_sel_cons = vc_cons[fg_console].d;
 		dev_warn(tty->dev,
-			"Selection: mark console not the same as cut\n");
+			 "Selection: mark console not the same as cut\n");
 		return -EINVAL;
 	}
 
@@ -99,7 +99,7 @@ int speakup_set_selection(struct tty_struct *tty)
 	sel_start = new_sel_start;
 	sel_end = new_sel_end;
 	/* Allocate a new buffer before freeing the old one ... */
-	bp = kmalloc((sel_end-sel_start)/2+1, GFP_ATOMIC);
+	bp = kmalloc((sel_end - sel_start) / 2 + 1, GFP_ATOMIC);
 	if (!bp) {
 		speakup_clear_selection();
 		return -ENOMEM;
@@ -175,7 +175,7 @@ static struct speakup_paste_work speakup_paste_work = {
 
 int speakup_paste_selection(struct tty_struct *tty)
 {
-	if (cmpxchg(&speakup_paste_work.tty, NULL, tty) != NULL)
+	if (cmpxchg(&speakup_paste_work.tty, NULL, tty))
 		return -EBUSY;
 
 	tty_kref_get(tty);
diff --git a/drivers/staging/speakup/serialio.c b/drivers/staging/speakup/serialio.c
index ef89dc1c21c8..ba060d0ceca2 100644
--- a/drivers/staging/speakup/serialio.c
+++ b/drivers/staging/speakup/serialio.c
@@ -21,9 +21,21 @@ static void start_serial_interrupt(int irq);
 static const struct old_serial_port rs_table[] = {
 	SERIAL_PORT_DFNS
 };
+
 static const struct old_serial_port *serstate;
 static int timeouts;
 
+static int spk_serial_out(struct spk_synth *in_synth, const char ch);
+static void spk_serial_send_xchar(char ch);
+static void spk_serial_tiocmset(unsigned int set, unsigned int clear);
+
+struct spk_io_ops spk_serial_io_ops = {
+	.synth_out = spk_serial_out,
+	.send_xchar = spk_serial_send_xchar,
+	.tiocmset = spk_serial_tiocmset,
+};
+EXPORT_SYMBOL_GPL(spk_serial_io_ops);
+
 const struct old_serial_port *spk_serial_init(int index)
 {
 	int baud = 9600, quot = 0;
@@ -97,8 +109,7 @@ static irqreturn_t synth_readbuf_handler(int irq, void *dev_id)
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	while (inb_p(speakup_info.port_tts + UART_LSR) & UART_LSR_DR) {
-
-		c = inb_p(speakup_info.port_tts+UART_RX);
+		c = inb_p(speakup_info.port_tts + UART_RX);
 		synth->read_buff_add((u_char)c);
 	}
 	spin_unlock_irqrestore(&speakup_info.spinlock, flags);
@@ -119,17 +130,64 @@ static void start_serial_interrupt(int irq)
 		pr_err("Unable to request Speakup serial I R Q\n");
 	/* Set MCR */
 	outb(UART_MCR_DTR | UART_MCR_RTS | UART_MCR_OUT2,
-			speakup_info.port_tts + UART_MCR);
+	     speakup_info.port_tts + UART_MCR);
 	/* Turn on Interrupts */
 	outb(UART_IER_MSI|UART_IER_RLSI|UART_IER_RDI,
 			speakup_info.port_tts + UART_IER);
-	inb(speakup_info.port_tts+UART_LSR);
-	inb(speakup_info.port_tts+UART_RX);
-	inb(speakup_info.port_tts+UART_IIR);
-	inb(speakup_info.port_tts+UART_MSR);
+	inb(speakup_info.port_tts + UART_LSR);
+	inb(speakup_info.port_tts + UART_RX);
+	inb(speakup_info.port_tts + UART_IIR);
+	inb(speakup_info.port_tts + UART_MSR);
 	outb(1, speakup_info.port_tts + UART_FCR);	/* Turn FIFO On */
 }
 
+static void spk_serial_send_xchar(char ch)
+{
+	int timeout = SPK_XMITR_TIMEOUT;
+
+	while (spk_serial_tx_busy()) {
+		if (!--timeout)
+			break;
+		udelay(1);
+	}
+	outb(ch, speakup_info.port_tts);
+}
+
+static void spk_serial_tiocmset(unsigned int set, unsigned int clear)
+{
+	int old = inb(speakup_info.port_tts + UART_MCR);
+	outb((old & ~clear) | set, speakup_info.port_tts + UART_MCR);
+}
+
+int spk_serial_synth_probe(struct spk_synth *synth)
+{
+	const struct old_serial_port *ser;
+	int failed = 0;
+
+	if ((synth->ser >= SPK_LO_TTY) && (synth->ser <= SPK_HI_TTY)) {
+		ser = spk_serial_init(synth->ser);
+		if (!ser) {
+			failed = -1;
+		} else {
+			outb_p(0, ser->port);
+			mdelay(1);
+			outb_p('\r', ser->port);
+		}
+	} else {
+		failed = -1;
+		pr_warn("ttyS%i is an invalid port\n", synth->ser);
+	}
+	if (failed) {
+		pr_info("%s: not found\n", synth->long_name);
+		return -ENODEV;
+	}
+	pr_info("%s: ttyS%i, Driver Version %s\n",
+		synth->long_name, synth->ser, synth->version);
+	synth->alive = 1;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(spk_serial_synth_probe);
+
 void spk_stop_serial_interrupt(void)
 {
 	if (speakup_info.port_tts == 0)
@@ -139,19 +197,20 @@ void spk_stop_serial_interrupt(void)
 		return;
 
 	/* Turn off interrupts */
-	outb(0, speakup_info.port_tts+UART_IER);
+	outb(0, speakup_info.port_tts + UART_IER);
 	/* Free IRQ */
 	free_irq(serstate->irq, (void *)synth_readbuf_handler);
 }
+EXPORT_SYMBOL_GPL(spk_stop_serial_interrupt);
 
-int spk_wait_for_xmitr(void)
+int spk_wait_for_xmitr(struct spk_synth *in_synth)
 {
 	int tmout = SPK_XMITR_TIMEOUT;
 
-	if ((synth->alive) && (timeouts >= NUM_DISABLE_TIMEOUTS)) {
+	if ((in_synth->alive) && (timeouts >= NUM_DISABLE_TIMEOUTS)) {
 		pr_warn("%s: too many timeouts, deactivating speakup\n",
-			synth->long_name);
-		synth->alive = 0;
+			in_synth->long_name);
+		in_synth->alive = 0;
 		/* No synth any more, so nobody will restart TTYs, and we thus
 		 * need to do it ourselves.  Now that there is no synth we can
 		 * let application flood anyway
@@ -162,7 +221,7 @@ int spk_wait_for_xmitr(void)
 	}
 	while (spk_serial_tx_busy()) {
 		if (--tmout == 0) {
-			pr_warn("%s: timed out (tx busy)\n", synth->long_name);
+			pr_warn("%s: timed out (tx busy)\n", in_synth->long_name);
 			timeouts++;
 			return 0;
 		}
@@ -207,18 +266,35 @@ unsigned char spk_serial_in_nowait(void)
 }
 EXPORT_SYMBOL_GPL(spk_serial_in_nowait);
 
-int spk_serial_out(const char ch)
+static int spk_serial_out(struct spk_synth *in_synth, const char ch)
 {
-	if (synth->alive && spk_wait_for_xmitr()) {
+	if (in_synth->alive && spk_wait_for_xmitr(in_synth)) {
 		outb_p(ch, speakup_info.port_tts);
 		return 1;
 	}
 	return 0;
 }
-EXPORT_SYMBOL_GPL(spk_serial_out);
+
+const char *spk_serial_synth_immediate(struct spk_synth *synth, const char *buff)
+{
+	u_char ch;
+
+	while ((ch = *buff)) {
+		if (ch == '\n')
+			ch = synth->procspeech;
+		if (spk_wait_for_xmitr(synth))
+			outb(ch, speakup_info.port_tts);
+		else
+			return buff;
+		buff++;
+	}
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(spk_serial_synth_immediate);
 
 void spk_serial_release(void)
 {
+	spk_stop_serial_interrupt();
 	if (speakup_info.port_tts == 0)
 		return;
 	synth_release_region(speakup_info.port_tts, 8);
diff --git a/drivers/staging/speakup/speakup.h b/drivers/staging/speakup/speakup.h
index b203f0f883a9..a654334c98b9 100644
--- a/drivers/staging/speakup/speakup.h
+++ b/drivers/staging/speakup/speakup.h
@@ -20,7 +20,7 @@
 #define A_CAP 0x0007
 #define B_NUM 0x0008
 #define NUM 0x0009
-#define ALPHANUM (B_ALPHA|B_NUM)
+#define ALPHANUM (B_ALPHA | B_NUM)
 #define SOME 0x0010
 #define MOST 0x0020
 #define PUNC 0x0040
@@ -30,13 +30,14 @@
 #define B_EXNUM 0x0100
 #define CH_RPT 0x0200
 #define B_CTL 0x0400
-#define A_CTL (B_CTL+SYNTH_OK)
+#define A_CTL (B_CTL + SYNTH_OK)
 #define B_SYM 0x0800
-#define B_CAPSYM (B_CAP|B_SYM)
+#define B_CAPSYM (B_CAP | B_SYM)
 
-#define IS_WDLM(x) (spk_chartab[((u_char)x)]&B_WDLM)
-#define IS_CHAR(x, type) (spk_chartab[((u_char)x)]&type)
-#define IS_TYPE(x, type) ((spk_chartab[((u_char)x)]&type) == type)
+/* FIXME: u16 */
+#define IS_WDLM(x) (spk_chartab[((u_char)x)] & B_WDLM)
+#define IS_CHAR(x, type) (spk_chartab[((u_char)x)] & type)
+#define IS_TYPE(x, type) ((spk_chartab[((u_char)x)] & type) == type)
 
 int speakup_thread(void *data);
 void spk_reset_default_chars(void);
@@ -66,7 +67,7 @@ void synth_release(void);
 
 void spk_do_flush(void);
 void speakup_start_ttys(void);
-void synth_buffer_add(char ch);
+void synth_buffer_add(u16 ch);
 void synth_buffer_clear(void);
 void speakup_clear_selection(void);
 int speakup_set_selection(struct tty_struct *tty);
diff --git a/drivers/staging/speakup/speakup_acntpc.c b/drivers/staging/speakup/speakup_acntpc.c
index c7fab261d860..ad72f8e883fc 100644
--- a/drivers/staging/speakup/speakup_acntpc.c
+++ b/drivers/staging/speakup/speakup_acntpc.c
@@ -113,6 +113,7 @@ static struct spk_synth synth_acntpc = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = accent_release,
 	.synth_immediate = synth_immediate,
@@ -196,6 +197,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -233,7 +235,7 @@ static void do_catch_up(struct spk_synth *synth)
 			delay_time_val = delay_time->u.n.value;
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			schedule_timeout(msecs_to_jiffies(delay_time_val));
-			jiff_max = jiffies+jiffy_delta_val;
+			jiff_max = jiffies + jiffy_delta_val;
 		}
 	}
 	timeout = SPK_XMITR_TIMEOUT;
@@ -259,18 +261,18 @@ static int synth_probe(struct spk_synth *synth)
 	if (port_forced) {
 		speakup_info.port_tts = port_forced;
 		pr_info("probe forced to %x by kernel command line\n",
-				speakup_info.port_tts);
-		if (synth_request_region(speakup_info.port_tts-1,
-					SYNTH_IO_EXTENT)) {
+			speakup_info.port_tts);
+		if (synth_request_region(speakup_info.port_tts - 1,
+					 SYNTH_IO_EXTENT)) {
 			pr_warn("sorry, port already reserved\n");
 			return -EBUSY;
 		}
-		port_val = inw(speakup_info.port_tts-1);
-		synth_port_control = speakup_info.port_tts-1;
+		port_val = inw(speakup_info.port_tts - 1);
+		synth_port_control = speakup_info.port_tts - 1;
 	} else {
 		for (i = 0; synth_portlist[i]; i++) {
 			if (synth_request_region(synth_portlist[i],
-						SYNTH_IO_EXTENT)) {
+						 SYNTH_IO_EXTENT)) {
 				pr_warn
 				    ("request_region: failed with 0x%x, %d\n",
 				     synth_portlist[i], SYNTH_IO_EXTENT);
@@ -280,7 +282,7 @@ static int synth_probe(struct spk_synth *synth)
 			if (port_val == 0x53fc) {
 				/* 'S' and out&input bits */
 				synth_port_control = synth_portlist[i];
-				speakup_info.port_tts = synth_port_control+1;
+				speakup_info.port_tts = synth_port_control + 1;
 				break;
 			}
 		}
@@ -294,7 +296,7 @@ static int synth_probe(struct spk_synth *synth)
 		return -ENODEV;
 	}
 	pr_info("%s: %03x-%03x, driver version %s,\n", synth->long_name,
-		synth_port_control, synth_port_control+SYNTH_IO_EXTENT-1,
+		synth_port_control, synth_port_control + SYNTH_IO_EXTENT - 1,
 		synth->version);
 	synth->alive = 1;
 	return 0;
@@ -302,6 +304,7 @@ static int synth_probe(struct spk_synth *synth)
 
 static void accent_release(void)
 {
+	spk_stop_serial_interrupt();
 	if (speakup_info.port_tts)
 		synth_release_region(speakup_info.port_tts-1, SYNTH_IO_EXTENT);
 	speakup_info.port_tts = 0;
diff --git a/drivers/staging/speakup/speakup_acntsa.c b/drivers/staging/speakup/speakup_acntsa.c
index b2e352712766..de67ffda7d45 100644
--- a/drivers/staging/speakup/speakup_acntsa.c
+++ b/drivers/staging/speakup/speakup_acntsa.c
@@ -99,9 +99,10 @@ static struct spk_synth synth_acntsa = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -126,7 +127,7 @@ static int synth_probe(struct spk_synth *synth)
 
 	failed = spk_serial_synth_probe(synth);
 	if (failed == 0) {
-		spk_synth_immediate(synth, "\033=R\r");
+		synth->synth_immediate(synth, "\033=R\r");
 		mdelay(100);
 	}
 	synth->alive = !failed;
diff --git a/drivers/staging/speakup/speakup_apollo.c b/drivers/staging/speakup/speakup_apollo.c
index 3f43f8105bc0..cead8b1b1bfc 100644
--- a/drivers/staging/speakup/speakup_apollo.c
+++ b/drivers/staging/speakup/speakup_apollo.c
@@ -108,9 +108,10 @@ static struct spk_synth synth_apollo = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -160,6 +161,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -168,10 +170,9 @@ static void do_catch_up(struct spk_synth *synth)
 		set_current_state(TASK_INTERRUPTIBLE);
 		full_time_val = full_time->u.n.value;
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		if (!spk_serial_out(ch)) {
-			outb(UART_MCR_DTR, speakup_info.port_tts + UART_MCR);
-			outb(UART_MCR_DTR | UART_MCR_RTS,
-					speakup_info.port_tts + UART_MCR);
+		if (!synth->io_ops->synth_out(synth, ch)) {
+			synth->io_ops->tiocmset(0, UART_MCR_RTS);
+			synth->io_ops->tiocmset(UART_MCR_RTS, 0);
 			schedule_timeout(msecs_to_jiffies(full_time_val));
 			continue;
 		}
@@ -181,7 +182,7 @@ static void do_catch_up(struct spk_synth *synth)
 			full_time_val = full_time->u.n.value;
 			delay_time_val = delay_time->u.n.value;
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-			if (spk_serial_out(synth->procspeech))
+			if (synth->io_ops->synth_out(synth, synth->procspeech))
 				schedule_timeout(msecs_to_jiffies
 						 (delay_time_val));
 			else
@@ -194,7 +195,7 @@ static void do_catch_up(struct spk_synth *synth)
 		synth_buffer_getc();
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 	}
-	spk_serial_out(PROCSPEECH);
+	synth->io_ops->synth_out(synth, PROCSPEECH);
 }
 
 module_param_named(ser, synth_apollo.ser, int, 0444);
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index e696b87bf515..6880352a7b74 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -104,9 +104,10 @@ static struct spk_synth synth_audptr = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -127,15 +128,8 @@ static struct spk_synth synth_audptr = {
 
 static void synth_flush(struct spk_synth *synth)
 {
-	int timeout = SPK_XMITR_TIMEOUT;
-
-	while (spk_serial_tx_busy()) {
-		if (!--timeout)
-			break;
-		udelay(1);
-	}
-	outb(SYNTH_CLEAR, speakup_info.port_tts);
-	spk_serial_out(PROCSPEECH);
+	synth->io_ops->send_xchar(SYNTH_CLEAR);
+	synth->io_ops->synth_out(synth, PROCSPEECH);
 }
 
 static void synth_version(struct spk_synth *synth)
@@ -143,7 +137,7 @@ static void synth_version(struct spk_synth *synth)
 	unsigned char test = 0;
 	char synth_id[40] = "";
 
-	spk_synth_immediate(synth, "\x05[Q]");
+	synth->synth_immediate(synth, "\x05[Q]");
 	synth_id[test] = spk_serial_in();
 	if (synth_id[test] == 'A') {
 		do {
diff --git a/drivers/staging/speakup/speakup_bns.c b/drivers/staging/speakup/speakup_bns.c
index da158c968e7c..a972a5147c6b 100644
--- a/drivers/staging/speakup/speakup_bns.c
+++ b/drivers/staging/speakup/speakup_bns.c
@@ -96,9 +96,10 @@ static struct spk_synth synth_bns = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index 6b74a97385da..c564bf8e1531 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -127,9 +127,10 @@ static struct spk_synth synth_decext = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -175,6 +176,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -185,7 +187,7 @@ static void do_catch_up(struct spk_synth *synth)
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		if (ch == '\n')
 			ch = 0x0D;
-		if (synth_full() || !spk_serial_out(ch)) {
+		if (synth_full() || !synth->io_ops->synth_out(synth, ch)) {
 			schedule_timeout(msecs_to_jiffies(delay_time_val));
 			continue;
 		}
@@ -193,22 +195,22 @@ static void do_catch_up(struct spk_synth *synth)
 		spin_lock_irqsave(&speakup_info.spinlock, flags);
 		synth_buffer_getc();
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		if (ch == '[')
+		if (ch == '[') {
 			in_escape = 1;
-		else if (ch == ']')
+		} else if (ch == ']') {
 			in_escape = 0;
-		else if (ch <= SPACE) {
+		} else if (ch <= SPACE) {
 			if (!in_escape && strchr(",.!?;:", last))
-				spk_serial_out(PROCSPEECH);
+				synth->io_ops->synth_out(synth, PROCSPEECH);
 			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
-					spk_serial_out(PROCSPEECH);
+					synth->io_ops->synth_out(synth, PROCSPEECH);
 				spin_lock_irqsave(&speakup_info.spinlock,
-							flags);
+						  flags);
 				jiffy_delta_val = jiffy_delta->u.n.value;
 				delay_time_val = delay_time->u.n.value;
 				spin_unlock_irqrestore(&speakup_info.spinlock,
-							flags);
+						       flags);
 				schedule_timeout(msecs_to_jiffies
 						 (delay_time_val));
 				jiff_max = jiffies + jiffy_delta_val;
@@ -217,13 +219,13 @@ static void do_catch_up(struct spk_synth *synth)
 		last = ch;
 	}
 	if (!in_escape)
-		spk_serial_out(PROCSPEECH);
+		synth->io_ops->synth_out(synth, PROCSPEECH);
 }
 
 static void synth_flush(struct spk_synth *synth)
 {
 	in_escape = 0;
-	spk_synth_immediate(synth, "\033P;10z\033\\");
+	synth->synth_immediate(synth, "\033P;10z\033\\");
 }
 
 module_param_named(ser, synth_decext.ser, int, 0444);
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index 6bf38e49a96d..5d22c3b7edd4 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -33,15 +33,15 @@
 #include "spk_priv.h"
 #include "speakup.h"
 
-#define	MODULE_init		0x0dec		/* module in boot code */
-#define	MODULE_self_test	0x8800		/* module in self-test */
-#define	MODULE_reset		0xffff		/* reinit the whole module */
+#define	MODULE_init		0x0dec	/* module in boot code */
+#define	MODULE_self_test	0x8800	/* module in self-test */
+#define	MODULE_reset		0xffff	/* reinit the whole module */
 
-#define	MODE_mask		0xf000		/* mode bits in high nibble */
+#define	MODE_mask		0xf000	/* mode bits in high nibble */
 #define	MODE_null		0x0000
-#define	MODE_test		0x2000		/* in testing mode */
+#define	MODE_test		0x2000	/* in testing mode */
 #define	MODE_status		0x8000
-#define	STAT_int		0x0001		/* running in interrupt mode */
+#define	STAT_int		0x0001	/* running in interrupt mode */
 #define	STAT_tr_char		0x0002	/* character data to transmit */
 #define	STAT_rr_char		0x0004	/* ready to receive char data */
 #define	STAT_cmd_ready		0x0008	/* ready to accept commands */
@@ -61,33 +61,31 @@
 #define	CMD_mask		0xf000	/* mask for command nibble */
 #define	CMD_null		0x0000	/* post status */
 #define	CMD_control		0x1000	/* hard control command */
-#define	CTRL_mask		0x0F00	/*   mask off control nibble */
-#define	CTRL_data		0x00FF	/*   mask to get data byte */
-#define	CTRL_null		0x0000	/*   null control */
-#define	CTRL_vol_up		0x0100	/*   increase volume */
-#define	CTRL_vol_down		0x0200	/*   decrease volume */
-#define	CTRL_vol_set		0x0300	/*   set volume */
-#define	CTRL_pause		0x0400	/*   pause spc */
-#define	CTRL_resume		0x0500	/*   resume spc clock */
-#define	CTRL_resume_spc		0x0001	/*   resume spc soft pause */
-#define	CTRL_flush		0x0600	/*   flush all buffers */
-#define	CTRL_int_enable	0x0700	/*   enable status change ints */
-#define	CTRL_buff_free		0x0800	/*   buffer remain count */
-#define	CTRL_buff_used		0x0900	/*   buffer in use */
-#define	CTRL_speech		0x0a00	/*   immediate speech change */
-#define	   CTRL_SP_voice	0x0001	/*       voice change */
-#define	   CTRL_SP_rate		0x0002	/*       rate change */
-#define	   CTRL_SP_comma	0x0003	/*       comma pause change */
-#define	   CTRL_SP_period	0x0004	/*       period pause change */
-#define	   CTRL_SP_rate_delta	0x0005	/*       delta rate change */
-#define	   CTRL_SP_get_param	0x0006	/*       return the desired parameter */
-#define	CTRL_last_index		0x0b00	/*   get last index spoken */
-#define	CTRL_io_priority	0x0c00	/*   change i/o priority */
-#define	CTRL_free_mem		0x0d00	/*   get free paragraphs on module */
-#define	CTRL_get_lang		0x0e00	/* return bit mask of loaded
-					         * languages
-					         */
-#define	CMD_test			0x2000		/* self-test request */
+#define	CTRL_mask		0x0F00	/* mask off control nibble */
+#define	CTRL_data		0x00FF	/* mask to get data byte */
+#define	CTRL_null		0x0000	/* null control */
+#define	CTRL_vol_up		0x0100	/* increase volume */
+#define	CTRL_vol_down		0x0200	/* decrease volume */
+#define	CTRL_vol_set		0x0300	/* set volume */
+#define	CTRL_pause		0x0400	/* pause spc */
+#define	CTRL_resume		0x0500	/* resume spc clock */
+#define	CTRL_resume_spc		0x0001	/* resume spc soft pause */
+#define	CTRL_flush		0x0600	/* flush all buffers */
+#define	CTRL_int_enable		0x0700	/* enable status change ints */
+#define	CTRL_buff_free		0x0800	/* buffer remain count */
+#define	CTRL_buff_used		0x0900	/* buffer in use */
+#define	CTRL_speech		0x0a00	/* immediate speech change */
+#define	CTRL_SP_voice		0x0001	/* voice change */
+#define	CTRL_SP_rate		0x0002	/* rate change */
+#define	CTRL_SP_comma		0x0003	/* comma pause change */
+#define	CTRL_SP_period		0x0004	/* period pause change */
+#define	CTRL_SP_rate_delta	0x0005	/* delta rate change */
+#define	CTRL_SP_get_param	0x0006	/* return the desired parameter */
+#define	CTRL_last_index		0x0b00	/* get last index spoken */
+#define	CTRL_io_priority	0x0c00	/* change i/o priority */
+#define	CTRL_free_mem		0x0d00	/* get free paragraphs on module */
+#define	CTRL_get_lang		0x0e00	/* return bit mask of loaded languages */
+#define	CMD_test		0x2000	/* self-test request */
 #define	TEST_mask		0x0F00	/* isolate test field */
 #define	TEST_null		0x0000	/* no test requested */
 #define	TEST_isa_int		0x0100	/* assert isa irq */
@@ -101,19 +99,19 @@
 #define	ID_null			0x0000	/* null id */
 #define	ID_kernel		0x0100	/* kernel code executing */
 #define	ID_boot			0x0200	/* boot code executing */
-#define	CMD_dma			0x4000		/* force a dma start */
-#define	CMD_reset		0x5000		/* reset module status */
-#define	CMD_sync		0x6000		/* kernel sync command */
-#define	CMD_char_in		0x7000		/* single character send */
-#define	CMD_char_out		0x8000		/* single character get */
-#define	CHAR_count_1		0x0100	/*    one char in cmd_low */
-#define	CHAR_count_2		0x0200	/*	the second in data_low */
-#define	CHAR_count_3		0x0300	/*	the third in data_high */
-#define	CMD_spc_mode		0x9000		/* change spc mode */
-#define	CMD_spc_to_text		0x0100	/*   set to text mode */
-#define	CMD_spc_to_digit	0x0200	/*   set to digital mode */
-#define	CMD_spc_rate		0x0400	/*   change spc data rate */
-#define	CMD_error		0xf000		/* severe error */
+#define	CMD_dma			0x4000	/* force a dma start */
+#define	CMD_reset		0x5000	/* reset module status */
+#define	CMD_sync		0x6000	/* kernel sync command */
+#define	CMD_char_in		0x7000	/* single character send */
+#define	CMD_char_out		0x8000	/* single character get */
+#define	CHAR_count_1		0x0100	/* one char in cmd_low */
+#define	CHAR_count_2		0x0200	/* the second in data_low */
+#define	CHAR_count_3		0x0300	/* the third in data_high */
+#define	CMD_spc_mode		0x9000	/* change spc mode */
+#define	CMD_spc_to_text		0x0100	/* set to text mode */
+#define	CMD_spc_to_digit	0x0200	/* set to digital mode */
+#define	CMD_spc_rate		0x0400	/* change spc data rate */
+#define	CMD_error		0xf000	/* severe error */
 
 enum {	PRIMARY_DIC	= 0, USER_DIC, COMMAND_DIC, ABBREV_DIC };
 
@@ -220,6 +218,7 @@ static struct spk_synth synth_dec_pc = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = dtpc_release,
 	.synth_immediate = synth_immediate,
@@ -251,7 +250,7 @@ static int dt_getstatus(void)
 static void dt_sendcmd(u_int cmd)
 {
 	outb_p(cmd & 0xFF, speakup_info.port_tts);
-	outb_p((cmd >> 8) & 0xFF, speakup_info.port_tts+1);
+	outb_p((cmd >> 8) & 0xFF, speakup_info.port_tts + 1);
 }
 
 static int dt_waitbit(int bit)
@@ -287,11 +286,11 @@ static int dt_ctrl(u_int cmd)
 
 	if (!dt_waitbit(STAT_cmd_ready))
 		return -1;
-	outb_p(0, speakup_info.port_tts+2);
-	outb_p(0, speakup_info.port_tts+3);
+	outb_p(0, speakup_info.port_tts + 2);
+	outb_p(0, speakup_info.port_tts + 3);
 	dt_getstatus();
-	dt_sendcmd(CMD_control|cmd);
-	outb_p(0, speakup_info.port_tts+6);
+	dt_sendcmd(CMD_control | cmd);
+	outb_p(0, speakup_info.port_tts + 6);
 	while (dt_getstatus() & STAT_cmd_ready) {
 		udelay(20);
 		if (--timeout == 0)
@@ -319,8 +318,8 @@ udelay(50);
 			break;
 udelay(50);
 	}
-	outb_p(DMA_sync, speakup_info.port_tts+4);
-	outb_p(0, speakup_info.port_tts+4);
+	outb_p(DMA_sync, speakup_info.port_tts + 4);
+	outb_p(0, speakup_info.port_tts + 4);
 	udelay(100);
 	for (timeout = 0; timeout < 10; timeout++) {
 		if (!(dt_getstatus() & STAT_flushing))
@@ -338,8 +337,8 @@ static int dt_sendchar(char ch)
 		return -1;
 	if (!(dt_stat & STAT_rr_char))
 		return -2;
-	outb_p(DMA_single_in, speakup_info.port_tts+4);
-	outb_p(ch, speakup_info.port_tts+4);
+	outb_p(DMA_single_in, speakup_info.port_tts + 4);
+	outb_p(ch, speakup_info.port_tts + 4);
 	dma_state ^= STAT_dma_state;
 	return 0;
 }
@@ -355,7 +354,7 @@ static int testkernel(void)
 	dt_sendcmd(CMD_sync);
 	if (!dt_waitbit(STAT_cmd_ready))
 		status = -2;
-	else if (dt_stat&0x8000)
+	else if (dt_stat & 0x8000)
 		return 0;
 	else if (dt_stat == 0x0dec)
 		pr_warn("dec_pc at 0x%x, software not loaded\n",
@@ -392,6 +391,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -410,11 +410,11 @@ static void do_catch_up(struct spk_synth *synth)
 		spin_lock_irqsave(&speakup_info.spinlock, flags);
 		synth_buffer_getc();
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		if (ch == '[')
+		if (ch == '[') {
 			in_escape = 1;
-		else if (ch == ']')
+		} else if (ch == ']') {
 			in_escape = 0;
-		else if (ch <= SPACE) {
+		} else if (ch <= SPACE) {
 			if (!in_escape && strchr(",.!?;:", last))
 				dt_sendchar(PROCSPEECH);
 			if (time_after_eq(jiffies, jiff_max)) {
@@ -481,6 +481,7 @@ static int synth_probe(struct spk_synth *synth)
 
 static void dtpc_release(void)
 {
+	spk_stop_serial_interrupt();
 	if (speakup_info.port_tts)
 		synth_release_region(speakup_info.port_tts, SYNTH_IO_EXTENT);
 	speakup_info.port_tts = 0;
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index 26036050cdb2..0cdbd5e9b36b 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -130,9 +130,10 @@ static struct spk_synth synth_dectlk = {
 	.vars = vars,
 	.default_pitch = ap_defaults,
 	.default_vol = g5_defaults,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -239,6 +240,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -250,7 +252,7 @@ static void do_catch_up(struct spk_synth *synth)
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		if (ch == '\n')
 			ch = 0x0D;
-		if (synth_full_val || !spk_serial_out(ch)) {
+		if (synth_full_val || !synth->io_ops->synth_out(synth, ch)) {
 			schedule_timeout(msecs_to_jiffies(delay_time_val));
 			continue;
 		}
@@ -258,16 +260,16 @@ static void do_catch_up(struct spk_synth *synth)
 		spin_lock_irqsave(&speakup_info.spinlock, flags);
 		synth_buffer_getc();
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		if (ch == '[')
+		if (ch == '[') {
 			in_escape = 1;
-		else if (ch == ']')
+		} else if (ch == ']') {
 			in_escape = 0;
-		else if (ch <= SPACE) {
+		} else if (ch <= SPACE) {
 			if (!in_escape && strchr(",.!?;:", last))
-				spk_serial_out(PROCSPEECH);
+				synth->io_ops->synth_out(synth, PROCSPEECH);
 			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
-					spk_serial_out(PROCSPEECH);
+					synth->io_ops->synth_out(synth, PROCSPEECH);
 				spin_lock_irqsave(&speakup_info.spinlock,
 						flags);
 				jiffy_delta_val = jiffy_delta->u.n.value;
@@ -282,17 +284,17 @@ static void do_catch_up(struct spk_synth *synth)
 		last = ch;
 	}
 	if (!in_escape)
-		spk_serial_out(PROCSPEECH);
+		synth->io_ops->synth_out(synth, PROCSPEECH);
 }
 
 static void synth_flush(struct spk_synth *synth)
 {
 	if (in_escape)
 		/* if in command output ']' so we don't get an error */
-		spk_serial_out(']');
+		synth->io_ops->synth_out(synth, ']');
 	in_escape = 0;
 	is_flushing = 1;
-	spk_serial_out(SYNTH_CLEAR);
+	synth->io_ops->synth_out(synth, SYNTH_CLEAR);
 }
 
 module_param_named(ser, synth_dectlk.ser, int, 0444);
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index e2bf20806d8d..5973acc0a006 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -43,6 +43,7 @@ static int port_forced;
 static unsigned int synth_portlist[] = {
 		 0x25e, 0x29e, 0x2de, 0x31e, 0x35e, 0x39e, 0
 };
+
 static u_char synth_status;
 
 static struct var_t vars[] = {
@@ -128,6 +129,7 @@ static struct spk_synth synth_dtlk = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = dtlk_release,
 	.synth_immediate = synth_immediate,
@@ -209,6 +211,7 @@ static void do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -297,7 +300,7 @@ static struct synth_settings *synth_interrogate(struct spk_synth *synth)
 	t += 2;
 	for (i = 0; *t != '\r'; t++) {
 		status.rom_version[i] = *t;
-		if (i < sizeof(status.rom_version)-1)
+		if (i < sizeof(status.rom_version) - 1)
 			i++;
 	}
 	status.rom_version[i] = 0;
@@ -373,6 +376,7 @@ static int synth_probe(struct spk_synth *synth)
 
 static void dtlk_release(void)
 {
+	spk_stop_serial_interrupt();
 	if (speakup_info.port_tts)
 		synth_release_region(speakup_info.port_tts-1, SYNTH_IO_EXTENT);
 	speakup_info.port_tts = 0;
diff --git a/drivers/staging/speakup/speakup_dtlk.h b/drivers/staging/speakup/speakup_dtlk.h
index b3b3cfc3db07..46d885fcfb20 100644
--- a/drivers/staging/speakup/speakup_dtlk.h
+++ b/drivers/staging/speakup/speakup_dtlk.h
@@ -24,11 +24,11 @@
 				 * usec later.
 				 */
 #define TTS_ALMOST_FULL	0x08	/* mask for AF bit: When set to 1,
-				         * indicates that less than 300 bytes
-				         * are available in the TTS input
-				         * buffer. AF is always 0 in the PCM,
-				         * TGN and CVSD modes.
-				         */
+					 * indicates that less than 300 bytes
+					 * are available in the TTS input
+					 * buffer. AF is always 0 in the PCM,
+					 * TGN and CVSD modes.
+					 */
 #define TTS_ALMOST_EMPTY 0x04	/* mask for AE bit: When set to 1,
 				 * indicates that less than 300 bytes
 				 * are remaining in DoubleTalk's input
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index cb7cef30c124..8db7aa358f31 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -98,9 +98,10 @@ static struct spk_synth synth_dummy = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index 10f4964782e2..ba7901178e0b 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -105,6 +105,7 @@ static struct spk_synth synth_keypc = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = keynote_release,
 	.synth_immediate = synth_immediate,
@@ -141,9 +142,9 @@ static char *oops(void)
 	int s1, s2, s3, s4;
 
 	s1 = inb_p(synth_port);
-	s2 = inb_p(synth_port+1);
-	s3 = inb_p(synth_port+2);
-	s4 = inb_p(synth_port+3);
+	s2 = inb_p(synth_port + 1);
+	s3 = inb_p(synth_port + 2);
+	s4 = inb_p(synth_port + 3);
 	pr_warn("synth timeout %d %d %d %d\n", s1, s2, s3, s4);
 	return NULL;
 }
@@ -198,6 +199,7 @@ spin_lock_irqsave(&speakup_info.spinlock, flags);
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -304,6 +306,7 @@ static int synth_probe(struct spk_synth *synth)
 
 static void keynote_release(void)
 {
+	spk_stop_serial_interrupt();
 	if (synth_port)
 		synth_release_region(synth_port, SYNTH_IO_EXTENT);
 	synth_port = 0;
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index 9d22198a0339..11275f49bea4 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -111,9 +111,10 @@ static struct spk_synth synth_ltlk = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -138,13 +139,13 @@ static void synth_interrogate(struct spk_synth *synth)
 	unsigned char *t, i;
 	unsigned char buf[50], rom_v[20];
 
-	spk_synth_immediate(synth, "\x18\x01?");
+	synth->synth_immediate(synth, "\x18\x01?");
 	for (i = 0; i < 50; i++) {
 		buf[i] = spk_serial_in();
 		if (i > 2 && buf[i] == 0x7f)
 			break;
 	}
-	t = buf+2;
+	t = buf + 2;
 	for (i = 0; *t != '\r'; t++) {
 		rom_v[i] = *t;
 		if (++i >= 19)
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index d2ff0afd685a..e454f5685f70 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -29,6 +29,7 @@
 
 #define DRV_VERSION "2.6"
 #define SOFTSYNTH_MINOR 26 /* might as well give it one more than /dev/synth */
+#define SOFTSYNTHU_MINOR 27 /* might as well give it one more than /dev/synth */
 #define PROCSPEECH 0x0d
 #define CLEAR_SYNTH 0x18
 
@@ -37,7 +38,7 @@ static void softsynth_release(void);
 static int softsynth_is_alive(struct spk_synth *synth);
 static unsigned char get_index(void);
 
-static struct miscdevice synth_device;
+static struct miscdevice synth_device, synthu_device;
 static int init_pos;
 static int misc_registered;
 
@@ -130,6 +131,7 @@ static struct spk_synth synth_soft = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = NULL,
 	.probe = softsynth_probe,
 	.release = softsynth_release,
 	.synth_immediate = NULL,
@@ -199,13 +201,13 @@ static int softsynth_close(struct inode *inode, struct file *fp)
 	return 0;
 }
 
-static ssize_t softsynth_read(struct file *fp, char __user *buf, size_t count,
-			      loff_t *pos)
+static ssize_t softsynthx_read(struct file *fp, char __user *buf, size_t count,
+			       loff_t *pos, int unicode)
 {
 	int chars_sent = 0;
 	char __user *cp;
 	char *init;
-	char ch;
+	u16 ch;
 	int empty;
 	unsigned long flags;
 	DEFINE_WAIT(wait);
@@ -213,6 +215,8 @@ static ssize_t softsynth_read(struct file *fp, char __user *buf, size_t count,
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	while (1) {
 		prepare_to_wait(&speakup_event, &wait, TASK_INTERRUPTIBLE);
+		if (!unicode)
+			synth_buffer_skip_nonlatin1();
 		if (!synth_buffer_empty() || speakup_info.flushing)
 			break;
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
@@ -231,23 +235,57 @@ static ssize_t softsynth_read(struct file *fp, char __user *buf, size_t count,
 
 	cp = buf;
 	init = get_initstring();
-	while (chars_sent < count) {
+
+	/* Keep 3 bytes available for a 16bit UTF-8-encoded character */
+	while (chars_sent <= count - 3) {
 		if (speakup_info.flushing) {
 			speakup_info.flushing = 0;
 			ch = '\x18';
-		} else if (synth_buffer_empty()) {
-			break;
 		} else if (init[init_pos]) {
 			ch = init[init_pos++];
 		} else {
+			if (!unicode)
+				synth_buffer_skip_nonlatin1();
+			if (synth_buffer_empty())
+				break;
 			ch = synth_buffer_getc();
 		}
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-		if (copy_to_user(cp, &ch, 1))
-			return -EFAULT;
+
+		if ((!unicode && ch < 0x100) || (unicode && ch < 0x80)) {
+			u_char c = ch;
+
+			if (copy_to_user(cp, &c, 1))
+				return -EFAULT;
+
+			chars_sent++;
+			cp++;
+		} else if (unicode && ch < 0x800) {
+			u_char s[2] = {
+				0xc0 | (ch >> 6),
+				0x80 | (ch & 0x3f)
+			};
+
+			if (copy_to_user(cp, s, sizeof(s)))
+				return -EFAULT;
+
+			chars_sent += sizeof(s);
+			cp += sizeof(s);
+		} else if (unicode) {
+			u_char s[3] = {
+				0xe0 | (ch >> 12),
+				0x80 | ((ch >> 6) & 0x3f),
+				0x80 | (ch & 0x3f)
+			};
+
+			if (copy_to_user(cp, s, sizeof(s)))
+				return -EFAULT;
+
+			chars_sent += sizeof(s);
+			cp += sizeof(s);
+		}
+
 		spin_lock_irqsave(&speakup_info.spinlock, flags);
-		chars_sent++;
-		cp++;
 	}
 	*pos += chars_sent;
 	empty = synth_buffer_empty();
@@ -259,6 +297,18 @@ static ssize_t softsynth_read(struct file *fp, char __user *buf, size_t count,
 	return chars_sent;
 }
 
+static ssize_t softsynth_read(struct file *fp, char __user *buf, size_t count,
+			      loff_t *pos)
+{
+	return softsynthx_read(fp, buf, count, pos, 0);
+}
+
+static ssize_t softsynthu_read(struct file *fp, char __user *buf, size_t count,
+			       loff_t *pos)
+{
+	return softsynthx_read(fp, buf, count, pos, 1);
+}
+
 static int last_index;
 
 static ssize_t softsynth_write(struct file *fp, const char __user *buf,
@@ -308,6 +358,15 @@ static const struct file_operations softsynth_fops = {
 	.release = softsynth_close,
 };
 
+static const struct file_operations softsynthu_fops = {
+	.owner = THIS_MODULE,
+	.poll = softsynth_poll,
+	.read = softsynthu_read,
+	.write = softsynth_write,
+	.open = softsynth_open,
+	.release = softsynth_close,
+};
+
 static int softsynth_probe(struct spk_synth *synth)
 {
 	if (misc_registered != 0)
@@ -321,16 +380,28 @@ static int softsynth_probe(struct spk_synth *synth)
 		return -ENODEV;
 	}
 
+	memset(&synthu_device, 0, sizeof(synthu_device));
+	synthu_device.minor = SOFTSYNTHU_MINOR;
+	synthu_device.name = "softsynthu";
+	synthu_device.fops = &softsynthu_fops;
+	if (misc_register(&synthu_device)) {
+		pr_warn("Couldn't initialize miscdevice /dev/softsynth.\n");
+		return -ENODEV;
+	}
+
 	misc_registered = 1;
 	pr_info("initialized device: /dev/softsynth, node (MAJOR 10, MINOR 26)\n");
+	pr_info("initialized device: /dev/softsynthu, node (MAJOR 10, MINOR 27)\n");
 	return 0;
 }
 
 static void softsynth_release(void)
 {
 	misc_deregister(&synth_device);
+	misc_deregister(&synthu_device);
 	misc_registered = 0;
 	pr_info("unregistered /dev/softsynth\n");
+	pr_info("unregistered /dev/softsynthu\n");
 }
 
 static int softsynth_is_alive(struct spk_synth *synth)
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index 143fadabfe6c..d95c375a0736 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -102,9 +102,10 @@ static struct spk_synth synth_spkout = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
@@ -125,14 +126,7 @@ static struct spk_synth synth_spkout = {
 
 static void synth_flush(struct spk_synth *synth)
 {
-	int timeout = SPK_XMITR_TIMEOUT;
-
-	while (spk_serial_tx_busy()) {
-		if (!--timeout)
-			break;
-		udelay(1);
-	}
-	outb(SYNTH_CLEAR, speakup_info.port_tts);
+	synth->io_ops->send_xchar(SYNTH_CLEAR);
 }
 
 module_param_named(ser, synth_spkout.ser, int, 0444);
diff --git a/drivers/staging/speakup/speakup_txprt.c b/drivers/staging/speakup/speakup_txprt.c
index aa2f338d15d8..3f531fb99fd3 100644
--- a/drivers/staging/speakup/speakup_txprt.c
+++ b/drivers/staging/speakup/speakup_txprt.c
@@ -95,9 +95,10 @@ static struct spk_synth synth_txprt = {
 	.startup = SYNTH_START,
 	.checkval = SYNTH_CHECK,
 	.vars = vars,
+	.io_ops = &spk_serial_io_ops,
 	.probe = spk_serial_synth_probe,
 	.release = spk_serial_release,
-	.synth_immediate = spk_synth_immediate,
+	.synth_immediate = spk_serial_synth_immediate,
 	.catch_up = spk_do_catch_up,
 	.flush = spk_synth_flush,
 	.is_alive = spk_synth_is_alive_restart,
diff --git a/drivers/staging/speakup/spk_priv.h b/drivers/staging/speakup/spk_priv.h
index d5aa41d82122..995f586bddcd 100644
--- a/drivers/staging/speakup/spk_priv.h
+++ b/drivers/staging/speakup/spk_priv.h
@@ -42,14 +42,14 @@
 
 const struct old_serial_port *spk_serial_init(int index);
 void spk_stop_serial_interrupt(void);
-int spk_wait_for_xmitr(void);
+int spk_wait_for_xmitr(struct spk_synth *in_synth);
 unsigned char spk_serial_in(void);
 unsigned char spk_serial_in_nowait(void);
-int spk_serial_out(const char ch);
 void spk_serial_release(void);
 
-char synth_buffer_getc(void);
-char synth_buffer_peek(void);
+void synth_buffer_skip_nonlatin1(void);
+u16 synth_buffer_getc(void);
+u16 synth_buffer_peek(void);
 int synth_buffer_empty(void);
 struct var_t *spk_get_var(enum var_id_t var_id);
 ssize_t spk_var_show(struct kobject *kobj, struct kobj_attribute *attr,
@@ -58,12 +58,17 @@ ssize_t spk_var_store(struct kobject *kobj, struct kobj_attribute *attr,
 		      const char *buf, size_t count);
 
 int spk_serial_synth_probe(struct spk_synth *synth);
-const char *spk_synth_immediate(struct spk_synth *synth, const char *buff);
+const char *spk_serial_synth_immediate(struct spk_synth *synth, const char *buff);
 void spk_do_catch_up(struct spk_synth *synth);
 void spk_synth_flush(struct spk_synth *synth);
 int spk_synth_is_alive_nop(struct spk_synth *synth);
 int spk_synth_is_alive_restart(struct spk_synth *synth);
+__printf(1, 2)
 void synth_printf(const char *buf, ...);
+void synth_putwc(u16 wc);
+void synth_putwc_s(u16 wc);
+void synth_putws(const u16 *buf);
+void synth_putws_s(const u16 *buf);
 int synth_request_region(unsigned long start, unsigned long n);
 int synth_release_region(unsigned long start, unsigned long n);
 int synth_add(struct spk_synth *in_synth);
@@ -73,4 +78,6 @@ extern struct speakup_info_t speakup_info;
 
 extern struct var_t synth_time_vars[];
 
+extern struct spk_io_ops spk_serial_io_ops;
+
 #endif
diff --git a/drivers/staging/speakup/spk_types.h b/drivers/staging/speakup/spk_types.h
index b07f6cc4f284..c156975392c8 100644
--- a/drivers/staging/speakup/spk_types.h
+++ b/drivers/staging/speakup/spk_types.h
@@ -55,7 +55,7 @@ struct spk_highlight_color_track {
 	/* Count of each background color */
 	unsigned int bgcount[8];
 	/* Buffer for characters drawn with each background color */
-	char highbuf[8][COLOR_BUFFER_SIZE];
+	u16 highbuf[8][COLOR_BUFFER_SIZE];
 	/* Current index into highbuf */
 	unsigned int highsize[8];
 	/* Reading Position for each color */
@@ -146,6 +146,14 @@ struct synth_indexing {
 	unsigned char currindex;
 };
 
+struct spk_synth;
+
+struct spk_io_ops {
+	int (*synth_out)(struct spk_synth *synth, const char ch);
+	void (*send_xchar)(char ch);
+	void (*tiocmset)(unsigned int set, unsigned int clear);
+};
+
 struct spk_synth {
 	const char *name;
 	const char *version;
@@ -164,6 +172,7 @@ struct spk_synth {
 	struct var_t *vars;
 	int *default_pitch;
 	int *default_vol;
+	struct spk_io_ops *io_ops;
 	int (*probe)(struct spk_synth *synth);
 	void (*release)(void);
 	const char *(*synth_immediate)(struct spk_synth *synth,
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index a61c02ba06da..352e9eebc3de 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -44,35 +44,6 @@ EXPORT_SYMBOL_GPL(speakup_info);
 
 static int do_synth_init(struct spk_synth *in_synth);
 
-int spk_serial_synth_probe(struct spk_synth *synth)
-{
-	const struct old_serial_port *ser;
-	int failed = 0;
-
-	if ((synth->ser >= SPK_LO_TTY) && (synth->ser <= SPK_HI_TTY)) {
-		ser = spk_serial_init(synth->ser);
-		if (ser == NULL) {
-			failed = -1;
-		} else {
-			outb_p(0, ser->port);
-			mdelay(1);
-			outb_p('\r', ser->port);
-		}
-	} else {
-		failed = -1;
-		pr_warn("ttyS%i is an invalid port\n", synth->ser);
-	}
-	if (failed) {
-		pr_info("%s: not found\n", synth->long_name);
-		return -ENODEV;
-	}
-	pr_info("%s: ttyS%i, Driver Version %s\n",
-		synth->long_name, synth->ser, synth->version);
-	synth->alive = 1;
-	return 0;
-}
-EXPORT_SYMBOL_GPL(spk_serial_synth_probe);
-
 /*
  * Main loop of the progression thread: keep eating from the buffer
  * and push to the serial port, waiting as needed
@@ -109,6 +80,7 @@ void spk_do_catch_up(struct spk_synth *synth)
 			synth->flush(synth);
 			continue;
 		}
+		synth_buffer_skip_nonlatin1();
 		if (synth_buffer_empty()) {
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 			break;
@@ -119,7 +91,7 @@ void spk_do_catch_up(struct spk_synth *synth)
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 		if (ch == '\n')
 			ch = synth->procspeech;
-		if (!spk_serial_out(ch)) {
+		if (!synth->io_ops->synth_out(synth, ch)) {
 			schedule_timeout(msecs_to_jiffies(full_time_val));
 			continue;
 		}
@@ -129,7 +101,7 @@ void spk_do_catch_up(struct spk_synth *synth)
 			delay_time_val = delay_time->u.n.value;
 			full_time_val = full_time->u.n.value;
 			spin_unlock_irqrestore(&speakup_info.spinlock, flags);
-			if (spk_serial_out(synth->procspeech))
+			if (synth->io_ops->synth_out(synth, synth->procspeech))
 				schedule_timeout(
 					msecs_to_jiffies(delay_time_val));
 			else
@@ -142,30 +114,13 @@ void spk_do_catch_up(struct spk_synth *synth)
 		synth_buffer_getc();
 		spin_unlock_irqrestore(&speakup_info.spinlock, flags);
 	}
-	spk_serial_out(synth->procspeech);
+	synth->io_ops->synth_out(synth, synth->procspeech);
 }
 EXPORT_SYMBOL_GPL(spk_do_catch_up);
 
-const char *spk_synth_immediate(struct spk_synth *synth, const char *buff)
-{
-	u_char ch;
-
-	while ((ch = *buff)) {
-		if (ch == '\n')
-			ch = synth->procspeech;
-		if (spk_wait_for_xmitr())
-			outb(ch, speakup_info.port_tts);
-		else
-			return buff;
-		buff++;
-	}
-	return NULL;
-}
-EXPORT_SYMBOL_GPL(spk_synth_immediate);
-
 void spk_synth_flush(struct spk_synth *synth)
 {
-	spk_serial_out(synth->clear);
+	synth->io_ops->synth_out(synth, synth->clear);
 }
 EXPORT_SYMBOL_GPL(spk_synth_flush);
 
@@ -180,7 +135,7 @@ int spk_synth_is_alive_restart(struct spk_synth *synth)
 {
 	if (synth->alive)
 		return 1;
-	if (spk_wait_for_xmitr() > 0) {
+	if (spk_wait_for_xmitr(synth) > 0) {
 		/* restart */
 		synth->alive = 1;
 		synth_printf("%s", synth->init);
@@ -255,6 +210,35 @@ void synth_printf(const char *fmt, ...)
 }
 EXPORT_SYMBOL_GPL(synth_printf);
 
+void synth_putwc(u16 wc)
+{
+	synth_buffer_add(wc);
+}
+EXPORT_SYMBOL_GPL(synth_putwc);
+
+void synth_putwc_s(u16 wc)
+{
+	synth_buffer_add(wc);
+	synth_start();
+}
+EXPORT_SYMBOL_GPL(synth_putwc_s);
+
+void synth_putws(const u16 *buf)
+{
+	const u16 *p;
+
+	for (p = buf; *p; p++)
+		synth_buffer_add(*p);
+}
+EXPORT_SYMBOL_GPL(synth_putws);
+
+void synth_putws_s(const u16 *buf)
+{
+	synth_putws(buf);
+	synth_start();
+}
+EXPORT_SYMBOL_GPL(synth_putws_s);
+
 static int index_count;
 static int sentence_count;
 
@@ -272,7 +256,7 @@ void spk_reset_index_count(int sc)
 
 int synth_supports_indexing(void)
 {
-	if (synth->get_index != NULL)
+	if (synth->get_index)
 		return 1;
 	return 0;
 }
@@ -350,7 +334,7 @@ int synth_init(char *synth_name)
 	int ret = 0;
 	struct spk_synth *synth = NULL;
 
-	if (synth_name == NULL)
+	if (!synth_name)
 		return 0;
 
 	if (strcmp(synth_name, "none") == 0) {
@@ -362,7 +346,7 @@ int synth_init(char *synth_name)
 
 	mutex_lock(&spk_mutex);
 	/* First, check if we already have it loaded. */
-	for (i = 0; i < MAXSYNTHS && synths[i] != NULL; i++)
+	for (i = 0; i < MAXSYNTHS && synths[i]; i++)
 		if (strcmp(synths[i]->name, synth_name) == 0)
 			synth = synths[i];
 
@@ -406,8 +390,8 @@ static int do_synth_init(struct spk_synth *in_synth)
 		speakup_register_var(var);
 	if (!spk_quiet_boot)
 		synth_printf("%s found\n", synth->long_name);
-	if (synth->attributes.name && sysfs_create_group(speakup_kobj,
-							 &synth->attributes) < 0)
+	if (synth->attributes.name &&
+	    sysfs_create_group(speakup_kobj, &synth->attributes) < 0)
 		return -ENOMEM;
 	synth_flags = synth->flags;
 	wake_up_interruptible_all(&speakup_event);
@@ -421,7 +405,7 @@ void synth_release(void)
 	struct var_t *var;
 	unsigned long flags;
 
-	if (synth == NULL)
+	if (!synth)
 		return;
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	pr_info("releasing synth %s\n", synth->name);
@@ -432,7 +416,6 @@ void synth_release(void)
 		sysfs_remove_group(speakup_kobj, &synth->attributes);
 	for (var = synth->vars; var->var_id != MAXVARS; var++)
 		speakup_unregister_var(var->var_id);
-	spk_stop_serial_interrupt();
 	synth->release();
 	synth = NULL;
 }
@@ -444,7 +427,7 @@ int synth_add(struct spk_synth *in_synth)
 	int status = 0;
 
 	mutex_lock(&spk_mutex);
-	for (i = 0; i < MAXSYNTHS && synths[i] != NULL; i++)
+	for (i = 0; i < MAXSYNTHS && synths[i]; i++)
 		/* synth_remove() is responsible for rotating the array down */
 		if (in_synth == synths[i]) {
 			mutex_unlock(&spk_mutex);
@@ -471,11 +454,11 @@ void synth_remove(struct spk_synth *in_synth)
 	mutex_lock(&spk_mutex);
 	if (synth == in_synth)
 		synth_release();
-	for (i = 0; synths[i] != NULL; i++) {
+	for (i = 0; synths[i]; i++) {
 		if (in_synth == synths[i])
 			break;
 	}
-	for ( ; synths[i] != NULL; i++) /* compress table */
+	for ( ; synths[i]; i++) /* compress table */
 		synths[i] = synths[i + 1];
 	module_status = 0;
 	mutex_unlock(&spk_mutex);
diff --git a/drivers/staging/speakup/varhandlers.c b/drivers/staging/speakup/varhandlers.c
index cc984196020f..d37d24e26641 100644
--- a/drivers/staging/speakup/varhandlers.c
+++ b/drivers/staging/speakup/varhandlers.c
@@ -98,7 +98,7 @@ void speakup_register_var(struct var_t *var)
 		}
 	}
 	p_header = var_ptrs[var->var_id];
-	if (p_header->data != NULL)
+	if (p_header->data)
 		return;
 	p_header->data = var;
 	switch (p_header->var_type) {
@@ -210,11 +210,11 @@ int spk_set_num_var(int input, struct st_var_header *var, int how)
 		return -ERANGE;
 
 	var_data->u.n.value = val;
-	if (var->var_type == VAR_TIME && p_val != NULL) {
+	if (var->var_type == VAR_TIME && p_val) {
 		*p_val = msecs_to_jiffies(val);
 		return 0;
 	}
-	if (p_val != NULL)
+	if (p_val)
 		*p_val = val;
 	if (var->var_id == PUNC_LEVEL) {
 		spk_punc_mask = spk_punc_masks[val];
@@ -258,10 +258,11 @@ int spk_set_string_var(const char *page, struct st_var_header *var, int len)
 		if (var->p_val != var_data->u.s.default_val)
 			strcpy((char *)var->p_val, var_data->u.s.default_val);
 		return -ERESTART;
-	} else if (var->p_val)
+	} else if (var->p_val) {
 		strcpy((char *)var->p_val, page);
-	else
+	} else {
 		return -E2BIG;
+	}
 	return 0;
 }
 
@@ -281,17 +282,18 @@ int spk_set_mask_bits(const char *input, const int which, const int how)
 			spk_chartab[*cp] &= ~mask;
 	}
 	cp = (u_char *)input;
-	if (!cp)
+	if (!cp) {
 		cp = spk_punc_info[which].value;
-	else {
+	} else {
 		for (; *cp; cp++) {
 			if (*cp < SPACE)
 				break;
 			if (mask < PUNC) {
 				if (!(spk_chartab[*cp] & PUNC))
 					break;
-			} else if (spk_chartab[*cp] & B_NUM)
+			} else if (spk_chartab[*cp] & B_NUM) {
 				break;
+			}
 		}
 		if (*cp)
 			return -EINVAL;
diff --git a/drivers/staging/unisys/include/channel.h b/drivers/staging/unisys/include/channel.h
index 1c95302f7f1b..057421eeb1ae 100644
--- a/drivers/staging/unisys/include/channel.h
+++ b/drivers/staging/unisys/include/channel.h
@@ -20,35 +20,19 @@
 #include <linux/io.h>
 #include <linux/uuid.h>
 
-/*
- * Whenever this file is changed a corresponding change must be made in
- * the Console/ServicePart/visordiag_early/supervisor_channel.h file
- * which is needed for Linux kernel compiles. These two files must be
- * in sync.
- */
-
-/* define the following to prevent include nesting in kernel header
- * files of similar abbreviated content
- */
 #define __SUPERVISOR_CHANNEL_H__
 
-#define SIGNATURE_16(A, B) ((A) | (B << 8))
+#define SIGNATURE_16(A, B) ((A) | ((B) << 8))
 #define SIGNATURE_32(A, B, C, D) \
 	(SIGNATURE_16(A, B) | (SIGNATURE_16(C, D) << 16))
 #define SIGNATURE_64(A, B, C, D, E, F, G, H) \
 	(SIGNATURE_32(A, B, C, D) | ((u64)(SIGNATURE_32(E, F, G, H)) << 32))
 
-#ifndef lengthof
-#define lengthof(TYPE, MEMBER) (sizeof(((TYPE *)0)->MEMBER))
-#endif
-#ifndef COVERQ
-#define COVERQ(v, d)  (((v) + (d) - 1) / (d))
-#endif
 #ifndef COVER
-#define COVER(v, d)   ((d) * COVERQ(v, d))
+#define COVER(v, d) ((d) * DIV_ROUND_UP(v, d))
 #endif
 
-#define ULTRA_CHANNEL_PROTOCOL_SIGNATURE  SIGNATURE_32('E', 'C', 'N', 'L')
+#define ULTRA_CHANNEL_PROTOCOL_SIGNATURE SIGNATURE_32('E', 'C', 'N', 'L')
 
 enum channel_serverstate {
 	CHANNELSRV_UNINITIALIZED = 0,	/* channel is in an undefined state */
@@ -78,7 +62,7 @@ enum channel_clientstate {
 #define SPAR_CHANNEL_SERVER_READY(ch) \
 	(readl(&(ch)->srv_state) == CHANNELSRV_READY)
 
-#define ULTRA_VALID_CHANNELCLI_TRANSITION(o, n)				\
+#define ULTRA_VALID_CHANNELCLI_TRANSITION(o, n) \
 	(((((o) == CHANNELCLI_DETACHED) && ((n) == CHANNELCLI_DISABLED)) || \
 	  (((o) == CHANNELCLI_ATTACHING) && ((n) == CHANNELCLI_DISABLED)) || \
 	  (((o) == CHANNELCLI_ATTACHED) && ((n) == CHANNELCLI_DISABLED)) || \
@@ -100,7 +84,7 @@ enum channel_clientstate {
 /* throttling invalid boot channel statetransition error due to client
  * disabled
  */
-#define ULTRA_CLIERRORBOOT_THROTTLEMSG_DISABLED    0x01
+#define ULTRA_CLIERRORBOOT_THROTTLEMSG_DISABLED 0x01
 
 /* throttling invalid boot channel statetransition error due to client
  * not attached
@@ -108,7 +92,7 @@ enum channel_clientstate {
 #define ULTRA_CLIERRORBOOT_THROTTLEMSG_NOTATTACHED 0x02
 
 /* throttling invalid boot channel statetransition error due to busy channel */
-#define ULTRA_CLIERRORBOOT_THROTTLEMSG_BUSY        0x04
+#define ULTRA_CLIERRORBOOT_THROTTLEMSG_BUSY 0x04
 
 /* Values for ULTRA_CHANNEL_PROTOCOL.Features: This define exists so
  * that windows guest can look at the FeatureFlags in the io channel,
@@ -214,189 +198,65 @@ struct signal_queue_header {
 	u8 filler[12];		/* Pad out to 64 byte cacheline */
 } __packed;
 
-#define spar_signal_init(chan, QHDRFLD, QDATAFLD, QDATATYPE, ver, typ)	\
-	do {								\
-		memset(&chan->QHDRFLD, 0, sizeof(chan->QHDRFLD));	\
-		chan->QHDRFLD.version = ver;				\
-		chan->QHDRFLD.chtype = typ;				\
-		chan->QHDRFLD.size = sizeof(chan->QDATAFLD);		\
-		chan->QHDRFLD.signal_size = sizeof(QDATATYPE);		\
-		chan->QHDRFLD.sig_base_offset = (u64)(chan->QDATAFLD) -	\
-			(u64)(&chan->QHDRFLD);				\
-		chan->QHDRFLD.max_slots =				\
-			sizeof(chan->QDATAFLD) / sizeof(QDATATYPE);	\
-		chan->QHDRFLD.max_signals = chan->QHDRFLD.max_slots - 1;\
-	} while (0)
-
 /* Generic function useful for validating any type of channel when it is
  * received by the client that will be accessing the channel.
  * Note that <logCtx> is only needed for callers in the EFI environment, and
  * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
  */
 static inline int
-spar_check_channel_client(void __iomem *ch,
-			  uuid_le expected_uuid,
-			  char *chname,
-			  u64 expected_min_bytes,
-			  u32 expected_version,
-			  u64 expected_signature)
+spar_check_channel(struct channel_header *ch,
+		   uuid_le expected_uuid,
+		   char *chname,
+		   u64 expected_min_bytes,
+		   u32 expected_version,
+		   u64 expected_signature)
 {
 	if (uuid_le_cmp(expected_uuid, NULL_UUID_LE) != 0) {
-		uuid_le guid;
-
-		memcpy_fromio(&guid,
-			      &((struct channel_header __iomem *)(ch))->chtype,
-			      sizeof(guid));
 		/* caller wants us to verify type GUID */
-		if (uuid_le_cmp(guid, expected_uuid) != 0) {
+		if (uuid_le_cmp(ch->chtype, expected_uuid) != 0) {
 			pr_err("Channel mismatch on channel=%s(%pUL) field=type expected=%pUL actual=%pUL\n",
 			       chname, &expected_uuid,
-			       &expected_uuid, &guid);
+			       &expected_uuid, &ch->chtype);
 			return 0;
 		}
 	}
 	if (expected_min_bytes > 0) {	/* verify channel size */
-		unsigned long long bytes =
-				readq(&((struct channel_header __iomem *)
-					(ch))->size);
-		if (bytes < expected_min_bytes) {
+		if (ch->size < expected_min_bytes) {
 			pr_err("Channel mismatch on channel=%s(%pUL) field=size expected=0x%-8.8Lx actual=0x%-8.8Lx\n",
 			       chname, &expected_uuid,
-			       (unsigned long long)expected_min_bytes, bytes);
+			       (unsigned long long)expected_min_bytes,
+			       ch->size);
 			return 0;
 		}
 	}
 	if (expected_version > 0) {	/* verify channel version */
-		unsigned long ver = readl(&((struct channel_header __iomem *)
-				    (ch))->version_id);
-		if (ver != expected_version) {
-			pr_err("Channel mismatch on channel=%s(%pUL) field=version expected=0x%-8.8lx actual=0x%-8.8lx\n",
+		if (ch->version_id != expected_version) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=version expected=0x%-8.8lx actual=0x%-8.8x\n",
 			       chname, &expected_uuid,
-			       (unsigned long)expected_version, ver);
+			       (unsigned long)expected_version,
+			       ch->version_id);
 			return 0;
 		}
 	}
 	if (expected_signature > 0) {	/* verify channel signature */
-		unsigned long long sig =
-				readq(&((struct channel_header __iomem *)
-					(ch))->signature);
-		if (sig != expected_signature) {
-			pr_err("Channel mismatch on channel=%s(%pUL) field=signature expected=0x%-8.8llx actual=0x%-8.8llx\n",
+		if (ch->signature != expected_signature) {
+			pr_err("Channel mismatch on channel=%s(%pUL) field=signature expected=0x%-8.8Lx actual=0x%-8.8Lx\n",
 			       chname, &expected_uuid,
-			       expected_signature, sig);
+			       expected_signature, ch->signature);
 			return 0;
 		}
 	}
 	return 1;
 }
 
-/* Generic function useful for validating any type of channel when it is about
- * to be initialized by the server of the channel.
- * Note that <logCtx> is only needed for callers in the EFI environment, and
- * is used to pass the EFI_DIAG_CAPTURE_PROTOCOL needed to log messages.
- */
-static inline int spar_check_channel_server(uuid_le typeuuid, char *name,
-					    u64 expected_min_bytes,
-					    u64 actual_bytes)
-{
-	if (expected_min_bytes > 0)	/* verify channel size */
-		if (actual_bytes < expected_min_bytes) {
-			pr_err("Channel mismatch on channel=%s(%pUL) field=size expected=0x%-8.8llx actual=0x%-8.8llx\n",
-			       name, &typeuuid, expected_min_bytes,
-			       actual_bytes);
-			return 0;
-		}
-	return 1;
-}
-
-/*
- * Routine Description:
- * Tries to insert the prebuilt signal pointed to by pSignal into the nth
- * Queue of the Channel pointed to by pChannel
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to the signal
- *
- * Assumptions:
- * - pChannel, Queue and pSignal are valid.
- * - If insertion fails due to a full queue, the caller will determine the
- * retry policy (e.g. wait & try again, report an error, etc.).
- *
- * Return value: 1 if the insertion succeeds, 0 if the queue was
- * full.
- */
-
-unsigned char spar_signal_insert(struct channel_header __iomem *ch, u32 queue,
-				 void *sig);
-
-/*
- * Routine Description:
- * Removes one signal from Channel pChannel's nth Queue at the
- * time of the call and copies it into the memory pointed to by
- * pSignal.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to where the signals are to be copied
- *
- * Assumptions:
- * - pChannel and Queue are valid.
- * - pSignal points to a memory area large enough to hold queue's SignalSize
- *
- * Return value: 1 if the removal succeeds, 0 if the queue was
- * empty.
- */
-
-unsigned char spar_signal_remove(struct channel_header __iomem *ch, u32 queue,
-				 void *sig);
-
-/*
- * Routine Description:
- * Removes all signals present in Channel pChannel's nth Queue at the
- * time of the call and copies them into the memory pointed to by
- * pSignal.  Returns the # of signals copied as the value of the routine.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- * pSignal: (IN) pointer to where the signals are to be copied
- *
- * Assumptions:
- * - pChannel and Queue are valid.
- * - pSignal points to a memory area large enough to hold Queue's MaxSignals
- * # of signals, each of which is Queue's SignalSize.
- *
- * Return value:
- * # of signals copied.
- */
-unsigned int spar_signal_remove_all(struct channel_header *ch, u32 queue,
-				    void *sig);
-
-/*
- * Routine Description:
- * Determine whether a signal queue is empty.
- *
- * Parameters:
- * pChannel: (IN) points to the IO Channel
- * Queue: (IN) nth Queue of the IO Channel
- *
- * Return value:
- * 1 if the signal queue is empty, 0 otherwise.
- */
-unsigned char spar_signalqueue_empty(struct channel_header __iomem *ch,
-				     u32 queue);
-
 /*
  * CHANNEL Guids
  */
 
 /* {414815ed-c58c-11da-95a9-00e08161165f} */
 #define SPAR_VHBA_CHANNEL_PROTOCOL_UUID \
-		UUID_LE(0x414815ed, 0xc58c, 0x11da, \
-				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+	UUID_LE(0x414815ed, 0xc58c, 0x11da, \
+		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
 static const uuid_le spar_vhba_channel_protocol_uuid =
 	SPAR_VHBA_CHANNEL_PROTOCOL_UUID;
 #define SPAR_VHBA_CHANNEL_PROTOCOL_UUID_STR \
@@ -404,8 +264,8 @@ static const uuid_le spar_vhba_channel_protocol_uuid =
 
 /* {8cd5994d-c58e-11da-95a9-00e08161165f} */
 #define SPAR_VNIC_CHANNEL_PROTOCOL_UUID \
-		UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
-				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+	UUID_LE(0x8cd5994d, 0xc58e, 0x11da, \
+		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
 static const uuid_le spar_vnic_channel_protocol_uuid =
 	SPAR_VNIC_CHANNEL_PROTOCOL_UUID;
 #define SPAR_VNIC_CHANNEL_PROTOCOL_UUID_STR \
@@ -413,21 +273,8 @@ static const uuid_le spar_vnic_channel_protocol_uuid =
 
 /* {72120008-4AAB-11DC-8530-444553544200} */
 #define SPAR_SIOVM_UUID \
-		UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
-				0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
+	UUID_LE(0x72120008, 0x4AAB, 0x11DC, \
+		0x85, 0x30, 0x44, 0x45, 0x53, 0x54, 0x42, 0x00)
 static const uuid_le spar_siovm_uuid = SPAR_SIOVM_UUID;
 
-/* {5b52c5ac-e5f5-4d42-8dff-429eaecd221f} */
-#define SPAR_CONTROLDIRECTOR_CHANNEL_PROTOCOL_UUID  \
-		UUID_LE(0x5b52c5ac, 0xe5f5, 0x4d42, \
-				0x8d, 0xff, 0x42, 0x9e, 0xae, 0xcd, 0x22, 0x1f)
-
-static const uuid_le spar_controldirector_channel_protocol_uuid =
-	SPAR_CONTROLDIRECTOR_CHANNEL_PROTOCOL_UUID;
-
-/* {b4e79625-aede-4eAA-9e11-D3eddcd4504c} */
-#define SPAR_DIAG_POOL_CHANNEL_PROTOCOL_UUID				\
-		UUID_LE(0xb4e79625, 0xaede, 0x4eaa, \
-				0x9e, 0x11, 0xd3, 0xed, 0xdc, 0xd4, 0x50, 0x4c)
-
 #endif
diff --git a/drivers/staging/unisys/include/iochannel.h b/drivers/staging/unisys/include/iochannel.h
index 54f490090a59..9bde848a321c 100644
--- a/drivers/staging/unisys/include/iochannel.h
+++ b/drivers/staging/unisys/include/iochannel.h
@@ -50,17 +50,17 @@
 #define ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID 2
 #define ULTRA_VSWITCH_CHANNEL_PROTOCOL_VERSIONID 1
 
-#define SPAR_VHBA_CHANNEL_OK_CLIENT(ch)			\
-	(spar_check_channel_client(ch, spar_vhba_channel_protocol_uuid, \
-				   "vhba", MIN_IO_CHANNEL_SIZE,	\
-				   ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID, \
-				   ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE))
-
-#define SPAR_VNIC_CHANNEL_OK_CLIENT(ch)			\
-	(spar_check_channel_client(ch, spar_vnic_channel_protocol_uuid, \
-				   "vnic", MIN_IO_CHANNEL_SIZE,	\
-				   ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID, \
-				   ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE))
+#define SPAR_VHBA_CHANNEL_OK_CLIENT(ch) \
+	(spar_check_channel(ch, spar_vhba_channel_protocol_uuid, \
+			    "vhba", MIN_IO_CHANNEL_SIZE,	\
+			    ULTRA_VHBA_CHANNEL_PROTOCOL_VERSIONID, \
+			    ULTRA_VHBA_CHANNEL_PROTOCOL_SIGNATURE))
+
+#define SPAR_VNIC_CHANNEL_OK_CLIENT(ch) \
+	(spar_check_channel(ch, spar_vnic_channel_protocol_uuid, \
+			    "vnic", MIN_IO_CHANNEL_SIZE,	\
+			    ULTRA_VNIC_CHANNEL_PROTOCOL_VERSIONID, \
+			    ULTRA_VNIC_CHANNEL_PROTOCOL_SIGNATURE))
 
 /*
  * Everything necessary to handle SCSI & NIC traffic between Guest Partition and
@@ -92,11 +92,11 @@ enum net_types {
 				 */
 	/* visornic -> uisnic */
 	NET_RCV,		/* incoming packet received */
-	/* uisnic -> virtpci */
+	/* uisnic -> visornic */
 	NET_XMIT,		/* for outgoing net packets */
 	/* visornic -> uisnic */
 	NET_XMIT_DONE,		/* outgoing packet xmitted */
-	/* uisnic -> virtpci */
+	/* uisnic -> visornic */
 	NET_RCV_ENBDIS,		/* enable/disable packet reception */
 	/* visornic -> uisnic */
 	NET_RCV_ENBDIS_ACK,	/* acknowledge enable/disable packet */
@@ -200,7 +200,7 @@ struct uiscmdrsp_scsi {
 	int linuxstat;		/* original Linux status used by Linux vdisk */
 	u8 scsistat;		/* the scsi status */
 	u8 addlstat;		/* non-scsi status */
-#define ADDL_SEL_TIMEOUT	4
+#define ADDL_SEL_TIMEOUT 4
 
 	/* The following fields are need to determine the result of command. */
 	 u8 sensebuf[MAX_SENSE_SIZE];	/* sense info in case cmd failed; */
@@ -308,8 +308,8 @@ struct net_pkt_xmt {
 		u8 valid;	/* 1 = struct is valid - else ignore */
 		u8 hrawoffv;	/* 1 = hwrafoff is valid */
 		u8 nhrawoffv;	/* 1 = nhwrafoff is valid */
-		u16 protocol;	/* specifies packet protocol */
-		u32 csum;	/* value used to set skb->csum at IOPart */
+		__be16 protocol;	/* specifies packet protocol */
+		__wsum csum;	/* value used to set skb->csum at IOPart */
 		u32 hrawoff;	/* value used to set skb->h.raw at IOPart */
 		/* hrawoff points to the start of the TRANSPORT LAYER HEADER */
 		u32 nhrawoff;	/* value used to set skb->nh.raw at IOPart */
@@ -340,7 +340,7 @@ struct net_pkt_xmtdone {
 #define RCVPOST_BUF_SIZE 4032
 #define MAX_NET_RCV_CHAIN \
 	((VISOR_ETH_MAX_MTU + ETH_HLEN + RCVPOST_BUF_SIZE - 1) \
-	/ RCVPOST_BUF_SIZE)
+	 / RCVPOST_BUF_SIZE)
 
 /*
  * rcv buf size must be large enough to include ethernet data len + ethernet
@@ -441,7 +441,7 @@ struct uiscmdrsp_scsitaskmgmt {
 	/* Result of taskmgmt command - set by IOPart - values are: */
 	char result;
 
-#define TASK_MGMT_FAILED  0
+#define TASK_MGMT_FAILED 0
 } __packed;
 
 /* Used by uissd to send disk add/remove notifications to Guest. */
@@ -496,11 +496,11 @@ struct uiscmdrsp {
 	char cmdtype;
 
 /* Describes what type of information is in the struct */
-#define CMD_SCSI_TYPE		1
-#define CMD_NET_TYPE		2
-#define CMD_SCSITASKMGMT_TYPE	3
-#define CMD_NOTIFYGUEST_TYPE	4
-#define CMD_VDISKMGMT_TYPE	5
+#define CMD_SCSI_TYPE	      1
+#define CMD_NET_TYPE	      2
+#define CMD_SCSITASKMGMT_TYPE 3
+#define CMD_NOTIFYGUEST_TYPE  4
+#define CMD_VDISKMGMT_TYPE    5
 	union {
 		struct uiscmdrsp_scsi scsi;
 		struct uiscmdrsp_net net;
@@ -548,44 +548,7 @@ struct spar_io_channel_protocol {
 #define SIZEOF_CMDRSP (COVER(sizeof(struct uiscmdrsp), 64))
 
 /* Use 4K page sizes when passing page info between Guest and IOPartition. */
-#define PI_PAGE_SIZE  0x1000
-#define PI_PAGE_MASK  0x0FFF
-
-/* Returns next non-zero index on success or 0 on failure (i.e. out of room). */
-static inline u16
-add_physinfo_entries(u64 inp_pfn, u16 inp_off, u32 inp_len, u16 index,
-		     u16 max_pi_arr_entries, struct phys_info pi_arr[])
-{
-	u32 len;
-	u16 i, firstlen;
-
-	firstlen = PI_PAGE_SIZE - inp_off;
-	if (inp_len <= firstlen) {
-		/* The input entry spans only one page - add as is. */
-		if (index >= max_pi_arr_entries)
-			return 0;
-		pi_arr[index].pi_pfn = inp_pfn;
-		pi_arr[index].pi_off = (u16)inp_off;
-		pi_arr[index].pi_len = (u16)inp_len;
-		return index + 1;
-	}
-
-	/* This entry spans multiple pages. */
-	for (len = inp_len, i = 0; len;
-		len -= pi_arr[index + i].pi_len, i++) {
-		if (index + i >= max_pi_arr_entries)
-			return 0;
-		pi_arr[index + i].pi_pfn = inp_pfn + i;
-		if (i == 0) {
-			pi_arr[index].pi_off = inp_off;
-			pi_arr[index].pi_len = firstlen;
-		} else {
-			pi_arr[index + i].pi_off = 0;
-			pi_arr[index + i].pi_len =
-			    (u16)MINNUM(len, (u32)PI_PAGE_SIZE);
-		}
-	}
-	return index + i;
-}
+#define PI_PAGE_SIZE 0x1000
+#define PI_PAGE_MASK 0x0FFF
 
 #endif /* __IOCHANNEL_H__ */
diff --git a/drivers/staging/unisys/include/visorbus.h b/drivers/staging/unisys/include/visorbus.h
index 03d56f818a86..de0635542fbd 100644
--- a/drivers/staging/unisys/include/visorbus.h
+++ b/drivers/staging/unisys/include/visorbus.h
@@ -172,15 +172,15 @@ struct visor_device {
 
 #define to_visor_device(x) container_of(x, struct visor_device, device)
 
-int visorbus_register_visor_driver(struct visor_driver *);
-void visorbus_unregister_visor_driver(struct visor_driver *);
+int visorbus_register_visor_driver(struct visor_driver *drv);
+void visorbus_unregister_visor_driver(struct visor_driver *drv);
 int visorbus_read_channel(struct visor_device *dev,
 			  unsigned long offset, void *dest,
 			  unsigned long nbytes);
 int visorbus_write_channel(struct visor_device *dev,
 			   unsigned long offset, void *src,
 			   unsigned long nbytes);
-void visorbus_enable_channel_interrupts(struct visor_device *dev);
+int visorbus_enable_channel_interrupts(struct visor_device *dev);
 void visorbus_disable_channel_interrupts(struct visor_device *dev);
 
 /* Levels of severity for diagnostic events, in order from lowest severity to
@@ -209,7 +209,7 @@ int visorchannel_signalinsert(struct visorchannel *channel, u32 queue,
 bool visorchannel_signalempty(struct visorchannel *channel, u32 queue);
 uuid_le visorchannel_get_uuid(struct visorchannel *channel);
 
-#define BUS_ROOT_DEVICE		UINT_MAX
+#define BUS_ROOT_DEVICE UINT_MAX
 struct visor_device *visorbus_get_device_by_id(u32 bus_no, u32 dev_no,
 					       struct visor_device *from);
 #endif
diff --git a/drivers/staging/unisys/visorbus/controlvmchannel.h b/drivers/staging/unisys/visorbus/controlvmchannel.h
index 859345243afe..274f72422166 100644
--- a/drivers/staging/unisys/visorbus/controlvmchannel.h
+++ b/drivers/staging/unisys/visorbus/controlvmchannel.h
@@ -19,9 +19,9 @@
 #include "channel.h"
 
 /* {2B3C2D10-7EF5-4ad8-B966-3448B7386B3D} */
-#define SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID	\
-		UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
-			0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
+#define SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID \
+	UUID_LE(0x2b3c2d10, 0x7ef5, 0x4ad8, \
+		0xb9, 0x66, 0x34, 0x48, 0xb7, 0x38, 0x6b, 0x3d)
 
 #define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE \
 	ULTRA_CHANNEL_PROTOCOL_SIGNATURE
@@ -33,24 +33,24 @@
  * software.  Note that you can usually add fields to the END of the
  * channel struct withOUT needing to increment this.
  */
-#define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID  1
+#define ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID 1
 
-#define SPAR_CONTROLVM_CHANNEL_OK_CLIENT(ch)           \
-	spar_check_channel_client(ch, \
-		SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID, \
-		"controlvm", \
-		sizeof(struct spar_controlvm_channel_protocol), \
-		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID, \
-		ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE)
+#define SPAR_CONTROLVM_CHANNEL_OK_CLIENT(ch) \
+	(spar_check_channel(ch, \
+			    SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID, \
+			    "controlvm", \
+			    sizeof(struct spar_controlvm_channel_protocol), \
+			    ULTRA_CONTROLVM_CHANNEL_PROTOCOL_VERSIONID, \
+			    ULTRA_CONTROLVM_CHANNEL_PROTOCOL_SIGNATURE))
 
 /* Defines for various channel queues */
-#define CONTROLVM_QUEUE_REQUEST		0
-#define CONTROLVM_QUEUE_RESPONSE	1
-#define CONTROLVM_QUEUE_EVENT		2
-#define CONTROLVM_QUEUE_ACK		3
+#define CONTROLVM_QUEUE_REQUEST	 0
+#define CONTROLVM_QUEUE_RESPONSE 1
+#define CONTROLVM_QUEUE_EVENT	 2
+#define CONTROLVM_QUEUE_ACK	 3
 
 /* Max num of messages stored during IOVM creation to be reused after crash */
-#define CONTROLVM_CRASHMSG_MAX		2
+#define CONTROLVM_CRASHMSG_MAX 2
 
 struct spar_segment_state  {
 	/* Bit 0: May enter other states */
@@ -69,10 +69,12 @@ struct spar_segment_state  {
 	u16 ready:1;
 	/* Bit 7: resource is configured and operating */
 	u16 operating:1;
+	/* Natural alignment*/
+	u16 reserved:8;
 /* Note: don't use high bit unless we need to switch to ushort
  * which is non-compliant
  */
-};
+} __packed;
 
 static const struct spar_segment_state segment_state_running = {
 	1, 1, 1, 0, 1, 1, 1, 1
@@ -145,15 +147,7 @@ struct irq_info {
      */
 	u8 recv_irq_shared;
 	u8 reserved[3];	/* Natural alignment purposes */
-};
-
-struct pci_id {
-	u16 domain;
-	u8 bus;
-	u8 slot;
-	u8 func;
-	u8 reserved[3];	/* Natural alignment purposes */
-};
+} __packed;
 
 struct efi_spar_indication  {
 	u64 boot_to_fw_ui:1;		/* Bit 0: Stop in uefi ui */
@@ -161,7 +155,8 @@ struct efi_spar_indication  {
 	u64 clear_cmos:1;		/* Bit 2: Clear CMOS */
 	u64 boot_to_tool:1;		/* Bit 3: Run install tool */
 	/* remaining bits are available */
-};
+	u64 reserved:60;		/* Natural alignment */
+} __packed;
 
 enum ultra_chipset_feature {
 	ULTRA_CHIPSET_FEATURE_REPLY = 0x00000001,
@@ -203,7 +198,9 @@ struct controlvm_message_header  {
 		u32 preserve:1;
 		/* =1 the DiagWriter is active in the Diagnostic Partition */
 		u32 writer_in_diag:1;
-	} flags;
+		/* Natural alignment */
+		u32 reserve:25;
+	} __packed flags;
 	/* Natural alignment */
 	u32 reserved;
 	/* Identifies the particular message instance */
@@ -216,7 +213,7 @@ struct controlvm_message_header  {
 	/* Actual number of bytes of payload area to copy between IO/Command */
 	u32 payload_bytes;
 	/* if non-zero, there is a payload to copy. */
-};
+} __packed;
 
 struct controlvm_packet_device_create  {
 	u32 bus_no;		/* bus # (0..n-1) from the msg receiver's end */
@@ -229,24 +226,24 @@ struct controlvm_packet_device_create  {
 	uuid_le data_type_uuid;	/* specifies format of data in channel */
 	uuid_le dev_inst_uuid;	/* instance guid for the device */
 	struct irq_info intr;	/* specifies interrupt information */
-};	/* for CONTROLVM_DEVICE_CREATE */
+} __packed;	/* for CONTROLVM_DEVICE_CREATE */
 
 struct controlvm_packet_device_configure  {
 	/* bus # (0..n-1) from the msg receiver's perspective */
 	u32 bus_no;
 	/* Control uses header SegmentIndex field to access bus number... */
 	u32 dev_no;	      /* bus-relative (0..n-1) device number */
-} ;	/* for CONTROLVM_DEVICE_CONFIGURE */
+} __packed;	/* for CONTROLVM_DEVICE_CONFIGURE */
 
 struct controlvm_message_device_create {
 	struct controlvm_message_header header;
 	struct controlvm_packet_device_create packet;
-};	/* total 128 bytes */
+} __packed;	/* total 128 bytes */
 
 struct controlvm_message_device_configure  {
 	struct controlvm_message_header header;
 	struct controlvm_packet_device_configure packet;
-};	/* total 56 bytes */
+} __packed;	/* total 56 bytes */
 
 /* This is the format for a message in any ControlVm queue. */
 struct controlvm_message_packet  {
@@ -264,12 +261,12 @@ struct controlvm_message_packet  {
 	/* indicates format of data in bus channel*/
 			uuid_le bus_data_type_uuid;
 			uuid_le bus_inst_uuid;	/* instance uuid for the bus */
-		} create_bus;	/* for CONTROLVM_BUS_CREATE */
+		} __packed create_bus;	/* for CONTROLVM_BUS_CREATE */
 		struct  {
 	/* bus # (0..n-1) from the msg receiver's perspective */
 			u32 bus_no;
 			u32 reserved;	/* Natural alignment purposes */
-		} destroy_bus;	/* for CONTROLVM_BUS_DESTROY */
+		} __packed destroy_bus;	/* for CONTROLVM_BUS_DESTROY */
 		struct  {
 	/* bus # (0..n-1) from the receiver's perspective */
 			u32 bus_no;
@@ -283,26 +280,27 @@ struct controlvm_message_packet  {
 				 * notifications.  The corresponding
 				 * sendBusInterruptHandle is kept in CP.
 				 */
-		} configure_bus;	/* for CONTROLVM_BUS_CONFIGURE */
+		} __packed configure_bus;      /* for CONTROLVM_BUS_CONFIGURE */
 		/* for CONTROLVM_DEVICE_CREATE */
 		struct controlvm_packet_device_create create_device;
 		struct  {
 		/* bus # (0..n-1) from the msg receiver's perspective */
 			u32 bus_no;
 			u32 dev_no;	/* bus-relative (0..n-1) device # */
-		} destroy_device;	/* for CONTROLVM_DEVICE_DESTROY */
+		} __packed destroy_device;    /* for CONTROLVM_DEVICE_DESTROY */
 		/* for CONTROLVM_DEVICE_CONFIGURE */
 		struct controlvm_packet_device_configure configure_device;
 		struct  {
 		/* bus # (0..n-1) from the msg receiver's perspective */
 			u32 bus_no;
 			u32 dev_no;	/* bus-relative (0..n-1) device # */
-		} reconfigure_device;	/* for CONTROLVM_DEVICE_RECONFIGURE */
+		} __packed reconfigure_device;
+			/* for CONTROLVM_DEVICE_RECONFIGURE */
 		struct  {
 			u32 bus_no;
 			struct spar_segment_state state;
 			u8 reserved[2];	/* Natural alignment purposes */
-		} bus_change_state;	/* for CONTROLVM_BUS_CHANGESTATE */
+		} __packed bus_change_state; /* for CONTROLVM_BUS_CHANGESTATE */
 		struct  {
 			u32 bus_no;
 			u32 dev_no;
@@ -310,15 +308,18 @@ struct controlvm_message_packet  {
 			struct  {
 				/* =1 if message is for a physical device */
 				u32 phys_device:1;
-			} flags;
+				u32 reserved:31;	/* Natural alignment */
+				u32 reserved1;		/* Natural alignment */
+			} __packed flags;
 			u8 reserved[2];	/* Natural alignment purposes */
-		} device_change_state;	/* for CONTROLVM_DEVICE_CHANGESTATE */
+		} __packed device_change_state;
+			/* for CONTROLVM_DEVICE_CHANGESTATE */
 		struct  {
 			u32 bus_no;
 			u32 dev_no;
 			struct spar_segment_state state;
 			u8 reserved[6];	/* Natural alignment purposes */
-		} device_change_state_event;
+		} __packed device_change_state_event;
 			/* for CONTROLVM_DEVICE_CHANGESTATE_EVENT */
 		struct  {
 			/* indicates the max number of busses */
@@ -327,11 +328,12 @@ struct controlvm_message_packet  {
 			u32 switch_count;
 			enum ultra_chipset_feature features;
 			u32 platform_number;	/* Platform Number */
-		} init_chipset;	/* for CONTROLVM_CHIPSET_INIT */
+		} __packed init_chipset;	/* for CONTROLVM_CHIPSET_INIT */
 		struct  {
 			u32 options;	/* reserved */
 			u32 test;	/* bit 0 set to run embedded selftest */
-		} chipset_selftest;	/* for CONTROLVM_CHIPSET_SELFTEST */
+		} __packed chipset_selftest;
+			/* for CONTROLVM_CHIPSET_SELFTEST */
 		/* a physical address of something, that can be dereferenced
 		 * by the receiver of this ControlVm command
 		 */
@@ -339,13 +341,13 @@ struct controlvm_message_packet  {
 		/* a handle of something (depends on command id) */
 		u64 handle;
 	};
-};
+} __packed;
 
 /* All messages in any ControlVm queue have this layout. */
 struct controlvm_message {
 	struct controlvm_message_header hdr;
 	struct controlvm_message_packet cmd;
-};
+} __packed;
 
 struct spar_controlvm_channel_protocol {
 	struct channel_header header;
@@ -432,7 +434,7 @@ struct spar_controlvm_channel_protocol {
 
 	 /* Message stored during IOVM creation to be reused after crash */
 	 struct controlvm_message saved_crash_msg[CONTROLVM_CRASHMSG_MAX];
-};
+} __packed;
 
 /* The following header will be located at the beginning of PayloadVmOffset for
  * various ControlVm commands. The receiver of a ControlVm command with a
@@ -458,81 +460,81 @@ struct spar_controlvm_parameters_header {
 	uuid_le id;
 	u32 revision;
 	u32 reserved;		/* Natural alignment */
-};
+} __packed;
 
 /* General Errors------------------------------------------------------[0-99] */
-#define CONTROLVM_RESP_SUCCESS                                  0
-#define CONTROLVM_RESP_ALREADY_DONE                             1
-#define CONTROLVM_RESP_IOREMAP_FAILED                           2
-#define CONTROLVM_RESP_KMALLOC_FAILED                           3
-#define CONTROLVM_RESP_ID_UNKNOWN                               4
-#define CONTROLVM_RESP_ID_INVALID_FOR_CLIENT                    5
+#define CONTROLVM_RESP_SUCCESS			   0
+#define CONTROLVM_RESP_ALREADY_DONE		   1
+#define CONTROLVM_RESP_IOREMAP_FAILED		   2
+#define CONTROLVM_RESP_KMALLOC_FAILED		   3
+#define CONTROLVM_RESP_ID_UNKNOWN		   4
+#define CONTROLVM_RESP_ID_INVALID_FOR_CLIENT	   5
 
 /* CONTROLVM_INIT_CHIPSET-------------------------------------------[100-199] */
-#define CONTROLVM_RESP_CLIENT_SWITCHCOUNT_NONZERO               100
-#define CONTROLVM_RESP_EXPECTED_CHIPSET_INIT                    101
+#define CONTROLVM_RESP_CLIENT_SWITCHCOUNT_NONZERO  100
+#define CONTROLVM_RESP_EXPECTED_CHIPSET_INIT	   101
 
 /* Maximum Limit----------------------------------------------------[200-299] */
-#define CONTROLVM_RESP_ERROR_MAX_BUSES		201	/* BUS_CREATE */
-#define CONTROLVM_RESP_ERROR_MAX_DEVICES        202	/* DEVICE_CREATE */
+#define CONTROLVM_RESP_ERROR_MAX_BUSES		   201 /* BUS_CREATE */
+#define CONTROLVM_RESP_ERROR_MAX_DEVICES	   202 /* DEVICE_CREATE */
 /* Payload and Parameter Related------------------------------------[400-499] */
-#define CONTROLVM_RESP_PAYLOAD_INVALID		400	/* SWITCH_ATTACHEXTPORT,
-							 * DEVICE_CONFIGURE
-							 */
-#define CONTROLVM_RESP_INITIATOR_PARAMETER_INVALID 401  /* Multiple */
-#define CONTROLVM_RESP_TARGET_PARAMETER_INVALID    402  /* DEVICE_CONFIGURE */
-#define CONTROLVM_RESP_CLIENT_PARAMETER_INVALID    403  /* DEVICE_CONFIGURE */
+#define CONTROLVM_RESP_PAYLOAD_INVALID		   400 /* SWITCH_ATTACHEXTPORT,
+							* DEVICE_CONFIGURE
+							*/
+#define CONTROLVM_RESP_INITIATOR_PARAMETER_INVALID 401 /* Multiple */
+#define CONTROLVM_RESP_TARGET_PARAMETER_INVALID	   402 /* DEVICE_CONFIGURE */
+#define CONTROLVM_RESP_CLIENT_PARAMETER_INVALID	   403 /* DEVICE_CONFIGURE */
 /* Specified[Packet Structure] Value-------------------------------[500-599] */
-#define CONTROLVM_RESP_BUS_INVALID                 500	/* SWITCH_ATTACHINTPORT,
-							 * BUS_CONFIGURE,
-							 * DEVICE_CREATE,
-							 * DEVICE_CONFIG
-							 * DEVICE_DESTROY
-							 */
-#define CONTROLVM_RESP_DEVICE_INVALID           501 /* SWITCH_ATTACHINTPORT */
-						    /* DEVICE_CREATE,
-						     * DEVICE_CONFIGURE,
-						     * DEVICE_DESTROY
-						     */
-#define CONTROLVM_RESP_CHANNEL_INVALID          502 /* DEVICE_CREATE,
-						     * DEVICE_CONFIGURE
-						     */
+#define CONTROLVM_RESP_BUS_INVALID		   500 /* SWITCH_ATTACHINTPORT,
+							* BUS_CONFIGURE,
+							* DEVICE_CREATE,
+							* DEVICE_CONFIG
+							* DEVICE_DESTROY
+							*/
+#define CONTROLVM_RESP_DEVICE_INVALID		   501 /* SWITCH_ATTACHINTPORT*/
+						       /* DEVICE_CREATE,
+							* DEVICE_CONFIGURE,
+							* DEVICE_DESTROY
+							*/
+#define CONTROLVM_RESP_CHANNEL_INVALID		   502 /* DEVICE_CREATE,
+							* DEVICE_CONFIGURE
+							*/
 /* Partition Driver Callback Interface----------------------[600-699] */
-#define CONTROLVM_RESP_VIRTPCI_DRIVER_FAILURE   604       /* BUS_CREATE,
-							   * BUS_DESTROY,
-							   * DEVICE_CREATE,
-							   * DEVICE_DESTROY
-							   */
+#define CONTROLVM_RESP_VIRTPCI_DRIVER_FAILURE	   604 /* BUS_CREATE,
+							* BUS_DESTROY,
+							* DEVICE_CREATE,
+							* DEVICE_DESTROY
+							*/
 /* Unable to invoke VIRTPCI callback */
-#define CONTROLVM_RESP_VIRTPCI_DRIVER_CALLBACK_ERROR 605  /* BUS_CREATE,
-							   * BUS_DESTROY,
-							   * DEVICE_CREATE,
-							   * DEVICE_DESTROY
-							   */
+#define CONTROLVM_RESP_VIRTPCI_DRIVER_CALLBACK_ERROR   605 /* BUS_CREATE,
+							    * BUS_DESTROY,
+							    * DEVICE_CREATE,
+							    * DEVICE_DESTROY
+							    */
 /* VIRTPCI Callback returned error */
-#define CONTROLVM_RESP_GENERIC_DRIVER_CALLBACK_ERROR 606
-							/* SWITCH_ATTACHEXTPORT,
-							 * SWITCH_DETACHEXTPORT
-							 * DEVICE_CONFIGURE
-							 */
+#define CONTROLVM_RESP_GENERIC_DRIVER_CALLBACK_ERROR   606
+						       /* SWITCH_ATTACHEXTPORT,
+							* SWITCH_DETACHEXTPORT
+							* DEVICE_CONFIGURE
+							*/
 
 /* generic device callback returned error */
 /* Bus Related------------------------------------------------------[700-799] */
-#define CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED 700	/* BUS_DESTROY */
+#define CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED       700 /* BUS_DESTROY */
 /* Channel Related--------------------------------------------------[800-899] */
-#define CONTROLVM_RESP_CHANNEL_TYPE_UNKNOWN 800	        /* GET_CHANNELINFO,
-							 * DEVICE_DESTROY
-							 */
-#define CONTROLVM_RESP_CHANNEL_SIZE_TOO_SMALL 801	/* DEVICE_CREATE */
+#define CONTROLVM_RESP_CHANNEL_TYPE_UNKNOWN	       800 /* GET_CHANNELINFO,
+							    * DEVICE_DESTROY
+							    */
+#define CONTROLVM_RESP_CHANNEL_SIZE_TOO_SMALL	       801 /* DEVICE_CREATE */
 /* Chipset Shutdown Related---------------------------------------[1000-1099] */
-#define CONTROLVM_RESP_CHIPSET_SHUTDOWN_FAILED            1000
-#define CONTROLVM_RESP_CHIPSET_SHUTDOWN_ALREADY_ACTIVE    1001
+#define CONTROLVM_RESP_CHIPSET_SHUTDOWN_FAILED	       1000
+#define CONTROLVM_RESP_CHIPSET_SHUTDOWN_ALREADY_ACTIVE 1001
 
 /* Chipset Stop Related-------------------------------------------[1100-1199] */
-#define CONTROLVM_RESP_CHIPSET_STOP_FAILED_BUS            1100
-#define CONTROLVM_RESP_CHIPSET_STOP_FAILED_SWITCH         1101
+#define CONTROLVM_RESP_CHIPSET_STOP_FAILED_BUS	       1100
+#define CONTROLVM_RESP_CHIPSET_STOP_FAILED_SWITCH      1101
 
 /* Device Related-------------------------------------------------[1400-1499] */
-#define CONTROLVM_RESP_DEVICE_UDEV_TIMEOUT                1400
+#define CONTROLVM_RESP_DEVICE_UDEV_TIMEOUT	       1400
 
 #endif				/* __CONTROLVMCHANNEL_H__ */
diff --git a/drivers/staging/unisys/visorbus/vbuschannel.h b/drivers/staging/unisys/visorbus/vbuschannel.h
index b0df26155d02..f0ef5ecf3d7d 100644
--- a/drivers/staging/unisys/visorbus/vbuschannel.h
+++ b/drivers/staging/unisys/visorbus/vbuschannel.h
@@ -28,8 +28,8 @@
 
 /* {193b331b-c58f-11da-95a9-00e08161165f} */
 #define SPAR_VBUS_CHANNEL_PROTOCOL_UUID \
-		UUID_LE(0x193b331b, 0xc58f, 0x11da, \
-				0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
+	UUID_LE(0x193b331b, 0xc58f, 0x11da, \
+		0x95, 0xa9, 0x0, 0xe0, 0x81, 0x61, 0x16, 0x5f)
 static const uuid_le spar_vbus_channel_protocol_uuid =
 	SPAR_VBUS_CHANNEL_PROTOCOL_UUID;
 
@@ -43,16 +43,6 @@ static const uuid_le spar_vbus_channel_protocol_uuid =
  */
 #define SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID 1
 
-#define SPAR_VBUS_CHANNEL_OK_CLIENT(ch)       \
-	spar_check_channel_client(ch,				\
-				   spar_vbus_channel_protocol_uuid,	\
-				   "vbus",				\
-				   sizeof(struct spar_vbus_channel_protocol),\
-				   SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID, \
-				   SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE)
-
-#pragma pack(push, 1)		/* both GCC and VC now allow this pragma */
-
 /*
  * An array of this struct is present in the channel area for each vbus.
  * (See vbuschannel.h.)
@@ -64,42 +54,7 @@ struct ultra_vbus_deviceinfo {
 	u8 drvname[16];		/* driver .sys file name */
 	u8 infostrs[96];	/* kernel version */
 	u8 reserved[128];	/* pad size to 256 bytes */
-};
-
-/**
- * vbuschannel_print_devinfo() - format a struct ultra_vbus_deviceinfo
- *                               and write it to a seq_file
- * @devinfo: the struct ultra_vbus_deviceinfo to format
- * @seq: seq_file to write to
- * @devix: the device index to be included in the output data, or -1 if no
- *         device index is to be included
- *
- * Reads @devInfo, and writes it in human-readable notation to @seq.
- */
-static inline void
-vbuschannel_print_devinfo(struct ultra_vbus_deviceinfo *devinfo,
-			  struct seq_file *seq, int devix)
-{
-	if (!isprint(devinfo->devtype[0]))
-		return; /* uninitialized vbus device entry */
-
-	if (devix >= 0)
-		seq_printf(seq, "[%d]", devix);
-	else
-		/* vbus device entry is for bus or chipset */
-		seq_puts(seq, "   ");
-
-	/*
-	 * Note: because the s-Par back-end is free to scribble in this area,
-	 * we never assume '\0'-termination.
-	 */
-	seq_printf(seq, "%-*.*s ", (int)sizeof(devinfo->devtype),
-		   (int)sizeof(devinfo->devtype), devinfo->devtype);
-	seq_printf(seq, "%-*.*s ", (int)sizeof(devinfo->drvname),
-		   (int)sizeof(devinfo->drvname), devinfo->drvname);
-	seq_printf(seq, "%.*s\n", (int)sizeof(devinfo->infostrs),
-		   devinfo->infostrs);
-}
+} __packed;
 
 struct spar_vbus_headerinfo {
 	u32 struct_bytes;	/* size of this struct in bytes */
@@ -113,7 +68,7 @@ struct spar_vbus_headerinfo {
 	u32 dev_info_offset;	/* byte offset from beginning of this struct */
 	/* to the DevInfo array (below) */
 	u8 reserved[104];
-};
+} __packed;
 
 struct spar_vbus_channel_protocol {
 	struct channel_header channel_header;	/* initialized by server */
@@ -125,8 +80,6 @@ struct spar_vbus_channel_protocol {
 	/* describes client bus device and driver */
 	struct ultra_vbus_deviceinfo dev_info[0];
 	/* describes client device and driver for each device on the bus */
-};
-
-#pragma pack(pop)
+} __packed;
 
 #endif
diff --git a/drivers/staging/unisys/visorbus/visorbus_main.c b/drivers/staging/unisys/visorbus/visorbus_main.c
index 55f29ae8e015..a692561c81c8 100644
--- a/drivers/staging/unisys/visorbus/visorbus_main.c
+++ b/drivers/staging/unisys/visorbus/visorbus_main.c
@@ -19,21 +19,16 @@
 
 #include "visorbus.h"
 #include "visorbus_private.h"
-#include "vmcallinterface.h"
 
 #define MYDRVNAME "visorbus"
 
-/* module parameters */
-static int visorbus_forcematch;
-static int visorbus_forcenomatch;
-
 /* Display string that is guaranteed to be no longer the 99 characters*/
 #define LINESIZE 99
 
 #define CURRENT_FILE_PC VISOR_BUS_PC_visorbus_main_c
-#define POLLJIFFIES_NORMALCHANNEL     10
+#define POLLJIFFIES_NORMALCHANNEL 10
 
-static int busreg_rc = -ENODEV; /* stores the result from bus registration */
+static bool initialized; /* stores whether bus_registration was successful */
 static struct dentry *visorbus_debugfs_dir;
 
 /*
@@ -87,12 +82,10 @@ visorbus_uevent(struct device *xdev, struct kobj_uevent_env *env)
 	dev = to_visor_device(xdev);
 	guid = visorchannel_get_uuid(dev->visorchannel);
 
-	if (add_uevent_var(env, "MODALIAS=visorbus:%pUl", &guid))
-		return -ENOMEM;
-	return 0;
+	return add_uevent_var(env, "MODALIAS=visorbus:%pUl", &guid);
 }
 
-/**
+/*
  * visorbus_match() - called automatically upon adding a visor_device
  *                    (device_add), or adding a visor_driver
  *                    (visorbus_register_visor_driver)
@@ -113,10 +106,6 @@ visorbus_match(struct device *xdev, struct device_driver *xdrv)
 	drv = to_visor_driver(xdrv);
 	channel_type = visorchannel_get_uuid(dev->visorchannel);
 
-	if (visorbus_forcematch)
-		return 1;
-	if (visorbus_forcenomatch)
-		return 0;
 	if (!drv->channel_types)
 		return 0;
 
@@ -142,10 +131,10 @@ struct bus_type visorbus_type = {
 	.dev_groups = visorbus_dev_groups,
 };
 
-/**
- * visorbus_releae_busdevice() - called when device_unregister() is called for
- *                               the bus device instance, after all other tasks
- *                               involved with destroying the dev are complete
+/*
+ * visorbus_release_busdevice() - called when device_unregister() is called for
+ *                                the bus device instance, after all other tasks
+ *                                involved with destroying the dev are complete
  * @xdev: struct device for the bus being released
  */
 static void
@@ -158,7 +147,7 @@ visorbus_release_busdevice(struct device *xdev)
 	kfree(dev);
 }
 
-/**
+/*
  * visorbus_release_device() - called when device_unregister() is called for
  *                             each child device instance
  * @xdev: struct device for the visor device being released
@@ -185,8 +174,6 @@ static ssize_t physaddr_show(struct device *dev, struct device_attribute *attr,
 {
 	struct visor_device *vdev = to_visor_device(dev);
 
-	if (!vdev->visorchannel)
-		return 0;
 	return sprintf(buf, "0x%llx\n",
 		       visorchannel_get_physaddr(vdev->visorchannel));
 }
@@ -197,8 +184,6 @@ static ssize_t nbytes_show(struct device *dev, struct device_attribute *attr,
 {
 	struct visor_device *vdev = to_visor_device(dev);
 
-	if (!vdev->visorchannel)
-		return 0;
 	return sprintf(buf, "0x%lx\n",
 			visorchannel_get_nbytes(vdev->visorchannel));
 }
@@ -209,8 +194,6 @@ static ssize_t clientpartition_show(struct device *dev,
 {
 	struct visor_device *vdev = to_visor_device(dev);
 
-	if (!vdev->visorchannel)
-		return 0;
 	return sprintf(buf, "0x%llx\n",
 		       visorchannel_get_clientpartition(vdev->visorchannel));
 }
@@ -222,8 +205,6 @@ static ssize_t typeguid_show(struct device *dev, struct device_attribute *attr,
 	struct visor_device *vdev = to_visor_device(dev);
 	char typeid[LINESIZE];
 
-	if (!vdev->visorchannel)
-		return 0;
 	return sprintf(buf, "%s\n",
 		       visorchannel_id(vdev->visorchannel, typeid));
 }
@@ -235,8 +216,6 @@ static ssize_t zoneguid_show(struct device *dev, struct device_attribute *attr,
 	struct visor_device *vdev = to_visor_device(dev);
 	char zoneid[LINESIZE];
 
-	if (!vdev->visorchannel)
-		return 0;
 	return sprintf(buf, "%s\n",
 		       visorchannel_zoneid(vdev->visorchannel, zoneid));
 }
@@ -245,13 +224,12 @@ static DEVICE_ATTR_RO(zoneguid);
 static ssize_t typename_show(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
-	struct visor_device *vdev = to_visor_device(dev);
 	int i = 0;
 	struct bus_type *xbus = dev->bus;
 	struct device_driver *xdrv = dev->driver;
 	struct visor_driver *drv = NULL;
 
-	if (!vdev->visorchannel || !xbus || !xdrv)
+	if (!xbus || !xdrv)
 		return 0;
 	i = xbus->match(dev, xdrv);
 	if (!i)
@@ -344,11 +322,10 @@ static ssize_t channel_id_show(struct device *dev,
 	struct visor_device *vdev = to_visor_device(dev);
 	int len = 0;
 
-	if (vdev->visorchannel) {
-		visorchannel_id(vdev->visorchannel, buf);
-		len = strlen(buf);
-		buf[len++] = '\n';
-	}
+	visorchannel_id(vdev->visorchannel, buf);
+	len = strlen(buf);
+	buf[len++] = '\n';
+
 	return len;
 }
 static DEVICE_ATTR_RO(channel_id);
@@ -378,6 +355,40 @@ static const struct attribute_group *visorbus_groups[] = {
  *  define & implement display of debugfs attributes under
  *  /sys/kernel/debug/visorbus/visorbus<n>.
  */
+/*
+ * vbuschannel_print_devinfo() - format a struct ultra_vbus_deviceinfo
+ *                               and write it to a seq_file
+ * @devinfo: the struct ultra_vbus_deviceinfo to format
+ * @seq: seq_file to write to
+ * @devix: the device index to be included in the output data, or -1 if no
+ *         device index is to be included
+ *
+ * Reads @devInfo, and writes it in human-readable notation to @seq.
+ */
+static void
+vbuschannel_print_devinfo(struct ultra_vbus_deviceinfo *devinfo,
+			  struct seq_file *seq, int devix)
+{
+	if (!isprint(devinfo->devtype[0]))
+		return; /* uninitialized vbus device entry */
+
+	if (devix >= 0)
+		seq_printf(seq, "[%d]", devix);
+	else
+		/* vbus device entry is for bus or chipset */
+		seq_puts(seq, "   ");
+
+	/*
+	 * Note: because the s-Par back-end is free to scribble in this area,
+	 * we never assume '\0'-termination.
+	 */
+	seq_printf(seq, "%-*.*s ", (int)sizeof(devinfo->devtype),
+		   (int)sizeof(devinfo->devtype), devinfo->devtype);
+	seq_printf(seq, "%-*.*s ", (int)sizeof(devinfo->drvname),
+		   (int)sizeof(devinfo->drvname), devinfo->drvname);
+	seq_printf(seq, "%.*s\n", (int)sizeof(devinfo->infostrs),
+		   devinfo->infostrs);
+}
 
 static int client_bus_info_debugfs_show(struct seq_file *seq, void *v)
 {
@@ -442,16 +453,17 @@ dev_periodic_work(unsigned long __opaque)
 	mod_timer(&dev->timer, jiffies + POLLJIFFIES_NORMALCHANNEL);
 }
 
-static void
+static int
 dev_start_periodic_work(struct visor_device *dev)
 {
 	if (dev->being_removed || dev->timer_active)
-		return;
+		return -EINVAL;
 	/* now up by at least 2 */
 	get_device(&dev->device);
 	dev->timer.expires = jiffies + POLLJIFFIES_NORMALCHANNEL;
 	add_timer(&dev->timer);
 	dev->timer_active = true;
+	return 0;
 }
 
 static void
@@ -464,7 +476,7 @@ dev_stop_periodic_work(struct visor_device *dev)
 	put_device(&dev->device);
 }
 
-/**
+/*
  * visordriver_remove_device() - handle visor device going away
  * @xdev: struct device for the visor device being removed
  *
@@ -557,17 +569,17 @@ EXPORT_SYMBOL_GPL(visorbus_write_channel);
  * Currently we don't yet have a real interrupt, so for now we just call the
  * interrupt function periodically via a timer.
  */
-void
+int
 visorbus_enable_channel_interrupts(struct visor_device *dev)
 {
 	struct visor_driver *drv = to_visor_driver(dev->device.driver);
 
 	if (!drv->channel_interrupt) {
 		dev_err(&dev->device, "%s no interrupt function!\n", __func__);
-		return;
+		return -ENOENT;
 	}
 
-	dev_start_periodic_work(dev);
+	return dev_start_periodic_work(dev);
 }
 EXPORT_SYMBOL_GPL(visorbus_enable_channel_interrupts);
 
@@ -583,7 +595,7 @@ visorbus_disable_channel_interrupts(struct visor_device *dev)
 }
 EXPORT_SYMBOL_GPL(visorbus_disable_channel_interrupts);
 
-/**
+/*
  * create_visor_device() - create visor device as a result of receiving the
  *                         controlvm device_create message for a new device
  * @dev: a freshly-zeroed struct visor_device, containing only filled-in values
@@ -613,9 +625,6 @@ create_visor_device(struct visor_device *dev)
 	u32 chipset_bus_no = dev->chipset_bus_no;
 	u32 chipset_dev_no = dev->chipset_dev_no;
 
-	POSTCODE_LINUX(DEVICE_CREATE_ENTRY_PC, chipset_dev_no, chipset_bus_no,
-		       DIAG_SEVERITY_PRINT);
-
 	mutex_init(&dev->visordriver_callback_lock);
 	dev->device.bus = &visorbus_type;
 	dev->device.groups = visorbus_channel_groups;
@@ -630,8 +639,10 @@ create_visor_device(struct visor_device *dev)
 	 * (NOT bus instance).  That's why we need to include the bus
 	 * number within the name.
 	 */
-	dev_set_name(&dev->device, "vbus%u:dev%u",
-		     chipset_bus_no, chipset_dev_no);
+	err = dev_set_name(&dev->device, "vbus%u:dev%u",
+			   chipset_bus_no, chipset_dev_no);
+	if (err)
+		goto err_put;
 
 	/*
 	 * device_add does this:
@@ -651,17 +662,15 @@ create_visor_device(struct visor_device *dev)
 	 *  bus_type.klist_devices regardless (use bus_for_each_dev).
 	 */
 	err = device_add(&dev->device);
-	if (err < 0) {
-		POSTCODE_LINUX(DEVICE_ADD_PC, 0, chipset_bus_no,
-			       DIAG_SEVERITY_ERR);
+	if (err < 0)
 		goto err_put;
-	}
 
 	list_add_tail(&dev->list_all, &list_all_device_instances);
 	return 0; /* success: reference kept via unmatched get_device() */
 
 err_put:
 	put_device(&dev->device);
+	dev_err(&dev->device, "Creating visor device failed. %d\n", err);
 	return err;
 }
 
@@ -677,24 +686,32 @@ static int
 get_vbus_header_info(struct visorchannel *chan,
 		     struct spar_vbus_headerinfo *hdr_info)
 {
-	if (!SPAR_VBUS_CHANNEL_OK_CLIENT(visorchannel_get_header(chan)))
+	int err;
+
+	if (!spar_check_channel(visorchannel_get_header(chan),
+				spar_vbus_channel_protocol_uuid,
+				"vbus",
+				sizeof(struct spar_vbus_channel_protocol),
+				SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID,
+				SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE))
 		return -EINVAL;
 
-	if (visorchannel_read(chan, sizeof(struct channel_header), hdr_info,
-			      sizeof(*hdr_info)) < 0) {
-		return -EIO;
-	}
+	err = visorchannel_read(chan, sizeof(struct channel_header), hdr_info,
+				sizeof(*hdr_info));
+	if (err < 0)
+		return err;
+
 	if (hdr_info->struct_bytes < sizeof(struct spar_vbus_headerinfo))
 		return -EINVAL;
 
 	if (hdr_info->device_info_struct_bytes <
-	    sizeof(struct ultra_vbus_deviceinfo)) {
+	    sizeof(struct ultra_vbus_deviceinfo))
 		return -EINVAL;
-	}
+
 	return 0;
 }
 
-/**
+/*
  * write_vbus_chp_info() - write the contents of <info> to the struct
  *                         spar_vbus_channel_protocol.chp_info
  * @chan:     indentifies the s-Par channel that will be updated
@@ -720,7 +737,7 @@ write_vbus_chp_info(struct visorchannel *chan,
 	visorchannel_write(chan, off, info, sizeof(*info));
 }
 
-/**
+/*
  * write_vbus_bus_info() - write the contents of <info> to the struct
  *                         spar_vbus_channel_protocol.bus_info
  * @chan:     indentifies the s-Par channel that will be updated
@@ -746,7 +763,7 @@ write_vbus_bus_info(struct visorchannel *chan,
 	visorchannel_write(chan, off, info, sizeof(*info));
 }
 
-/**
+/*
  * write_vbus_dev_info() - write the contents of <info> to the struct
  *                         spar_vbus_channel_protocol.dev_info[<devix>]
  * @chan:     indentifies the s-Par channel that will be updated
@@ -775,10 +792,26 @@ write_vbus_dev_info(struct visorchannel *chan,
 	visorchannel_write(chan, off, info, sizeof(*info));
 }
 
-/**
+static void bus_device_info_init(
+		struct ultra_vbus_deviceinfo *bus_device_info_ptr,
+		const char *dev_type, const char *drv_name)
+{
+	memset(bus_device_info_ptr, 0, sizeof(struct ultra_vbus_deviceinfo));
+	snprintf(bus_device_info_ptr->devtype,
+		 sizeof(bus_device_info_ptr->devtype),
+		 "%s", (dev_type) ? dev_type : "unknownType");
+	snprintf(bus_device_info_ptr->drvname,
+		 sizeof(bus_device_info_ptr->drvname),
+		 "%s", (drv_name) ? drv_name : "unknownDriver");
+	snprintf(bus_device_info_ptr->infostrs,
+		 sizeof(bus_device_info_ptr->infostrs), "kernel ver. %s",
+		 utsname()->release);
+}
+
+/*
  * fix_vbus_dev_info() - for a child device just created on a client bus, fill
  *                       in information about the driver that is controlling
- *                       this device into the the appropriate slot within the
+ *                       this device into the appropriate slot within the
  *                       vbus channel of the bus instance
  * @visordev: struct visor_device for the desired device
  */
@@ -823,16 +856,12 @@ fix_vbus_dev_info(struct visor_device *visordev)
 	bus_device_info_init(&dev_info, chan_type_name, visordrv->name);
 	write_vbus_dev_info(bdev->visorchannel, hdr_info, &dev_info, dev_no);
 
-	/*
-	 * Re-write bus+chipset info, because it is possible that this
-	 * was previously written by our evil counterpart, virtpci.
-	 */
 	write_vbus_chp_info(bdev->visorchannel, hdr_info, &chipset_driverinfo);
 	write_vbus_bus_info(bdev->visorchannel, hdr_info,
 			    &clientbus_driverinfo);
 }
 
-/**
+/*
  * visordriver_probe_device() - handle new visor device coming online
  * @xdev: struct device for the visor device being probed
  *
@@ -925,10 +954,8 @@ visordriver_probe_device(struct device *xdev)
  */
 int visorbus_register_visor_driver(struct visor_driver *drv)
 {
-	int rc = 0;
-
-	if (busreg_rc < 0)
-		return -ENODEV; /*can't register on a nonexistent bus*/
+	if (!initialized)
+		return -ENODEV; /* can't register on a nonexistent bus */
 
 	drv->driver.name = drv->name;
 	drv->driver.bus = &visorbus_type;
@@ -949,14 +976,11 @@ int visorbus_register_visor_driver(struct visor_driver *drv)
 	 *                 dev.drv = NULL
 	 */
 
-	rc = driver_register(&drv->driver);
-	if (rc < 0)
-		driver_unregister(&drv->driver);
-	return rc;
+	return driver_register(&drv->driver);
 }
 EXPORT_SYMBOL_GPL(visorbus_register_visor_driver);
 
-/**
+/*
  * create_bus_instance() - create a device instance for the visor bus itself
  * @dev: struct visor_device indicating the bus instance
  *
@@ -970,8 +994,6 @@ create_bus_instance(struct visor_device *dev)
 	int err;
 	struct spar_vbus_headerinfo *hdr_info;
 
-	POSTCODE_LINUX(BUS_CREATE_ENTRY_PC, 0, 0, DIAG_SEVERITY_PRINT);
-
 	hdr_info = kzalloc(sizeof(*hdr_info), GFP_KERNEL);
 	if (!hdr_info)
 		return -ENOMEM;
@@ -983,51 +1005,38 @@ create_bus_instance(struct visor_device *dev)
 
 	dev->debugfs_dir = debugfs_create_dir(dev_name(&dev->device),
 					      visorbus_debugfs_dir);
-	if (!dev->debugfs_dir) {
-		err = -ENOMEM;
-		goto err_hdr_info;
-	}
 	dev->debugfs_client_bus_info =
 		debugfs_create_file("client_bus_info", 0440,
 				    dev->debugfs_dir, dev,
 				    &client_bus_info_debugfs_fops);
-	if (!dev->debugfs_client_bus_info) {
-		err = -ENOMEM;
+
+	dev_set_drvdata(&dev->device, dev);
+	err = get_vbus_header_info(dev->visorchannel, hdr_info);
+	if (err < 0)
 		goto err_debugfs_dir;
-	}
 
-	if (device_register(&dev->device) < 0) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, 0, id,
-			       DIAG_SEVERITY_ERR);
-		err = -ENODEV;
-		goto err_debugfs_created;
-	}
+	err = device_register(&dev->device);
+	if (err < 0)
+		goto err_debugfs_dir;
 
-	if (get_vbus_header_info(dev->visorchannel, hdr_info) >= 0) {
-		dev->vbus_hdr_info = (void *)hdr_info;
-		write_vbus_chp_info(dev->visorchannel, hdr_info,
-				    &chipset_driverinfo);
-		write_vbus_bus_info(dev->visorchannel, hdr_info,
-				    &clientbus_driverinfo);
-	} else {
-		kfree(hdr_info);
-	}
 	list_add_tail(&dev->list_all, &list_all_bus_instances);
-	dev_set_drvdata(&dev->device, dev);
-	return 0;
 
-err_debugfs_created:
-	debugfs_remove(dev->debugfs_client_bus_info);
+	dev->vbus_hdr_info = (void *)hdr_info;
+	write_vbus_chp_info(dev->visorchannel, hdr_info,
+			    &chipset_driverinfo);
+	write_vbus_bus_info(dev->visorchannel, hdr_info,
+			    &clientbus_driverinfo);
+
+	return 0;
 
 err_debugfs_dir:
 	debugfs_remove_recursive(dev->debugfs_dir);
-
-err_hdr_info:
 	kfree(hdr_info);
+	dev_err(&dev->device, "create_bus_instance failed: %d\n", err);
 	return err;
 }
 
-/**
+/*
  * remove_bus_instance() - remove a device instance for the visor bus itself
  * @dev: struct visor_device indentifying the bus to remove
  */
@@ -1051,30 +1060,7 @@ remove_bus_instance(struct visor_device *dev)
 	device_unregister(&dev->device);
 }
 
-/**
- * create_bus_type() - create and register the one-and-only one instance of
- *                     the visor bus type (visorbus_type)
- * Return: 0 for success, otherwise negative errno value returned by
- *         bus_register() indicating the reason for failure
- */
-static int
-create_bus_type(void)
-{
-	busreg_rc = bus_register(&visorbus_type);
-	return busreg_rc;
-}
-
-/**
- * remove_bus_type() - remove the one-and-only one instance of the visor bus
- *                     type (visorbus_type)
- */
-static void
-remove_bus_type(void)
-{
-	bus_unregister(&visorbus_type);
-}
-
-/**
+/*
  * remove_all_visor_devices() - remove all child visor bus device instances
  */
 static void
@@ -1090,24 +1076,19 @@ remove_all_visor_devices(void)
 	}
 }
 
-void
+int
 chipset_bus_create(struct visor_device *dev)
 {
-	int rc;
-	u32 bus_no = dev->chipset_bus_no;
+	int err;
 
-	POSTCODE_LINUX(BUS_CREATE_ENTRY_PC, 0, bus_no, DIAG_SEVERITY_PRINT);
-	rc = create_bus_instance(dev);
-	POSTCODE_LINUX(BUS_CREATE_EXIT_PC, 0, bus_no, DIAG_SEVERITY_PRINT);
+	err = create_bus_instance(dev);
 
-	if (rc < 0)
-		POSTCODE_LINUX(BUS_CREATE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
-	else
-		POSTCODE_LINUX(CHIPSET_INIT_SUCCESS_PC, 0, bus_no,
-			       DIAG_SEVERITY_PRINT);
+	if (err < 0)
+		return err;
+
+	bus_create_response(dev, err);
 
-	bus_create_response(dev, rc);
+	return 0;
 }
 
 void
@@ -1117,25 +1098,18 @@ chipset_bus_destroy(struct visor_device *dev)
 	bus_destroy_response(dev, 0);
 }
 
-void
+int
 chipset_device_create(struct visor_device *dev_info)
 {
-	int rc;
-	u32 bus_no = dev_info->chipset_bus_no;
-	u32 dev_no = dev_info->chipset_dev_no;
+	int err;
 
-	POSTCODE_LINUX(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
-		       DIAG_SEVERITY_PRINT);
+	err = create_visor_device(dev_info);
+	if (err < 0)
+		return err;
 
-	rc = create_visor_device(dev_info);
-	device_create_response(dev_info, rc);
+	device_create_response(dev_info, err);
 
-	if (rc < 0)
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
-	else
-		POSTCODE_LINUX(DEVICE_CREATE_SUCCESS_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_PRINT);
+	return 0;
 }
 
 void
@@ -1146,7 +1120,7 @@ chipset_device_destroy(struct visor_device *dev_info)
 	device_destroy_response(dev_info, 0);
 }
 
-/**
+/*
  * pause_state_change_complete() - the callback function to be called by a
  *                                 visorbus function driver when a
  *                                 pending "pause device" operation has
@@ -1166,7 +1140,7 @@ pause_state_change_complete(struct visor_device *dev, int status)
 	device_pause_response(dev, status);
 }
 
-/**
+/*
  * resume_state_change_complete() - the callback function to be called by a
  *                                  visorbus function driver when a
  *                                  pending "resume device" operation has
@@ -1191,7 +1165,7 @@ resume_state_change_complete(struct visor_device *dev, int status)
 	device_resume_response(dev, status);
 }
 
-/**
+/*
  * initiate_chipset_device_pause_resume() - start a pause or resume operation
  *                                          for a visor device
  * @dev: struct visor_device identifying the device being paused or resumed
@@ -1202,70 +1176,38 @@ resume_state_change_complete(struct visor_device *dev, int status)
  * via a callback function; see pause_state_change_complete() and
  * resume_state_change_complete().
  */
-static void
+static int
 initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
 {
-	int rc;
+	int err;
 	struct visor_driver *drv = NULL;
-	void (*notify_func)(struct visor_device *dev, int response) = NULL;
-
-	if (is_pause)
-		notify_func = device_pause_response;
-	else
-		notify_func = device_resume_response;
-	if (!notify_func)
-		return;
 
 	drv = to_visor_driver(dev->device.driver);
-	if (!drv) {
-		(*notify_func)(dev, -ENODEV);
-		return;
-	}
+	if (!drv)
+		return -ENODEV;
 
-	if (dev->pausing || dev->resuming) {
-		(*notify_func)(dev, -EBUSY);
-		return;
-	}
+	if (dev->pausing || dev->resuming)
+		return -EBUSY;
 
-	/*
-	 * Note that even though both drv->pause() and drv->resume
-	 * specify a callback function, it is NOT necessary for us to
-	 * increment our local module usage count.  Reason is, there
-	 * is already a linkage dependency between child function
-	 * drivers and visorbus, so it is already IMPOSSIBLE to unload
-	 * visorbus while child function drivers are still running.
-	 */
 	if (is_pause) {
-		if (!drv->pause) {
-			(*notify_func)(dev, -EINVAL);
-			return;
-		}
+		if (!drv->pause)
+			return -EINVAL;
 
 		dev->pausing = true;
-		rc = drv->pause(dev, pause_state_change_complete);
+		err = drv->pause(dev, pause_state_change_complete);
 	} else {
-		/* This should be done at BUS resume time, but an
-		 * existing problem prevents us from ever getting a bus
-		 * resume...  This hack would fail to work should we
-		 * ever have a bus that contains NO devices, since we
-		 * would never even get here in that case.
+		/* The vbus_dev_info structure in the channel was been
+		 * cleared, make sure it is valid.
 		 */
 		fix_vbus_dev_info(dev);
-		if (!drv->resume) {
-			(*notify_func)(dev, -EINVAL);
-			return;
-		}
+		if (!drv->resume)
+			return -EINVAL;
 
 		dev->resuming = true;
-		rc = drv->resume(dev, resume_state_change_complete);
-	}
-	if (rc < 0) {
-		if (is_pause)
-			dev->pausing = false;
-		else
-			dev->resuming = false;
-		(*notify_func)(dev, -EINVAL);
+		err = drv->resume(dev, resume_state_change_complete);
 	}
+
+	return err;
 }
 
 /**
@@ -1276,10 +1218,19 @@ initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
  * that device.  Success/failure result is returned asynchronously
  * via a callback function; see pause_state_change_complete().
  */
-void
+int
 chipset_device_pause(struct visor_device *dev_info)
 {
-	initiate_chipset_device_pause_resume(dev_info, true);
+	int err;
+
+	err = initiate_chipset_device_pause_resume(dev_info, true);
+
+	if (err < 0) {
+		dev_info->pausing = false;
+		return err;
+	}
+
+	return 0;
 }
 
 /**
@@ -1290,10 +1241,19 @@ chipset_device_pause(struct visor_device *dev_info)
  * that device.  Success/failure result is returned asynchronously
  * via a callback function; see resume_state_change_complete().
  */
-void
+int
 chipset_device_resume(struct visor_device *dev_info)
 {
-	initiate_chipset_device_pause_resume(dev_info, false);
+	int err;
+
+	err = initiate_chipset_device_pause_resume(dev_info, false);
+
+	if (err < 0) {
+		dev_info->resuming = false;
+		return err;
+	}
+
+	return 0;
 }
 
 int
@@ -1301,27 +1261,21 @@ visorbus_init(void)
 {
 	int err;
 
-	POSTCODE_LINUX(DRIVER_ENTRY_PC, 0, 0, DIAG_SEVERITY_PRINT);
-
 	visorbus_debugfs_dir = debugfs_create_dir("visorbus", NULL);
 	if (!visorbus_debugfs_dir)
 		return -ENOMEM;
 
 	bus_device_info_init(&clientbus_driverinfo, "clientbus", "visorbus");
 
-	err = create_bus_type();
-	if (err < 0) {
-		POSTCODE_LINUX(BUS_CREATE_ENTRY_PC, 0, 0, DIAG_SEVERITY_ERR);
-		goto error;
-	}
+	err = bus_register(&visorbus_type);
+	if (err < 0)
+		return err;
+
+	initialized = true;
 
 	bus_device_info_init(&chipset_driverinfo, "chipset", "visorchipset");
 
 	return 0;
-
-error:
-	POSTCODE_LINUX(CHIPSET_INIT_FAILURE_PC, 0, err, DIAG_SEVERITY_ERR);
-	return err;
 }
 
 void
@@ -1337,14 +1291,8 @@ visorbus_exit(void)
 						      list_all);
 		remove_bus_instance(dev);
 	}
-	remove_bus_type();
+
+	bus_unregister(&visorbus_type);
+	initialized = false;
 	debugfs_remove_recursive(visorbus_debugfs_dir);
 }
-
-module_param_named(forcematch, visorbus_forcematch, int, 0444);
-MODULE_PARM_DESC(visorbus_forcematch,
-		 "1 to force a successful dev <--> drv match");
-
-module_param_named(forcenomatch, visorbus_forcenomatch, int, 0444);
-MODULE_PARM_DESC(visorbus_forcenomatch,
-		 "1 to force an UNsuccessful dev <--> drv match");
diff --git a/drivers/staging/unisys/visorbus/visorbus_private.h b/drivers/staging/unisys/visorbus/visorbus_private.h
index 49bec1763e33..9f030b1f589f 100644
--- a/drivers/staging/unisys/visorbus/visorbus_private.h
+++ b/drivers/staging/unisys/visorbus/visorbus_private.h
@@ -27,28 +27,12 @@
  * command line
  */
 
-static inline void bus_device_info_init(
-		struct ultra_vbus_deviceinfo *bus_device_info_ptr,
-		const char *dev_type, const char *drv_name)
-{
-	memset(bus_device_info_ptr, 0, sizeof(struct ultra_vbus_deviceinfo));
-	snprintf(bus_device_info_ptr->devtype,
-		 sizeof(bus_device_info_ptr->devtype),
-		 "%s", (dev_type) ? dev_type : "unknownType");
-	snprintf(bus_device_info_ptr->drvname,
-		 sizeof(bus_device_info_ptr->drvname),
-		 "%s", (drv_name) ? drv_name : "unknownDriver");
-	snprintf(bus_device_info_ptr->infostrs,
-		 sizeof(bus_device_info_ptr->infostrs), "kernel ver. %s",
-		 utsname()->release);
-}
-
-void chipset_bus_create(struct visor_device *bus_info);
+int chipset_bus_create(struct visor_device *bus_info);
 void chipset_bus_destroy(struct visor_device *bus_info);
-void chipset_device_create(struct visor_device *dev_info);
+int chipset_device_create(struct visor_device *dev_info);
 void chipset_device_destroy(struct visor_device *dev_info);
-void chipset_device_pause(struct visor_device *dev_info);
-void chipset_device_resume(struct visor_device *dev_info);
+int chipset_device_pause(struct visor_device *dev_info);
+int chipset_device_resume(struct visor_device *dev_info);
 
 void bus_create_response(struct visor_device *p, int response);
 void bus_destroy_response(struct visor_device *p, int response);
@@ -81,5 +65,5 @@ u64 visorchannel_get_clientpartition(struct visorchannel *channel);
 int visorchannel_set_clientpartition(struct visorchannel *channel,
 				     u64 partition_handle);
 char *visorchannel_uuid_id(uuid_le *guid, char *s);
-void __iomem *visorchannel_get_header(struct visorchannel *channel);
+void *visorchannel_get_header(struct visorchannel *channel);
 #endif
diff --git a/drivers/staging/unisys/visorbus/visorchannel.c b/drivers/staging/unisys/visorbus/visorchannel.c
index e91febcc6c1e..9e1cea22ce68 100644
--- a/drivers/staging/unisys/visorbus/visorchannel.c
+++ b/drivers/staging/unisys/visorbus/visorchannel.c
@@ -29,8 +29,9 @@
 #define MYDRVNAME "visorchannel"
 
 #define SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID \
-	UUID_LE(0x3cd6e705, 0xd6a2, 0x4aa5,           \
+	UUID_LE(0x3cd6e705, 0xd6a2, 0x4aa5, \
 		0xad, 0x5c, 0x7b, 0x8, 0x88, 0x9d, 0xff, 0xe2)
+
 static const uuid_le spar_video_guid = SPAR_CONSOLEVIDEO_CHANNEL_PROTOCOL_GUID;
 
 struct visorchannel {
@@ -153,10 +154,10 @@ visorchannel_write(struct visorchannel *channel, ulong offset,
 	return 0;
 }
 
-void __iomem  *
+void *
 visorchannel_get_header(struct visorchannel *channel)
 {
-	return (void __iomem *)&channel->chan_hdr;
+	return &channel->chan_hdr;
 }
 
 /*
@@ -173,17 +174,17 @@ visorchannel_get_header(struct visorchannel *channel)
  */
 #define SIG_DATA_OFFSET(chan_hdr, q, sig_hdr, slot) \
 	(SIG_QUEUE_OFFSET(chan_hdr, q) + (sig_hdr)->sig_base_offset + \
-	    ((slot) * (sig_hdr)->signal_size))
+	 ((slot) * (sig_hdr)->signal_size))
 
 /*
  * Write the contents of a specific field within a SIGNAL_QUEUE_HEADER back
  * into host memory
  */
-#define SIG_WRITE_FIELD(channel, queue, sig_hdr, FIELD)			 \
-	visorchannel_write(channel,					 \
-			   SIG_QUEUE_OFFSET(&channel->chan_hdr, queue) +\
+#define SIG_WRITE_FIELD(channel, queue, sig_hdr, FIELD) \
+	visorchannel_write(channel, \
+			   SIG_QUEUE_OFFSET(&channel->chan_hdr, queue) + \
 			   offsetof(struct signal_queue_header, FIELD), \
-			   &((sig_hdr)->FIELD),			 \
+			   &((sig_hdr)->FIELD), \
 			   sizeof((sig_hdr)->FIELD))
 
 static int
@@ -199,7 +200,7 @@ sig_read_header(struct visorchannel *channel, u32 queue,
 				 sig_hdr, sizeof(struct signal_queue_header));
 }
 
-static inline int
+static int
 sig_read_data(struct visorchannel *channel, u32 queue,
 	      struct signal_queue_header *sig_hdr, u32 slot, void *data)
 {
@@ -210,7 +211,7 @@ sig_read_data(struct visorchannel *channel, u32 queue,
 				 data, sig_hdr->signal_size);
 }
 
-static inline int
+static int
 sig_write_data(struct visorchannel *channel, u32 queue,
 	       struct signal_queue_header *sig_hdr, u32 slot, void *data)
 {
@@ -286,16 +287,6 @@ visorchannel_signalremove(struct visorchannel *channel, u32 queue, void *msg)
 }
 EXPORT_SYMBOL_GPL(visorchannel_signalremove);
 
-/**
- * visorchannel_signalempty() - checks if the designated channel/queue
- *                              contains any messages
- * @channel: the channel to query
- * @queue:   the queue in the channel to query
- *
- * Return: boolean indicating whether any messages in the designated
- *         channel/queue are present
- */
-
 static bool
 queue_empty(struct visorchannel *channel, u32 queue)
 {
@@ -307,6 +298,15 @@ queue_empty(struct visorchannel *channel, u32 queue)
 	return (sig_hdr.head == sig_hdr.tail);
 }
 
+/**
+ * visorchannel_signalempty() - checks if the designated channel/queue
+ *                              contains any messages
+ * @channel: the channel to query
+ * @queue:   the queue in the channel to query
+ *
+ * Return: boolean indicating whether any messages in the designated
+ *         channel/queue are present
+ */
 bool
 visorchannel_signalempty(struct visorchannel *channel, u32 queue)
 {
@@ -328,27 +328,24 @@ static int
 signalinsert_inner(struct visorchannel *channel, u32 queue, void *msg)
 {
 	struct signal_queue_header sig_hdr;
-	int error;
+	int err;
 
-	error = sig_read_header(channel, queue, &sig_hdr);
-	if (error)
-		return error;
+	err = sig_read_header(channel, queue, &sig_hdr);
+	if (err)
+		return err;
 
 	sig_hdr.head = (sig_hdr.head + 1) % sig_hdr.max_slots;
 	if (sig_hdr.head == sig_hdr.tail) {
 		sig_hdr.num_overflows++;
-		visorchannel_write(channel,
-				   SIG_QUEUE_OFFSET(&channel->chan_hdr, queue) +
-				   offsetof(struct signal_queue_header,
-					    num_overflows),
-				   &sig_hdr.num_overflows,
-				   sizeof(sig_hdr.num_overflows));
+		err = SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_overflows);
+		if (err)
+			return err;
 		return -EIO;
 	}
 
-	error = sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg);
-	if (error)
-		return error;
+	err = sig_write_data(channel, queue, &sig_hdr, sig_hdr.head, msg);
+	if (err)
+		return err;
 
 	sig_hdr.num_sent++;
 
@@ -358,17 +355,17 @@ signalinsert_inner(struct visorchannel *channel, u32 queue, void *msg)
 	 */
 	mb(); /* required for channel synch */
 
-	error = SIG_WRITE_FIELD(channel, queue, &sig_hdr, head);
-	if (error)
-		return error;
-	error = SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent);
-	if (error)
-		return error;
+	err = SIG_WRITE_FIELD(channel, queue, &sig_hdr, head);
+	if (err)
+		return err;
+	err = SIG_WRITE_FIELD(channel, queue, &sig_hdr, num_sent);
+	if (err)
+		return err;
 
 	return 0;
 }
 
-/**
+/*
  * visorchannel_create_guts() - creates the struct visorchannel abstraction
  *                              for a data area in memory, but does NOT modify
  *                              this data area
@@ -418,12 +415,9 @@ visorchannel_create_guts(u64 physaddr, unsigned long channel_bytes,
 	 * release later on.
 	 */
 	channel->requested = request_mem_region(physaddr, size, MYDRVNAME);
-	if (!channel->requested) {
-		if (uuid_le_cmp(guid, spar_video_guid)) {
-			/* Not the video channel we care about this */
-			goto err_destroy_channel;
-		}
-	}
+	if (!channel->requested && uuid_le_cmp(guid, spar_video_guid))
+		/* we only care about errors if this is not the video channel */
+		goto err_destroy_channel;
 
 	channel->mapped = memremap(physaddr, size, MEMREMAP_WB);
 	if (!channel->mapped) {
@@ -451,12 +445,9 @@ visorchannel_create_guts(u64 physaddr, unsigned long channel_bytes,
 	channel->mapped = NULL;
 	channel->requested = request_mem_region(channel->physaddr,
 						channel_bytes, MYDRVNAME);
-	if (!channel->requested) {
-		if (uuid_le_cmp(guid, spar_video_guid)) {
-			/* Different we care about this */
-			goto err_destroy_channel;
-		}
-	}
+	if (!channel->requested && uuid_le_cmp(guid, spar_video_guid))
+		/* we only care about errors if this is not the video channel */
+		goto err_destroy_channel;
 
 	channel->mapped = memremap(channel->physaddr, channel_bytes,
 			MEMREMAP_WB);
diff --git a/drivers/staging/unisys/visorbus/visorchipset.c b/drivers/staging/unisys/visorbus/visorchipset.c
index 97778d733e1e..4cfd0fae9bd5 100644
--- a/drivers/staging/unisys/visorbus/visorchipset.c
+++ b/drivers/staging/unisys/visorbus/visorchipset.c
@@ -15,13 +15,11 @@
  */
 
 #include <linux/acpi.h>
-#include <linux/cdev.h>
 #include <linux/ctype.h>
 #include <linux/fs.h>
 #include <linux/mm.h>
 #include <linux/nls.h>
 #include <linux/netdevice.h>
-#include <linux/platform_device.h>
 #include <linux/uuid.h>
 #include <linux/crash_dump.h>
 
@@ -31,13 +29,11 @@
 
 #define CURRENT_FILE_PC VISOR_BUS_PC_visorchipset_c
 
-#define POLLJIFFIES_CONTROLVMCHANNEL_FAST   1
+#define POLLJIFFIES_CONTROLVMCHANNEL_FAST 1
 #define POLLJIFFIES_CONTROLVMCHANNEL_SLOW 100
 
 #define MAX_CONTROLVM_PAYLOAD_BYTES (1024 * 128)
 
-#define VISORCHIPSET_MMAP_CONTROLCHANOFFSET	0x00000000
-
 #define UNISYS_SPAR_LEAF_ID 0x40000000
 
 /* The s-Par leaf ID returns "UnisysSpar64" encoded across ebx, ecx, edx */
@@ -46,35 +42,11 @@
 #define UNISYS_SPAR_ID_EDX 0x34367261
 
 /*
- * Module parameters
- */
-static int visorchipset_major;
-
-static int
-visorchipset_open(struct inode *inode, struct file *file)
-{
-	unsigned int minor_number = iminor(inode);
-
-	if (minor_number)
-		return -ENODEV;
-	return 0;
-}
-
-static int
-visorchipset_release(struct inode *inode, struct file *file)
-{
-	return 0;
-}
-
-/*
  * When the controlvm channel is idle for at least MIN_IDLE_SECONDS,
  * we switch to slow polling mode. As soon as we get a controlvm
  * message, we switch back to fast polling mode.
  */
 #define MIN_IDLE_SECONDS 10
-static unsigned long poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-/* when we got our last controlvm message */
-static unsigned long most_recent_message_jiffies;
 
 struct parser_context {
 	unsigned long allocbytes;
@@ -85,22 +57,33 @@ struct parser_context {
 	char data[0];
 };
 
-static struct delayed_work periodic_controlvm_work;
-
-static struct cdev file_cdev;
-static struct visorchannel **file_controlvm_channel;
+struct vmcall_controlvm_addr {
+	struct vmcall_io_controlvm_addr_params params;
+	int err;
+	u64 physaddr;
+};
 
-static struct visorchannel *controlvm_channel;
-static unsigned long controlvm_payload_bytes_buffered;
+struct visorchipset_device {
+	struct acpi_device *acpi_device;
+	unsigned long poll_jiffies;
+	/* when we got our last controlvm message */
+	unsigned long most_recent_message_jiffies;
+	struct delayed_work periodic_controlvm_work;
+	struct visorchannel *controlvm_channel;
+	unsigned long controlvm_payload_bytes_buffered;
+	/*
+	 * The following variables are used to handle the scenario where we are
+	 * unable to offload the payload from a controlvm message due to memory
+	 * requirements. In this scenario, we simply stash the controlvm
+	 * message, then attempt to process it again the next time
+	 * controlvm_periodic_work() runs.
+	 */
+	struct controlvm_message controlvm_pending_msg;
+	bool controlvm_pending_msg_valid;
+	struct vmcall_controlvm_addr controlvm_addr;
+};
 
-/*
- * The following globals are used to handle the scenario where we are unable to
- * offload the payload from a controlvm message due to memory requirements. In
- * this scenario, we simply stash the controlvm message, then attempt to
- * process it again the next time controlvm_periodic_work() runs.
- */
-static struct controlvm_message controlvm_pending_msg;
-static bool controlvm_pending_msg_valid;
+static struct visorchipset_device *chipset_dev;
 
 struct parahotplug_request {
 	struct list_head list;
@@ -109,19 +92,21 @@ struct parahotplug_request {
 	struct controlvm_message msg;
 };
 
-/* info for /dev/visorchipset */
-static dev_t major_dev = -1; /*< indicates major num for device */
-
 /* prototypes for attributes */
 static ssize_t toolaction_show(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
 	u8 tool_action = 0;
+	int err;
+
+	err = visorchannel_read(chipset_dev->controlvm_channel,
+				offsetof(struct spar_controlvm_channel_protocol,
+					 tool_action),
+				&tool_action, sizeof(u8));
+	if (err)
+		return err;
 
-	visorchannel_read(controlvm_channel,
-			  offsetof(struct spar_controlvm_channel_protocol,
-				   tool_action), &tool_action, sizeof(u8));
 	return sprintf(buf, "%u\n", tool_action);
 }
 
@@ -130,19 +115,19 @@ static ssize_t toolaction_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	u8 tool_action;
-	int ret;
+	int err;
 
 	if (kstrtou8(buf, 10, &tool_action))
 		return -EINVAL;
 
-	ret = visorchannel_write
-		(controlvm_channel,
+	err = visorchannel_write
+		(chipset_dev->controlvm_channel,
 		 offsetof(struct spar_controlvm_channel_protocol,
 			  tool_action),
 		 &tool_action, sizeof(u8));
 
-	if (ret)
-		return ret;
+	if (err)
+		return err;
 	return count;
 }
 static DEVICE_ATTR_RW(toolaction);
@@ -152,11 +137,16 @@ static ssize_t boottotool_show(struct device *dev,
 			       char *buf)
 {
 	struct efi_spar_indication efi_spar_indication;
+	int err;
+
+	err = visorchannel_read(chipset_dev->controlvm_channel,
+				offsetof(struct spar_controlvm_channel_protocol,
+					 efi_spar_ind),
+				&efi_spar_indication,
+				sizeof(struct efi_spar_indication));
 
-	visorchannel_read(controlvm_channel,
-			  offsetof(struct spar_controlvm_channel_protocol,
-				   efi_spar_ind), &efi_spar_indication,
-			  sizeof(struct efi_spar_indication));
+	if (err)
+		return err;
 	return sprintf(buf, "%u\n", efi_spar_indication.boot_to_tool);
 }
 
@@ -164,21 +154,21 @@ static ssize_t boottotool_store(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
-	int val, ret;
+	int val, err;
 	struct efi_spar_indication efi_spar_indication;
 
 	if (kstrtoint(buf, 10, &val))
 		return -EINVAL;
 
 	efi_spar_indication.boot_to_tool = val;
-	ret = visorchannel_write
-		(controlvm_channel,
+	err = visorchannel_write
+		(chipset_dev->controlvm_channel,
 		 offsetof(struct spar_controlvm_channel_protocol,
 			  efi_spar_ind), &(efi_spar_indication),
 		 sizeof(struct efi_spar_indication));
 
-	if (ret)
-		return ret;
+	if (err)
+		return err;
 	return count;
 }
 static DEVICE_ATTR_RW(boottotool);
@@ -187,11 +177,14 @@ static ssize_t error_show(struct device *dev, struct device_attribute *attr,
 			  char *buf)
 {
 	u32 error = 0;
+	int err;
 
-	visorchannel_read(controlvm_channel,
-			  offsetof(struct spar_controlvm_channel_protocol,
-				   installation_error),
-			  &error, sizeof(u32));
+	err = visorchannel_read(chipset_dev->controlvm_channel,
+				offsetof(struct spar_controlvm_channel_protocol,
+					 installation_error),
+				&error, sizeof(u32));
+	if (err)
+		return err;
 	return sprintf(buf, "%i\n", error);
 }
 
@@ -199,18 +192,18 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 			   const char *buf, size_t count)
 {
 	u32 error;
-	int ret;
+	int err;
 
 	if (kstrtou32(buf, 10, &error))
 		return -EINVAL;
 
-	ret = visorchannel_write
-		(controlvm_channel,
+	err = visorchannel_write
+		(chipset_dev->controlvm_channel,
 		 offsetof(struct spar_controlvm_channel_protocol,
 			  installation_error),
 		 &error, sizeof(u32));
-	if (ret)
-		return ret;
+	if (err)
+		return err;
 	return count;
 }
 static DEVICE_ATTR_RW(error);
@@ -219,12 +212,16 @@ static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
 			   char *buf)
 {
 	u32 text_id = 0;
+	int err;
+
+	err = visorchannel_read
+			(chipset_dev->controlvm_channel,
+			 offsetof(struct spar_controlvm_channel_protocol,
+				  installation_text_id),
+			 &text_id, sizeof(u32));
+	if (err)
+		return err;
 
-	visorchannel_read
-		(controlvm_channel,
-		 offsetof(struct spar_controlvm_channel_protocol,
-			  installation_text_id),
-		 &text_id, sizeof(u32));
 	return sprintf(buf, "%i\n", text_id);
 }
 
@@ -232,18 +229,18 @@ static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
 			    const char *buf, size_t count)
 {
 	u32 text_id;
-	int ret;
+	int err;
 
 	if (kstrtou32(buf, 10, &text_id))
 		return -EINVAL;
 
-	ret = visorchannel_write
-		(controlvm_channel,
+	err = visorchannel_write
+		(chipset_dev->controlvm_channel,
 		 offsetof(struct spar_controlvm_channel_protocol,
 			  installation_text_id),
 		 &text_id, sizeof(u32));
-	if (ret)
-		return ret;
+	if (err)
+		return err;
 	return count;
 }
 static DEVICE_ATTR_RW(textid);
@@ -252,11 +249,15 @@ static ssize_t remaining_steps_show(struct device *dev,
 				    struct device_attribute *attr, char *buf)
 {
 	u16 remaining_steps = 0;
+	int err;
+
+	err = visorchannel_read(chipset_dev->controlvm_channel,
+				offsetof(struct spar_controlvm_channel_protocol,
+					 installation_remaining_steps),
+				&remaining_steps, sizeof(u16));
+	if (err)
+		return err;
 
-	visorchannel_read(controlvm_channel,
-			  offsetof(struct spar_controlvm_channel_protocol,
-				   installation_remaining_steps),
-			  &remaining_steps, sizeof(u16));
 	return sprintf(buf, "%hu\n", remaining_steps);
 }
 
@@ -265,18 +266,18 @@ static ssize_t remaining_steps_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	u16 remaining_steps;
-	int ret;
+	int err;
 
 	if (kstrtou16(buf, 10, &remaining_steps))
 		return -EINVAL;
 
-	ret = visorchannel_write
-		(controlvm_channel,
+	err = visorchannel_write
+		(chipset_dev->controlvm_channel,
 		 offsetof(struct spar_controlvm_channel_protocol,
 			  installation_remaining_steps),
 		 &remaining_steps, sizeof(u16));
-	if (ret)
-		return ret;
+	if (err)
+		return err;
 	return count;
 }
 static DEVICE_ATTR_RW(remaining_steps);
@@ -292,7 +293,7 @@ parser_id_get(struct parser_context *ctx)
 
 static void parser_done(struct parser_context *ctx)
 {
-	controlvm_payload_bytes_buffered -= ctx->param_bytes;
+	chipset_dev->controlvm_payload_bytes_buffered -= ctx->param_bytes;
 	kfree(ctx);
 }
 
@@ -318,7 +319,7 @@ parser_string_get(struct parser_context *ctx)
 		}
 	if (value_length < 0)	/* '\0' was not included in the length */
 		value_length = nscan;
-	value = kmalloc(value_length + 1, GFP_KERNEL | __GFP_NORETRY);
+	value = kmalloc(value_length + 1, GFP_KERNEL);
 	if (!value)
 		return NULL;
 	if (value_length > 0)
@@ -405,7 +406,7 @@ controlvm_respond_chipset_init(struct controlvm_message_header *msg_hdr,
 
 	controlvm_init_response(&outmsg, msg_hdr, response);
 	outmsg.cmd.init_chipset.features = features;
-	return visorchannel_signalinsert(controlvm_channel,
+	return visorchannel_signalinsert(chipset_dev->controlvm_channel,
 					 CONTROLVM_QUEUE_REQUEST, &outmsg);
 }
 
@@ -417,14 +418,12 @@ chipset_init(struct controlvm_message *inmsg)
 	int rc = CONTROLVM_RESP_SUCCESS;
 	int res = 0;
 
-	POSTCODE_LINUX(CHIPSET_INIT_ENTRY_PC, 0, 0, DIAG_SEVERITY_PRINT);
 	if (chipset_inited) {
 		rc = -CONTROLVM_RESP_ALREADY_DONE;
 		res = -EIO;
 		goto out_respond;
 	}
 	chipset_inited = 1;
-	POSTCODE_LINUX(CHIPSET_INIT_EXIT_PC, 0, 0, DIAG_SEVERITY_PRINT);
 
 	/*
 	 * Set features to indicate we support parahotplug (if Command
@@ -447,7 +446,8 @@ out_respond:
 }
 
 static int
-controlvm_respond(struct controlvm_message_header *msg_hdr, int response)
+controlvm_respond(struct controlvm_message_header *msg_hdr, int response,
+		  struct spar_segment_state *state)
 {
 	struct controlvm_message outmsg;
 
@@ -455,20 +455,12 @@ controlvm_respond(struct controlvm_message_header *msg_hdr, int response)
 	if (outmsg.hdr.flags.test_message == 1)
 		return -EINVAL;
 
-	return visorchannel_signalinsert(controlvm_channel,
-					 CONTROLVM_QUEUE_REQUEST, &outmsg);
-}
-
-static int controlvm_respond_physdev_changestate(
-		struct controlvm_message_header *msg_hdr, int response,
-		struct spar_segment_state state)
-{
-	struct controlvm_message outmsg;
+	if (state) {
+		outmsg.cmd.device_change_state.state = *state;
+		outmsg.cmd.device_change_state.flags.phys_device = 1;
+	}
 
-	controlvm_init_response(&outmsg, msg_hdr, response);
-	outmsg.cmd.device_change_state.state = state;
-	outmsg.cmd.device_change_state.flags.phys_device = 1;
-	return visorchannel_signalinsert(controlvm_channel,
+	return visorchannel_signalinsert(chipset_dev->controlvm_channel,
 					 CONTROLVM_QUEUE_REQUEST, &outmsg);
 }
 
@@ -484,68 +476,68 @@ save_crash_message(struct controlvm_message *msg, enum crash_obj_type typ)
 	u16 local_crash_msg_count;
 	int err;
 
-	err = visorchannel_read(controlvm_channel,
+	err = visorchannel_read(chipset_dev->controlvm_channel,
 				offsetof(struct spar_controlvm_channel_protocol,
 					 saved_crash_message_count),
 				&local_crash_msg_count, sizeof(u16));
 	if (err) {
-		POSTCODE_LINUX(CRASH_DEV_CTRL_RD_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read message count\n");
 		return err;
 	}
 
 	if (local_crash_msg_count != CONTROLVM_CRASHMSG_MAX) {
-		POSTCODE_LINUX(CRASH_DEV_COUNT_FAILURE_PC, 0,
-			       local_crash_msg_count,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"invalid number of messages\n");
 		return -EIO;
 	}
 
-	err = visorchannel_read(controlvm_channel,
+	err = visorchannel_read(chipset_dev->controlvm_channel,
 				offsetof(struct spar_controlvm_channel_protocol,
 					 saved_crash_message_offset),
 				&local_crash_msg_offset, sizeof(u32));
 	if (err) {
-		POSTCODE_LINUX(CRASH_DEV_CTRL_RD_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read offset\n");
 		return err;
 	}
 
 	switch (typ) {
 	case CRASH_DEV:
 		local_crash_msg_offset += sizeof(struct controlvm_message);
-		err = visorchannel_write(controlvm_channel,
+		err = visorchannel_write(chipset_dev->controlvm_channel,
 					 local_crash_msg_offset,
 					 msg,
 					 sizeof(struct controlvm_message));
 		if (err) {
-			POSTCODE_LINUX(SAVE_MSG_DEV_FAILURE_PC, 0, 0,
-				       DIAG_SEVERITY_ERR);
+			dev_err(&chipset_dev->acpi_device->dev,
+				"failed to write dev msg\n");
 			return err;
 		}
 		break;
 	case CRASH_BUS:
-		err = visorchannel_write(controlvm_channel,
+		err = visorchannel_write(chipset_dev->controlvm_channel,
 					 local_crash_msg_offset,
 					 msg,
 					 sizeof(struct controlvm_message));
 		if (err) {
-			POSTCODE_LINUX(SAVE_MSG_BUS_FAILURE_PC, 0, 0,
-				       DIAG_SEVERITY_ERR);
+			dev_err(&chipset_dev->acpi_device->dev,
+				"failed to write bus msg\n");
 			return err;
 		}
 		break;
 	default:
-		pr_info("Invalid crash_obj_type\n");
+		dev_err(&chipset_dev->acpi_device->dev,
+			"Invalid crash_obj_type\n");
 		break;
 	}
 	return 0;
 }
 
 static int
-bus_responder(enum controlvm_id cmd_id,
-	      struct controlvm_message_header *pending_msg_hdr,
-	      int response)
+controlvm_responder(enum controlvm_id cmd_id,
+		    struct controlvm_message_header *pending_msg_hdr,
+		    int response)
 {
 	if (!pending_msg_hdr)
 		return -EIO;
@@ -553,7 +545,7 @@ bus_responder(enum controlvm_id cmd_id,
 	if (pending_msg_hdr->id != (u32)cmd_id)
 		return -EINVAL;
 
-	return controlvm_respond(pending_msg_hdr, response);
+	return controlvm_respond(pending_msg_hdr, response, NULL);
 }
 
 static int
@@ -576,25 +568,11 @@ device_changestate_responder(enum controlvm_id cmd_id,
 	outmsg.cmd.device_change_state.dev_no = dev_no;
 	outmsg.cmd.device_change_state.state = response_state;
 
-	return visorchannel_signalinsert(controlvm_channel,
+	return visorchannel_signalinsert(chipset_dev->controlvm_channel,
 					 CONTROLVM_QUEUE_REQUEST, &outmsg);
 }
 
 static int
-device_responder(enum controlvm_id cmd_id,
-		 struct controlvm_message_header *pending_msg_hdr,
-		 int response)
-{
-	if (!pending_msg_hdr)
-		return -EIO;
-
-	if (pending_msg_hdr->id != (u32)cmd_id)
-		return -EINVAL;
-
-	return controlvm_respond(pending_msg_hdr, response);
-}
-
-static int
 bus_create(struct controlvm_message *inmsg)
 {
 	struct controlvm_message_packet *cmd = &inmsg->cmd;
@@ -606,16 +584,14 @@ bus_create(struct controlvm_message *inmsg)
 
 	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (bus_info && (bus_info->state.created == 1)) {
-		POSTCODE_LINUX(BUS_CREATE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed bus_create: already exists\n");
 		err = -EEXIST;
 		goto err_respond;
 	}
 
 	bus_info = kzalloc(sizeof(*bus_info), GFP_KERNEL);
 	if (!bus_info) {
-		POSTCODE_LINUX(BUS_CREATE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -ENOMEM;
 		goto err_respond;
 	}
@@ -624,8 +600,6 @@ bus_create(struct controlvm_message *inmsg)
 	bus_info->chipset_bus_no = bus_no;
 	bus_info->chipset_dev_no = BUS_ROOT_DEVICE;
 
-	POSTCODE_LINUX(BUS_CREATE_ENTRY_PC, 0, bus_no, DIAG_SEVERITY_PRINT);
-
 	if (uuid_le_cmp(cmd->create_bus.bus_inst_uuid, spar_siovm_uuid) == 0) {
 		err = save_crash_message(inmsg, CRASH_BUS);
 		if (err)
@@ -636,9 +610,6 @@ bus_create(struct controlvm_message *inmsg)
 		pmsg_hdr = kzalloc(sizeof(*pmsg_hdr),
 				   GFP_KERNEL);
 		if (!pmsg_hdr) {
-			POSTCODE_LINUX(MALLOC_FAILURE_PC, cmd,
-				       bus_info->chipset_bus_no,
-				       DIAG_SEVERITY_ERR);
 			err = -ENOMEM;
 			goto err_free_bus_info;
 		}
@@ -654,19 +625,22 @@ bus_create(struct controlvm_message *inmsg)
 					   cmd->create_bus.bus_data_type_uuid);
 
 	if (!visorchannel) {
-		POSTCODE_LINUX(BUS_CREATE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -ENOMEM;
 		goto err_free_pending_msg;
 	}
 	bus_info->visorchannel = visorchannel;
 
 	/* Response will be handled by chipset_bus_create */
-	chipset_bus_create(bus_info);
+	err = chipset_bus_create(bus_info);
+	/* If error chipset_bus_create didn't respond, need to respond here */
+	if (err)
+		goto err_destroy_channel;
 
-	POSTCODE_LINUX(BUS_CREATE_EXIT_PC, 0, bus_no, DIAG_SEVERITY_PRINT);
 	return 0;
 
+err_destroy_channel:
+	visorchannel_destroy(visorchannel);
+
 err_free_pending_msg:
 	kfree(bus_info->pending_msg_hdr);
 
@@ -675,7 +649,7 @@ err_free_bus_info:
 
 err_respond:
 	if (inmsg->hdr.flags.response_expected == 1)
-		bus_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -705,9 +679,6 @@ bus_destroy(struct controlvm_message *inmsg)
 	if (inmsg->hdr.flags.response_expected == 1) {
 		pmsg_hdr = kzalloc(sizeof(*pmsg_hdr), GFP_KERNEL);
 		if (!pmsg_hdr) {
-			POSTCODE_LINUX(MALLOC_FAILURE_PC, cmd,
-				       bus_info->chipset_bus_no,
-				       DIAG_SEVERITY_ERR);
 			err = -ENOMEM;
 			goto err_respond;
 		}
@@ -723,7 +694,7 @@ bus_destroy(struct controlvm_message *inmsg)
 
 err_respond:
 	if (inmsg->hdr.flags.response_expected == 1)
-		bus_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -737,23 +708,14 @@ bus_configure(struct controlvm_message *inmsg,
 	int err = 0;
 
 	bus_no = cmd->configure_bus.bus_no;
-	POSTCODE_LINUX(BUS_CONFIGURE_ENTRY_PC, 0, bus_no,
-		       DIAG_SEVERITY_PRINT);
-
 	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (!bus_info) {
-		POSTCODE_LINUX(BUS_CONFIGURE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -EINVAL;
 		goto err_respond;
 	} else if (bus_info->state.created == 0) {
-		POSTCODE_LINUX(BUS_CONFIGURE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -EINVAL;
 		goto err_respond;
 	} else if (bus_info->pending_msg_hdr) {
-		POSTCODE_LINUX(BUS_CONFIGURE_FAILURE_PC, 0, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -EIO;
 		goto err_respond;
 	}
@@ -769,16 +731,15 @@ bus_configure(struct controlvm_message *inmsg,
 		bus_info->name = parser_name_get(parser_ctx);
 	}
 
-	POSTCODE_LINUX(BUS_CONFIGURE_EXIT_PC, 0, bus_no,
-		       DIAG_SEVERITY_PRINT);
-
 	if (inmsg->hdr.flags.response_expected == 1)
-		bus_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return 0;
 
 err_respond:
+	dev_err(&chipset_dev->acpi_device->dev,
+		"bus_configured exited with err: %d\n", err);
 	if (inmsg->hdr.flags.response_expected == 1)
-		bus_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -796,31 +757,29 @@ my_device_create(struct controlvm_message *inmsg)
 
 	bus_info = visorbus_get_device_by_id(bus_no, BUS_ROOT_DEVICE, NULL);
 	if (!bus_info) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to get bus by id: %d\n", bus_no);
 		err = -ENODEV;
 		goto err_respond;
 	}
 
 	if (bus_info->state.created == 0) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"bus not created, id: %d\n", bus_no);
 		err = -EINVAL;
 		goto err_respond;
 	}
 
 	dev_info = visorbus_get_device_by_id(bus_no, dev_no, NULL);
 	if (dev_info && (dev_info->state.created == 1)) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to get bus by id: %d/%d\n", bus_no, dev_no);
 		err = -EEXIST;
 		goto err_respond;
 	}
 
 	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
 	if (!dev_info) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -ENOMEM;
 		goto err_respond;
 	}
@@ -832,9 +791,6 @@ my_device_create(struct controlvm_message *inmsg)
 	/* not sure where the best place to set the 'parent' */
 	dev_info->device.parent = &bus_info->device;
 
-	POSTCODE_LINUX(DEVICE_CREATE_ENTRY_PC, dev_no, bus_no,
-		       DIAG_SEVERITY_PRINT);
-
 	visorchannel =
 	       visorchannel_create_with_lock(cmd->create_device.channel_addr,
 					     cmd->create_device.channel_bytes,
@@ -842,8 +798,9 @@ my_device_create(struct controlvm_message *inmsg)
 					     cmd->create_device.data_type_uuid);
 
 	if (!visorchannel) {
-		POSTCODE_LINUX(DEVICE_CREATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to create visorchannel: %d/%d\n",
+			bus_no, dev_no);
 		err = -ENOMEM;
 		goto err_free_dev_info;
 	}
@@ -853,14 +810,14 @@ my_device_create(struct controlvm_message *inmsg)
 			spar_vhba_channel_protocol_uuid) == 0) {
 		err = save_crash_message(inmsg, CRASH_DEV);
 		if (err)
-			goto err_free_dev_info;
+			goto err_destroy_visorchannel;
 	}
 
 	if (inmsg->hdr.flags.response_expected == 1) {
 		pmsg_hdr = kzalloc(sizeof(*pmsg_hdr), GFP_KERNEL);
 		if (!pmsg_hdr) {
 			err = -ENOMEM;
-			goto err_free_dev_info;
+			goto err_destroy_visorchannel;
 		}
 
 		memcpy(pmsg_hdr, &inmsg->hdr,
@@ -868,17 +825,21 @@ my_device_create(struct controlvm_message *inmsg)
 		dev_info->pending_msg_hdr = pmsg_hdr;
 	}
 	/* Chipset_device_create will send response */
-	chipset_device_create(dev_info);
-	POSTCODE_LINUX(DEVICE_CREATE_EXIT_PC, dev_no, bus_no,
-		       DIAG_SEVERITY_PRINT);
+	err = chipset_device_create(dev_info);
+	if (err)
+		goto err_destroy_visorchannel;
+
 	return 0;
 
+err_destroy_visorchannel:
+	visorchannel_destroy(visorchannel);
+
 err_free_dev_info:
 	kfree(dev_info);
 
 err_respond:
 	if (inmsg->hdr.flags.response_expected == 1)
-		device_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -891,18 +852,14 @@ my_device_changestate(struct controlvm_message *inmsg)
 	u32 dev_no = cmd->device_change_state.dev_no;
 	struct spar_segment_state state = cmd->device_change_state.state;
 	struct visor_device *dev_info;
-	int err;
+	int err = 0;
 
 	dev_info = visorbus_get_device_by_id(bus_no, dev_no, NULL);
 	if (!dev_info) {
-		POSTCODE_LINUX(DEVICE_CHANGESTATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -ENODEV;
 		goto err_respond;
 	}
 	if (dev_info->state.created == 0) {
-		POSTCODE_LINUX(DEVICE_CHANGESTATE_FAILURE_PC, dev_no, bus_no,
-			       DIAG_SEVERITY_ERR);
 		err = -EINVAL;
 		goto err_respond;
 	}
@@ -926,7 +883,7 @@ my_device_changestate(struct controlvm_message *inmsg)
 	if (state.alive == segment_state_running.alive &&
 	    state.operating == segment_state_running.operating)
 		/* Response will be sent from chipset_device_resume */
-		chipset_device_resume(dev_info);
+		err = chipset_device_resume(dev_info);
 	/* ServerNotReady / ServerLost / SegmentStateStandby */
 	else if (state.alive == segment_state_standby.alive &&
 		 state.operating == segment_state_standby.operating)
@@ -934,12 +891,16 @@ my_device_changestate(struct controlvm_message *inmsg)
 		 * technically this is standby case where server is lost.
 		 * Response will be sent from chipset_device_pause.
 		 */
-		chipset_device_pause(dev_info);
+		err = chipset_device_pause(dev_info);
+	if (err)
+		goto err_respond;
+
 	return 0;
 
 err_respond:
+	dev_err(&chipset_dev->acpi_device->dev, "failed: %d\n", err);
 	if (inmsg->hdr.flags.response_expected == 1)
-		device_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -985,7 +946,7 @@ my_device_destroy(struct controlvm_message *inmsg)
 
 err_respond:
 	if (inmsg->hdr.flags.response_expected == 1)
-		device_responder(inmsg->hdr.id, &inmsg->hdr, err);
+		controlvm_responder(inmsg->hdr.id, &inmsg->hdr, err);
 	return err;
 }
 
@@ -1004,7 +965,7 @@ err_respond:
 
 #define PARAHOTPLUG_TIMEOUT_MS 2000
 
-/**
+/*
  * parahotplug_next_id() - generate unique int to match an outstanding
  *                         CONTROLVM message with a udev script /sys
  *                         response
@@ -1019,7 +980,7 @@ parahotplug_next_id(void)
 	return atomic_inc_return(&id);
 }
 
-/**
+/*
  * parahotplug_next_expiration() - returns the time (in jiffies) when a
  *                                 CONTROLVM message on the list should expire
  *                                 -- PARAHOTPLUG_TIMEOUT_MS in the future
@@ -1032,7 +993,7 @@ parahotplug_next_expiration(void)
 	return jiffies + msecs_to_jiffies(PARAHOTPLUG_TIMEOUT_MS);
 }
 
-/**
+/*
  * parahotplug_request_create() - create a parahotplug_request, which is
  *                                basically a wrapper for a CONTROLVM_MESSAGE
  *                                that we can stick on a list
@@ -1045,7 +1006,7 @@ parahotplug_request_create(struct controlvm_message *msg)
 {
 	struct parahotplug_request *req;
 
-	req = kmalloc(sizeof(*req), GFP_KERNEL | __GFP_NORETRY);
+	req = kmalloc(sizeof(*req), GFP_KERNEL);
 	if (!req)
 		return NULL;
 
@@ -1056,7 +1017,7 @@ parahotplug_request_create(struct controlvm_message *msg)
 	return req;
 }
 
-/**
+/*
  * parahotplug_request_destroy() - free a parahotplug_request
  * @req: the request to deallocate
  */
@@ -1069,7 +1030,7 @@ parahotplug_request_destroy(struct parahotplug_request *req)
 static LIST_HEAD(parahotplug_request_list);
 static DEFINE_SPINLOCK(parahotplug_request_list_lock);	/* lock for above */
 
-/**
+/*
  * parahotplug_request_complete() - mark request as complete
  * @id:     the id of the request
  * @active: indicates whether the request is assigned to active partition
@@ -1101,9 +1062,9 @@ parahotplug_request_complete(int id, u16 active)
 			spin_unlock(&parahotplug_request_list_lock);
 			req->msg.cmd.device_change_state.state.active = active;
 			if (req->msg.hdr.flags.response_expected)
-				controlvm_respond_physdev_changestate(
-					&req->msg.hdr, CONTROLVM_RESP_SUCCESS,
-					req->msg.cmd.device_change_state.state);
+				controlvm_respond(
+				       &req->msg.hdr, CONTROLVM_RESP_SUCCESS,
+				       &req->msg.cmd.device_change_state.state);
 			parahotplug_request_destroy(req);
 			return 0;
 		}
@@ -1113,7 +1074,7 @@ parahotplug_request_complete(int id, u16 active)
 	return -EINVAL;
 }
 
-/**
+/*
  * devicedisabled_store() - disables the hotplug device
  * @dev:   sysfs interface variable not utilized in this function
  * @attr:  sysfs interface variable not utilized in this function
@@ -1143,7 +1104,7 @@ static ssize_t devicedisabled_store(struct device *dev,
 }
 static DEVICE_ATTR_WO(devicedisabled);
 
-/**
+/*
  * deviceenabled_store() - enables the hotplug device
  * @dev:   sysfs interface variable not utilized in this function
  * @attr:  sysfs interface variable not utilized in this function
@@ -1201,26 +1162,14 @@ static const struct attribute_group *visorchipset_dev_groups[] = {
 	NULL
 };
 
-static void visorchipset_dev_release(struct device *dev)
-{
-}
-
-/* /sys/devices/platform/visorchipset */
-static struct platform_device visorchipset_platform_device = {
-	.name = "visorchipset",
-	.id = -1,
-	.dev.groups = visorchipset_dev_groups,
-	.dev.release = visorchipset_dev_release,
-};
-
-/**
+/*
  * parahotplug_request_kickoff() - initiate parahotplug request
  * @req: the request to initiate
  *
  * Cause uevent to run the user level script to do the disable/enable specified
  * in the parahotplug_request.
  */
-static void
+static int
 parahotplug_request_kickoff(struct parahotplug_request *req)
 {
 	struct controlvm_message_packet *cmd = &req->msg.cmd;
@@ -1241,56 +1190,59 @@ parahotplug_request_kickoff(struct parahotplug_request *req)
 	sprintf(env_func, "SPAR_PARAHOTPLUG_FUNCTION=%d",
 		cmd->device_change_state.dev_no & 0x7);
 
-	kobject_uevent_env(&visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
-			   envp);
+	return kobject_uevent_env(&chipset_dev->acpi_device->dev.kobj,
+				  KOBJ_CHANGE, envp);
 }
 
-/**
+/*
  * parahotplug_process_message() - enables or disables a PCI device by kicking
  *                                 off a udev script
  * @inmsg: the message indicating whether to enable or disable
  */
-static void
+static int
 parahotplug_process_message(struct controlvm_message *inmsg)
 {
 	struct parahotplug_request *req;
+	int err;
 
 	req = parahotplug_request_create(inmsg);
 
 	if (!req)
-		return;
+		return -ENOMEM;
 
+	/*
+	 * For enable messages, just respond with success right away, we don't
+	 * need to wait to see if the enable was successful.
+	 */
 	if (inmsg->cmd.device_change_state.state.active) {
-		/*
-		 * For enable messages, just respond with success
-		 * right away. This is a bit of a hack, but there are
-		 * issues with the early enable messages we get (with
-		 * either the udev script not detecting that the device
-		 * is up, or not getting called at all). Fortunately
-		 * the messages that get lost don't matter anyway, as
-		 *
-		 * devices are automatically enabled at
-		 * initialization.
-		 */
-		parahotplug_request_kickoff(req);
-		controlvm_respond_physdev_changestate
-			(&inmsg->hdr,
-			 CONTROLVM_RESP_SUCCESS,
-			 inmsg->cmd.device_change_state.state);
+		err = parahotplug_request_kickoff(req);
+		if (err)
+			goto err_respond;
+		controlvm_respond(&inmsg->hdr, CONTROLVM_RESP_SUCCESS,
+				  &inmsg->cmd.device_change_state.state);
 		parahotplug_request_destroy(req);
-	} else {
-		/*
-		 * For disable messages, add the request to the
-		 * request list before kicking off the udev script. It
-		 * won't get responded to until the script has
-		 * indicated it's done.
-		 */
-		spin_lock(&parahotplug_request_list_lock);
-		list_add_tail(&req->list, &parahotplug_request_list);
-		spin_unlock(&parahotplug_request_list_lock);
-
-		parahotplug_request_kickoff(req);
+		return 0;
 	}
+
+	/*
+	 * For disable messages, add the request to the
+	 * request list before kicking off the udev script. It
+	 * won't get responded to until the script has
+	 * indicated it's done.
+	 */
+	spin_lock(&parahotplug_request_list_lock);
+	list_add_tail(&req->list, &parahotplug_request_list);
+	spin_unlock(&parahotplug_request_list_lock);
+
+	err = parahotplug_request_kickoff(req);
+	if (err)
+		goto err_respond;
+	return 0;
+
+err_respond:
+	controlvm_respond(&inmsg->hdr, err,
+			  &inmsg->cmd.device_change_state.state);
+	return err;
 }
 
 /*
@@ -1303,12 +1255,15 @@ parahotplug_process_message(struct controlvm_message *inmsg)
 static int
 chipset_ready_uevent(struct controlvm_message_header *msg_hdr)
 {
-	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_ONLINE);
+	int res;
+
+	res = kobject_uevent(&chipset_dev->acpi_device->dev.kobj,
+			     KOBJ_ONLINE);
 
 	if (msg_hdr->flags.response_expected)
-		return controlvm_respond(msg_hdr, CONTROLVM_RESP_SUCCESS);
+		controlvm_respond(msg_hdr, res, NULL);
 
-	return 0;
+	return res;
 }
 
 /*
@@ -1323,15 +1278,16 @@ chipset_selftest_uevent(struct controlvm_message_header *msg_hdr)
 {
 	char env_selftest[20];
 	char *envp[] = { env_selftest, NULL };
+	int res;
 
 	sprintf(env_selftest, "SPARSP_SELFTEST=%d", 1);
-	kobject_uevent_env(&visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
-			   envp);
+	res = kobject_uevent_env(&chipset_dev->acpi_device->dev.kobj,
+				 KOBJ_CHANGE, envp);
 
 	if (msg_hdr->flags.response_expected)
-		return controlvm_respond(msg_hdr, CONTROLVM_RESP_SUCCESS);
+		controlvm_respond(msg_hdr, res, NULL);
 
-	return 0;
+	return res;
 }
 
 /*
@@ -1344,28 +1300,62 @@ chipset_selftest_uevent(struct controlvm_message_header *msg_hdr)
 static int
 chipset_notready_uevent(struct controlvm_message_header *msg_hdr)
 {
-	kobject_uevent(&visorchipset_platform_device.dev.kobj, KOBJ_OFFLINE);
+	int res;
 
+	res = kobject_uevent(&chipset_dev->acpi_device->dev.kobj,
+			     KOBJ_OFFLINE);
 	if (msg_hdr->flags.response_expected)
-		return controlvm_respond(msg_hdr, CONTROLVM_RESP_SUCCESS);
+		controlvm_respond(msg_hdr, res, NULL);
 
-	return 0;
+	return res;
 }
 
-static inline unsigned int
-issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
+static int unisys_vmcall(unsigned long tuple, unsigned long param)
 {
-	struct vmcall_io_controlvm_addr_params params;
-	int result = VMCALL_SUCCESS;
-	u64 physaddr;
+	int result = 0;
+	unsigned int cpuid_eax, cpuid_ebx, cpuid_ecx, cpuid_edx;
+	unsigned long reg_ebx;
+	unsigned long reg_ecx;
+
+	reg_ebx = param & 0xFFFFFFFF;
+	reg_ecx = param >> 32;
+
+	cpuid(0x00000001, &cpuid_eax, &cpuid_ebx, &cpuid_ecx, &cpuid_edx);
+	if (!(cpuid_ecx & 0x80000000))
+		return -EPERM;
+
+	__asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
+		"a"(tuple), "b"(reg_ebx), "c"(reg_ecx));
+
+	if (result)
+		goto error;
 
-	physaddr = virt_to_phys(&params);
-	ISSUE_IO_VMCALL(VMCALL_IO_CONTROLVM_ADDR, physaddr, result);
-	if (VMCALL_SUCCESSFUL(result)) {
-		*control_addr = params.address;
-		*control_bytes = params.channel_bytes;
+	return 0;
+
+error: /* Need to convert from VMCALL error codes to Linux */
+	switch (result) {
+	case VMCALL_RESULT_INVALID_PARAM:
+		return -EINVAL;
+	case VMCALL_RESULT_DATA_UNAVAILABLE:
+		return -ENODEV;
+	default:
+		return -EFAULT;
 	}
-	return result;
+}
+static unsigned int
+issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
+{
+	chipset_dev->controlvm_addr.physaddr = virt_to_phys(
+					   &chipset_dev->controlvm_addr.params);
+	chipset_dev->controlvm_addr.err = unisys_vmcall(VMCALL_CONTROLVM_ADDR,
+					  chipset_dev->controlvm_addr.physaddr);
+	if (chipset_dev->controlvm_addr.err)
+		return chipset_dev->controlvm_addr.err;
+
+	*control_addr = chipset_dev->controlvm_addr.params.address;
+	*control_bytes = chipset_dev->controlvm_addr.params.channel_bytes;
+
+	return 0;
 }
 
 static u64 controlvm_get_channel_address(void)
@@ -1373,7 +1363,7 @@ static u64 controlvm_get_channel_address(void)
 	u64 addr = 0;
 	u32 size = 0;
 
-	if (!VMCALL_SUCCESSFUL(issue_vmcall_io_controlvm_addr(&addr, &size)))
+	if (issue_vmcall_io_controlvm_addr(&addr, &size))
 		return 0;
 
 	return addr;
@@ -1388,8 +1378,6 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	u32 local_crash_msg_offset;
 	u16 local_crash_msg_count;
 
-	POSTCODE_LINUX(CRASH_DEV_ENTRY_PC, 0, 0, DIAG_SEVERITY_PRINT);
-
 	/* send init chipset msg */
 	msg.hdr.id = CONTROLVM_CHIPSET_INIT;
 	msg.cmd.init_chipset.bus_count = 23;
@@ -1398,71 +1386,67 @@ setup_crash_devices_work_queue(struct work_struct *work)
 	chipset_init(&msg);
 
 	/* get saved message count */
-	if (visorchannel_read(controlvm_channel,
+	if (visorchannel_read(chipset_dev->controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_count),
 			      &local_crash_msg_count, sizeof(u16)) < 0) {
-		POSTCODE_LINUX(CRASH_DEV_CTRL_RD_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read channel\n");
 		return;
 	}
 
 	if (local_crash_msg_count != CONTROLVM_CRASHMSG_MAX) {
-		POSTCODE_LINUX(CRASH_DEV_COUNT_FAILURE_PC, 0,
-			       local_crash_msg_count,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"invalid count\n");
 		return;
 	}
 
 	/* get saved crash message offset */
-	if (visorchannel_read(controlvm_channel,
+	if (visorchannel_read(chipset_dev->controlvm_channel,
 			      offsetof(struct spar_controlvm_channel_protocol,
 				       saved_crash_message_offset),
 			      &local_crash_msg_offset, sizeof(u32)) < 0) {
-		POSTCODE_LINUX(CRASH_DEV_CTRL_RD_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read channel\n");
 		return;
 	}
 
 	/* read create device message for storage bus offset */
-	if (visorchannel_read(controlvm_channel,
+	if (visorchannel_read(chipset_dev->controlvm_channel,
 			      local_crash_msg_offset,
 			      &local_crash_bus_msg,
 			      sizeof(struct controlvm_message)) < 0) {
-		POSTCODE_LINUX(CRASH_DEV_RD_BUS_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read channel\n");
 		return;
 	}
 
 	/* read create device message for storage device */
-	if (visorchannel_read(controlvm_channel,
+	if (visorchannel_read(chipset_dev->controlvm_channel,
 			      local_crash_msg_offset +
 			      sizeof(struct controlvm_message),
 			      &local_crash_dev_msg,
 			      sizeof(struct controlvm_message)) < 0) {
-		POSTCODE_LINUX(CRASH_DEV_RD_DEV_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+		dev_err(&chipset_dev->acpi_device->dev,
+			"failed to read channel\n");
 		return;
 	}
 
 	/* reuse IOVM create bus message */
-	if (local_crash_bus_msg.cmd.create_bus.channel_addr) {
-		bus_create(&local_crash_bus_msg);
-	} else {
-		POSTCODE_LINUX(CRASH_DEV_BUS_NULL_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+	if (!local_crash_bus_msg.cmd.create_bus.channel_addr) {
+		dev_err(&chipset_dev->acpi_device->dev,
+			"no valid create_bus message\n");
 		return;
 	}
+	bus_create(&local_crash_bus_msg);
 
 	/* reuse create device message for storage device */
-	if (local_crash_dev_msg.cmd.create_device.channel_addr) {
-		my_device_create(&local_crash_dev_msg);
-	} else {
-		POSTCODE_LINUX(CRASH_DEV_DEV_NULL_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
+	if (!local_crash_dev_msg.cmd.create_device.channel_addr) {
+		dev_err(&chipset_dev->acpi_device->dev,
+			"no valid create_device message\n");
 		return;
 	}
-	POSTCODE_LINUX(CRASH_DEV_EXIT_PC, 0, 0, DIAG_SEVERITY_PRINT);
+	my_device_create(&local_crash_dev_msg);
 }
 
 void
@@ -1471,8 +1455,8 @@ bus_create_response(struct visor_device *bus_info, int response)
 	if (response >= 0)
 		bus_info->state.created = 1;
 
-	bus_responder(CONTROLVM_BUS_CREATE, bus_info->pending_msg_hdr,
-		      response);
+	controlvm_responder(CONTROLVM_BUS_CREATE, bus_info->pending_msg_hdr,
+			    response);
 
 	kfree(bus_info->pending_msg_hdr);
 	bus_info->pending_msg_hdr = NULL;
@@ -1481,8 +1465,8 @@ bus_create_response(struct visor_device *bus_info, int response)
 void
 bus_destroy_response(struct visor_device *bus_info, int response)
 {
-	bus_responder(CONTROLVM_BUS_DESTROY, bus_info->pending_msg_hdr,
-		      response);
+	controlvm_responder(CONTROLVM_BUS_DESTROY, bus_info->pending_msg_hdr,
+			    response);
 
 	kfree(bus_info->pending_msg_hdr);
 	bus_info->pending_msg_hdr = NULL;
@@ -1494,8 +1478,8 @@ device_create_response(struct visor_device *dev_info, int response)
 	if (response >= 0)
 		dev_info->state.created = 1;
 
-	device_responder(CONTROLVM_DEVICE_CREATE, dev_info->pending_msg_hdr,
-			 response);
+	controlvm_responder(CONTROLVM_DEVICE_CREATE, dev_info->pending_msg_hdr,
+			    response);
 
 	kfree(dev_info->pending_msg_hdr);
 	dev_info->pending_msg_hdr = NULL;
@@ -1504,8 +1488,8 @@ device_create_response(struct visor_device *dev_info, int response)
 void
 device_destroy_response(struct visor_device *dev_info, int response)
 {
-	device_responder(CONTROLVM_DEVICE_DESTROY, dev_info->pending_msg_hdr,
-			 response);
+	controlvm_responder(CONTROLVM_DEVICE_DESTROY, dev_info->pending_msg_hdr,
+			    response);
 
 	kfree(dev_info->pending_msg_hdr);
 	dev_info->pending_msg_hdr = NULL;
@@ -1534,136 +1518,6 @@ device_resume_response(struct visor_device *dev_info, int response)
 	dev_info->pending_msg_hdr = NULL;
 }
 
-static int
-visorchipset_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	unsigned long physaddr = 0;
-	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	u64 addr = 0;
-
-	/* sv_enable_dfp(); */
-	if (offset & (PAGE_SIZE - 1))
-		return -ENXIO;	/* need aligned offsets */
-
-	switch (offset) {
-	case VISORCHIPSET_MMAP_CONTROLCHANOFFSET:
-		vma->vm_flags |= VM_IO;
-		if (!*file_controlvm_channel)
-			return -ENXIO;
-
-		visorchannel_read
-			(*file_controlvm_channel,
-			 offsetof(struct spar_controlvm_channel_protocol,
-				  gp_control_channel),
-			 &addr, sizeof(addr));
-		if (!addr)
-			return -ENXIO;
-
-		physaddr = (unsigned long)addr;
-		if (remap_pfn_range(vma, vma->vm_start,
-				    physaddr >> PAGE_SHIFT,
-				    vma->vm_end - vma->vm_start,
-				    /*pgprot_noncached */
-				    (vma->vm_page_prot))) {
-			return -EAGAIN;
-		}
-		break;
-	default:
-		return -ENXIO;
-	}
-	return 0;
-}
-
-static inline s64 issue_vmcall_query_guest_virtual_time_offset(void)
-{
-	u64 result = VMCALL_SUCCESS;
-	u64 physaddr = 0;
-
-	ISSUE_IO_VMCALL(VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET, physaddr,
-			result);
-	return result;
-}
-
-static inline int issue_vmcall_update_physical_time(u64 adjustment)
-{
-	int result = VMCALL_SUCCESS;
-
-	ISSUE_IO_VMCALL(VMCALL_UPDATE_PHYSICAL_TIME, adjustment, result);
-	return result;
-}
-
-static long visorchipset_ioctl(struct file *file, unsigned int cmd,
-			       unsigned long arg)
-{
-	u64 adjustment;
-	s64 vrtc_offset;
-
-	switch (cmd) {
-	case VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET:
-		/* get the physical rtc offset */
-		vrtc_offset = issue_vmcall_query_guest_virtual_time_offset();
-		if (copy_to_user((void __user *)arg, &vrtc_offset,
-				 sizeof(vrtc_offset))) {
-			return -EFAULT;
-		}
-		return 0;
-	case VMCALL_UPDATE_PHYSICAL_TIME:
-		if (copy_from_user(&adjustment, (void __user *)arg,
-				   sizeof(adjustment))) {
-			return -EFAULT;
-		}
-		return issue_vmcall_update_physical_time(adjustment);
-	default:
-		return -EFAULT;
-	}
-}
-
-static const struct file_operations visorchipset_fops = {
-	.owner = THIS_MODULE,
-	.open = visorchipset_open,
-	.read = NULL,
-	.write = NULL,
-	.unlocked_ioctl = visorchipset_ioctl,
-	.release = visorchipset_release,
-	.mmap = visorchipset_mmap,
-};
-
-static int
-visorchipset_file_init(dev_t major_dev, struct visorchannel **controlvm_channel)
-{
-	int rc = 0;
-
-	file_controlvm_channel = controlvm_channel;
-	cdev_init(&file_cdev, &visorchipset_fops);
-	file_cdev.owner = THIS_MODULE;
-	if (MAJOR(major_dev) == 0) {
-		rc = alloc_chrdev_region(&major_dev, 0, 1, "visorchipset");
-		/* dynamic major device number registration required */
-		if (rc < 0)
-			return rc;
-	} else {
-		/* static major device number registration required */
-		rc = register_chrdev_region(major_dev, 1, "visorchipset");
-		if (rc < 0)
-			return rc;
-	}
-	rc = cdev_add(&file_cdev, MKDEV(MAJOR(major_dev), 0), 1);
-	if (rc < 0) {
-		unregister_chrdev_region(major_dev, 1);
-		return rc;
-	}
-	return 0;
-}
-
-static void
-visorchipset_file_cleanup(dev_t major_dev)
-{
-	if (file_cdev.ops)
-		cdev_del(&file_cdev);
-	file_cdev.ops = NULL;
-	unregister_chrdev_region(major_dev, 1);
-}
-
 static struct parser_context *
 parser_init_byte_stream(u64 addr, u32 bytes, bool local, bool *retry)
 {
@@ -1677,12 +1531,12 @@ parser_init_byte_stream(u64 addr, u32 bytes, bool local, bool *retry)
 	 * '\0'-terminated
 	 */
 	allocbytes++;
-	if ((controlvm_payload_bytes_buffered + bytes)
+	if ((chipset_dev->controlvm_payload_bytes_buffered + bytes)
 	    > MAX_CONTROLVM_PAYLOAD_BYTES) {
 		*retry = true;
 		return NULL;
 	}
-	ctx = kzalloc(allocbytes, GFP_KERNEL | __GFP_NORETRY);
+	ctx = kzalloc(allocbytes, GFP_KERNEL);
 	if (!ctx) {
 		*retry = true;
 		return NULL;
@@ -1710,7 +1564,7 @@ parser_init_byte_stream(u64 addr, u32 bytes, bool local, bool *retry)
 	}
 
 	ctx->byte_stream = true;
-	controlvm_payload_bytes_buffered += ctx->param_bytes;
+	chipset_dev->controlvm_payload_bytes_buffered += ctx->param_bytes;
 
 	return ctx;
 
@@ -1719,22 +1573,20 @@ err_finish_ctx:
 	return NULL;
 }
 
-/**
+/*
  * handle_command() - process a controlvm message
  * @inmsg:        the message to process
  * @channel_addr: address of the controlvm channel
  *
  * Return:
- *    false - this function will return false only in the case where the
- *            controlvm message was NOT processed, but processing must be
- *            retried before reading the next controlvm message; a
- *            scenario where this can occur is when we need to throttle
- *            the allocation of memory in which to copy out controlvm
- *            payload data
- *    true  - processing of the controlvm message completed,
- *            either successfully or with an error
+ *	0	- Successfully processed the message
+ *	-EAGAIN - ControlVM message was not processed and should be retried
+ *		  reading the next controlvm message; a scenario where this can
+ *		  occur is when we need to throttle the allocation of memory in
+ *		  which to copy out controlvm payload data.
+ *	< 0	- error: ControlVM message was processed but an error occurred.
  */
-static bool
+static int
 handle_command(struct controlvm_message inmsg, u64 channel_addr)
 {
 	struct controlvm_message_packet *cmd = &inmsg.cmd;
@@ -1743,11 +1595,13 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 	struct parser_context *parser_ctx = NULL;
 	bool local_addr;
 	struct controlvm_message ackmsg;
+	int err = 0;
 
 	/* create parsing context if necessary */
 	local_addr = (inmsg.hdr.flags.test_message == 1);
 	if (channel_addr == 0)
-		return true;
+		return -EINVAL;
+
 	parm_addr = channel_addr + inmsg.hdr.payload_vm_offset;
 	parm_bytes = inmsg.hdr.payload_bytes;
 
@@ -1763,66 +1617,69 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 		    parser_init_byte_stream(parm_addr, parm_bytes,
 					    local_addr, &retry);
 		if (!parser_ctx && retry)
-			return false;
+			return -EAGAIN;
 	}
 
 	if (!local_addr) {
 		controlvm_init_response(&ackmsg, &inmsg.hdr,
 					CONTROLVM_RESP_SUCCESS);
-		if (controlvm_channel)
-			visorchannel_signalinsert(controlvm_channel,
-						  CONTROLVM_QUEUE_ACK,
-						  &ackmsg);
+		err = visorchannel_signalinsert(chipset_dev->controlvm_channel,
+						CONTROLVM_QUEUE_ACK,
+						&ackmsg);
+		if (err)
+			return err;
 	}
 	switch (inmsg.hdr.id) {
 	case CONTROLVM_CHIPSET_INIT:
-		chipset_init(&inmsg);
+		err = chipset_init(&inmsg);
 		break;
 	case CONTROLVM_BUS_CREATE:
-		bus_create(&inmsg);
+		err = bus_create(&inmsg);
 		break;
 	case CONTROLVM_BUS_DESTROY:
-		bus_destroy(&inmsg);
+		err = bus_destroy(&inmsg);
 		break;
 	case CONTROLVM_BUS_CONFIGURE:
-		bus_configure(&inmsg, parser_ctx);
+		err = bus_configure(&inmsg, parser_ctx);
 		break;
 	case CONTROLVM_DEVICE_CREATE:
-		my_device_create(&inmsg);
+		err = my_device_create(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CHANGESTATE:
 		if (cmd->device_change_state.flags.phys_device) {
-			parahotplug_process_message(&inmsg);
+			err = parahotplug_process_message(&inmsg);
 		} else {
 			/*
 			 * save the hdr and cmd structures for later use
 			 * when sending back the response to Command
 			 */
-			my_device_changestate(&inmsg);
+			err = my_device_changestate(&inmsg);
 			break;
 		}
 		break;
 	case CONTROLVM_DEVICE_DESTROY:
-		my_device_destroy(&inmsg);
+		err = my_device_destroy(&inmsg);
 		break;
 	case CONTROLVM_DEVICE_CONFIGURE:
-		/* no op for now, just send a respond that we passed */
+		/* no op just send a respond that we passed */
 		if (inmsg.hdr.flags.response_expected)
-			controlvm_respond(&inmsg.hdr, CONTROLVM_RESP_SUCCESS);
+			controlvm_respond(&inmsg.hdr, CONTROLVM_RESP_SUCCESS,
+					  NULL);
 		break;
 	case CONTROLVM_CHIPSET_READY:
-		chipset_ready_uevent(&inmsg.hdr);
+		err = chipset_ready_uevent(&inmsg.hdr);
 		break;
 	case CONTROLVM_CHIPSET_SELFTEST:
-		chipset_selftest_uevent(&inmsg.hdr);
+		err = chipset_selftest_uevent(&inmsg.hdr);
 		break;
 	case CONTROLVM_CHIPSET_STOP:
-		chipset_notready_uevent(&inmsg.hdr);
+		err = chipset_notready_uevent(&inmsg.hdr);
 		break;
 	default:
+		err = -ENOMSG;
 		if (inmsg.hdr.flags.response_expected)
-			controlvm_respond
-				(&inmsg.hdr, -CONTROLVM_RESP_ID_UNKNOWN);
+			controlvm_respond(&inmsg.hdr,
+					  -CONTROLVM_RESP_ID_UNKNOWN, NULL);
 		break;
 	}
 
@@ -1830,31 +1687,35 @@ handle_command(struct controlvm_message inmsg, u64 channel_addr)
 		parser_done(parser_ctx);
 		parser_ctx = NULL;
 	}
-	return true;
+	return err;
 }
 
-/**
+/*
  * read_controlvm_event() - retreives the next message from the
  *                          CONTROLVM_QUEUE_EVENT queue in the controlvm
  *                          channel
  * @msg: pointer to the retrieved message
  *
- * Return: true if a valid message was retrieved or false otherwise
+ * Return: 0 if valid message was retrieved or -error
  */
-static bool
+static int
 read_controlvm_event(struct controlvm_message *msg)
 {
-	if (!visorchannel_signalremove(controlvm_channel,
-				       CONTROLVM_QUEUE_EVENT, msg)) {
-		/* got a message */
-		if (msg->hdr.flags.test_message == 1)
-			return false;
-		return true;
-	}
-	return false;
+	int err;
+
+	err = visorchannel_signalremove(chipset_dev->controlvm_channel,
+					CONTROLVM_QUEUE_EVENT, msg);
+	if (err)
+		return err;
+
+	/* got a message */
+	if (msg->hdr.flags.test_message == 1)
+		return -EINVAL;
+
+	return 0;
 }
 
-/**
+/*
  * parahotplug_process_list() - remove any request from the list that's been on
  *                              there too long and respond with an error
  */
@@ -1875,10 +1736,10 @@ parahotplug_process_list(void)
 
 		list_del(pos);
 		if (req->msg.hdr.flags.response_expected)
-			controlvm_respond_physdev_changestate(
+			controlvm_respond(
 				&req->msg.hdr,
 				CONTROLVM_RESP_DEVICE_UDEV_TIMEOUT,
-				req->msg.cmd.device_change_state.state);
+				&req->msg.cmd.device_change_state.state);
 		parahotplug_request_destroy(req);
 	}
 
@@ -1889,67 +1750,70 @@ static void
 controlvm_periodic_work(struct work_struct *work)
 {
 	struct controlvm_message inmsg;
-	bool got_command = false;
-	bool handle_command_failed = false;
-
-	while (!visorchannel_signalremove(controlvm_channel,
-					  CONTROLVM_QUEUE_RESPONSE,
-					  &inmsg))
-		;
-	if (!got_command) {
-		if (controlvm_pending_msg_valid) {
-			/*
-			 * we throttled processing of a prior
-			 * msg, so try to process it again
-			 * rather than reading a new one
-			 */
-			inmsg = controlvm_pending_msg;
-			controlvm_pending_msg_valid = false;
-			got_command = true;
-		} else {
-			got_command = read_controlvm_event(&inmsg);
-		}
+	int count = 0;
+	int err;
+
+	/* Drain the RESPONSE queue make it empty */
+	do {
+		err = visorchannel_signalremove(chipset_dev->controlvm_channel,
+						CONTROLVM_QUEUE_RESPONSE,
+						&inmsg);
+	} while ((!err) && (++count < CONTROLVM_MESSAGE_MAX));
+
+	if (err != -EAGAIN)
+		goto schedule_out;
+
+	if (chipset_dev->controlvm_pending_msg_valid) {
+		/*
+		 * we throttled processing of a prior
+		 * msg, so try to process it again
+		 * rather than reading a new one
+		 */
+		inmsg = chipset_dev->controlvm_pending_msg;
+		chipset_dev->controlvm_pending_msg_valid = false;
+		err = 0;
+	} else {
+		err = read_controlvm_event(&inmsg);
 	}
 
-	handle_command_failed = false;
-	while (got_command && (!handle_command_failed)) {
-		most_recent_message_jiffies = jiffies;
-		if (handle_command(inmsg,
-				   visorchannel_get_physaddr
-				   (controlvm_channel)))
-			got_command = read_controlvm_event(&inmsg);
-		else {
-			/*
-			 * this is a scenario where throttling
-			 * is required, but probably NOT an
-			 * error...; we stash the current
-			 * controlvm msg so we will attempt to
-			 * reprocess it on our next loop
-			 */
-			handle_command_failed = true;
-			controlvm_pending_msg = inmsg;
-			controlvm_pending_msg_valid = true;
+	while (!err) {
+		chipset_dev->most_recent_message_jiffies = jiffies;
+		err = handle_command(inmsg,
+				     visorchannel_get_physaddr
+				     (chipset_dev->controlvm_channel));
+		if (err == -EAGAIN) {
+			chipset_dev->controlvm_pending_msg = inmsg;
+			chipset_dev->controlvm_pending_msg_valid = true;
+			break;
 		}
+
+		err = read_controlvm_event(&inmsg);
 	}
 
 	/* parahotplug_worker */
 	parahotplug_process_list();
 
-	if (time_after(jiffies,
-		       most_recent_message_jiffies + (HZ * MIN_IDLE_SECONDS))) {
+schedule_out:
+	if (time_after(jiffies, chipset_dev->most_recent_message_jiffies +
+				(HZ * MIN_IDLE_SECONDS))) {
 		/*
 		 * it's been longer than MIN_IDLE_SECONDS since we
 		 * processed our last controlvm message; slow down the
 		 * polling
 		 */
-		if (poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_SLOW)
-			poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
+		if (chipset_dev->poll_jiffies !=
+					      POLLJIFFIES_CONTROLVMCHANNEL_SLOW)
+			chipset_dev->poll_jiffies =
+					      POLLJIFFIES_CONTROLVMCHANNEL_SLOW;
 	} else {
-		if (poll_jiffies != POLLJIFFIES_CONTROLVMCHANNEL_FAST)
-			poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+		if (chipset_dev->poll_jiffies !=
+					      POLLJIFFIES_CONTROLVMCHANNEL_FAST)
+			chipset_dev->poll_jiffies =
+					      POLLJIFFIES_CONTROLVMCHANNEL_FAST;
 	}
 
-	schedule_delayed_work(&periodic_controlvm_work, poll_jiffies);
+	schedule_delayed_work(&chipset_dev->periodic_controlvm_work,
+			      chipset_dev->poll_jiffies);
 }
 
 static int
@@ -1958,81 +1822,84 @@ visorchipset_init(struct acpi_device *acpi_device)
 	int err = -ENODEV;
 	u64 addr;
 	uuid_le uuid = SPAR_CONTROLVM_CHANNEL_PROTOCOL_UUID;
+	struct visorchannel *controlvm_channel;
+
+	chipset_dev = kzalloc(sizeof(*chipset_dev), GFP_KERNEL);
+	if (!chipset_dev)
+		goto error;
 
 	addr = controlvm_get_channel_address();
 	if (!addr)
 		goto error;
 
-	controlvm_channel = visorchannel_create_with_lock(addr, 0,
-							  GFP_KERNEL, uuid);
+	acpi_device->driver_data = chipset_dev;
+
+	chipset_dev->acpi_device = acpi_device;
+	chipset_dev->poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+	controlvm_channel = visorchannel_create_with_lock(addr,
+							  0, GFP_KERNEL, uuid);
+
 	if (!controlvm_channel)
-		goto error;
+		goto error_free_chipset_dev;
 
-	if (!SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
-				visorchannel_get_header(controlvm_channel)))
-		goto error_destroy_channel;
+	chipset_dev->controlvm_channel = controlvm_channel;
 
-	major_dev = MKDEV(visorchipset_major, 0);
-	err = visorchipset_file_init(major_dev, &controlvm_channel);
+	err = sysfs_create_groups(&chipset_dev->acpi_device->dev.kobj,
+				  visorchipset_dev_groups);
 	if (err < 0)
 		goto error_destroy_channel;
 
+	if (!SPAR_CONTROLVM_CHANNEL_OK_CLIENT(
+				visorchannel_get_header(controlvm_channel)))
+		goto error_delete_groups;
+
 	/* if booting in a crash kernel */
 	if (is_kdump_kernel())
-		INIT_DELAYED_WORK(&periodic_controlvm_work,
+		INIT_DELAYED_WORK(&chipset_dev->periodic_controlvm_work,
 				  setup_crash_devices_work_queue);
 	else
-		INIT_DELAYED_WORK(&periodic_controlvm_work,
+		INIT_DELAYED_WORK(&chipset_dev->periodic_controlvm_work,
 				  controlvm_periodic_work);
 
-	most_recent_message_jiffies = jiffies;
-	poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
-	schedule_delayed_work(&periodic_controlvm_work, poll_jiffies);
-
-	visorchipset_platform_device.dev.devt = major_dev;
-	if (platform_device_register(&visorchipset_platform_device) < 0) {
-		POSTCODE_LINUX(DEVICE_REGISTER_FAILURE_PC, 0, 0,
-			       DIAG_SEVERITY_ERR);
-		err = -ENODEV;
-		goto error_cancel_work;
-	}
-	POSTCODE_LINUX(CHIPSET_INIT_SUCCESS_PC, 0, 0, DIAG_SEVERITY_PRINT);
+	chipset_dev->most_recent_message_jiffies = jiffies;
+	chipset_dev->poll_jiffies = POLLJIFFIES_CONTROLVMCHANNEL_FAST;
+	schedule_delayed_work(&chipset_dev->periodic_controlvm_work,
+			      chipset_dev->poll_jiffies);
 
 	err = visorbus_init();
 	if (err < 0)
-		goto error_unregister;
+		goto error_cancel_work;
 
 	return 0;
 
-error_unregister:
-	platform_device_unregister(&visorchipset_platform_device);
-
 error_cancel_work:
-	cancel_delayed_work_sync(&periodic_controlvm_work);
-	visorchipset_file_cleanup(major_dev);
+	cancel_delayed_work_sync(&chipset_dev->periodic_controlvm_work);
+
+error_delete_groups:
+	sysfs_remove_groups(&chipset_dev->acpi_device->dev.kobj,
+			    visorchipset_dev_groups);
 
 error_destroy_channel:
-	visorchannel_destroy(controlvm_channel);
+	visorchannel_destroy(chipset_dev->controlvm_channel);
+
+error_free_chipset_dev:
+	kfree(chipset_dev);
 
 error:
-	POSTCODE_LINUX(CHIPSET_INIT_FAILURE_PC, 0, err, DIAG_SEVERITY_ERR);
+	dev_err(&acpi_device->dev, "failed with error %d\n", err);
 	return err;
 }
 
 static int
 visorchipset_exit(struct acpi_device *acpi_device)
 {
-	POSTCODE_LINUX(DRIVER_EXIT_PC, 0, 0, DIAG_SEVERITY_PRINT);
-
 	visorbus_exit();
+	cancel_delayed_work_sync(&chipset_dev->periodic_controlvm_work);
+	sysfs_remove_groups(&chipset_dev->acpi_device->dev.kobj,
+			    visorchipset_dev_groups);
 
-	cancel_delayed_work_sync(&periodic_controlvm_work);
-
-	visorchannel_destroy(controlvm_channel);
-
-	visorchipset_file_cleanup(visorchipset_platform_device.dev.devt);
-	platform_device_unregister(&visorchipset_platform_device);
-	POSTCODE_LINUX(DRIVER_EXIT_PC, 0, 0, DIAG_SEVERITY_PRINT);
+	visorchannel_destroy(chipset_dev->controlvm_channel);
+	kfree(chipset_dev);
 
 	return 0;
 }
@@ -2050,12 +1917,12 @@ static struct acpi_driver unisys_acpi_driver = {
 	.ops = {
 		.add = visorchipset_init,
 		.remove = visorchipset_exit,
-		},
+	},
 };
 
 MODULE_DEVICE_TABLE(acpi, unisys_device_ids);
 
-static __init uint32_t visorutil_spar_detect(void)
+static __init int visorutil_spar_detect(void)
 {
 	unsigned int eax, ebx, ecx, edx;
 
@@ -2090,10 +1957,6 @@ static void exit_unisys(void)
 	acpi_bus_unregister_driver(&unisys_acpi_driver);
 }
 
-module_param_named(major, visorchipset_major, int, 0444);
-MODULE_PARM_DESC(visorchipset_major,
-		 "major device number to use for the device node");
-
 module_init(init_unisys);
 module_exit(exit_unisys);
 
diff --git a/drivers/staging/unisys/visorbus/vmcallinterface.h b/drivers/staging/unisys/visorbus/vmcallinterface.h
index d1d72c184e29..cc70e1b16bda 100644
--- a/drivers/staging/unisys/visorbus/vmcallinterface.h
+++ b/drivers/staging/unisys/visorbus/vmcallinterface.h
@@ -12,54 +12,9 @@
  * details.
  */
 
-#ifndef __IOMONINTF_H__
-#define __IOMONINTF_H__
+#ifndef __VMCALLINTERFACE_H__
+#define __VMCALLINTERFACE_H__
 
-/*
- * This file contains all structures needed to support the VMCALLs for IO
- * Virtualization.  The VMCALLs are provided by Monitor and used by IO code
- * running on IO Partitions.
- */
-static inline unsigned long
-__unisys_vmcall_gnuc(unsigned long tuple, unsigned long reg_ebx,
-		     unsigned long reg_ecx)
-{
-	unsigned long result = 0;
-	unsigned int cpuid_eax, cpuid_ebx, cpuid_ecx, cpuid_edx;
-
-	cpuid(0x00000001, &cpuid_eax, &cpuid_ebx, &cpuid_ecx, &cpuid_edx);
-	if (!(cpuid_ecx & 0x80000000))
-		return -EPERM;
-
-	__asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
-		"a"(tuple), "b"(reg_ebx), "c"(reg_ecx));
-	return result;
-}
-
-static inline unsigned long
-__unisys_extended_vmcall_gnuc(unsigned long long tuple,
-			      unsigned long long reg_ebx,
-			      unsigned long long reg_ecx,
-			      unsigned long long reg_edx)
-{
-	unsigned long result = 0;
-	unsigned int cpuid_eax, cpuid_ebx, cpuid_ecx, cpuid_edx;
-
-	cpuid(0x00000001, &cpuid_eax, &cpuid_ebx, &cpuid_ecx, &cpuid_edx);
-	if (!(cpuid_ecx & 0x80000000))
-		return -EPERM;
-
-	__asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
-		"a"(tuple), "b"(reg_ebx), "c"(reg_ecx), "d"(reg_edx));
-	return result;
-}
-
-#ifdef VMCALL_IO_CONTROLVM_ADDR
-#undef VMCALL_IO_CONTROLVM_ADDR
-#endif	/*  */
-
-/* define subsystem number for AppOS, used in uislib driver  */
-#define MDS_APPOS 0x4000000000000000L	/* subsystem = 62 - AppOS */
 enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 	    /* Note: when a new VMCALL is added:
 	     * - the 1st 2 hex digits correspond to one of the
@@ -72,30 +27,20 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 	     *   type of VMCALL
 	     */
 	/* used by all Guests, not just IO */
-	VMCALL_IO_CONTROLVM_ADDR = 0x0501,
-	/* Allow caller to query virtual time offset */
-	VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET = 0x0708,
-	/* LOGEVENT Post Code (RDX) with specified subsystem mask */
-	/* (RCX - monitor_subsystems.h) and severity (RDX) */
-	VMCALL_POST_CODE_LOGEVENT = 0x070B,
-	/* Allow ULTRA_SERVICE_CAPABILITY_TIME capable guest to make VMCALL */
-	VMCALL_UPDATE_PHYSICAL_TIME = 0x0a02
+	VMCALL_CONTROLVM_ADDR = 0x0501,
 };
 
-#define VMCALL_SUCCESS 0
-#define VMCALL_SUCCESSFUL(result)	(result == 0)
-
-#define unisys_vmcall(tuple, reg_ebx, reg_ecx) \
-	__unisys_vmcall_gnuc(tuple, reg_ebx, reg_ecx)
-#define unisys_extended_vmcall(tuple, reg_ebx, reg_ecx, reg_edx) \
-	__unisys_extended_vmcall_gnuc(tuple, reg_ebx, reg_ecx, reg_edx)
-#define ISSUE_IO_VMCALL(method, param, result) \
-	(result = unisys_vmcall(method, (param) & 0xFFFFFFFF,	\
-				(param) >> 32))
+enum vmcall_result {
+	VMCALL_RESULT_SUCCESS = 0,
+	VMCALL_RESULT_INVALID_PARAM = 1,
+	VMCALL_RESULT_DATA_UNAVAILABLE = 2,
+	VMCALL_RESULT_FAILURE_UNAVAILABLE = 3,
+	VMCALL_RESULT_DEVICE_ERROR = 4,
+	VMCALL_RESULT_DEVICE_NOT_READY = 5
+};
 
 /* Structures for IO VMCALLs */
-
-/* Parameters to VMCALL_IO_CONTROLVM_ADDR interface */
+/* Parameters to VMCALL_CONTROLVM_ADDR interface */
 struct vmcall_io_controlvm_addr_params {
 	/* The Guest-relative physical address of the ControlVm channel. */
 	/* This VMCall fills this in with the appropriate address. */
@@ -106,72 +51,4 @@ struct vmcall_io_controlvm_addr_params {
 	u8 unused[4];		/* Unused Bytes in the 64-Bit Aligned Struct */
 } __packed;
 
-/******* INFO ON ISSUE_POSTCODE_LINUX() BELOW *******/
-enum driver_pc {		/* POSTCODE driver identifier tuples */
-	/* visorbus driver files */
-	VISOR_BUS_PC = 0xF0,
-	VISOR_BUS_PC_visorbus_main_c = 0xFF,
-	VISOR_BUS_PC_visorchipset_c = 0xFE,
-};
-
-enum event_pc {			/* POSTCODE event identifier tuples */
-	BUS_CREATE_ENTRY_PC = 0x001,
-	BUS_CREATE_FAILURE_PC = 0x002,
-	BUS_CREATE_EXIT_PC = 0x003,
-	BUS_CONFIGURE_ENTRY_PC = 0x004,
-	BUS_CONFIGURE_FAILURE_PC = 0x005,
-	BUS_CONFIGURE_EXIT_PC = 0x006,
-	CHIPSET_INIT_ENTRY_PC = 0x007,
-	CHIPSET_INIT_SUCCESS_PC = 0x008,
-	CHIPSET_INIT_FAILURE_PC = 0x009,
-	CHIPSET_INIT_EXIT_PC = 0x00A,
-	CONTROLVM_INIT_FAILURE_PC = 0x00B,
-	DEVICE_CREATE_ENTRY_PC = 0x00C,
-	DEVICE_CREATE_FAILURE_PC = 0x00D,
-	DEVICE_CREATE_SUCCESS_PC = 0x00E,
-	DEVICE_CREATE_EXIT_PC = 0x00F,
-	DEVICE_ADD_PC = 0x010,
-	DEVICE_REGISTER_FAILURE_PC = 0x011,
-	DEVICE_CHANGESTATE_FAILURE_PC = 0x012,
-	DRIVER_ENTRY_PC = 0x013,
-	DRIVER_EXIT_PC = 0x014,
-	MALLOC_FAILURE_PC = 0x015,
-	CRASH_DEV_ENTRY_PC = 0x016,
-	CRASH_DEV_EXIT_PC = 0x017,
-	CRASH_DEV_RD_BUS_FAILURE_PC = 0x018,
-	CRASH_DEV_RD_DEV_FAILURE_PC = 0x019,
-	CRASH_DEV_BUS_NULL_FAILURE_PC = 0x01A,
-	CRASH_DEV_DEV_NULL_FAILURE_PC = 0x01B,
-	CRASH_DEV_CTRL_RD_FAILURE_PC = 0x01C,
-	CRASH_DEV_COUNT_FAILURE_PC = 0x01D,
-	SAVE_MSG_BUS_FAILURE_PC = 0x01E,
-	SAVE_MSG_DEV_FAILURE_PC = 0x01F,
-};
-
-/* Write a 64-bit value to the hypervisor's log file
- * POSTCODE_LINUX generates a value in the form 0xAABBBCCCDDDDEEEE where
- *	A is an identifier for the file logging the postcode
- *	B is an identifier for the event logging the postcode
- *	C is the line logging the postcode
- *	D is additional information the caller wants to log
- *	E is additional information the caller wants to log
- * Please also note that the resulting postcode is in hex, so if you are
- * searching for the __LINE__ number, convert it first to decimal.  The line
- * number combined with driver and type of call, will allow you to track down
- * exactly what line an error occurred on, or where the last driver
- * entered/exited from.
- */
-
-#define POSTCODE_LINUX(EVENT_PC, pc16bit1, pc16bit2, severity)		\
-do {									\
-	unsigned long long post_code_temp;				\
-	post_code_temp = (((u64)CURRENT_FILE_PC) << 56) |		\
-		(((u64)EVENT_PC) << 44) |				\
-		((((u64)__LINE__) & 0xFFF) << 32) |			\
-		((((u64)pc16bit1) & 0xFFFF) << 16) |			\
-		(((u64)pc16bit2) & 0xFFFF);				\
-	unisys_extended_vmcall(VMCALL_POST_CODE_LOGEVENT, severity,     \
-			       MDS_APPOS, post_code_temp);              \
-} while (0)
-
-#endif /* __IOMONINTF_H__ */
+#endif /* __VMCALLINTERFACE_H__ */
diff --git a/drivers/staging/unisys/visorhba/visorhba_main.c b/drivers/staging/unisys/visorhba/visorhba_main.c
index 0ce92c85157c..6997b16b4dcd 100644
--- a/drivers/staging/unisys/visorhba/visorhba_main.c
+++ b/drivers/staging/unisys/visorhba/visorhba_main.c
@@ -28,9 +28,9 @@
 
 /* The Send and Receive Buffers of the IO Queue may both be full */
 
-#define IOS_ERROR_THRESHOLD	1000
-#define MAX_PENDING_REQUESTS	(MIN_NUMSIGNALS * 2)
-#define VISORHBA_ERROR_COUNT	30
+#define IOS_ERROR_THRESHOLD  1000
+#define MAX_PENDING_REQUESTS (MIN_NUMSIGNALS * 2)
+#define VISORHBA_ERROR_COUNT 30
 
 static struct dentry *visorhba_debugfs_dir;
 
@@ -101,12 +101,13 @@ struct visorhba_devices_open {
 	struct visorhba_devdata *devdata;
 };
 
-#define for_each_vdisk_match(iter, list, match)			  \
+#define for_each_vdisk_match(iter, list, match) \
 	for (iter = &list->head; iter->next; iter = iter->next) \
-		if ((iter->channel == match->channel) &&		  \
-		    (iter->id == match->id) &&			  \
+		if ((iter->channel == match->channel) && \
+		    (iter->id == match->id) && \
 		    (iter->lun == match->lun))
-/**
+
+/*
  *	visor_thread_start - starts a thread for the device
  *	@threadfn: Function the thread starts
  *	@thrcontext: Context to pass to the thread, i.e. devdata
@@ -130,7 +131,7 @@ static struct task_struct *visor_thread_start
 	return task;
 }
 
-/**
+/*
  *      visor_thread_stop - stops the thread if it is running
  */
 static void visor_thread_stop(struct task_struct *task)
@@ -140,7 +141,7 @@ static void visor_thread_stop(struct task_struct *task)
 	kthread_stop(task);
 }
 
-/**
+/*
  *	add_scsipending_entry - save off io command that is pending in
  *				Service Partition
  *	@devdata: Pointer to devdata
@@ -183,8 +184,8 @@ static int add_scsipending_entry(struct visorhba_devdata *devdata,
 	return insert_location;
 }
 
-/**
- *	del_scsipending_enty - removes an entry from the pending array
+/*
+ *	del_scsipending_ent - removes an entry from the pending array
  *	@devdata: Device holding the pending array
  *	@del: Entry to remove
  *
@@ -210,9 +211,9 @@ static void *del_scsipending_ent(struct visorhba_devdata *devdata,
 	return sent;
 }
 
-/**
+/*
  *	get_scsipending_cmdrsp - return the cmdrsp stored in a pending entry
- *	#ddata: Device holding the pending array
+ *	@ddata: Device holding the pending array
  *	@ent: Entry that stores the cmdrsp
  *
  *	Each scsipending entry has a cmdrsp in it. The cmdrsp is only valid
@@ -228,7 +229,7 @@ static struct uiscmdrsp *get_scsipending_cmdrsp(struct visorhba_devdata *ddata,
 	return NULL;
 }
 
-/**
+/*
  *      simple_idr_get - associate a provided pointer with an int value
  *                       1 <= value <= INT_MAX, and return this int value;
  *                       the pointer value can be obtained later by passing
@@ -253,7 +254,7 @@ static unsigned int simple_idr_get(struct idr *idrtable, void *p,
 	return (unsigned int)(id);  /* idr_alloc() guarantees > 0 */
 }
 
-/**
+/*
  *      setup_scsitaskmgmt_handles - stash the necessary handles so that the
  *                                   completion processing logic for a taskmgmt
  *                                   cmd will be able to find who to wake up
@@ -271,7 +272,7 @@ static void setup_scsitaskmgmt_handles(struct idr *idrtable, spinlock_t *lock,
 		simple_idr_get(idrtable, result, lock);
 }
 
-/**
+/*
  *      cleanup_scsitaskmgmt_handles - forget handles created by
  *                                     setup_scsitaskmgmt_handles()
  */
@@ -284,7 +285,7 @@ static void cleanup_scsitaskmgmt_handles(struct idr *idrtable,
 		idr_remove(idrtable, cmdrsp->scsitaskmgmt.notifyresult_handle);
 }
 
-/**
+/*
  *	forward_taskmgmt_command - send taskmegmt command to the Service
  *				   Partition
  *	@tasktype: Type of taskmgmt command
@@ -363,7 +364,7 @@ err_del_scsipending_ent:
 	return FAILED;
 }
 
-/**
+/*
  *	visorhba_abort_handler - Send TASK_MGMT_ABORT_TASK
  *	@scsicmd: The scsicmd that needs aborted
  *
@@ -388,7 +389,7 @@ static int visorhba_abort_handler(struct scsi_cmnd *scsicmd)
 	return forward_taskmgmt_command(TASK_MGMT_ABORT_TASK, scsicmd);
 }
 
-/**
+/*
  *	visorhba_device_reset_handler - Send TASK_MGMT_LUN_RESET
  *	@scsicmd: The scsicmd that needs aborted
  *
@@ -412,7 +413,7 @@ static int visorhba_device_reset_handler(struct scsi_cmnd *scsicmd)
 	return forward_taskmgmt_command(TASK_MGMT_LUN_RESET, scsicmd);
 }
 
-/**
+/*
  *	visorhba_bus_reset_handler - Send TASK_MGMT_TARGET_RESET for each
  *				     target on the bus
  *	@scsicmd: The scsicmd that needs aborted
@@ -436,7 +437,7 @@ static int visorhba_bus_reset_handler(struct scsi_cmnd *scsicmd)
 	return forward_taskmgmt_command(TASK_MGMT_BUS_RESET, scsicmd);
 }
 
-/**
+/*
  *	visorhba_host_reset_handler - Not supported
  *	@scsicmd: The scsicmd that needs aborted
  *
@@ -450,7 +451,7 @@ visorhba_host_reset_handler(struct scsi_cmnd *scsicmd)
 	return SUCCESS;
 }
 
-/**
+/*
  *	visorhba_get_info
  *	@shp: Scsi host that is requesting information
  *
@@ -462,7 +463,7 @@ static const char *visorhba_get_info(struct Scsi_Host *shp)
 	return "visorhba";
 }
 
-/**
+/*
  *	visorhba_queue_command_lck -- queues command to the Service Partition
  *	@scsicmd: Command to be queued
  *	@vsiorhba_cmnd_done: Done command to call when scsicmd is returned
@@ -553,7 +554,7 @@ static DEF_SCSI_QCMD(visorhba_queue_command)
 #define visorhba_queue_command visorhba_queue_command_lck
 #endif
 
-/**
+/*
  *	visorhba_slave_alloc - called when new disk is discovered
  *	@scsidev: New disk
  *
@@ -590,7 +591,7 @@ static int visorhba_slave_alloc(struct scsi_device *scsidev)
 	return 0;
 }
 
-/**
+/*
  *	visorhba_slave_destroy - disk is going away
  *	@scsidev: scsi device going away
  *
@@ -633,7 +634,7 @@ static struct scsi_host_template visorhba_driver_template = {
 	.use_clustering = ENABLE_CLUSTERING,
 };
 
-/**
+/*
  *	info_debugfs_show - debugfs interface to dump visorhba states
  *
  *      This presents a file in the debugfs tree named:
@@ -677,7 +678,7 @@ static const struct file_operations info_debugfs_fops = {
 	.release = single_release,
 };
 
-/**
+/*
  *	complete_taskmgmt_command - complete task management
  *	@cmdrsp: Response from the IOVM
  *
@@ -685,8 +686,8 @@ static const struct file_operations info_debugfs_fops = {
  *	command. Wake up anyone waiting for it.
  *	Returns void
  */
-static inline void complete_taskmgmt_command
-(struct idr *idrtable, struct uiscmdrsp *cmdrsp, int result)
+static void complete_taskmgmt_command(struct idr *idrtable,
+				      struct uiscmdrsp *cmdrsp, int result)
 {
 	wait_queue_head_t *wq =
 		idr_find(idrtable, cmdrsp->scsitaskmgmt.notify_handle);
@@ -706,7 +707,7 @@ static inline void complete_taskmgmt_command
 	wake_up_all(wq);
 }
 
-/**
+/*
  *	visorhba_serverdown_complete - Called when we are done cleaning up
  *				       from serverdown
  *	@work: work structure for this serverdown request
@@ -756,7 +757,7 @@ static void visorhba_serverdown_complete(struct visorhba_devdata *devdata)
 	devdata->serverchangingstate = false;
 }
 
-/**
+/*
  *	visorhba_serverdown - Got notified that the IOVM is down
  *	@devdata: visorhba that is being serviced by downed IOVM.
  *
@@ -775,7 +776,7 @@ static int visorhba_serverdown(struct visorhba_devdata *devdata)
 	return 0;
 }
 
-/**
+/*
  *	do_scsi_linuxstat - scsi command returned linuxstat
  *	@cmdrsp: response from IOVM
  *	@scsicmd: Command issued.
@@ -826,7 +827,7 @@ static int set_no_disk_inquiry_result(unsigned char *buf,
 	return 0;
 }
 
-/**
+/*
  *	do_scsi_nolinuxstat - scsi command didn't have linuxstat
  *	@cmdrsp: response from IOVM
  *	@scsicmd: Command issued.
@@ -838,7 +839,7 @@ static void
 do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 {
 	struct scsi_device *scsidev;
-	unsigned char buf[36];
+	unsigned char *buf;
 	struct scatterlist *sg;
 	unsigned int i;
 	char *this_page;
@@ -853,6 +854,10 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 		if (cmdrsp->scsi.no_disk_result == 0)
 			return;
 
+		buf = kzalloc(sizeof(char) * 36, GFP_KERNEL);
+		if (!buf)
+			return;
+
 		/* Linux scsi code wants a device at Lun 0
 		 * to issue report luns, but we don't want
 		 * a disk there so we'll present a processor
@@ -864,6 +869,7 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 		if (scsi_sg_count(scsicmd) == 0) {
 			memcpy(scsi_sglist(scsicmd), buf,
 			       cmdrsp->scsi.bufflen);
+			kfree(buf);
 			return;
 		}
 
@@ -875,6 +881,7 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 			memcpy(this_page, buf + bufind, sg[i].length);
 			kunmap_atomic(this_page_orig);
 		}
+		kfree(buf);
 	} else {
 		devdata = (struct visorhba_devdata *)scsidev->host->hostdata;
 		for_each_vdisk_match(vdisk, devdata, scsidev) {
@@ -887,7 +894,7 @@ do_scsi_nolinuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 	}
 }
 
-/**
+/*
  *	complete_scsi_command - complete a scsi command
  *	@uiscmdrsp: Response from Service Partition
  *	@scsicmd: The scsi command
@@ -909,7 +916,7 @@ complete_scsi_command(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 	scsicmd->scsi_done(scsicmd);
 }
 
-/**
+/*
  *	drain_queue - pull responses out of iochannel
  *	@cmdrsp: Response from the IOSP
  *	@devdata: device that owns this iochannel
@@ -951,7 +958,7 @@ drain_queue(struct uiscmdrsp *cmdrsp, struct visorhba_devdata *devdata)
 	}
 }
 
-/**
+/*
  *	process_incoming_rsps - Process responses from IOSP
  *	@v: void pointer to visorhba_devdata
  *
@@ -983,7 +990,7 @@ static int process_incoming_rsps(void *v)
 	return 0;
 }
 
-/**
+/*
  *	visorhba_pause - function to handle visorbus pause messages
  *	@dev: device that is pausing.
  *	@complete_func: function to call when finished
@@ -1003,7 +1010,7 @@ static int visorhba_pause(struct visor_device *dev,
 	return 0;
 }
 
-/**
+/*
  *	visorhba_resume - function called when the IO Service Partition is back
  *	@dev: device that is pausing.
  *	@complete_func: function to call when finished
@@ -1033,7 +1040,7 @@ static int visorhba_resume(struct visor_device *dev,
 	return 0;
 }
 
-/**
+/*
  *	visorhba_probe - device has been discovered, do acquire
  *	@dev: visor_device that was discovered
  *
@@ -1132,7 +1139,7 @@ err_scsi_host_put:
 	return err;
 }
 
-/**
+/*
  *	visorhba_remove - remove a visorhba device
  *	@dev: Device to remove
  *
@@ -1174,7 +1181,7 @@ static struct visor_driver visorhba_driver = {
 	.channel_interrupt = NULL,
 };
 
-/**
+/*
  *	visorhba_init		- driver init routine
  *
  *	Initialize the visorhba driver and register it with visorbus
@@ -1200,8 +1207,8 @@ cleanup_debugfs:
 	return rc;
 }
 
-/**
- *	visorhba_cleanup	- driver exit routine
+/*
+ *	visorhba_exit	- driver exit routine
  *
  *	Unregister driver from the bus and free up memory.
  */
diff --git a/drivers/staging/unisys/visorinput/visorinput.c b/drivers/staging/unisys/visorinput/visorinput.c
index 949cce680b29..cdd35437f0a0 100644
--- a/drivers/staging/unisys/visorinput/visorinput.c
+++ b/drivers/staging/unisys/visorinput/visorinput.c
@@ -33,21 +33,21 @@
 #include "ultrainputreport.h"
 
 /* Keyboard channel {c73416d0-b0b8-44af-b304-9d2ae99f1b3d} */
-#define SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID				\
-	UUID_LE(0xc73416d0, 0xb0b8, 0x44af,				\
+#define SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID \
+	UUID_LE(0xc73416d0, 0xb0b8, 0x44af, \
 		0xb3, 0x4, 0x9d, 0x2a, 0xe9, 0x9f, 0x1b, 0x3d)
 #define SPAR_KEYBOARD_CHANNEL_PROTOCOL_UUID_STR "c73416d0-b0b8-44af-b304-9d2ae99f1b3d"
 
 /* Mouse channel {addf07d4-94a9-46e2-81c3-61abcdbdbd87} */
-#define SPAR_MOUSE_CHANNEL_PROTOCOL_UUID  \
+#define SPAR_MOUSE_CHANNEL_PROTOCOL_UUID \
 	UUID_LE(0xaddf07d4, 0x94a9, 0x46e2, \
 		0x81, 0xc3, 0x61, 0xab, 0xcd, 0xbd, 0xbd, 0x87)
 #define SPAR_MOUSE_CHANNEL_PROTOCOL_UUID_STR \
 	"addf07d4-94a9-46e2-81c3-61abcdbdbd87"
 
-#define PIXELS_ACROSS_DEFAULT	800
-#define PIXELS_DOWN_DEFAULT	600
-#define KEYCODE_TABLE_BYTES	256
+#define PIXELS_ACROSS_DEFAULT 800
+#define PIXELS_DOWN_DEFAULT   600
+#define KEYCODE_TABLE_BYTES   256
 
 enum visorinput_device_type {
 	visorinput_keyboard,
@@ -539,13 +539,10 @@ handle_locking_key(struct input_dev *visorinput_dev,
 static int
 scancode_to_keycode(int scancode)
 {
-	int keycode;
-
 	if (scancode > 0xff)
-		keycode = visorkbd_ext_keycode[(scancode >> 8) & 0xff];
-	else
-		keycode = visorkbd_keycode[scancode];
-	return keycode;
+		return visorkbd_ext_keycode[(scancode >> 8) & 0xff];
+
+	return  visorkbd_keycode[scancode];
 }
 
 static int
diff --git a/drivers/staging/unisys/visornic/visornic_main.c b/drivers/staging/unisys/visornic/visornic_main.c
index 73a01a70b106..adebf224f73a 100644
--- a/drivers/staging/unisys/visornic/visornic_main.c
+++ b/drivers/staging/unisys/visornic/visornic_main.c
@@ -141,7 +141,44 @@ struct visornic_devdata {
 	struct uiscmdrsp cmdrsp[SIZEOF_CMDRSP];
 };
 
-/**
+/* Returns next non-zero index on success or 0 on failure (i.e. out of room). */
+static u16
+add_physinfo_entries(u64 inp_pfn, u16 inp_off, u32 inp_len, u16 index,
+		     u16 max_pi_arr_entries, struct phys_info pi_arr[])
+{
+	u32 len;
+	u16 i, firstlen;
+
+	firstlen = PI_PAGE_SIZE - inp_off;
+	if (inp_len <= firstlen) {
+		/* The input entry spans only one page - add as is. */
+		if (index >= max_pi_arr_entries)
+			return 0;
+		pi_arr[index].pi_pfn = inp_pfn;
+		pi_arr[index].pi_off = (u16)inp_off;
+		pi_arr[index].pi_len = (u16)inp_len;
+		return index + 1;
+	}
+
+	/* This entry spans multiple pages. */
+	for (len = inp_len, i = 0; len;
+		len -= pi_arr[index + i].pi_len, i++) {
+		if (index + i >= max_pi_arr_entries)
+			return 0;
+		pi_arr[index + i].pi_pfn = inp_pfn + i;
+		if (i == 0) {
+			pi_arr[index].pi_off = inp_off;
+			pi_arr[index].pi_len = firstlen;
+		} else {
+			pi_arr[index + i].pi_off = 0;
+			pi_arr[index + i].pi_len =
+			    (u16)MINNUM(len, (u32)PI_PAGE_SIZE);
+		}
+	}
+	return index + i;
+}
+
+/*
  *	visor_copy_fragsinfo_from_skb(
  *	@skb_in: skbuff that we are pulling the frags from
  *	@firstfraglen: length of first fragment in skb
@@ -250,7 +287,7 @@ static const struct file_operations debugfs_enable_ints_fops = {
 	.write = enable_ints_write,
 };
 
-/**
+/*
  *	visornic_serverdown_complete - IOPART went down, pause device
  *	@work: Work queue it was scheduled on
  *
@@ -285,7 +322,7 @@ visornic_serverdown_complete(struct visornic_devdata *devdata)
 	devdata->server_down_complete_func = NULL;
 }
 
-/**
+/*
  *	visornic_serverdown - Command has notified us that IOPART is down
  *	@devdata: device that is being managed by IOPART
  *
@@ -332,7 +369,7 @@ err_unlock:
 	return err;
 }
 
-/**
+/*
  *	alloc_rcv_buf	- alloc rcv buffer to be given to the IO Partition.
  *	@netdev: network adapter the rcv bufs are attached too.
  *
@@ -363,19 +400,21 @@ alloc_rcv_buf(struct net_device *netdev)
 	return skb;
 }
 
-/**
+/*
  *	post_skb	- post a skb to the IO Partition.
  *	@cmdrsp: cmdrsp packet to be send to the IO Partition
  *	@devdata: visornic_devdata to post the skb too
  *	@skb: skb to give to the IO partition
  *
  *	Send the skb to the IO Partition.
- *	Returns void
+ *	Returns 0 or error
  */
-static inline void
+static int
 post_skb(struct uiscmdrsp *cmdrsp,
 	 struct visornic_devdata *devdata, struct sk_buff *skb)
 {
+	int err;
+
 	cmdrsp->net.buf = skb;
 	cmdrsp->net.rcvpost.frag.pi_pfn = page_to_pfn(virt_to_page(skb->data));
 	cmdrsp->net.rcvpost.frag.pi_off =
@@ -383,21 +422,26 @@ post_skb(struct uiscmdrsp *cmdrsp,
 	cmdrsp->net.rcvpost.frag.pi_len = skb->len;
 	cmdrsp->net.rcvpost.unique_num = devdata->incarnation_id;
 
-	if ((cmdrsp->net.rcvpost.frag.pi_off + skb->len) <= PI_PAGE_SIZE) {
-		cmdrsp->net.type = NET_RCV_POST;
-		cmdrsp->cmdtype = CMD_NET_TYPE;
-		if (!visorchannel_signalinsert(devdata->dev->visorchannel,
-					       IOCHAN_TO_IOPART,
-					       cmdrsp)) {
-			atomic_inc(&devdata->num_rcvbuf_in_iovm);
-			devdata->chstat.sent_post++;
-		} else {
-			devdata->chstat.sent_post_failed++;
-		}
+	if ((cmdrsp->net.rcvpost.frag.pi_off + skb->len) > PI_PAGE_SIZE)
+		return -EINVAL;
+
+	cmdrsp->net.type = NET_RCV_POST;
+	cmdrsp->cmdtype = CMD_NET_TYPE;
+	err = visorchannel_signalinsert(devdata->dev->visorchannel,
+					IOCHAN_TO_IOPART,
+					cmdrsp);
+	if (err) {
+		devdata->chstat.sent_post_failed++;
+		return err;
 	}
+
+	atomic_inc(&devdata->num_rcvbuf_in_iovm);
+	devdata->chstat.sent_post++;
+
+	return 0;
 }
 
-/**
+/*
  *	send_enbdis	- send NET_RCV_ENBDIS to IO Partition
  *	@netdev: netdevice we are enable/disable, used as context
  *		 return value
@@ -405,23 +449,28 @@ post_skb(struct uiscmdrsp *cmdrsp,
  *	@devdata: visornic device we are enabling/disabling
  *
  *	Send the enable/disable message to the IO Partition.
- *	Returns void
+ *	Returns 0 or error
  */
-static void
+static int
 send_enbdis(struct net_device *netdev, int state,
 	    struct visornic_devdata *devdata)
 {
+	int err;
+
 	devdata->cmdrsp_rcv->net.enbdis.enable = state;
 	devdata->cmdrsp_rcv->net.enbdis.context = netdev;
 	devdata->cmdrsp_rcv->net.type = NET_RCV_ENBDIS;
 	devdata->cmdrsp_rcv->cmdtype = CMD_NET_TYPE;
-	if (!visorchannel_signalinsert(devdata->dev->visorchannel,
-				       IOCHAN_TO_IOPART,
-				       devdata->cmdrsp_rcv))
-		devdata->chstat.sent_enbdis++;
+	err = visorchannel_signalinsert(devdata->dev->visorchannel,
+					IOCHAN_TO_IOPART,
+					devdata->cmdrsp_rcv);
+	if (err)
+		return err;
+	devdata->chstat.sent_enbdis++;
+	return 0;
 }
 
-/**
+/*
  *	visornic_disable_with_timeout - Disable network adapter
  *	@netdev: netdevice to disable
  *	@timeout: timeout to wait for disable
@@ -439,6 +488,7 @@ visornic_disable_with_timeout(struct net_device *netdev, const int timeout)
 	int i;
 	unsigned long flags;
 	int wait = 0;
+	int err;
 
 	/* send a msg telling the other end we are stopping incoming pkts */
 	spin_lock_irqsave(&devdata->priv_lock, flags);
@@ -448,8 +498,11 @@ visornic_disable_with_timeout(struct net_device *netdev, const int timeout)
 
 	/* send disable and wait for ack -- don't hold lock when sending
 	 * disable because if the queue is full, insert might sleep.
+	 * If an error occurs, don't wait for the timeout.
 	 */
-	send_enbdis(netdev, 0, devdata);
+	err = send_enbdis(netdev, 0, devdata);
+	if (err)
+		return err;
 
 	/* wait for ack to arrive before we try to free rcv buffers
 	 * NOTE: the other end automatically unposts the rcv buffers when
@@ -507,7 +560,7 @@ visornic_disable_with_timeout(struct net_device *netdev, const int timeout)
 	return 0;
 }
 
-/**
+/*
  *	init_rcv_bufs  -- initialize receive bufs and send them to the IO Part
  *	@netdev: struct netdevice
  *	@devdata: visornic_devdata
@@ -518,7 +571,7 @@ visornic_disable_with_timeout(struct net_device *netdev, const int timeout)
 static int
 init_rcv_bufs(struct net_device *netdev, struct visornic_devdata *devdata)
 {
-	int i, count;
+	int i, j, count, err;
 
 	/* allocate fixed number of receive buffers to post to uisnic
 	 * post receive buffers after we've allocated a required amount
@@ -548,13 +601,30 @@ init_rcv_bufs(struct net_device *netdev, struct visornic_devdata *devdata)
 	 * lock - we've not enabled nor started the queue so there shouldn't
 	 * be any rcv or xmit activity
 	 */
-	for (i = 0; i < count; i++)
-		post_skb(devdata->cmdrsp_rcv, devdata, devdata->rcvbuf[i]);
+	for (i = 0; i < count; i++) {
+		err = post_skb(devdata->cmdrsp_rcv, devdata,
+			       devdata->rcvbuf[i]);
+		if (!err)
+			continue;
+
+		/* Error handling -
+		 * If we posted at least one skb, we should return success,
+		 * but need to free the resources that we have not successfully
+		 * posted.
+		 */
+		for (j = i; j < count; j++) {
+			kfree_skb(devdata->rcvbuf[j]);
+			devdata->rcvbuf[j] = NULL;
+		}
+		if (i == 0)
+			return err;
+		break;
+	}
 
 	return 0;
 }
 
-/**
+/*
  *	visornic_enable_with_timeout	- send enable to IO Part
  *	@netdev: struct net_device
  *	@timeout: Time to wait for the ACK from the enable
@@ -566,7 +636,7 @@ init_rcv_bufs(struct net_device *netdev, struct visornic_devdata *devdata)
 static int
 visornic_enable_with_timeout(struct net_device *netdev, const int timeout)
 {
-	int i;
+	int err = 0;
 	struct visornic_devdata *devdata = netdev_priv(netdev);
 	unsigned long flags;
 	int wait = 0;
@@ -576,11 +646,11 @@ visornic_enable_with_timeout(struct net_device *netdev, const int timeout)
 	/* NOTE: the other end automatically unposts the rcv buffers when it
 	 * gets a disable.
 	 */
-	i = init_rcv_bufs(netdev, devdata);
-	if (i < 0) {
+	err = init_rcv_bufs(netdev, devdata);
+	if (err < 0) {
 		dev_err(&netdev->dev,
-			"%s failed to init rcv bufs (%d)\n", __func__, i);
-		return i;
+			"%s failed to init rcv bufs\n", __func__);
+		return err;
 	}
 
 	spin_lock_irqsave(&devdata->priv_lock, flags);
@@ -594,9 +664,12 @@ visornic_enable_with_timeout(struct net_device *netdev, const int timeout)
 	spin_unlock_irqrestore(&devdata->priv_lock, flags);
 
 	/* send enable and wait for ack -- don't hold lock when sending enable
-	 * because if the queue is full, insert might sleep.
+	 * because if the queue is full, insert might sleep. If an error
+	 * occurs error out.
 	 */
-	send_enbdis(netdev, 1, devdata);
+	err = send_enbdis(netdev, 1, devdata);
+	if (err)
+		return err;
 
 	spin_lock_irqsave(&devdata->priv_lock, flags);
 	while ((timeout == VISORNIC_INFINITE_RSP_WAIT) ||
@@ -626,7 +699,7 @@ visornic_enable_with_timeout(struct net_device *netdev, const int timeout)
 	return 0;
 }
 
-/**
+/*
  *	visornic_timeout_reset	- handle xmit timeout resets
  *	@work	work item that scheduled the work
  *
@@ -669,7 +742,7 @@ call_serverdown:
 	rtnl_unlock();
 }
 
-/**
+/*
  *	visornic_open - Enable the visornic device and mark the queue started
  *	@netdev: netdevice to start
  *
@@ -684,7 +757,7 @@ visornic_open(struct net_device *netdev)
 	return 0;
 }
 
-/**
+/*
  *	visornic_close - Disables the visornic device and stops the queues
  *	@netdev: netdevice to start
  *
@@ -699,7 +772,7 @@ visornic_close(struct net_device *netdev)
 	return 0;
 }
 
-/**
+/*
  *	devdata_xmits_outstanding - compute outstanding xmits
  *	@devdata: visornic_devdata for device
  *
@@ -714,7 +787,7 @@ static unsigned long devdata_xmits_outstanding(struct visornic_devdata *devdata)
 		+ devdata->chstat.sent_xmit + 1);
 }
 
-/**
+/*
  *	vnic_hit_high_watermark
  *	@devdata: indicates visornic device we are checking
  *	@high_watermark: max num of unacked xmits we will tolerate,
@@ -723,13 +796,13 @@ static unsigned long devdata_xmits_outstanding(struct visornic_devdata *devdata)
  *      Returns true iff the number of unacked xmits sent to
  *      the IO partition is >= high_watermark.
  */
-static inline bool vnic_hit_high_watermark(struct visornic_devdata *devdata,
-					   ulong high_watermark)
+static bool vnic_hit_high_watermark(struct visornic_devdata *devdata,
+				    ulong high_watermark)
 {
 	return (devdata_xmits_outstanding(devdata) >= high_watermark);
 }
 
-/**
+/*
  *	vnic_hit_low_watermark
  *	@devdata: indicates visornic device we are checking
  *	@low_watermark: we will wait until the num of unacked xmits
@@ -739,13 +812,13 @@ static inline bool vnic_hit_high_watermark(struct visornic_devdata *devdata,
  *      Returns true iff the number of unacked xmits sent to
  *      the IO partition is <= low_watermark.
  */
-static inline bool vnic_hit_low_watermark(struct visornic_devdata *devdata,
-					  ulong low_watermark)
+static bool vnic_hit_low_watermark(struct visornic_devdata *devdata,
+				   ulong low_watermark)
 {
 	return (devdata_xmits_outstanding(devdata) <= low_watermark);
 }
 
-/**
+/*
  *	visornic_xmit - send a packet to the IO Partition
  *	@skb: Packet to be sent
  *	@netdev: net device the packet is being sent from
@@ -764,6 +837,7 @@ visornic_xmit(struct sk_buff *skb, struct net_device *netdev)
 	int len, firstfraglen, padlen;
 	struct uiscmdrsp *cmdrsp = NULL;
 	unsigned long flags;
+	int err;
 
 	devdata = netdev_priv(netdev);
 	spin_lock_irqsave(&devdata->priv_lock, flags);
@@ -880,8 +954,9 @@ visornic_xmit(struct sk_buff *skb, struct net_device *netdev)
 		return NETDEV_TX_OK;
 	}
 
-	if (visorchannel_signalinsert(devdata->dev->visorchannel,
-				      IOCHAN_TO_IOPART, cmdrsp)) {
+	err = visorchannel_signalinsert(devdata->dev->visorchannel,
+					IOCHAN_TO_IOPART, cmdrsp);
+	if (err) {
 		netif_stop_queue(netdev);
 		spin_unlock_irqrestore(&devdata->priv_lock, flags);
 		devdata->busy_cnt++;
@@ -916,7 +991,7 @@ visornic_xmit(struct sk_buff *skb, struct net_device *netdev)
 	return NETDEV_TX_OK;
 }
 
-/**
+/*
  *	visornic_get_stats - returns net_stats of the visornic device
  *	@netdev: netdevice
  *
@@ -930,7 +1005,7 @@ visornic_get_stats(struct net_device *netdev)
 	return &devdata->net_stats;
 }
 
-/**
+/*
  *	visornic_change_mtu - changes mtu of device.
  *	@netdev: netdevice
  *	@new_mtu: value of new mtu
@@ -947,7 +1022,7 @@ visornic_change_mtu(struct net_device *netdev, int new_mtu)
 	return -EINVAL;
 }
 
-/**
+/*
  *	visornic_set_multi - changes mtu of device.
  *	@netdev: netdevice
  *
@@ -959,6 +1034,7 @@ visornic_set_multi(struct net_device *netdev)
 {
 	struct uiscmdrsp *cmdrsp;
 	struct visornic_devdata *devdata = netdev_priv(netdev);
+	int err = 0;
 
 	if (devdata->old_flags == netdev->flags)
 		return;
@@ -975,16 +1051,18 @@ visornic_set_multi(struct net_device *netdev)
 	cmdrsp->net.enbdis.context = netdev;
 	cmdrsp->net.enbdis.enable =
 		netdev->flags & IFF_PROMISC;
-	visorchannel_signalinsert(devdata->dev->visorchannel,
-				  IOCHAN_TO_IOPART,
-				  cmdrsp);
+	err = visorchannel_signalinsert(devdata->dev->visorchannel,
+					IOCHAN_TO_IOPART,
+					cmdrsp);
 	kfree(cmdrsp);
+	if (err)
+		return;
 
 out_save_flags:
 	devdata->old_flags = netdev->flags;
 }
 
-/**
+/*
  *	visornic_xmit_timeout - request to timeout the xmit
  *	@netdev
  *
@@ -1019,7 +1097,7 @@ visornic_xmit_timeout(struct net_device *netdev)
 	spin_unlock_irqrestore(&devdata->priv_lock, flags);
 }
 
-/**
+/*
  *	repost_return	- repost rcv bufs that have come back
  *	@cmdrsp: io channel command struct to post
  *	@devdata: visornic devdata for the device
@@ -1030,7 +1108,7 @@ visornic_xmit_timeout(struct net_device *netdev)
  *	we are finished with them.
  *	Returns 0 for success, -1 for error.
  */
-static inline int
+static int
 repost_return(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata,
 	      struct sk_buff *skb, struct net_device *netdev)
 {
@@ -1071,7 +1149,12 @@ repost_return(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata,
 				status = -ENOMEM;
 				break;
 			}
-			post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			status = post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			if (status) {
+				kfree_skb(devdata->rcvbuf[i]);
+				devdata->rcvbuf[i] = NULL;
+				break;
+			}
 			numreposted++;
 			break;
 		}
@@ -1091,7 +1174,7 @@ repost_return(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata,
 	return status;
 }
 
-/**
+/*
  *	visornic_rx - Handle receive packets coming back from IO Part
  *	@cmdrsp: Receive packet returned from IO Part
  *
@@ -1293,7 +1376,7 @@ visornic_rx(struct uiscmdrsp *cmdrsp)
 	return 1;
 }
 
-/**
+/*
  *	devdata_initialize	- Initialize devdata structure
  *	@devdata: visornic_devdata structure to initialize
  *	#dev: visorbus_deviced it belongs to
@@ -1310,7 +1393,7 @@ devdata_initialize(struct visornic_devdata *devdata, struct visor_device *dev)
 	return devdata;
 }
 
-/**
+/*
  *	devdata_release	- Frees up references in devdata
  *	@devdata: struct to clean up
  *
@@ -1487,24 +1570,25 @@ static const struct file_operations debugfs_info_fops = {
 	.read = info_debugfs_read,
 };
 
-/**
+/*
  *	send_rcv_posts_if_needed
  *	@devdata: visornic device
  *
  *	Send receive buffers to the IO Partition.
  *	Returns void
  */
-static void
+static int
 send_rcv_posts_if_needed(struct visornic_devdata *devdata)
 {
 	int i;
 	struct net_device *netdev;
 	struct uiscmdrsp *cmdrsp = devdata->cmdrsp_rcv;
 	int cur_num_rcv_bufs_to_alloc, rcv_bufs_allocated;
+	int err;
 
 	/* don't do this until vnic is marked ready */
 	if (!(devdata->enabled && devdata->enab_dis_acked))
-		return;
+		return 0;
 
 	netdev = devdata->netdev;
 	rcv_bufs_allocated = 0;
@@ -1523,14 +1607,20 @@ send_rcv_posts_if_needed(struct visornic_devdata *devdata)
 				break;
 			}
 			rcv_bufs_allocated++;
-			post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			err = post_skb(cmdrsp, devdata, devdata->rcvbuf[i]);
+			if (err) {
+				kfree_skb(devdata->rcvbuf[i]);
+				devdata->rcvbuf[i] = NULL;
+				break;
+			}
 			devdata->chstat.extra_rcvbufs_sent++;
 		}
 	}
 	devdata->num_rcv_bufs_could_not_alloc -= rcv_bufs_allocated;
+	return 0;
 }
 
-/**
+/*
  *	drain_resp_queue  - drains and ignores all messages from the resp queue
  *	@cmdrsp: io channel command response message
  *	@devdata: visornic device to drain
@@ -1544,7 +1634,7 @@ drain_resp_queue(struct uiscmdrsp *cmdrsp, struct visornic_devdata *devdata)
 		;
 }
 
-/**
+/*
  *	service_resp_queue	- drains the response queue
  *	@cmdrsp: io channel command response message
  *	@devdata: visornic device to drain
@@ -1650,8 +1740,12 @@ static int visornic_poll(struct napi_struct *napi, int budget)
 							struct visornic_devdata,
 							napi);
 	int rx_count = 0;
+	int err;
+
+	err = send_rcv_posts_if_needed(devdata);
+	if (err)
+		return err;
 
-	send_rcv_posts_if_needed(devdata);
 	service_resp_queue(devdata->cmdrsp, devdata, &rx_count, budget);
 
 	/* If there aren't any more packets to receive stop the poll */
@@ -1661,7 +1755,7 @@ static int visornic_poll(struct napi_struct *napi, int budget)
 	return rx_count;
 }
 
-/**
+/*
  *	poll_for_irq	- Checks the status of the response queue.
  *	@v: void pointer to the visronic devdata
  *
@@ -1684,7 +1778,7 @@ poll_for_irq(unsigned long v)
 	mod_timer(&devdata->irq_poll_timer, msecs_to_jiffies(2));
 }
 
-/**
+/*
  *	visornic_probe	- probe function for visornic devices
  *	@dev: The visor device discovered
  *
@@ -1881,7 +1975,7 @@ cleanup_netdev:
 	return err;
 }
 
-/**
+/*
  *	host_side_disappeared	- IO part is gone.
  *	@devdata: device object
  *
@@ -1897,7 +1991,7 @@ static void host_side_disappeared(struct visornic_devdata *devdata)
 	spin_unlock_irqrestore(&devdata->priv_lock, flags);
 }
 
-/**
+/*
  *	visornic_remove		- Called when visornic dev goes away
  *	@dev: visornic device that is being removed
  *
@@ -1944,7 +2038,7 @@ static void visornic_remove(struct visor_device *dev)
 	free_netdev(netdev);
 }
 
-/**
+/*
  *	visornic_pause		- Called when IO Part disappears
  *	@dev: visornic device that is being serviced
  *	@complete_func: call when finished.
@@ -1966,7 +2060,7 @@ static int visornic_pause(struct visor_device *dev,
 	return 0;
 }
 
-/**
+/*
  *	visornic_resume		- Called when IO part has recovered
  *	@dev: visornic device that is being serviced
  *	@compelte_func: call when finished
@@ -2036,7 +2130,7 @@ static struct visor_driver visornic_driver = {
 	.channel_interrupt = NULL,
 };
 
-/**
+/*
  *	visornic_init	- Init function
  *
  *	Init function for the visornic driver. Do initial driver setup
@@ -2052,11 +2146,11 @@ static int visornic_init(void)
 	if (!visornic_debugfs_dir)
 		return err;
 
-	ret = debugfs_create_file("info", S_IRUSR, visornic_debugfs_dir, NULL,
+	ret = debugfs_create_file("info", 0400, visornic_debugfs_dir, NULL,
 				  &debugfs_info_fops);
 	if (!ret)
 		goto cleanup_debugfs;
-	ret = debugfs_create_file("enable_ints", S_IWUSR, visornic_debugfs_dir,
+	ret = debugfs_create_file("enable_ints", 0200, visornic_debugfs_dir,
 				  NULL, &debugfs_enable_ints_fops);
 	if (!ret)
 		goto cleanup_debugfs;
@@ -2073,7 +2167,7 @@ cleanup_debugfs:
 	return err;
 }
 
-/**
+/*
  *	visornic_cleanup	- driver exit routine
  *
  *	Unregister driver from the bus and free up memory.
diff --git a/drivers/staging/vc04_services/Kconfig b/drivers/staging/vc04_services/Kconfig
index 74094fff4367..9e2763663ab8 100644
--- a/drivers/staging/vc04_services/Kconfig
+++ b/drivers/staging/vc04_services/Kconfig
@@ -1,11 +1,39 @@
-config BCM2835_VCHIQ
-	tristate "Videocore VCHIQ"
+menuconfig BCM_VIDEOCORE
+	tristate "Broadcom VideoCore support"
 	depends on HAS_DMA
 	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"
+	help
 		Kernel to VideoCore communication interface for the
 		BCM2835 family of products.
 		Defaults to Y when the Broadcom Videocore services
 		are included in the build, N otherwise.
+
+if BCM2835_VCHIQ
+
+config BCM2835_VCHIQ_SUPPORT_MEMDUMP
+	bool "Support dumping memory contents to debug log"
+	help
+		BCM2835 VCHIQ supports the ability to dump the
+		contents of memory to the debug log.  This
+		is typically only needed by diagnostic tools used
+		to debug issues with VideoCore.
+
+endif
+
+source "drivers/staging/vc04_services/bcm2835-audio/Kconfig"
+
+source "drivers/staging/vc04_services/bcm2835-camera/Kconfig"
+
+endif
+
diff --git a/drivers/staging/vc04_services/Makefile b/drivers/staging/vc04_services/Makefile
index 1a9e742ee40d..e9a8e1343cbb 100644
--- a/drivers/staging/vc04_services/Makefile
+++ b/drivers/staging/vc04_services/Makefile
@@ -10,5 +10,8 @@ vchiq-objs := \
    interface/vchiq_arm/vchiq_util.o \
    interface/vchiq_arm/vchiq_connected.o \
 
+obj-$(CONFIG_SND_BCM2835)	+= bcm2835-audio/
+obj-$(CONFIG_VIDEO_BCM2835)	+= bcm2835-camera/
+
 ccflags-y += -DVCOS_VERIFY_BKPTS=1 -Idrivers/staging/vc04_services -DUSE_VCHIQ_ARM -D__VCCOREVER__=0x04000000
 
diff --git a/drivers/staging/vc04_services/bcm2835-audio/Kconfig b/drivers/staging/vc04_services/bcm2835-audio/Kconfig
new file mode 100644
index 000000000000..9f536533c257
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/Kconfig
@@ -0,0 +1,8 @@
+config SND_BCM2835
+        tristate "BCM2835 Audio"
+        depends on ARCH_BCM2835 && SND
+        select SND_PCM
+        select BCM2835_VCHIQ
+        help
+          Say Y or M if you want to support BCM2835 built in audio
+
diff --git a/drivers/staging/bcm2835-audio/Makefile b/drivers/staging/vc04_services/bcm2835-audio/Makefile
index d7b88d164d15..d7b88d164d15 100644
--- a/drivers/staging/bcm2835-audio/Makefile
+++ b/drivers/staging/vc04_services/bcm2835-audio/Makefile
diff --git a/drivers/staging/bcm2835-audio/TODO b/drivers/staging/vc04_services/bcm2835-audio/TODO
index 73d41fa631ac..73d41fa631ac 100644
--- a/drivers/staging/bcm2835-audio/TODO
+++ b/drivers/staging/vc04_services/bcm2835-audio/TODO
diff --git a/drivers/staging/bcm2835-audio/bcm2835-ctl.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c
index a4ffa1bf53e5..f484bb055df7 100644
--- a/drivers/staging/bcm2835-audio/bcm2835-ctl.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-ctl.c
@@ -41,7 +41,6 @@
 static int snd_bcm2835_ctl_info(struct snd_kcontrol *kcontrol,
 				struct snd_ctl_elem_info *uinfo)
 {
-	audio_info(" ... IN\n");
 	if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) {
 		uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
 		uinfo->count = 1;
@@ -58,7 +57,6 @@ static int snd_bcm2835_ctl_info(struct snd_kcontrol *kcontrol,
 		uinfo->value.integer.min = 0;
 		uinfo->value.integer.max = AUDIO_DEST_MAX - 1;
 	}
-	audio_info(" ... OUT\n");
 	return 0;
 }
 
@@ -107,7 +105,7 @@ static int snd_bcm2835_ctl_get(struct snd_kcontrol *kcontrol,
 }
 
 static int snd_bcm2835_ctl_put(struct snd_kcontrol *kcontrol,
-	struct snd_ctl_elem_value *ucontrol)
+				struct snd_ctl_elem_value *ucontrol)
 {
 	struct bcm2835_chip *chip = snd_kcontrol_chip(kcontrol);
 	int changed = 0;
@@ -116,15 +114,13 @@ static int snd_bcm2835_ctl_put(struct snd_kcontrol *kcontrol,
 		return -EINTR;
 
 	if (kcontrol->private_value == PCM_PLAYBACK_VOLUME) {
-		audio_info("Volume change attempted.. volume = %d new_volume = %d\n", chip->volume, (int) ucontrol->value.integer.value[0]);
+		audio_info("Volume change attempted.. volume = %d new_volume = %d\n", chip->volume, (int)ucontrol->value.integer.value[0]);
 		if (chip->mute == CTRL_VOL_MUTE) {
 			/* changed = toggle_mute(chip, CTRL_VOL_UNMUTE); */
 			changed = 1; /* should return 0 to signify no change but the mixer takes this as the opposite sign (no idea why) */
 			goto unlock;
 		}
-		if (changed
-			|| (ucontrol->value.integer.value[0] != chip2alsa(chip->volume))) {
-
+		if (changed || (ucontrol->value.integer.value[0] != chip2alsa(chip->volume))) {
 			chip->volume = alsa2chip(ucontrol->value.integer.value[0]);
 			changed = 1;
 		}
@@ -141,10 +137,8 @@ static int snd_bcm2835_ctl_put(struct snd_kcontrol *kcontrol,
 		}
 	}
 
-	if (changed) {
-		if (bcm2835_audio_set_ctls(chip))
-			printk(KERN_ERR "Failed to set ALSA controls..\n");
-	}
+	if (changed && bcm2835_audio_set_ctls(chip))
+		dev_err(chip->card->dev, "Failed to set ALSA controls..\n");
 
 unlock:
 	mutex_unlock(&chip->audio_mutex);
@@ -226,7 +220,7 @@ static int snd_bcm2835_spdif_default_put(struct snd_kcontrol *kcontrol,
 		return -EINTR;
 
 	for (i = 0; i < 4; i++)
-		val |= (unsigned int) ucontrol->value.iec958.status[i] << (i * 8);
+		val |= (unsigned int)ucontrol->value.iec958.status[i] << (i * 8);
 
 	change = val != chip->spdif_status;
 	chip->spdif_status = val;
@@ -246,9 +240,10 @@ static int snd_bcm2835_spdif_mask_info(struct snd_kcontrol *kcontrol,
 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 */
+	/*
+	 * 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;
 }
@@ -289,7 +284,7 @@ static int snd_bcm2835_spdif_stream_put(struct snd_kcontrol *kcontrol,
 		return -EINTR;
 
 	for (i = 0; i < 4; i++)
-		val |= (unsigned int) ucontrol->value.iec958.status[i] << (i * 8);
+		val |= (unsigned int)ucontrol->value.iec958.status[i] << (i * 8);
 	change = val != chip->spdif_status;
 	chip->spdif_status = val;
 
@@ -343,3 +338,89 @@ int snd_bcm2835_new_ctl(struct bcm2835_chip *chip)
 	}
 	return 0;
 }
+
+static struct snd_kcontrol_new snd_bcm2835_headphones_ctl[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "Headphone Playback Volume",
+		.index = 0,
+		.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,
+		.count = 1,
+		.tlv = {.p = snd_bcm2835_db_scale}
+	},
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "Headphone Playback Switch",
+		.index = 0,
+		.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,
+		.count = 1,
+	}
+};
+
+int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip)
+{
+	int err;
+	unsigned int idx;
+
+	strcpy(chip->card->mixername, "Broadcom Mixer");
+	for (idx = 0; idx < ARRAY_SIZE(snd_bcm2835_headphones_ctl); idx++) {
+		err = snd_ctl_add(chip->card,
+				  snd_ctl_new1(&snd_bcm2835_headphones_ctl[idx],
+					       chip));
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static struct snd_kcontrol_new snd_bcm2835_hdmi[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "HDMI Playback Volume",
+		.index = 0,
+		.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,
+		.count = 1,
+		.tlv = {.p = snd_bcm2835_db_scale}
+	},
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "HDMI Playback Switch",
+		.index = 0,
+		.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,
+		.count = 1,
+	}
+};
+
+int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip)
+{
+	int err;
+	unsigned int idx;
+
+	strcpy(chip->card->mixername, "Broadcom Mixer");
+	for (idx = 0; idx < ARRAY_SIZE(snd_bcm2835_hdmi); idx++) {
+		err = snd_ctl_add(chip->card,
+				  snd_ctl_new1(&snd_bcm2835_hdmi[idx], chip));
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
diff --git a/drivers/staging/bcm2835-audio/bcm2835-pcm.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
index 16127e062661..e8cf0b97bf02 100644
--- a/drivers/staging/bcm2835-audio/bcm2835-pcm.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
@@ -56,8 +56,7 @@ static struct snd_pcm_hardware snd_bcm2835_playback_spdif_hw = {
 static void snd_bcm2835_playback_free(struct snd_pcm_runtime *runtime)
 {
 	audio_info("Freeing up alsa stream here ..\n");
-	if (runtime->private_data)
-		kfree(runtime->private_data);
+	kfree(runtime->private_data);
 	runtime->private_data = NULL;
 }
 
@@ -66,7 +65,6 @@ void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream)
 	unsigned int consumed = 0;
 	int new_period = 0;
 
-	audio_info(" .. IN\n");
 
 	audio_info("alsa_stream=%p substream=%p\n", alsa_stream,
 		alsa_stream ? alsa_stream->substream : 0);
@@ -91,7 +89,7 @@ void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream)
 		frames_to_bytes(alsa_stream->substream->runtime, alsa_stream->substream->runtime->status->hw_ptr),
 		new_period);
 	if (alsa_stream->buffer_size) {
-		alsa_stream->pos += consumed &~(1 << 30);
+		alsa_stream->pos += consumed & ~(1 << 30);
 		alsa_stream->pos %= alsa_stream->buffer_size;
 	}
 
@@ -101,7 +99,6 @@ void bcm2835_playback_fifo(struct bcm2835_alsa_stream *alsa_stream)
 	} else {
 		audio_warning(" unexpected NULL substream\n");
 	}
-	audio_info(" .. OUT\n");
 }
 
 /* open callback */
@@ -114,7 +111,6 @@ static int snd_bcm2835_playback_open_generic(
 	int idx;
 	int err;
 
-	audio_info(" .. IN (%d)\n", substream->number);
 
 	if (mutex_lock_interruptible(&chip->audio_mutex)) {
 		audio_error("Interrupted whilst waiting for lock\n");
@@ -157,8 +153,6 @@ static int snd_bcm2835_playback_open_generic(
 	alsa_stream->substream = substream;
 	alsa_stream->idx = idx;
 
-	sema_init(&alsa_stream->buffers_update_sem, 0);
-	sema_init(&alsa_stream->control_sem, 0);
 	spin_lock_init(&alsa_stream->lock);
 
 	err = bcm2835_audio_open(alsa_stream);
@@ -190,7 +184,6 @@ static int snd_bcm2835_playback_open_generic(
 out:
 	mutex_unlock(&chip->audio_mutex);
 
-	audio_info(" .. OUT =%d\n", err);
 
 	return err;
 }
@@ -214,7 +207,6 @@ static int snd_bcm2835_playback_close(struct snd_pcm_substream *substream)
 	struct snd_pcm_runtime *runtime;
 	struct bcm2835_alsa_stream *alsa_stream;
 
-	audio_info(" .. IN\n");
 
 	chip = snd_pcm_substream_chip(substream);
 	if (mutex_lock_interruptible(&chip->audio_mutex)) {
@@ -255,7 +247,6 @@ static int snd_bcm2835_playback_close(struct snd_pcm_substream *substream)
 	chip->opened &= ~(1 << substream->number);
 
 	mutex_unlock(&chip->audio_mutex);
-	audio_info(" .. OUT\n");
 
 	return 0;
 }
@@ -268,7 +259,6 @@ static int snd_bcm2835_pcm_hw_params(struct snd_pcm_substream *substream,
 	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
 	int err;
 
-	audio_info(" .. IN\n");
 
 	err = snd_pcm_lib_malloc_pages(substream, params_buffer_bytes(params));
 	if (err < 0) {
@@ -280,7 +270,6 @@ static int snd_bcm2835_pcm_hw_params(struct snd_pcm_substream *substream,
 	alsa_stream->channels = params_channels(params);
 	alsa_stream->params_rate = params_rate(params);
 	alsa_stream->pcm_format_width = snd_pcm_format_width(params_format(params));
-	audio_info(" .. OUT\n");
 
 	return err;
 }
@@ -288,7 +277,6 @@ static int snd_bcm2835_pcm_hw_params(struct snd_pcm_substream *substream,
 /* hw_free callback */
 static int snd_bcm2835_pcm_hw_free(struct snd_pcm_substream *substream)
 {
-	audio_info(" .. IN\n");
 	return snd_pcm_lib_free_pages(substream);
 }
 
@@ -301,7 +289,6 @@ static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream)
 	int channels;
 	int err;
 
-	audio_info(" .. IN\n");
 
 	if (mutex_lock_interruptible(&chip->audio_mutex))
 		return -EINTR;
@@ -317,9 +304,9 @@ static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream)
 	err = bcm2835_audio_set_params(alsa_stream, channels,
 		alsa_stream->params_rate,
 		alsa_stream->pcm_format_width);
-	if (err < 0) {
+	if (err < 0)
 		audio_error(" error setting hw params\n");
-	}
+
 
 	bcm2835_audio_setup(alsa_stream);
 
@@ -342,7 +329,6 @@ static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream)
 		alsa_stream->pos, runtime->frame_bits);
 
 	mutex_unlock(&chip->audio_mutex);
-	audio_info(" .. OUT\n");
 	return 0;
 }
 
@@ -379,7 +365,6 @@ static int snd_bcm2835_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
 	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
 	int err = 0;
 
-	audio_info(" .. IN\n");
 
 	switch (cmd) {
 	case SNDRV_PCM_TRIGGER_START:
@@ -422,7 +407,6 @@ static int snd_bcm2835_pcm_trigger(struct snd_pcm_substream *substream, int cmd)
 		err = -EINVAL;
 	}
 
-	audio_info(" .. OUT\n");
 	return err;
 }
 
@@ -433,14 +417,12 @@ 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;
 
-	audio_info(" .. IN\n");
 
 	audio_debug("pcm_pointer... (%d) hwptr=%d appl=%d pos=%d\n", 0,
 		frames_to_bytes(runtime, runtime->status->hw_ptr),
 		frames_to_bytes(runtime, runtime->control->appl_ptr),
 		alsa_stream->pos);
 
-	audio_info(" .. OUT\n");
 	return snd_pcm_indirect_playback_pointer(substream,
 		&alsa_stream->pcm_indirect,
 		alsa_stream->pos);
@@ -482,18 +464,17 @@ static struct snd_pcm_ops snd_bcm2835_playback_spdif_ops = {
 };
 
 /* create a pcm device */
-int snd_bcm2835_new_pcm(struct bcm2835_chip *chip)
+int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, u32 numchannels)
 {
 	struct snd_pcm *pcm;
 	int err;
 
-	audio_info(" .. IN\n");
 	mutex_init(&chip->audio_mutex);
 	if (mutex_lock_interruptible(&chip->audio_mutex)) {
 		audio_error("Interrupted whilst waiting for lock\n");
 		return -EINTR;
 	}
-	err = snd_pcm_new(chip->card, "bcm2835 ALSA", 0, MAX_SUBSTREAMS, 0, &pcm);
+	err = snd_pcm_new(chip->card, "bcm2835 ALSA", 0, numchannels, 0, &pcm);
 	if (err < 0)
 		goto out;
 	pcm->private_data = chip;
@@ -516,7 +497,6 @@ int snd_bcm2835_new_pcm(struct bcm2835_chip *chip)
 
 out:
 	mutex_unlock(&chip->audio_mutex);
-	audio_info(" .. OUT\n");
 
 	return 0;
 }
@@ -526,7 +506,6 @@ int snd_bcm2835_new_spdif_pcm(struct bcm2835_chip *chip)
 	struct snd_pcm *pcm;
 	int err;
 
-	audio_info(" .. IN\n");
 	if (mutex_lock_interruptible(&chip->audio_mutex)) {
 		audio_error("Interrupted whilst waiting for lock\n");
 		return -EINTR;
@@ -548,7 +527,42 @@ int snd_bcm2835_new_spdif_pcm(struct bcm2835_chip *chip)
 		snd_bcm2835_playback_spdif_hw.buffer_bytes_max, snd_bcm2835_playback_spdif_hw.buffer_bytes_max);
 out:
 	mutex_unlock(&chip->audio_mutex);
-	audio_info(" .. OUT\n");
 
 	return 0;
 }
+
+int snd_bcm2835_new_simple_pcm(struct bcm2835_chip *chip,
+			       const char *name,
+			       enum snd_bcm2835_route route,
+			       u32 numchannels)
+{
+	struct snd_pcm *pcm;
+	int err;
+
+	mutex_init(&chip->audio_mutex);
+
+	err = snd_pcm_new(chip->card, name, 0, numchannels,
+			  0, &pcm);
+	if (err)
+		return err;
+
+	pcm->private_data = chip;
+	strcpy(pcm->name, name);
+	chip->pcm = pcm;
+	chip->dest = route;
+	chip->volume = alsa2chip(0);
+	chip->mute = CTRL_VOL_UNMUTE;
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_PLAYBACK,
+			&snd_bcm2835_playback_ops);
+
+	snd_pcm_lib_preallocate_pages_for_all(
+		pcm,
+		SNDRV_DMA_TYPE_CONTINUOUS,
+		snd_dma_continuous_data(GFP_KERNEL),
+		snd_bcm2835_playback_hw.buffer_bytes_max,
+		snd_bcm2835_playback_hw.buffer_bytes_max);
+
+	return 0;
+}
+
diff --git a/drivers/staging/bcm2835-audio/bcm2835-vchiq.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
index fa23a13f8d95..5f3d8f2339e3 100644
--- a/drivers/staging/bcm2835-audio/bcm2835-vchiq.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
@@ -109,7 +109,6 @@ static void my_wq_function(struct work_struct *work)
 		container_of(work, struct bcm2835_audio_work, my_work);
 	int ret = -9;
 
-	LOG_DBG(" .. IN %p:%d\n", w->alsa_stream, w->cmd);
 	switch (w->cmd) {
 	case BCM2835_AUDIO_START:
 		ret = bcm2835_audio_start_worker(w->alsa_stream);
@@ -125,86 +124,81 @@ static void my_wq_function(struct work_struct *work)
 		LOG_ERR(" Unexpected work: %p:%d\n", w->alsa_stream, w->cmd);
 		break;
 	}
-	kfree((void *) work);
-	LOG_DBG(" .. OUT %d\n", ret);
+	kfree((void *)work);
 }
 
 int bcm2835_audio_start(struct bcm2835_alsa_stream *alsa_stream)
 {
-	int ret = -1;
-
-	LOG_DBG(" .. IN\n");
 	if (alsa_stream->my_wq) {
 		struct bcm2835_audio_work *work;
 
 		work = kmalloc(sizeof(*work), GFP_ATOMIC);
 		/*--- Queue some work (item 1) ---*/
-		if (work) {
-			INIT_WORK(&work->my_work, my_wq_function);
-			work->alsa_stream = alsa_stream;
-			work->cmd = BCM2835_AUDIO_START;
-			if (queue_work(alsa_stream->my_wq, &work->my_work))
-				ret = 0;
-		} else
+		if (!work) {
 			LOG_ERR(" .. Error: NULL work kmalloc\n");
+			return -ENOMEM;
+		}
+		INIT_WORK(&work->my_work, my_wq_function);
+		work->alsa_stream = alsa_stream;
+		work->cmd = BCM2835_AUDIO_START;
+		if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+			kfree(work);
+			return -EBUSY;
+		}
 	}
-	LOG_DBG(" .. OUT %d\n", ret);
-	return ret;
+	return 0;
 }
 
 int bcm2835_audio_stop(struct bcm2835_alsa_stream *alsa_stream)
 {
-	int ret = -1;
-
-	LOG_DBG(" .. IN\n");
 	if (alsa_stream->my_wq) {
 		struct bcm2835_audio_work *work;
 
 		work = kmalloc(sizeof(*work), GFP_ATOMIC);
 		/*--- Queue some work (item 1) ---*/
-		if (work) {
-			INIT_WORK(&work->my_work, my_wq_function);
-			work->alsa_stream = alsa_stream;
-			work->cmd = BCM2835_AUDIO_STOP;
-			if (queue_work(alsa_stream->my_wq, &work->my_work))
-				ret = 0;
-		} else
+		if (!work) {
 			LOG_ERR(" .. Error: NULL work kmalloc\n");
+			return -ENOMEM;
+		}
+		INIT_WORK(&work->my_work, my_wq_function);
+		work->alsa_stream = alsa_stream;
+		work->cmd = BCM2835_AUDIO_STOP;
+		if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+			kfree(work);
+			return -EBUSY;
+		}
 	}
-	LOG_DBG(" .. OUT %d\n", ret);
-	return ret;
+	return 0;
 }
 
 int bcm2835_audio_write(struct bcm2835_alsa_stream *alsa_stream,
 			unsigned int count, void *src)
 {
-	int ret = -1;
-
-	LOG_DBG(" .. IN\n");
 	if (alsa_stream->my_wq) {
 		struct bcm2835_audio_work *work;
 
 		work = kmalloc(sizeof(*work), GFP_ATOMIC);
 		/*--- Queue some work (item 1) ---*/
-		if (work) {
-			INIT_WORK(&work->my_work, my_wq_function);
-			work->alsa_stream = alsa_stream;
-			work->cmd = BCM2835_AUDIO_WRITE;
-			work->src = src;
-			work->count = count;
-			if (queue_work(alsa_stream->my_wq, &work->my_work))
-				ret = 0;
-		} else
+		if (!work) {
 			LOG_ERR(" .. Error: NULL work kmalloc\n");
+			return -ENOMEM;
+		}
+		INIT_WORK(&work->my_work, my_wq_function);
+		work->alsa_stream = alsa_stream;
+		work->cmd = BCM2835_AUDIO_WRITE;
+		work->src = src;
+		work->count = count;
+		if (!queue_work(alsa_stream->my_wq, &work->my_work)) {
+			kfree(work);
+			return -EBUSY;
+		}
 	}
-	LOG_DBG(" .. OUT %d\n", ret);
-	return ret;
+	return 0;
 }
 
 static void my_workqueue_init(struct bcm2835_alsa_stream *alsa_stream)
 {
 	alsa_stream->my_wq = alloc_workqueue("my_queue", WQ_HIGHPRI, 1);
-	return;
 }
 
 static void my_workqueue_quit(struct bcm2835_alsa_stream *alsa_stream)
@@ -214,7 +208,6 @@ static void my_workqueue_quit(struct bcm2835_alsa_stream *alsa_stream)
 		destroy_workqueue(alsa_stream->my_wq);
 		alsa_stream->my_wq = NULL;
 	}
-	return;
 }
 
 static void audio_vchi_callback(void *param,
@@ -226,12 +219,9 @@ static void audio_vchi_callback(void *param,
 	int msg_len;
 	struct vc_audio_msg m;
 
-	LOG_DBG(" .. IN instance=%p, handle=%p, alsa=%p, reason=%d, handle=%p\n",
-		instance, instance ? instance->vchi_handle[0] : NULL, instance ? instance->alsa_stream : NULL, reason, msg_handle);
-
-	if (reason != VCHI_CALLBACK_MSG_AVAILABLE) {
+	if (reason != VCHI_CALLBACK_MSG_AVAILABLE)
 		return;
-	}
+
 	if (!instance) {
 		LOG_ERR(" .. instance is null\n");
 		BUG();
@@ -268,7 +258,6 @@ static void audio_vchi_callback(void *param,
 	} else {
 		LOG_ERR(" .. unexpected m.type=%d\n", m.type);
 	}
-	LOG_DBG(" .. OUT\n");
 }
 
 static struct bcm2835_audio_instance *
@@ -279,6 +268,7 @@ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_instance,
 	unsigned int i;
 	struct bcm2835_audio_instance *instance;
 	int status;
+	int ret;
 
 	LOG_DBG("%s: start", __func__);
 
@@ -286,14 +276,13 @@ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_instance,
 		LOG_ERR("%s: unsupported number of connections %u (max=%u)\n",
 			__func__, num_connections, VCHI_MAX_NUM_CONNECTIONS);
 
-		return NULL;
+		return ERR_PTR(-EINVAL);
 	}
 	/* Allocate memory for this instance */
-	instance = kmalloc(sizeof(*instance), GFP_KERNEL);
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
 	if (!instance)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
-	memset(instance, 0, sizeof(*instance));
 	instance->num_connections = num_connections;
 
 	/* Create a lock for exclusive, serialized VCHI connection access */
@@ -301,26 +290,27 @@ vc_vchi_audio_init(VCHI_INSTANCE_T vchi_instance,
 	/* Open the VCHI service connections */
 	for (i = 0; i < num_connections; i++) {
 		SERVICE_CREATION_T params = {
-			VCHI_VERSION_EX(VC_AUDIOSERV_VER, VC_AUDIOSERV_MIN_VER),
-			VC_AUDIO_SERVER_NAME, // 4cc service code
-			vchi_connections[i], // passed in fn pointers
-			0, // rx fifo size (unused)
-			0, // tx fifo size (unused)
-			audio_vchi_callback, // service callback
-			instance, // service callback parameter
-			1, //TODO: remove VCOS_FALSE,   // unaligned bulk recieves
-			1, //TODO: remove VCOS_FALSE,   // unaligned bulk transmits
-			0 // want crc check on bulk transfers
+			.version		= VCHI_VERSION_EX(VC_AUDIOSERV_VER, VC_AUDIOSERV_MIN_VER),
+			.service_id		= VC_AUDIO_SERVER_NAME,
+			.connection		= vchi_connections[i],
+			.rx_fifo_size		= 0,
+			.tx_fifo_size		= 0,
+			.callback		= audio_vchi_callback,
+			.callback_param		= instance,
+			.want_unaligned_bulk_rx = 1, //TODO: remove VCOS_FALSE
+			.want_unaligned_bulk_tx = 1, //TODO: remove VCOS_FALSE
+			.want_crc		= 0
 		};
 
 		LOG_DBG("%s: about to open %i\n", __func__, i);
 		status = vchi_service_open(vchi_instance, &params,
-			&instance->vchi_handle[i]);
+					   &instance->vchi_handle[i]);
+
 		LOG_DBG("%s: opened %i: %p=%d\n", __func__, i, instance->vchi_handle[i], status);
 		if (status) {
 			LOG_ERR("%s: failed to open VCHI service connection (status=%d)\n",
 				__func__, status);
-
+			ret = -EPERM;
 			goto err_close_services;
 		}
 		/* Finished with the service for now */
@@ -340,14 +330,13 @@ err_close_services:
 	kfree(instance);
 	LOG_ERR("%s: error\n", __func__);
 
-	return NULL;
+	return ERR_PTR(ret);
 }
 
 static int vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
 {
 	unsigned int i;
 
-	LOG_DBG(" .. IN\n");
 
 	if (!instance) {
 		LOG_ERR("%s: invalid handle %p\n", __func__, instance);
@@ -380,7 +369,6 @@ static int vc_vchi_audio_deinit(struct bcm2835_audio_instance *instance)
 
 	kfree(instance);
 
-	LOG_DBG(" .. OUT\n");
 
 	return 0;
 }
@@ -394,7 +382,6 @@ static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream
 		(struct bcm2835_audio_instance *)alsa_stream->instance;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	LOG_INFO("%s: start\n", __func__);
 	BUG_ON(instance);
@@ -431,10 +418,10 @@ static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream
 	/* Initialize an instance of the audio service */
 	instance = vc_vchi_audio_init(vchi_instance, &vchi_connection, 1);
 
-	if (!instance) {
+	if (IS_ERR(instance)) {
 		LOG_ERR("%s: failed to initialize audio service\n", __func__);
 
-		ret = -EPERM;
+		ret = PTR_ERR(instance);
 		goto err_free_mem;
 	}
 
@@ -444,7 +431,7 @@ static int bcm2835_audio_open_connection(struct bcm2835_alsa_stream *alsa_stream
 	LOG_DBG(" success !\n");
 	ret = 0;
 err_free_mem:
-	LOG_DBG(" .. OUT\n");
+	kfree(vchi_instance);
 
 	return ret;
 }
@@ -456,7 +443,6 @@ int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream)
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	my_workqueue_init(alsa_stream);
 
@@ -494,7 +480,6 @@ unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
 exit:
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -506,7 +491,6 @@ static int bcm2835_audio_set_ctls_chan(struct bcm2835_alsa_stream *alsa_stream,
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	LOG_INFO(" Setting ALSA dest(%d), volume(%d)\n",
 		 chip->dest, chip->volume);
@@ -555,7 +539,6 @@ unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
 
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -564,7 +547,6 @@ int bcm2835_audio_set_ctls(struct bcm2835_chip *chip)
 	int i;
 	int ret = 0;
 
-	LOG_DBG(" .. IN\n");
 	LOG_DBG(" Setting ALSA dest(%d), volume(%d)\n", chip->dest, chip->volume);
 
 	/* change ctls for all substreams */
@@ -581,7 +563,6 @@ int bcm2835_audio_set_ctls(struct bcm2835_chip *chip)
 			}
 		}
 	}
-	LOG_DBG(" .. OUT ret=%d\n", ret);
 	return ret;
 }
 
@@ -594,10 +575,9 @@ int bcm2835_audio_set_params(struct bcm2835_alsa_stream *alsa_stream,
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	LOG_INFO(" Setting ALSA channels(%d), samplerate(%d), bits-per-sample(%d)\n",
-		channels, samplerate, bps);
+		 channels, samplerate, bps);
 
 	/* resend ctls - alsa_stream may not have been open when first send */
 	ret = bcm2835_audio_set_ctls_chan(alsa_stream, alsa_stream->chip);
@@ -650,15 +630,12 @@ unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
 
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
 int bcm2835_audio_setup(struct bcm2835_alsa_stream *alsa_stream)
 {
-	LOG_DBG(" .. IN\n");
 
-	LOG_DBG(" .. OUT\n");
 
 	return 0;
 }
@@ -670,7 +647,6 @@ static int bcm2835_audio_start_worker(struct bcm2835_alsa_stream *alsa_stream)
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	if (mutex_lock_interruptible(&instance->vchi_mutex)) {
 		LOG_DBG("Interrupted whilst waiting for lock on (%d)\n",
@@ -698,7 +674,6 @@ static int bcm2835_audio_start_worker(struct bcm2835_alsa_stream *alsa_stream)
 unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -709,7 +684,6 @@ static int bcm2835_audio_stop_worker(struct bcm2835_alsa_stream *alsa_stream)
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	if (mutex_lock_interruptible(&instance->vchi_mutex)) {
 		LOG_DBG("Interrupted whilst waiting for lock on (%d)\n",
@@ -738,7 +712,6 @@ static int bcm2835_audio_stop_worker(struct bcm2835_alsa_stream *alsa_stream)
 unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -749,7 +722,6 @@ int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream)
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	my_workqueue_quit(alsa_stream);
 
@@ -797,7 +769,6 @@ unlock:
 	vc_vchi_audio_deinit(instance);
 	alsa_stream->instance = NULL;
 
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -809,7 +780,6 @@ static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
 	int status;
 	int ret;
 
-	LOG_DBG(" .. IN\n");
 
 	LOG_INFO(" Writing %d bytes from %p\n", count, src);
 
@@ -820,9 +790,10 @@ static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
 	}
 	vchi_service_use(instance->vchi_handle[0]);
 
-	if (instance->peer_version == 0 && vchi_get_peer_version(instance->vchi_handle[0], &instance->peer_version) == 0) {
+	if (instance->peer_version == 0 &&
+	    vchi_get_peer_version(instance->vchi_handle[0], &instance->peer_version) == 0)
 		LOG_DBG("%s: client version %d connected\n", __func__, instance->peer_version);
-	}
+
 	m.type = VC_AUDIO_MSG_TYPE_WRITE;
 	m.u.write.count = count;
 	// old version uses bulk, new version uses control
@@ -846,16 +817,14 @@ static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
 		if (!m.u.write.max_packet) {
 			/* Send the message to the videocore */
 			status = vchi_bulk_queue_transmit(instance->vchi_handle[0],
-				src, count,
-				0 *
-				VCHI_FLAGS_BLOCK_UNTIL_QUEUED
-				+
-				1 *
-				VCHI_FLAGS_BLOCK_UNTIL_DATA_READ,
-				NULL);
+							  src, count,
+							  0 * VCHI_FLAGS_BLOCK_UNTIL_QUEUED
+							  +
+							  1 * VCHI_FLAGS_BLOCK_UNTIL_DATA_READ,
+							  NULL);
 		} else {
 			while (count > 0) {
-				int bytes = min((int) m.u.write.max_packet, (int) count);
+				int bytes = min_t(int, m.u.write.max_packet, count);
 
 				status = bcm2835_vchi_msg_queue(instance->vchi_handle[0],
 								src, bytes);
@@ -876,7 +845,6 @@ static int bcm2835_audio_write_worker(struct bcm2835_alsa_stream *alsa_stream,
 unlock:
 	vchi_service_release(instance->vchi_handle[0]);
 	mutex_unlock(&instance->vchi_mutex);
-	LOG_DBG(" .. OUT\n");
 	return ret;
 }
 
@@ -885,9 +853,6 @@ unlock:
  */
 void bcm2835_audio_flush_buffers(struct bcm2835_alsa_stream *alsa_stream)
 {
-	LOG_DBG(" .. IN\n");
-	LOG_DBG(" .. OUT\n");
-	return;
 }
 
 /**
@@ -896,8 +861,6 @@ void bcm2835_audio_flush_buffers(struct bcm2835_alsa_stream *alsa_stream)
  */
 void bcm2835_audio_flush_playback_buffers(struct bcm2835_alsa_stream *alsa_stream)
 {
-	LOG_DBG(" .. IN\n");
-	LOG_DBG(" .. OUT\n");
 }
 
 unsigned int bcm2835_audio_retrieve_buffers(struct bcm2835_alsa_stream *alsa_stream)
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 000000000000..8f2d508183b2
--- /dev/null
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
@@ -0,0 +1,472 @@
+/*****************************************************************************
+ * Copyright 2011 Broadcom Corporation.  All rights reserved.
+ *
+ * Unless you and Broadcom execute a separate written software license
+ * agreement governing use of this software, this software is licensed to you
+ * under the terms of the GNU General Public License version 2, available at
+ * http://www.broadcom.com/licenses/GPLv2.php (the "GPL").
+ *
+ * Notwithstanding the above, under no circumstances may you combine this
+ * software in any way with any other Broadcom software provided under a
+ * license other than the GPL, without Broadcom's express prior written
+ * consent.
+ *****************************************************************************/
+
+#include <linux/platform_device.h>
+
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#include "bcm2835.h"
+
+static bool enable_hdmi;
+static bool enable_headphones;
+static bool enable_compat_alsa = true;
+
+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(enable_compat_alsa, bool, 0444);
+MODULE_PARM_DESC(enable_compat_alsa,
+		 "Enables ALSA compatibility virtual audio device");
+
+static void snd_devm_unregister_child(struct device *dev, void *res)
+{
+	struct device *childdev = *(struct device **)res;
+
+	device_unregister(childdev);
+}
+
+static int snd_devm_add_child(struct device *dev, struct device *child)
+{
+	struct device **dr;
+	int ret;
+
+	dr = devres_alloc(snd_devm_unregister_child, sizeof(*dr), GFP_KERNEL);
+	if (!dr)
+		return -ENOMEM;
+
+	ret = device_add(child);
+	if (ret) {
+		devres_free(dr);
+		return ret;
+	}
+
+	*dr = child;
+	devres_add(dev, dr);
+
+	return 0;
+}
+
+static struct device *
+snd_create_device(struct device *parent,
+		  struct device_driver *driver,
+		  const char *name)
+{
+	struct device *device;
+	int ret;
+
+	device = devm_kzalloc(parent, sizeof(*device), GFP_KERNEL);
+	if (!device)
+		return ERR_PTR(-ENOMEM);
+
+	device_initialize(device);
+	device->parent = parent;
+	device->driver = driver;
+
+	dev_set_name(device, "%s", name);
+
+	ret = snd_devm_add_child(parent, device);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return device;
+}
+
+static int snd_bcm2835_free(struct bcm2835_chip *chip)
+{
+	kfree(chip);
+	return 0;
+}
+
+/* component-destructor
+ * (see "Management of Cards and Components")
+ */
+static int snd_bcm2835_dev_free(struct snd_device *device)
+{
+	return snd_bcm2835_free(device->device_data);
+}
+
+/* chip-specific constructor
+ * (see "Management of Cards and Components")
+ */
+static int snd_bcm2835_create(struct snd_card *card,
+			      struct bcm2835_chip **rchip)
+{
+	struct bcm2835_chip *chip;
+	int err;
+	static struct snd_device_ops ops = {
+		.dev_free = snd_bcm2835_dev_free,
+	};
+
+	*rchip = NULL;
+
+	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->card = card;
+
+	err = snd_device_new(card, SNDRV_DEV_LOWLEVEL, chip, &ops);
+	if (err) {
+		snd_bcm2835_free(chip);
+		return err;
+	}
+
+	*rchip = chip;
+	return 0;
+}
+
+static void snd_devm_card_free(struct device *dev, void *res)
+{
+	struct snd_card *snd_card = *(struct snd_card **)res;
+
+	snd_card_free(snd_card);
+}
+
+static struct snd_card *snd_devm_card_new(struct device *dev)
+{
+	struct snd_card **dr;
+	struct snd_card *card;
+	int ret;
+
+	dr = devres_alloc(snd_devm_card_free, sizeof(*dr), GFP_KERNEL);
+	if (!dr)
+		return ERR_PTR(-ENOMEM);
+
+	ret = snd_card_new(dev, -1, NULL, THIS_MODULE, 0, &card);
+	if (ret) {
+		devres_free(dr);
+		return ERR_PTR(ret);
+	}
+
+	*dr = card;
+	devres_add(dev, dr);
+
+	return card;
+}
+
+typedef int (*bcm2835_audio_newpcm_func)(struct bcm2835_chip *chip,
+					 const char *name,
+					 enum snd_bcm2835_route route,
+					 u32 numchannels);
+
+typedef int (*bcm2835_audio_newctl_func)(struct bcm2835_chip *chip);
+
+struct bcm2835_audio_driver {
+	struct device_driver driver;
+	const char *shortname;
+	const char *longname;
+	int minchannels;
+	bcm2835_audio_newpcm_func newpcm;
+	bcm2835_audio_newctl_func newctl;
+	enum snd_bcm2835_route route;
+};
+
+static int bcm2835_audio_alsa_newpcm(struct bcm2835_chip *chip,
+				     const char *name,
+				     enum snd_bcm2835_route route,
+				     u32 numchannels)
+{
+	int err;
+
+	err = snd_bcm2835_new_pcm(chip, numchannels - 1);
+	if (err)
+		return err;
+
+	err = snd_bcm2835_new_spdif_pcm(chip);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static struct bcm2835_audio_driver bcm2835_audio_alsa = {
+	.driver = {
+		.name = "bcm2835_alsa",
+		.owner = THIS_MODULE,
+	},
+	.shortname = "bcm2835 ALSA",
+	.longname  = "bcm2835 ALSA",
+	.minchannels = 2,
+	.newpcm = bcm2835_audio_alsa_newpcm,
+	.newctl = snd_bcm2835_new_ctl,
+};
+
+static struct bcm2835_audio_driver bcm2835_audio_hdmi = {
+	.driver = {
+		.name = "bcm2835_hdmi",
+		.owner = THIS_MODULE,
+	},
+	.shortname = "bcm2835 HDMI",
+	.longname  = "bcm2835 HDMI",
+	.minchannels = 1,
+	.newpcm = snd_bcm2835_new_simple_pcm,
+	.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 = snd_bcm2835_new_simple_pcm,
+	.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_alsa,
+		.is_enabled = &enable_compat_alsa,
+	},
+	{
+		.audio_driver = &bcm2835_audio_hdmi,
+		.is_enabled = &enable_hdmi,
+	},
+	{
+		.audio_driver = &bcm2835_audio_headphones,
+		.is_enabled = &enable_headphones,
+	},
+};
+
+static int snd_add_child_device(struct device *device,
+				struct bcm2835_audio_driver *audio_driver,
+				u32 numchans)
+{
+	struct snd_card *card;
+	struct device *child;
+	struct bcm2835_chip *chip;
+	int err, i;
+
+	child = snd_create_device(device, &audio_driver->driver,
+				  audio_driver->driver.name);
+	if (IS_ERR(child)) {
+		dev_err(device,
+			"Unable to create child device %p, error %ld",
+			audio_driver->driver.name,
+			PTR_ERR(child));
+		return PTR_ERR(child);
+	}
+
+	card = snd_devm_card_new(child);
+	if (IS_ERR(card)) {
+		dev_err(child, "Failed to create card");
+		return PTR_ERR(card);
+	}
+
+	snd_card_set_dev(card, child);
+	strcpy(card->driver, audio_driver->driver.name);
+	strcpy(card->shortname, audio_driver->shortname);
+	strcpy(card->longname, audio_driver->longname);
+
+	err = snd_bcm2835_create(card, &chip);
+	if (err) {
+		dev_err(child, "Failed to create chip, error %d\n", err);
+		return err;
+	}
+
+	chip->dev = child;
+
+	err = audio_driver->newpcm(chip, audio_driver->shortname,
+		audio_driver->route,
+		numchans);
+	if (err) {
+		dev_err(child, "Failed to create pcm, error %d\n", err);
+		return err;
+	}
+
+	err = audio_driver->newctl(chip);
+	if (err) {
+		dev_err(child, "Failed to create controls, error %d\n", err);
+		return err;
+	}
+
+	for (i = 0; i < numchans; i++)
+		chip->avail_substreams |= (1 << i);
+
+	err = snd_card_register(card);
+	if (err) {
+		dev_err(child, "Failed to register card, error %d\n", err);
+		return err;
+	}
+
+	dev_set_drvdata(child, card);
+	dev_info(child, "card created with %d channels\n", numchans);
+
+	return 0;
+}
+
+static int snd_add_child_devices(struct device *device, u32 numchans)
+{
+	int i;
+	int count_devices = 0;
+	int minchannels = 0;
+	int extrachannels = 0;
+	int extrachannels_per_driver = 0;
+	int extrachannels_remainder = 0;
+
+	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++) {
+		int err;
+		int numchannels_this_device;
+		struct bcm2835_audio_driver *audio_driver;
+
+		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_dt(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	u32 numchans;
+	int err;
+
+	err = of_property_read_u32(dev->of_node, "brcm,pwm-channels",
+				   &numchans);
+	if (err) {
+		dev_err(dev, "Failed to get DT property 'brcm,pwm-channels'");
+		return err;
+	}
+
+	if (numchans == 0 || numchans > MAX_SUBSTREAMS) {
+		numchans = MAX_SUBSTREAMS;
+		dev_warn(dev,
+			 "Illegal 'brcm,pwm-channels' value, will use %u\n",
+			 numchans);
+	}
+
+	err = snd_add_child_devices(dev, numchans);
+	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 const struct of_device_id snd_bcm2835_of_match_table[] = {
+	{ .compatible = "brcm,bcm2835-audio",},
+	{},
+};
+MODULE_DEVICE_TABLE(of, snd_bcm2835_of_match_table);
+
+static struct platform_driver bcm2835_alsa0_driver = {
+	.probe = snd_bcm2835_alsa_probe_dt,
+#ifdef CONFIG_PM
+	.suspend = snd_bcm2835_alsa_suspend,
+	.resume = snd_bcm2835_alsa_resume,
+#endif
+	.driver = {
+		.name = "bcm2835_audio",
+		.owner = THIS_MODULE,
+		.of_match_table = snd_bcm2835_of_match_table,
+	},
+};
+
+static int bcm2835_alsa_device_init(void)
+{
+	int retval;
+
+	retval = platform_driver_register(&bcm2835_alsa0_driver);
+	if (retval)
+		pr_err("Error registering bcm2835_audio driver %d .\n", retval);
+
+	return retval;
+}
+
+static void bcm2835_alsa_device_exit(void)
+{
+	platform_driver_unregister(&bcm2835_alsa0_driver);
+}
+
+late_initcall(bcm2835_alsa_device_init);
+module_exit(bcm2835_alsa_device_exit);
+
+MODULE_AUTHOR("Dom Cobley");
+MODULE_DESCRIPTION("Alsa driver for BCM2835 chip");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/bcm2835-audio/bcm2835.h b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
index 36e3ef80e60c..379604d3554e 100644
--- a/drivers/staging/bcm2835-audio/bcm2835.h
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
@@ -27,8 +27,8 @@
 #include <linux/workqueue.h>
 
 /*
-#define AUDIO_DEBUG_ENABLE
-#define AUDIO_VERBOSE_DEBUG_ENABLE
+ * #define AUDIO_DEBUG_ENABLE
+ * #define AUDIO_VERBOSE_DEBUG_ENABLE
  */
 
 /* Debug macros */
@@ -37,10 +37,10 @@
 #ifdef AUDIO_VERBOSE_DEBUG_ENABLE
 
 #define audio_debug(fmt, arg...) \
-	printk(KERN_INFO"%s:%d " fmt, __func__, __LINE__, ##arg)
+	pr_info("%s:%d " fmt, __func__, __LINE__, ##arg)
 
 #define audio_info(fmt, arg...) \
-	printk(KERN_INFO"%s:%d " fmt, __func__, __LINE__, ##arg)
+	pr_info("%s:%d " fmt, __func__, __LINE__, ##arg)
 
 #else
 
@@ -59,13 +59,13 @@
 #endif /* AUDIO_DEBUG_ENABLE */
 
 #define audio_error(fmt, arg...) \
-	printk(KERN_ERR"%s:%d " fmt, __func__, __LINE__, ##arg)
+	pr_err("%s:%d " fmt, __func__, __LINE__, ##arg)
 
 #define audio_warning(fmt, arg...) \
-	printk(KERN_WARNING"%s:%d " fmt, __func__, __LINE__, ##arg)
+	pr_warn("%s:%d " fmt, __func__, __LINE__, ##arg)
 
 #define audio_alert(fmt, arg...) \
-	printk(KERN_ALERT"%s:%d " fmt, __func__, __LINE__, ##arg)
+	pr_alert("%s:%d " fmt, __func__, __LINE__, ##arg)
 
 #define MAX_SUBSTREAMS   (8)
 #define AVAIL_SUBSTREAMS_MASK  (0xff)
@@ -77,8 +77,11 @@ enum {
 
 /* macros for alsa2chip and chip2alsa, instead of functions */
 
-#define alsa2chip(vol) (uint)(-((vol << 8) / 100)) /* convert alsa to chip volume (defined as macro rather than function call) */
-#define chip2alsa(vol) -((vol * 100) >> 8)   /* convert chip to alsa volume */
+// 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)
 
 /* Some constants for values .. */
 enum snd_bcm2835_route {
@@ -101,7 +104,7 @@ struct bcm2835_chip {
 	struct snd_pcm *pcm_spdif;
 	/* Bitmat for valid reg_base and irq numbers */
 	unsigned int avail_substreams;
-	struct platform_device *pdev[MAX_SUBSTREAMS];
+	struct device *dev;
 	struct bcm2835_alsa_stream *alsa_stream[MAX_SUBSTREAMS];
 
 	int volume;
@@ -119,8 +122,6 @@ struct bcm2835_alsa_stream {
 	struct snd_pcm_substream *substream;
 	struct snd_pcm_indirect pcm_indirect;
 
-	struct semaphore buffers_update_sem;
-	struct semaphore control_sem;
 	spinlock_t lock;
 	volatile unsigned int control;
 	volatile unsigned int status;
@@ -144,8 +145,15 @@ struct bcm2835_alsa_stream {
 };
 
 int snd_bcm2835_new_ctl(struct bcm2835_chip *chip);
-int snd_bcm2835_new_pcm(struct bcm2835_chip *chip);
+int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, u32 numchannels);
 int snd_bcm2835_new_spdif_pcm(struct bcm2835_chip *chip);
+int snd_bcm2835_new_simple_pcm(struct bcm2835_chip *chip,
+			       const char *name,
+			       enum snd_bcm2835_route route,
+			       u32 numchannels);
+
+int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip);
+int snd_bcm2835_new_headphones_ctl(struct bcm2835_chip *chip);
 
 int bcm2835_audio_open(struct bcm2835_alsa_stream *alsa_stream);
 int bcm2835_audio_close(struct bcm2835_alsa_stream *alsa_stream);
diff --git a/drivers/staging/bcm2835-audio/vc_vchi_audioserv_defs.h b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h
index da96f1bc2516..da96f1bc2516 100644
--- a/drivers/staging/bcm2835-audio/vc_vchi_audioserv_defs.h
+++ b/drivers/staging/vc04_services/bcm2835-audio/vc_vchi_audioserv_defs.h
diff --git a/drivers/staging/media/platform/bcm2835/Kconfig b/drivers/staging/vc04_services/bcm2835-camera/Kconfig
index 7c5245dc3225..b8b01aa4e426 100644
--- a/drivers/staging/media/platform/bcm2835/Kconfig
+++ b/drivers/staging/vc04_services/bcm2835-camera/Kconfig
@@ -1,9 +1,10 @@
 config VIDEO_BCM2835
-	tristate "Broadcom BCM2835 camera driver"
+	tristate "BCM2835 Camera"
+	depends on MEDIA_SUPPORT
 	depends on VIDEO_V4L2 && (ARCH_BCM2835 || COMPILE_TEST)
-	depends on BCM2835_VCHIQ
-	depends on ARM
+	select BCM2835_VCHIQ
 	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
diff --git a/drivers/staging/media/platform/bcm2835/Makefile b/drivers/staging/vc04_services/bcm2835-camera/Makefile
index 8307f30517d5..8307f30517d5 100644
--- a/drivers/staging/media/platform/bcm2835/Makefile
+++ b/drivers/staging/vc04_services/bcm2835-camera/Makefile
diff --git a/drivers/staging/media/platform/bcm2835/TODO b/drivers/staging/vc04_services/bcm2835-camera/TODO
index 61a509992b9a..0ab9e88d769a 100644
--- a/drivers/staging/media/platform/bcm2835/TODO
+++ b/drivers/staging/vc04_services/bcm2835-camera/TODO
@@ -32,8 +32,3 @@ We should have VCHI create a platform device once it's initialized,
 and have this driver bind to it, so that we automatically load the
 v4l2 module after VCHI loads.
 
-5) Drop the gstreamer workaround.
-
-This was a temporary workaround for a bug that was fixed mid-2014, and
-we should remove it before stabilizing the driver.
-
diff --git a/drivers/staging/media/platform/bcm2835/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
index ca15a698e018..a11e047734f9 100644
--- a/drivers/staging/media/platform/bcm2835/bcm2835-camera.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
@@ -61,24 +61,11 @@ 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, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+module_param(max_video_width, int, 0644);
 MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
-module_param(max_video_height, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+module_param(max_video_height, int, 0644);
 MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
 
-/* Gstreamer bug https://bugzilla.gnome.org/show_bug.cgi?id=726521
- * v4l2src does bad (and actually wrong) things when the vidioc_enum_framesizes
- * function says type V4L2_FRMSIZE_TYPE_STEPWISE, which we do by default.
- * It's happier if we just don't say anything at all, when it then
- * sets up a load of defaults that it thinks might work.
- * If gst_v4l2src_is_broken is non-zero, then we remove the function from
- * our function table list (actually switch to an alternate set, but same
- * result).
- */
-static int gst_v4l2src_is_broken;
-module_param(gst_v4l2src_is_broken, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
-MODULE_PARM_DESC(gst_v4l2src_is_broken, "If non-zero, enable workaround for Gstreamer");
-
 /* global device data array */
 static struct bm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
 
@@ -229,18 +216,16 @@ static struct mmal_fmt *get_format(struct v4l2_format *f)
 	for (k = 0; k < ARRAY_SIZE(formats); k++) {
 		fmt = &formats[k];
 		if (fmt->fourcc == f->fmt.pix.pixelformat)
-			break;
+			return fmt;
 	}
 
-	if (k == ARRAY_SIZE(formats))
-		return NULL;
-
-	return &formats[k];
+	return NULL;
 }
 
 /* ------------------------------------------------------------------
-	Videobuf queue operations
-   ------------------------------------------------------------------*/
+ *	Videobuf queue operations
+ * ------------------------------------------------------------------
+ */
 
 static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
@@ -250,7 +235,7 @@ static int queue_setup(struct vb2_queue *vq,
 	unsigned long size;
 
 	/* refuse queue setup if port is not configured */
-	if (dev->capture.port == NULL) {
+	if (!dev->capture.port) {
 		v4l2_err(&dev->v4l2_dev,
 			 "%s: capture port not configured\n", __func__);
 		return -EINVAL;
@@ -289,8 +274,8 @@ static int buffer_prepare(struct vb2_buffer *vb)
 	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 		 __func__, dev);
 
-	BUG_ON(dev->capture.port == NULL);
-	BUG_ON(dev->capture.fmt == NULL);
+	BUG_ON(!dev->capture.port);
+	BUG_ON(!dev->capture.fmt);
 
 	size = dev->capture.stride * dev->capture.height;
 	if (vb2_plane_size(vb, 0) < size) {
@@ -324,14 +309,14 @@ static void buffer_cb(struct vchiq_mmal_instance *instance,
 
 	if (status != 0) {
 		/* error in transfer */
-		if (buf != NULL) {
+		if (buf) {
 			/* there was a buffer with the error so return it */
 			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 		}
 		return;
 	} else if (length == 0) {
 		/* stream ended */
-		if (buf != NULL) {
+		if (buf) {
 			/* this should only ever happen if the port is
 			 * disabled and there are buffers still queued
 			 */
@@ -513,7 +498,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		 __func__, dev);
 
 	/* ensure a format has actually been set */
-	if (dev->capture.port == NULL)
+	if (!dev->capture.port)
 		return -EINVAL;
 
 	if (enable_camera(dev) < 0) {
@@ -604,7 +589,7 @@ static void stop_streaming(struct vb2_queue *vq)
 	dev->capture.frame_count = 0;
 
 	/* ensure a format has actually been set */
-	if (dev->capture.port == NULL) {
+	if (!dev->capture.port) {
 		v4l2_err(&dev->v4l2_dev,
 			 "no capture port - stream not started?\n");
 		return;
@@ -668,13 +653,13 @@ static struct vb2_ops bm2835_mmal_video_qops = {
 };
 
 /* ------------------------------------------------------------------
-	IOCTL operations
-   ------------------------------------------------------------------*/
+ *	IOCTL operations
+ * ------------------------------------------------------------------
+ */
 
 static int set_overlay_params(struct bm2835_mmal_dev *dev,
 			      struct vchiq_mmal_port *port)
 {
-	int ret;
 	struct mmal_parameter_displayregion prev_config = {
 	.set = MMAL_DISPLAY_SET_LAYER | MMAL_DISPLAY_SET_ALPHA |
 	    MMAL_DISPLAY_SET_DEST_RECT | MMAL_DISPLAY_SET_FULLSCREEN,
@@ -688,11 +673,9 @@ static int set_overlay_params(struct bm2835_mmal_dev *dev,
 		      .height = dev->overlay.w.height,
 		      },
 	};
-	ret = vchiq_mmal_port_parameter_set(dev->instance, port,
-					    MMAL_PARAMETER_DISPLAYREGION,
-					    &prev_config, sizeof(prev_config));
-
-	return ret;
+	return vchiq_mmal_port_parameter_set(dev->instance, port,
+					     MMAL_PARAMETER_DISPLAYREGION,
+					     &prev_config, sizeof(prev_config));
 }
 
 /* overlay ioctl */
@@ -834,7 +817,8 @@ 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. */
+	 * positioned independently.
+	 */
 	struct bm2835_mmal_dev *dev = video_drvdata(file);
 	struct vchiq_mmal_port *preview_port =
 		    &dev->component[MMAL_COMPONENT_CAMERA]->
@@ -1291,7 +1275,8 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	}
 
 	/* If the format is unsupported v4l2 says we should switch to
-	 * a supported one and not return an error. */
+	 * a supported one and not return an error.
+	 */
 	mfmt = get_format(f);
 	if (!mfmt) {
 		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
@@ -1456,50 +1441,10 @@ static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
-static const struct v4l2_ioctl_ops camera0_ioctl_ops_gstreamer = {
-	/* 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,
-	/* Remove this function ptr to fix gstreamer bug
-	.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
-   ------------------------------------------------------------------*/
+ *	Driver init/finalise
+ * ------------------------------------------------------------------
+ */
 
 static const struct v4l2_file_operations camera0_fops = {
 	.owner = THIS_MODULE,
@@ -1545,9 +1490,7 @@ static int get_num_cameras(struct vchiq_mmal_instance *instance,
 		pr_info("Failed to get camera info\n");
 	}
 	for (i = 0;
-	     i < (cam_info.num_cameras > num_resolutions ?
-			num_resolutions :
-			cam_info.num_cameras);
+	     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;
@@ -1591,7 +1534,7 @@ static int set_camera_parameters(struct vchiq_mmal_instance *instance,
 static int __init mmal_init(struct bm2835_mmal_dev *dev)
 {
 	int ret;
-	struct mmal_es_format *format;
+	struct mmal_es_format_local *format;
 	u32 bool_true = 1;
 	u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
 	int param_size;
@@ -1813,11 +1756,6 @@ static int __init bm2835_mmal_init_device(struct bm2835_mmal_dev *dev,
 	int ret;
 
 	*vfd = vdev_template;
-	if (gst_v4l2src_is_broken) {
-		v4l2_info(&dev->v4l2_dev,
-			  "Work-around for gstreamer issue is active.\n");
-		vfd->ioctl_ops = &camera0_ioctl_ops_gstreamer;
-	}
 
 	vfd->v4l2_dev = &dev->v4l2_dev;
 
@@ -1901,6 +1839,7 @@ static int __init bm2835_mmal_init(void)
 	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)
@@ -1913,9 +1852,11 @@ static int __init bm2835_mmal_init(void)
 		num_cameras = MAX_BCM2835_CAMERAS;
 
 	for (camera = 0; camera < num_cameras; camera++) {
-		dev = kzalloc(sizeof(struct bm2835_mmal_dev), GFP_KERNEL);
-		if (!dev)
-			return -ENOMEM;
+		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto cleanup_gdev;
+		}
 
 		dev->camera_num = camera;
 		dev->max_width = resolutions[camera][0];
@@ -1998,9 +1939,10 @@ unreg_dev:
 free_dev:
 	kfree(dev);
 
-	for ( ; camera > 0; camera--) {
-		bcm2835_cleanup_instance(gdev[camera]);
-		gdev[camera] = NULL;
+cleanup_gdev:
+	for (i = 0; i < camera; i++) {
+		bcm2835_cleanup_instance(gdev[i]);
+		gdev[i] = NULL;
 	}
 	pr_info("%s: error %d while loading driver\n",
 		BM2835_MMAL_MODULE_NAME, ret);
diff --git a/drivers/staging/media/platform/bcm2835/bcm2835-camera.h b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
index e6aeb7e7e381..404037476bc5 100644
--- a/drivers/staging/media/platform/bcm2835/bcm2835-camera.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
@@ -59,7 +59,7 @@ struct bm2835_mmal_dev {
 	unsigned int		  manual_shutter_speed;
 	bool			  exp_auto_priority;
 	bool manual_iso_enabled;
-	uint32_t iso;
+	u32 iso;
 
 	/* allocated mmal instance and components */
 	struct vchiq_mmal_instance   *instance;
@@ -125,7 +125,7 @@ int set_framerate_params(struct bm2835_mmal_dev *dev);
 {	\
 	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 == NULL ? "" : desc,	\
+		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); \
@@ -135,7 +135,7 @@ int set_framerate_params(struct bm2835_mmal_dev *dev);
 	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 == NULL ? "" : desc,	\
+		desc,	\
 		(win_fmt)->w.width, (win_fmt)->w.height, \
 		(win_fmt)->w.left, (win_fmt)->w.top, \
 		(win_fmt)->field,	\
diff --git a/drivers/staging/media/platform/bcm2835/controls.c b/drivers/staging/vc04_services/bcm2835-camera/controls.c
index a40987b2e75d..77a5d6f4e1eb 100644
--- a/drivers/staging/media/platform/bcm2835/controls.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/controls.c
@@ -90,7 +90,8 @@ struct bm2835_mmal_v4l2_ctrl {
 	u32 id; /* v4l2 control identifier */
 	enum bm2835_mmal_ctrl_type type;
 	/* control minimum value or
-	 * mask for MMAL_CONTROL_TYPE_STD_MENU */
+	 * mask for MMAL_CONTROL_TYPE_STD_MENU
+	 */
 	s32 min;
 	s32 max; /* maximum value of control */
 	s32 def;  /* default value of control */
@@ -218,9 +219,7 @@ static int ctrl_set_iso(struct bm2835_mmal_dev *dev,
 		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 ?
-							true :
-							false);
+				(ctrl->val == V4L2_ISO_SENSITIVITY_MANUAL);
 
 	control = &dev->component[MMAL_COMPONENT_CAMERA]->control;
 
@@ -398,10 +397,10 @@ static int ctrl_set_metering_mode(struct bm2835_mmal_dev *dev,
 		break;
 
 	/* todo matrix weighting not added to Linux API till 3.9
-	case V4L2_EXPOSURE_METERING_MATRIX:
-		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX;
-		break;
-	*/
+	 * case V4L2_EXPOSURE_METERING_MATRIX:
+	 *	dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX;
+	 *	break;
+	 */
 	}
 
 	if (dev->scene_mode == V4L2_SCENE_MODE_NONE) {
@@ -911,9 +910,7 @@ static int bm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl)
 	const struct bm2835_mmal_v4l2_ctrl *mmal_ctrl = ctrl->priv;
 	int ret;
 
-	if ((mmal_ctrl == NULL) ||
-	    (mmal_ctrl->id != ctrl->id) ||
-	    (mmal_ctrl->setter == NULL)) {
+	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;
 	}
@@ -982,8 +979,9 @@ static const struct bm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
 		false
 	},
 /*	{
-		0, MMAL_CONTROL_TYPE_CLUSTER, 3, 1, 0, NULL, 0, NULL
-	}, */
+ *		0, MMAL_CONTROL_TYPE_CLUSTER, 3, 1, 0, NULL, 0, NULL
+ *	},
+ */
 	{
 		V4L2_CID_EXPOSURE_AUTO, MMAL_CONTROL_TYPE_STD_MENU,
 		~0x03, 3, V4L2_EXPOSURE_AUTO, 0, NULL,
@@ -992,9 +990,9 @@ static const struct bm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
 		false
 	},
 /* todo this needs mixing in with set exposure
-	{
-	       V4L2_CID_SCENE_MODE, MMAL_CONTROL_TYPE_STD_MENU,
-	},
+ *	{
+ *		V4L2_CID_SCENE_MODE, MMAL_CONTROL_TYPE_STD_MENU,
+ *	},
  */
 	{
 		V4L2_CID_EXPOSURE_ABSOLUTE, MMAL_CONTROL_TYPE_STD,
@@ -1275,11 +1273,11 @@ int bm2835_mmal_init_controls(struct bm2835_mmal_dev *dev,
 				 * mismatches.
 				 */
 				int i;
-				mask = 1<<V4L2_SCENE_MODE_NONE;
+				mask = 1 << V4L2_SCENE_MODE_NONE;
 				for (i = 0;
 				     i < ARRAY_SIZE(scene_configs);
 				     i++) {
-					mask |= 1<<scene_configs[i].v4l2_scene;
+					mask |= 1 << scene_configs[i].v4l2_scene;
 				}
 				mask = ~mask;
 			}
diff --git a/drivers/staging/media/platform/bcm2835/mmal-common.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h
index 840fd139e033..840fd139e033 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-common.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h
diff --git a/drivers/staging/media/platform/bcm2835/mmal-encodings.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-encodings.h
index 024d620dc1df..e71d9600b278 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-encodings.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-encodings.h
@@ -95,7 +95,6 @@
 /** 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
diff --git a/drivers/staging/media/platform/bcm2835/mmal-msg-common.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-common.h
index 66e8a6edf628..66e8a6edf628 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-msg-common.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-common.h
diff --git a/drivers/staging/media/platform/bcm2835/mmal-msg-format.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-format.h
index 123d86ef582b..24b002e8df0c 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-msg-format.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-format.h
@@ -20,7 +20,6 @@
 
 /* MMAL_ES_FORMAT_T */
 
-
 struct mmal_audio_format {
 	u32 channels;           /**< Number of audio channels */
 	u32 sample_rate;        /**< Sample rate */
@@ -54,7 +53,7 @@ union mmal_es_specific_format {
 };
 
 /** Definition of an elementary stream format (MMAL_ES_FORMAT_T) */
-struct mmal_es_format {
+struct mmal_es_format_local {
 	u32 type;      /* enum mmal_es_type */
 
 	u32 encoding;  /* FourCC specifying encoding of the elementary stream.*/
@@ -63,10 +62,7 @@ struct mmal_es_format {
 			       * stream.
 			       */
 
-	union mmal_es_specific_format *es; /* TODO: pointers in
-					    * message serialisation?!?
-					    */
-					    /* Type specific
+	union mmal_es_specific_format *es;  /* Type specific
 					     * information for the
 					     * elementary stream
 					     */
@@ -78,4 +74,26 @@ struct mmal_es_format {
 	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/media/platform/bcm2835/mmal-msg-port.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-port.h
index a55c1ea2eceb..84a0f4b717ef 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-msg-port.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg-port.h
@@ -26,12 +26,14 @@ enum mmal_port_type {
 #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. */
+ * 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). */
+ * 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)
@@ -42,15 +44,15 @@ enum mmal_port_type {
  * buffer_num, buffer_size and userdata parameters are writable.
  */
 struct mmal_port {
-	void *priv; /* Private member used by the framework */
-	const char *name; /* Port name. Used for debugging purposes (RO) */
+	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) */
-	struct mmal_es_format *format; /* Format of the elementary stream */
+	u32 format; /* Format of the elementary stream */
 
 	u32 buffer_num_min; /* Minimum number of buffers the port
 			     *   requires (RO).  This is set by the
@@ -94,9 +96,9 @@ struct mmal_port {
 			  * the client.
 			  */
 
-	void *component; /* Component this port belongs to (Read Only) */
+	u32 component; /* Component this port belongs to (Read Only) */
 
-	void *userdata; /* Field reserved for use by the client */
+	u32 userdata; /* Field reserved for use by the client */
 
 	u32 capabilities; /* Flags describing the capabilities of a
 			   * port (RO).  Bitwise combination of \ref
diff --git a/drivers/staging/media/platform/bcm2835/mmal-msg.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg.h
index 67b1076015a5..52cdf4da1b47 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-msg.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-msg.h
@@ -84,9 +84,9 @@ struct mmal_msg_header {
 	u32 type; /** enum mmal_msg_type */
 
 	/* Opaque handle to the control service */
-	struct mmal_control_service *control_service;
+	u32 control_service;
 
-	struct mmal_msg_context *context; /** a u32 per message context */
+	u32 context; /** a u32 per message context */
 	u32 status; /** The status of the vchiq operation */
 	u32 padding;
 };
@@ -101,16 +101,16 @@ struct mmal_msg_version {
 
 /* request to VC to create component */
 struct mmal_msg_component_create {
-	void *client_component; /* component context */
+	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 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 */
@@ -126,7 +126,6 @@ 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;
@@ -136,7 +135,6 @@ 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;
@@ -162,7 +160,7 @@ struct mmal_msg_port_info_get_reply {
 	s32 found;             /* unused */
 	u32 port_handle;               /**< Handle to use for this port */
 	struct mmal_port port;
-	struct mmal_es_format format; /* elementry stream format */
+	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 */
 };
@@ -192,7 +190,6 @@ struct mmal_msg_port_info_set_reply {
 	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;
@@ -214,9 +211,6 @@ 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. */
@@ -234,10 +228,12 @@ struct mmal_msg_port_action_reply {
 /** Signals that the current payload is a keyframe (i.e. self decodable) */
 #define MMAL_BUFFER_HEADER_FLAG_KEYFRAME               (1<<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 */
+ * Can be used for instance by a decoder to reset its state
+ */
 #define MMAL_BUFFER_HEADER_FLAG_DISCONTINUITY          (1<<4)
 /** Signals a buffer containing some kind of config data for the component
- * (e.g. codec config data) */
+ * (e.g. codec config data)
+ */
 #define MMAL_BUFFER_HEADER_FLAG_CONFIG                 (1<<5)
 /** Signals an encrypted payload */
 #define MMAL_BUFFER_HEADER_FLAG_ENCRYPTED              (1<<6)
@@ -256,23 +252,23 @@ struct mmal_driver_buffer {
 	u32 magic;
 	u32 component_handle;
 	u32 port_handle;
-	void *client_context;
+	u32 client_context;
 };
 
 /* buffer header */
 struct mmal_buffer_header {
-	struct mmal_buffer_header *next; /* next header */
-	void *priv; /* framework private data */
+	u32 next; /* next header */
+	u32 priv; /* framework private data */
 	u32 cmd;
-	void *data;
+	u32 data;
 	u32 alloc_size;
 	u32 length;
 	u32 offset;
 	u32 flags;
 	s64 pts;
 	s64 dts;
-	void *type;
-	void *user_data;
+	u32 type;
+	u32 user_data;
 };
 
 struct mmal_buffer_header_type_specific {
@@ -307,7 +303,6 @@ struct mmal_msg_buffer_from_host {
 	u8 short_data[MMAL_VC_SHORT_DATA];
 };
 
-
 /* port parameter setting */
 
 #define MMAL_WORKER_PORT_PARAMETER_SPACE      96
@@ -321,9 +316,9 @@ struct mmal_msg_port_parameter_set {
 };
 
 struct mmal_msg_port_parameter_set_reply {
-	u32 status; /** enum mmal_msg_status todo: how does this
-		     * differ to the one in the header?
-		     */
+	u32 status;	/* enum mmal_msg_status todo: how does this
+			 * differ to the one in the header?
+			 */
 };
 
 /* port parameter getting */
@@ -346,7 +341,7 @@ struct mmal_msg_port_parameter_get_reply {
 #define MMAL_WORKER_EVENT_SPACE 256
 
 struct mmal_msg_event_to_host {
-	void *client_component; /* component context */
+	u32 client_component; /* component context */
 
 	u32 port_type;
 	u32 port_num;
@@ -354,7 +349,7 @@ struct mmal_msg_event_to_host {
 	u32 cmd;
 	u32 length;
 	u8 data[MMAL_WORKER_EVENT_SPACE];
-	struct mmal_buffer_header *delayed_buffer;
+	u32 delayed_buffer;
 };
 
 /* all mmal messages are serialised through this structure */
diff --git a/drivers/staging/media/platform/bcm2835/mmal-parameters.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h
index f6abb5cfa49d..e7300229842d 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-parameters.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h
@@ -191,7 +191,6 @@ struct mmal_parameter_fps_range {
 	struct mmal_parameter_rational	fps_high;
 };
 
-
 /* camera configuration parameter */
 struct mmal_parameter_camera_config {
 	/* Parameters for setting up the image pools */
@@ -223,7 +222,6 @@ struct mmal_parameter_camera_config {
 	s32 use_stc_timestamp;
 };
 
-
 enum mmal_parameter_exposuremode {
 	MMAL_PARAM_EXPOSUREMODE_OFF,
 	MMAL_PARAM_EXPOSUREMODE_AUTO,
diff --git a/drivers/staging/media/platform/bcm2835/mmal-vchiq.c b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c
index fdfb6a620a43..4360db6d4392 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-vchiq.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.c
@@ -24,6 +24,7 @@
 #include <linux/slab.h>
 #include <linux/completion.h>
 #include <linux/vmalloc.h>
+#include <linux/btree.h>
 #include <asm/cacheflush.h>
 #include <media/videobuf2-vmalloc.h>
 
@@ -108,8 +109,13 @@ static const char *const port_action_type_names[] = {
 #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;
+	u32 handle;
+
 	union {
 		struct {
 			/* work struct for defered callback - must come first */
@@ -146,6 +152,13 @@ struct mmal_msg_context {
 
 };
 
+struct vchiq_mmal_context_map {
+	/* ensure serialized access to the btree(contention should be low) */
+	struct mutex lock;
+	struct btree_head32 btree_head;
+	u32 last_handle;
+};
+
 struct vchiq_mmal_instance {
 	VCHI_SERVICE_HANDLE_T handle;
 
@@ -158,25 +171,125 @@ struct vchiq_mmal_instance {
 	/* vmalloc page to receive scratch bulk xfers into */
 	void *bulk_scratch;
 
+	/* mapping table between context handles and mmal_msg_contexts */
+	struct vchiq_mmal_context_map context_map;
+
 	/* component to use next */
 	int component_idx;
 	struct vchiq_mmal_component component[VCHIQ_MMAL_MAX_COMPONENTS];
 };
 
-static struct mmal_msg_context *get_msg_context(struct vchiq_mmal_instance
-						*instance)
+static int __must_check
+mmal_context_map_init(struct vchiq_mmal_context_map *context_map)
+{
+	mutex_init(&context_map->lock);
+	context_map->last_handle = 0;
+	return btree_init32(&context_map->btree_head);
+}
+
+static void mmal_context_map_destroy(struct vchiq_mmal_context_map *context_map)
+{
+	mutex_lock(&context_map->lock);
+	btree_destroy32(&context_map->btree_head);
+	mutex_unlock(&context_map->lock);
+}
+
+static u32
+mmal_context_map_create_handle(struct vchiq_mmal_context_map *context_map,
+			       struct mmal_msg_context *msg_context,
+			       gfp_t gfp)
+{
+	u32 handle;
+
+	mutex_lock(&context_map->lock);
+
+	while (1) {
+		/* just use a simple count for handles, but do not use 0 */
+		context_map->last_handle++;
+		if (!context_map->last_handle)
+			context_map->last_handle++;
+
+		handle = context_map->last_handle;
+
+		/* check if the handle is already in use */
+		if (!btree_lookup32(&context_map->btree_head, handle))
+			break;
+	}
+
+	if (btree_insert32(&context_map->btree_head, handle,
+			   msg_context, gfp)) {
+		/* probably out of memory */
+		mutex_unlock(&context_map->lock);
+		return 0;
+	}
+
+	mutex_unlock(&context_map->lock);
+	return handle;
+}
+
+static struct mmal_msg_context *
+mmal_context_map_lookup_handle(struct vchiq_mmal_context_map *context_map,
+			       u32 handle)
+{
+	struct mmal_msg_context *msg_context;
+
+	if (!handle)
+		return NULL;
+
+	mutex_lock(&context_map->lock);
+
+	msg_context = btree_lookup32(&context_map->btree_head, handle);
+
+	mutex_unlock(&context_map->lock);
+	return msg_context;
+}
+
+static void
+mmal_context_map_destroy_handle(struct vchiq_mmal_context_map *context_map,
+				u32 handle)
+{
+	mutex_lock(&context_map->lock);
+	btree_remove32(&context_map->btree_head, handle);
+	mutex_unlock(&context_map->lock);
+}
+
+static struct mmal_msg_context *
+get_msg_context(struct vchiq_mmal_instance *instance)
 {
 	struct mmal_msg_context *msg_context;
 
-	/* todo: should this be allocated from a pool to avoid kmalloc */
-	msg_context = kmalloc(sizeof(*msg_context), GFP_KERNEL);
-	memset(msg_context, 0, sizeof(*msg_context));
+	/* 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);
+
+	msg_context->instance = instance;
+	msg_context->handle =
+		mmal_context_map_create_handle(&instance->context_map,
+					       msg_context,
+					       GFP_KERNEL);
+
+	if (!msg_context->handle) {
+		kfree(msg_context);
+		return ERR_PTR(-ENOMEM);
+	}
 
 	return msg_context;
 }
 
-static void release_msg_context(struct mmal_msg_context *msg_context)
+static struct mmal_msg_context *
+lookup_msg_context(struct vchiq_mmal_instance *instance, u32 handle)
+{
+	return mmal_context_map_lookup_handle(&instance->context_map,
+		handle);
+}
+
+static void
+release_msg_context(struct mmal_msg_context *msg_context)
 {
+	mmal_context_map_destroy_handle(&msg_context->instance->context_map,
+					msg_context->handle);
 	kfree(msg_context);
 }
 
@@ -185,7 +298,7 @@ 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:%p port type:%d num:%d cmd:0x%x length:%d\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,
@@ -199,7 +312,8 @@ static void event_to_host_cb(struct vchiq_mmal_instance *instance,
  */
 static void buffer_work_cb(struct work_struct *work)
 {
-	struct mmal_msg_context *msg_context = (struct mmal_msg_context *)work;
+	struct mmal_msg_context *msg_context =
+		container_of(work, struct mmal_msg_context, u.bulk.work);
 
 	msg_context->u.bulk.port->buffer_cb(msg_context->u.bulk.instance,
 					    msg_context->u.bulk.port,
@@ -276,10 +390,6 @@ static int bulk_receive(struct vchiq_mmal_instance *instance,
 	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;
 
-	// only need to flush L1 cache here, as VCHIQ takes care of the L2
-	// cache.
-	__cpuc_flush_dcache_area(msg_context->u.bulk.buffer->buffer, rd_len);
-
 	/* queue the bulk submission */
 	vchi_service_use(instance->handle);
 	ret = vchi_bulk_queue_receive(instance->handle,
@@ -397,8 +507,8 @@ buffer_from_host(struct vchiq_mmal_instance *instance,
 
 	/* get context */
 	msg_context = get_msg_context(instance);
-	if (!msg_context) {
-		ret = -ENOMEM;
+	if (IS_ERR(msg_context)) {
+		ret = PTR_ERR(msg_context);
 		goto unlock;
 	}
 
@@ -416,18 +526,19 @@ buffer_from_host(struct vchiq_mmal_instance *instance,
 
 	m.h.type = MMAL_MSG_TYPE_BUFFER_FROM_HOST;
 	m.h.magic = MMAL_MAGIC;
-	m.h.context = msg_context;
+	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;
+	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 = buf->buffer;
+	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 */
@@ -505,12 +616,20 @@ 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("buffer_to_host_cb: instance:%p msg:%p msg_len:%d\n",
 		 instance, msg, msg_len);
 
 	if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) {
-		msg_context = msg->u.buffer_from_host.drvbuf.client_context;
+		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;
@@ -614,6 +733,7 @@ static void service_callback(void *param,
 	u32 msg_len;
 	struct mmal_msg *msg;
 	VCHI_HELD_MSG_T msg_handle;
+	struct mmal_msg_context *msg_context;
 
 	if (!instance) {
 		pr_err("Message callback passed NULL instance\n");
@@ -650,23 +770,25 @@ static void service_callback(void *param,
 
 		default:
 			/* messages dependent on header context to complete */
-
-			/* todo: the msg.context really ought to be sanity
-			 * checked before we just use it, afaict it comes back
-			 * and is used raw from the videocore. Perhaps it
-			 * should be verified the address lies in the kernel
-			 * address space.
-			 */
-			if (msg->h.context == NULL) {
+			if (!msg->h.context) {
 				pr_err("received message context was null!\n");
 				vchi_held_msg_release(&msg_handle);
 				break;
 			}
 
+			msg_context = lookup_msg_context(instance,
+							 msg->h.context);
+			if (!msg_context) {
+				pr_err("received invalid message context %u!\n",
+				       msg->h.context);
+				vchi_held_msg_release(&msg_handle);
+				break;
+			}
+
 			/* fill in context values */
-			msg->h.context->u.sync.msg_handle = msg_handle;
-			msg->h.context->u.sync.msg = msg;
-			msg->h.context->u.sync.msg_len = msg_len;
+			msg_context->u.sync.msg_handle = msg_handle;
+			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
@@ -678,7 +800,7 @@ static void service_callback(void *param,
 			 */
 
 			/* complete message so caller knows it happened */
-			complete(&msg->h.context->u.sync.cmplt);
+			complete(&msg_context->u.sync.cmplt);
 			break;
 		}
 
@@ -710,21 +832,26 @@ static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance,
 				     struct mmal_msg **msg_out,
 				     VCHI_HELD_MSG_T *msg_handle_out)
 {
-	struct mmal_msg_context msg_context;
+	struct mmal_msg_context *msg_context;
 	int ret;
 
 	/* 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,
-		       (MMAL_MSG_MAX_SIZE - sizeof(struct mmal_msg_header)));
+		      (int)(MMAL_MSG_MAX_SIZE -
+			    sizeof(struct mmal_msg_header)));
 		return -EINVAL;
 	}
 
-	init_completion(&msg_context.u.sync.cmplt);
+	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;
+	msg->h.context = msg_context->handle;
 	msg->h.status = 0;
 
 	DBG_DUMP_MSG(msg, (sizeof(struct mmal_msg_header) + payload_len),
@@ -741,20 +868,23 @@ static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance,
 
 	if (ret) {
 		pr_err("error %d queuing message\n", ret);
+		release_msg_context(msg_context);
 		return ret;
 	}
 
-	ret = wait_for_completion_timeout(&msg_context.u.sync.cmplt, 3 * HZ);
+	ret = wait_for_completion_timeout(&msg_context->u.sync.cmplt, 3 * HZ);
 	if (ret <= 0) {
 		pr_err("error %d waiting for sync completion\n", ret);
 		if (ret == 0)
 			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_out = msg_context.u.sync.msg_handle;
+	*msg_out = msg_context->u.sync.msg;
+	*msg_handle_out = msg_context->u.sync.msg_handle;
+	release_msg_context(msg_context);
 
 	return 0;
 }
@@ -776,7 +906,7 @@ static void dump_port_info(struct vchiq_mmal_port *port)
 		 port->current_buffer.num,
 		 port->current_buffer.size, port->current_buffer.alignment);
 
-	pr_debug("elementry stream: type:%d encoding:0x%x variant:0x%x\n",
+	pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n",
 		 port->format.type,
 		 port->format.encoding, port->format.encoding_variant);
 
@@ -816,7 +946,7 @@ static void port_to_mmal_msg(struct vchiq_mmal_port *port, struct mmal_port *p)
 	/* only three writable fields in a port */
 	p->buffer_num = port->current_buffer.num;
 	p->buffer_size = port->current_buffer.size;
-	p->userdata = port;
+	p->userdata = (u32)(unsigned long)port;
 }
 
 static int port_info_set(struct vchiq_mmal_instance *instance,
@@ -840,7 +970,7 @@ static int port_info_set(struct vchiq_mmal_instance *instance,
 
 	port_to_mmal_msg(port, &m.u.port_info_set.port);
 
-	/* elementry stream format setup */
+	/* 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 =
@@ -949,7 +1079,7 @@ static int port_info_get(struct vchiq_mmal_instance *instance,
 	port->format.bitrate = rmsg->u.port_info_get_reply.format.bitrate;
 	port->format.flags = rmsg->u.port_info_get_reply.format.flags;
 
-	/* elementry stream format */
+	/* elementary stream format */
 	memcpy(&port->es,
 	       &rmsg->u.port_info_get_reply.es,
 	       sizeof(union mmal_es_specific_format));
@@ -986,7 +1116,7 @@ static int create_component(struct vchiq_mmal_instance *instance,
 
 	/* build component create message */
 	m.h.type = MMAL_MSG_TYPE_COMPONENT_CREATE;
-	m.u.component_create.client_component = component;
+	m.u.component_create.client_component = (u32)(unsigned long)component;
 	strncpy(m.u.component_create.name, name,
 		sizeof(m.u.component_create.name));
 
@@ -1315,7 +1445,12 @@ static int port_parameter_get(struct vchiq_mmal_instance *instance,
 	}
 
 	ret = -rmsg->u.port_parameter_get_reply.status;
-	if (ret) {
+	/* 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
 		 */
@@ -1387,7 +1522,7 @@ static int port_enable(struct vchiq_mmal_instance *instance,
 		return 0;
 
 	/* ensure there are enough buffers queued to cover the buffer headers */
-	if (port->buffer_cb != NULL) {
+	if (port->buffer_cb) {
 		hdr_count = 0;
 		list_for_each(buf_head, &port->buffers) {
 			hdr_count++;
@@ -1429,7 +1564,8 @@ done:
 
 /* ------------------------------------------------------------------
  * Exported API
- *------------------------------------------------------------------*/
+ *------------------------------------------------------------------
+ */
 
 int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance,
 			       struct vchiq_mmal_port *port)
@@ -1547,7 +1683,7 @@ int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
 		return -EINTR;
 
 	/* disconnect ports if connected */
-	if (src->connected != NULL) {
+	if (src->connected) {
 		ret = port_disable(instance, src);
 		if (ret) {
 			pr_err("failed disabling src port(%d)\n", ret);
@@ -1570,7 +1706,7 @@ int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
 		src->connected = NULL;
 	}
 
-	if (dst == NULL) {
+	if (!dst) {
 		/* do not make new connection */
 		ret = 0;
 		pr_debug("not making new connection\n");
@@ -1817,7 +1953,7 @@ int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance)
 {
 	int status = 0;
 
-	if (instance == NULL)
+	if (!instance)
 		return -EINVAL;
 
 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
@@ -1833,6 +1969,8 @@ int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance)
 
 	vfree(instance->bulk_scratch);
 
+	mmal_context_map_destroy(&instance->context_map);
+
 	kfree(instance);
 
 	return status;
@@ -1845,16 +1983,16 @@ int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance)
 	static VCHI_CONNECTION_T *vchi_connection;
 	static VCHI_INSTANCE_T vchi_instance;
 	SERVICE_CREATION_T params = {
-		VCHI_VERSION_EX(VC_MMAL_VER, VC_MMAL_MIN_VER),
-		VC_MMAL_SERVER_NAME,
-		vchi_connection,
-		0,		/* rx fifo size (unused) */
-		0,		/* tx fifo size (unused) */
-		service_callback,
-		NULL,		/* service callback parameter */
-		1,		/* unaligned bulk receives */
-		1,		/* unaligned bulk transmits */
-		0		/* want crc check on bulk transfers */
+		.version		= VCHI_VERSION_EX(VC_MMAL_VER, VC_MMAL_MIN_VER),
+		.service_id		= VC_MMAL_SERVER_NAME,
+		.connection		= vchi_connection,
+		.rx_fifo_size		= 0,
+		.tx_fifo_size		= 0,
+		.callback		= service_callback,
+		.callback_param		= NULL,
+		.want_unaligned_bulk_rx = 1,
+		.want_unaligned_bulk_tx = 1,
+		.want_crc		= 0
 	};
 
 	/* compile time checks to ensure structure size as they are
@@ -1884,14 +2022,23 @@ int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance)
 		return -EIO;
 	}
 
-	instance = kmalloc(sizeof(*instance), GFP_KERNEL);
-	memset(instance, 0, sizeof(*instance));
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+
+	if (!instance)
+		return -ENOMEM;
 
 	mutex_init(&instance->vchiq_mutex);
 	mutex_init(&instance->bulk_mutex);
 
 	instance->bulk_scratch = vmalloc(PAGE_SIZE);
 
+	status = mmal_context_map_init(&instance->context_map);
+	if (status) {
+		pr_err("Failed to init context map (status=%d)\n", status);
+		kfree(instance);
+		return status;
+	}
+
 	params.callback_param = instance;
 
 	status = vchi_service_open(vchi_instance, &params, &instance->handle);
diff --git a/drivers/staging/media/platform/bcm2835/mmal-vchiq.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h
index 9d1d11e4a53e..63db053532bf 100644
--- a/drivers/staging/media/platform/bcm2835/mmal-vchiq.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-vchiq.h
@@ -74,8 +74,8 @@ struct vchiq_mmal_port {
 	struct vchiq_mmal_port_buffer current_buffer;
 
 	/* stream format */
-	struct mmal_es_format format;
-	/* elementry stream format */
+	struct mmal_es_format_local format;
+	/* elementary stream format */
 	union mmal_es_specific_format es;
 
 	/* data buffers to fill */
@@ -104,13 +104,12 @@ struct vchiq_mmal_component {
 	struct vchiq_mmal_port clock[MAX_PORT_COUNT]; /* clock ports */
 };
 
-
 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,
@@ -128,8 +127,6 @@ 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
@@ -147,7 +144,6 @@ int vchiq_mmal_port_enable(
 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,
diff --git a/drivers/staging/vc04_services/interface/vchi/TODO b/drivers/staging/vc04_services/interface/vchi/TODO
index 03aa65183b25..df93154b1aa6 100644
--- a/drivers/staging/vc04_services/interface/vchi/TODO
+++ b/drivers/staging/vc04_services/interface/vchi/TODO
@@ -1,24 +1,9 @@
-1) Port to aarch64
-
-This driver won't be very useful unless we also have it working on
-Raspberry Pi 3.  This requires, at least:
-
-  - Figure out an alternative to the dmac_map_area() hack.
-
-  - Decide what to use instead of dsb().
-
-  - Do something about (int) cast of bulk->data in
-    vchiq_bulk_transfer().
-
-    bulk->data is a bus address going across to the firmware.  We know
-    our bus addresses are <32bit.
-
-2) Write a DT binding doc and get the corresponding DT node merged to
+1) Write a DT binding doc and get the corresponding DT node merged to
    bcm2835.
 
 This will let the driver probe when enabled.
 
-3) Import drivers using VCHI.
+2) Import drivers using VCHI.
 
 VCHI is just a tool to let drivers talk to the firmware.  Here are
 some of the ones we want:
@@ -41,7 +26,7 @@ some of the ones we want:
   to manage these buffers as dmabufs so that we can zero-copy import
   camera images into vc4 for rendering/display.
 
-4) Garbage-collect unused code
+3) Garbage-collect unused code
 
 One of the reasons this driver wasn't upstreamed previously was that
 there's a lot code that got built that's probably unnecessary these
diff --git a/drivers/staging/vc04_services/interface/vchi/vchi_cfg.h b/drivers/staging/vc04_services/interface/vchi/vchi_cfg.h
index 26bc2d38d725..b6f42b86f206 100644
--- a/drivers/staging/vc04_services/interface/vchi/vchi_cfg.h
+++ b/drivers/staging/vc04_services/interface/vchi/vchi_cfg.h
@@ -173,7 +173,7 @@
  * under the carpet. */
 #if VCHI_RX_MSG_QUEUE_SIZE < (VCHI_MAX_MSG_SIZE/16 + 1) * VCHI_NUM_READ_SLOTS
 #  undef VCHI_RX_MSG_QUEUE_SIZE
-#  define VCHI_RX_MSG_QUEUE_SIZE (VCHI_MAX_MSG_SIZE/16 + 1) * VCHI_NUM_READ_SLOTS
+#  define VCHI_RX_MSG_QUEUE_SIZE ((VCHI_MAX_MSG_SIZE/16 + 1) * VCHI_NUM_READ_SLOTS)
 #endif
 
 /* How many bulk transmits can we have pending. Once exhausted, vchi_bulk_queue_transmit
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
index 3aeffcb9c87e..988ee61fb4a7 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_2835_arm.c
@@ -37,12 +37,11 @@
 #include <linux/interrupt.h>
 #include <linux/pagemap.h>
 #include <linux/dma-mapping.h>
-#include <linux/version.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/uaccess.h>
+#include <linux/mm.h>
 #include <linux/of.h>
-#include <asm/pgtable.h>
 #include <soc/bcm2835/raspberrypi-firmware.h>
 
 #define TOTAL_SLOTS (VCHIQ_SLOT_ZERO_SLOTS + 2 * 32)
@@ -208,6 +207,7 @@ VCHIQ_STATUS_T
 vchiq_platform_init_state(VCHIQ_STATE_T *state)
 {
 	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
+
 	state->platform_state = kzalloc(sizeof(VCHIQ_2835_ARM_STATE_T), GFP_KERNEL);
 	((VCHIQ_2835_ARM_STATE_T *)state->platform_state)->inited = 1;
 	status = vchiq_arm_init_state(state, &((VCHIQ_2835_ARM_STATE_T *)state->platform_state)->arm_state);
@@ -293,6 +293,7 @@ vchiq_dump_platform_state(void *dump_context)
 {
 	char buf[80];
 	int len;
+
 	len = snprintf(buf, sizeof(buf),
 		"  Platform: 2835 (VC master)");
 	vchiq_dump(dump_context, buf, len + 1);
@@ -406,7 +407,7 @@ create_pagelist(char __user *buf, size_t count, unsigned short type,
 	dma_addr_t dma_addr;
 
 	offset = ((unsigned int)(unsigned long)buf & (PAGE_SIZE - 1));
-	num_pages = (count + offset + PAGE_SIZE - 1) / PAGE_SIZE;
+	num_pages = DIV_ROUND_UP(count + offset, PAGE_SIZE);
 
 	pagelist_size = sizeof(PAGELIST_T) +
 			(num_pages * sizeof(u32)) +
@@ -591,6 +592,7 @@ free_pagelist(struct vchiq_pagelist_info *pagelistinfo,
 			(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) &
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
index 8a0d214f6e9b..e823f1d5d177 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -48,6 +48,7 @@
 #include <linux/list.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/compat.h>
 #include <soc/bcm2835/raspberrypi-firmware.h>
 
 #include "vchiq_core.h"
@@ -194,8 +195,10 @@ static const char *const ioctl_names[] = {
 vchiq_static_assert(ARRAY_SIZE(ioctl_names) ==
 		    (VCHIQ_IOC_MAX + 1));
 
+#if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
 static void
 dump_phys_mem(void *virt_addr, u32 num_bytes);
+#endif
 
 /****************************************************************************
 *
@@ -210,6 +213,7 @@ add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
 {
 	VCHIQ_COMPLETION_DATA_T *completion;
 	int insert;
+
 	DEBUG_INITIALISE(g_state.local)
 
 	insert = instance->completion_insert;
@@ -281,6 +285,7 @@ service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 	VCHIQ_SERVICE_T *service;
 	VCHIQ_INSTANCE_T instance;
 	bool skip_completion = false;
+
 	DEBUG_INITIALISE(g_state.local)
 
 	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
@@ -316,6 +321,7 @@ service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 			if ((user_service->message_available_pos -
 				instance->completion_remove) < 0) {
 				VCHIQ_STATUS_T status;
+
 				vchiq_log_info(vchiq_arm_log_level,
 					"Inserting extra MESSAGE_AVAILABLE");
 				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
@@ -407,7 +413,7 @@ static void close_delivered(USER_SERVICE_T *user_service)
 }
 
 struct vchiq_io_copy_callback_context {
-	VCHIQ_ELEMENT_T *current_element;
+	struct vchiq_element *current_element;
 	size_t current_element_offset;
 	unsigned long elements_to_go;
 	size_t current_offset;
@@ -484,7 +490,7 @@ vchiq_ioc_copy_element_data(
  **************************************************************************/
 static VCHIQ_STATUS_T
 vchiq_ioc_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
-			VCHIQ_ELEMENT_T *elements,
+			struct vchiq_element *elements,
 			unsigned long count)
 {
 	struct vchiq_io_copy_callback_context context;
@@ -520,6 +526,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	VCHIQ_SERVICE_T *service = NULL;
 	long ret = 0;
 	int i, rc;
+
 	DEBUG_INITIALISE(g_state.local)
 
 	vchiq_log_trace(vchiq_arm_log_level,
@@ -742,6 +749,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 	case VCHIQ_IOC_QUEUE_MESSAGE: {
 		VCHIQ_QUEUE_MESSAGE_T args;
+
 		if (copy_from_user
 			 (&args, (const void __user *)arg,
 			  sizeof(args)) != 0) {
@@ -753,9 +761,10 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 		if ((service != NULL) && (args.count <= MAX_ELEMENTS)) {
 			/* Copy elements into kernel space */
-			VCHIQ_ELEMENT_T elements[MAX_ELEMENTS];
+			struct vchiq_element elements[MAX_ELEMENTS];
+
 			if (copy_from_user(elements, args.elements,
-				args.count * sizeof(VCHIQ_ELEMENT_T)) == 0)
+				args.count * sizeof(struct vchiq_element)) == 0)
 				status = vchiq_ioc_queue_message
 					(args.handle,
 					elements, args.count);
@@ -770,6 +779,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case VCHIQ_IOC_QUEUE_BULK_RECEIVE: {
 		VCHIQ_QUEUE_BULK_TRANSFER_T args;
 		struct bulk_waiter_node *waiter = NULL;
+
 		VCHIQ_BULK_DIR_T dir =
 			(cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
 			VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
@@ -797,6 +807,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			args.userdata = &waiter->bulk_waiter;
 		} else if (args.mode == VCHIQ_BULK_MODE_WAITING) {
 			struct list_head *pos;
+
 			mutex_lock(&instance->bulk_waiter_list_mutex);
 			list_for_each(pos, &instance->bulk_waiter_list) {
 				if (list_entry(pos, struct bulk_waiter_node,
@@ -882,6 +893,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			instance->completion_insert)
 			&& !instance->closing) {
 			int rc;
+
 			DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 			mutex_unlock(&instance->completion_mutex);
 			rc = down_interruptible(&instance->insert_event);
@@ -1149,6 +1161,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				args.handle, args.option, args.value);
 	} break;
 
+#if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
 	case VCHIQ_IOC_DUMP_PHYS_MEM: {
 		VCHIQ_DUMP_MEM_T  args;
 
@@ -1160,6 +1173,7 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		}
 		dump_phys_mem(args.virt_addr, args.num_bytes);
 	} break;
+#endif
 
 	case VCHIQ_IOC_LIB_VERSION: {
 		unsigned int lib_version = (unsigned int)arg;
@@ -1219,6 +1233,491 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	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,
+	unsigned long arg)
+{
+	VCHIQ_CREATE_SERVICE_T __user *args;
+	struct vchiq_create_service32 __user *ptrargs32 =
+		(struct vchiq_create_service32 __user *)arg;
+	struct vchiq_create_service32 args32;
+	long ret;
+
+	args = compat_alloc_user_space(sizeof(*args));
+	if (!args)
+		return -EFAULT;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_create_service32 __user *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(args32.params.fourcc, &args->params.fourcc) ||
+	    put_user(compat_ptr(args32.params.callback),
+		     &args->params.callback) ||
+	    put_user(compat_ptr(args32.params.userdata),
+		     &args->params.userdata) ||
+	    put_user(args32.params.version, &args->params.version) ||
+	    put_user(args32.params.version_min,
+		     &args->params.version_min) ||
+	    put_user(args32.is_open, &args->is_open) ||
+	    put_user(args32.is_vchi, &args->is_vchi) ||
+	    put_user(args32.handle, &args->handle))
+		return -EFAULT;
+
+	ret = vchiq_ioctl(file, VCHIQ_IOC_CREATE_SERVICE, (unsigned long)args);
+
+	if (ret < 0)
+		return ret;
+
+	if (get_user(args32.handle, &args->handle))
+		return -EFAULT;
+
+	if (copy_to_user(&ptrargs32->handle,
+			 &args32.handle,
+			 sizeof(args32.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,
+				 unsigned long arg)
+{
+	VCHIQ_QUEUE_MESSAGE_T *args;
+	struct vchiq_element *elements;
+	struct vchiq_queue_message32 args32;
+	unsigned int count;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_queue_message32 __user *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	args = compat_alloc_user_space(sizeof(*args) +
+				       (sizeof(*elements) * MAX_ELEMENTS));
+
+	if (!args)
+		return -EFAULT;
+
+	if (put_user(args32.handle, &args->handle) ||
+	    put_user(args32.count, &args->count) ||
+	    put_user(compat_ptr(args32.elements), &args->elements))
+		return -EFAULT;
+
+	if (args32.count > MAX_ELEMENTS)
+		return -EINVAL;
+
+	if (args32.elements && args32.count) {
+		struct vchiq_element32 tempelement32[MAX_ELEMENTS];
+
+		elements = (struct vchiq_element __user *)(args + 1);
+
+		if (copy_from_user(&tempelement32,
+				   compat_ptr(args32.elements),
+				   sizeof(tempelement32)))
+			return -EFAULT;
+
+		for (count = 0; count < args32.count; count++) {
+			if (put_user(compat_ptr(tempelement32[count].data),
+				     &elements[count].data) ||
+			    put_user(tempelement32[count].size,
+				     &elements[count].size))
+				return -EFAULT;
+		}
+
+		if (put_user(elements, &args->elements))
+			return -EFAULT;
+	}
+
+	return vchiq_ioctl(file, VCHIQ_IOC_QUEUE_MESSAGE, (unsigned long)args);
+}
+
+struct vchiq_queue_bulk_transfer32 {
+	unsigned int handle;
+	compat_uptr_t data;
+	unsigned int size;
+	compat_uptr_t userdata;
+	VCHIQ_BULK_MODE_T 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,
+			      unsigned long arg)
+{
+	VCHIQ_QUEUE_BULK_TRANSFER_T *args;
+	struct vchiq_queue_bulk_transfer32 args32;
+	struct vchiq_queue_bulk_transfer32 *ptrargs32 =
+		(struct vchiq_queue_bulk_transfer32 *)arg;
+	long ret;
+
+	args = compat_alloc_user_space(sizeof(*args));
+	if (!args)
+		return -EFAULT;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_queue_bulk_transfer32 __user *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(args32.handle, &args->handle) ||
+	    put_user(compat_ptr(args32.data), &args->data) ||
+	    put_user(args32.size, &args->size) ||
+	    put_user(compat_ptr(args32.userdata), &args->userdata) ||
+	    put_user(args32.mode, &args->mode))
+		return -EFAULT;
+
+	if (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT32)
+		cmd = VCHIQ_IOC_QUEUE_BULK_TRANSMIT;
+	else
+		cmd = VCHIQ_IOC_QUEUE_BULK_RECEIVE;
+
+	ret = vchiq_ioctl(file, cmd, (unsigned long)args);
+
+	if (ret < 0)
+		return ret;
+
+	if (get_user(args32.mode, &args->mode))
+		return -EFAULT;
+
+	if (copy_to_user(&ptrargs32->mode,
+			 &args32.mode,
+			 sizeof(args32.mode)))
+		return -EFAULT;
+
+	return 0;
+}
+
+struct vchiq_completion_data32 {
+	VCHIQ_REASON_T reason;
+	compat_uptr_t header;
+	compat_uptr_t service_userdata;
+	compat_uptr_t bulk_userdata;
+};
+
+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,
+				    unsigned long arg)
+{
+	VCHIQ_AWAIT_COMPLETION_T *args;
+	VCHIQ_COMPLETION_DATA_T *completion;
+	VCHIQ_COMPLETION_DATA_T completiontemp;
+	struct vchiq_await_completion32 args32;
+	struct vchiq_completion_data32 completion32;
+	unsigned int *msgbufcount32;
+	compat_uptr_t msgbuf32;
+	void *msgbuf;
+	void **msgbufptr;
+	long ret;
+
+	args = compat_alloc_user_space(sizeof(*args) +
+				       sizeof(*completion) +
+				       sizeof(*msgbufptr));
+	if (!args)
+		return -EFAULT;
+
+	completion = (VCHIQ_COMPLETION_DATA_T *)(args + 1);
+	msgbufptr = (void __user **)(completion + 1);
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_completion_data32 *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(args32.count, &args->count) ||
+	    put_user(compat_ptr(args32.buf), &args->buf) ||
+	    put_user(args32.msgbufsize, &args->msgbufsize) ||
+	    put_user(args32.msgbufcount, &args->msgbufcount) ||
+	    put_user(compat_ptr(args32.msgbufs), &args->msgbufs))
+		return -EFAULT;
+
+	/* These are simple cases, so just fall into the native handler */
+	if (!args32.count || !args32.buf || !args32.msgbufcount)
+		return vchiq_ioctl(file,
+				   VCHIQ_IOC_AWAIT_COMPLETION,
+				   (unsigned long)args);
+
+	/*
+	 * These are the more complex cases.  Typical applications of this
+	 * ioctl will use a very large count, with a very large msgbufcount.
+	 * Since the native ioctl can asynchronously fill in the returned
+	 * buffers and the application can in theory begin processing messages
+	 * even before the ioctl returns, a bit of a trick is used here.
+	 *
+	 * By forcing both count and msgbufcount to be 1, it forces the native
+	 * ioctl to only claim at most 1 message is available.   This tricks
+	 * the calling application into thinking only 1 message was actually
+	 * available in the queue so like all good applications it will retry
+	 * waiting until all the required messages are received.
+	 *
+	 * This trick has been tested and proven to work with vchiq_test,
+	 * Minecraft_PI, the "hello pi" examples, and various other
+	 * applications that are included in Raspbian.
+	 */
+
+	if (copy_from_user(&msgbuf32,
+			   compat_ptr(args32.msgbufs) +
+			   (sizeof(compat_uptr_t) *
+			   (args32.msgbufcount - 1)),
+			   sizeof(msgbuf32)))
+		return -EFAULT;
+
+	msgbuf = compat_ptr(msgbuf32);
+
+	if (copy_to_user(msgbufptr,
+			 &msgbuf,
+			 sizeof(msgbuf)))
+		return -EFAULT;
+
+	if (copy_to_user(&args->msgbufs,
+			 &msgbufptr,
+			 sizeof(msgbufptr)))
+		return -EFAULT;
+
+	if (put_user(1U, &args->count) ||
+	    put_user(completion, &args->buf) ||
+	    put_user(1U, &args->msgbufcount))
+		return -EFAULT;
+
+	ret = vchiq_ioctl(file,
+			  VCHIQ_IOC_AWAIT_COMPLETION,
+			  (unsigned long)args);
+
+	/*
+	 * An return value of 0 here means that no messages where available
+	 * in the message queue.  In this case the native ioctl does not
+	 * return any data to the application at all.  Not even to update
+	 * msgbufcount.  This functionality needs to be kept here for
+	 * compatibility.
+	 *
+	 * Of course, < 0 means that an error occurred and no data is being
+	 * returned.
+	 *
+	 * Since count and msgbufcount was forced to 1, that means
+	 * the only other possible return value is 1. Meaning that 1 message
+	 * was available, so that multiple message case does not need to be
+	 * handled here.
+	 */
+	if (ret <= 0)
+		return ret;
+
+	if (copy_from_user(&completiontemp, completion, sizeof(*completion)))
+		return -EFAULT;
+
+	completion32.reason = completiontemp.reason;
+	completion32.header = ptr_to_compat(completiontemp.header);
+	completion32.service_userdata =
+		ptr_to_compat(completiontemp.service_userdata);
+	completion32.bulk_userdata =
+		ptr_to_compat(completiontemp.bulk_userdata);
+
+	if (copy_to_user(compat_ptr(args32.buf),
+			 &completion32,
+			 sizeof(completion32)))
+		return -EFAULT;
+
+	args32.msgbufcount--;
+
+	msgbufcount32 =
+		&((struct vchiq_await_completion32 __user *)arg)->msgbufcount;
+
+	if (copy_to_user(msgbufcount32,
+			 &args32.msgbufcount,
+			 sizeof(args32.msgbufcount)))
+		return -EFAULT;
+
+	return 1;
+}
+
+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,
+				   unsigned long arg)
+{
+	VCHIQ_DEQUEUE_MESSAGE_T *args;
+	struct vchiq_dequeue_message32 args32;
+
+	args = compat_alloc_user_space(sizeof(*args));
+	if (!args)
+		return -EFAULT;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_dequeue_message32 *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(args32.handle, &args->handle) ||
+	    put_user(args32.blocking, &args->blocking) ||
+	    put_user(args32.bufsize, &args->bufsize) ||
+	    put_user(compat_ptr(args32.buf), &args->buf))
+		return -EFAULT;
+
+	return vchiq_ioctl(file, VCHIQ_IOC_DEQUEUE_MESSAGE,
+			   (unsigned long)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,
+			      unsigned long arg)
+{
+	VCHIQ_GET_CONFIG_T *args;
+	struct vchiq_get_config32 args32;
+
+	args = compat_alloc_user_space(sizeof(*args));
+	if (!args)
+		return -EFAULT;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_get_config32 *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(args32.config_size, &args->config_size) ||
+	    put_user(compat_ptr(args32.pconfig), &args->pconfig))
+		return -EFAULT;
+
+	return vchiq_ioctl(file, VCHIQ_IOC_GET_CONFIG, (unsigned long)args);
+}
+
+#if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
+
+struct vchiq_dump_mem32 {
+	compat_uptr_t virt_addr;
+	u32 num_bytes;
+};
+
+#define VCHIQ_IOC_DUMP_PHYS_MEM32 \
+	_IOW(VCHIQ_IOC_MAGIC, 15, struct vchiq_dump_mem32)
+
+static long
+vchiq_compat_ioctl_dump_phys_mem(struct file *file,
+				 unsigned int cmd,
+				 unsigned long arg)
+{
+	VCHIQ_DUMP_MEM_T *args;
+	struct vchiq_dump_mem32 args32;
+
+	args = compat_alloc_user_space(sizeof(*args));
+	if (!args)
+		return -EFAULT;
+
+	if (copy_from_user(&args32,
+			   (struct vchiq_dump_mem32 *)arg,
+			   sizeof(args32)))
+		return -EFAULT;
+
+	if (put_user(compat_ptr(args32.virt_addr), &args->virt_addr) ||
+	    put_user(args32.num_bytes, &args->num_bytes))
+		return -EFAULT;
+
+	return vchiq_ioctl(file, VCHIQ_IOC_DUMP_PHYS_MEM, (unsigned long)args);
+}
+
+#endif
+
+static long
+vchiq_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case VCHIQ_IOC_CREATE_SERVICE32:
+		return vchiq_compat_ioctl_create_service(file, cmd, arg);
+	case VCHIQ_IOC_QUEUE_MESSAGE32:
+		return vchiq_compat_ioctl_queue_message(file, cmd, arg);
+	case VCHIQ_IOC_QUEUE_BULK_TRANSMIT32:
+	case VCHIQ_IOC_QUEUE_BULK_RECEIVE32:
+		return vchiq_compat_ioctl_queue_bulk(file, cmd, arg);
+	case VCHIQ_IOC_AWAIT_COMPLETION32:
+		return vchiq_compat_ioctl_await_completion(file, cmd, arg);
+	case VCHIQ_IOC_DEQUEUE_MESSAGE32:
+		return vchiq_compat_ioctl_dequeue_message(file, cmd, arg);
+	case VCHIQ_IOC_GET_CONFIG32:
+		return vchiq_compat_ioctl_get_config(file, cmd, arg);
+#if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
+	case VCHIQ_IOC_DUMP_PHYS_MEM32:
+		return vchiq_compat_ioctl_dump_phys_mem(file, cmd, arg);
+#endif
+	default:
+		return vchiq_ioctl(file, cmd, arg);
+	}
+}
+
+#endif
+
 /****************************************************************************
 *
 *   vchiq_open
@@ -1229,6 +1728,7 @@ static int
 vchiq_open(struct inode *inode, struct file *file)
 {
 	int dev = iminor(inode) & 0x0f;
+
 	vchiq_log_info(vchiq_arm_log_level, "vchiq_open");
 	switch (dev) {
 	case VCHIQ_MINOR: {
@@ -1284,6 +1784,7 @@ vchiq_release(struct inode *inode, struct file *file)
 {
 	int dev = iminor(inode) & 0x0f;
 	int ret = 0;
+
 	switch (dev) {
 	case VCHIQ_MINOR: {
 		VCHIQ_INSTANCE_T instance = file->private_data;
@@ -1365,6 +1866,7 @@ vchiq_release(struct inode *inode, struct file *file)
 			instance->completion_insert) {
 			VCHIQ_COMPLETION_DATA_T *completion;
 			VCHIQ_SERVICE_T *service;
+
 			completion = &instance->completions[
 				instance->completion_remove &
 				(MAX_COMPLETIONS - 1)];
@@ -1387,9 +1889,11 @@ vchiq_release(struct inode *inode, struct file *file)
 
 		{
 			struct list_head *pos, *next;
+
 			list_for_each_safe(pos, next,
 				&instance->bulk_waiter_list) {
 				struct bulk_waiter_node *waiter;
+
 				waiter = list_entry(pos,
 					struct bulk_waiter_node,
 					list);
@@ -1430,8 +1934,10 @@ vchiq_dump(void *dump_context, const char *str, int len)
 
 	if (context->actual < context->space) {
 		int copy_bytes;
+
 		if (context->offset > 0) {
 			int skip_bytes = min(len, (int)context->offset);
+
 			str += skip_bytes;
 			len -= skip_bytes;
 			context->offset -= skip_bytes;
@@ -1452,6 +1958,7 @@ vchiq_dump(void *dump_context, const char *str, int len)
 		** carriage return. */
 		if ((len == 0) && (str[copy_bytes - 1] == '\0')) {
 			char cr = '\n';
+
 			if (copy_to_user(context->buf + context->actual - 1,
 				&cr, 1))
 				context->actual = -EFAULT;
@@ -1547,6 +2054,8 @@ vchiq_dump_platform_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
 *
 ***************************************************************************/
 
+#if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
+
 static void
 dump_phys_mem(void *virt_addr, u32 num_bytes)
 {
@@ -1570,10 +2079,10 @@ dump_phys_mem(void *virt_addr, u32 num_bytes)
 	offset = (int)(long)virt_addr & (PAGE_SIZE - 1);
 	end_offset = (int)(long)end_virt_addr & (PAGE_SIZE - 1);
 
-	num_pages = (offset + num_bytes + PAGE_SIZE - 1) / PAGE_SIZE;
+	num_pages = DIV_ROUND_UP(offset + num_bytes, PAGE_SIZE);
 
 	pages = kmalloc(sizeof(struct page *) * num_pages, GFP_KERNEL);
-	if (pages == NULL) {
+	if (!pages) {
 		vchiq_log_error(vchiq_arm_log_level,
 			"Unable to allocation memory for %d pages\n",
 			num_pages);
@@ -1599,17 +2108,14 @@ dump_phys_mem(void *virt_addr, u32 num_bytes)
 	}
 
 	while (offset < end_offset) {
-
 		int page_offset = offset % PAGE_SIZE;
-		page_idx = offset / PAGE_SIZE;
 
+		page_idx = offset / PAGE_SIZE;
 		if (page_idx != prev_idx) {
-
 			if (page != NULL)
 				kunmap(page);
 			page = pages[page_idx];
 			kmapped_virt_ptr = kmap(page);
-
 			prev_idx = page_idx;
 		}
 
@@ -1632,6 +2138,8 @@ out:
 	kfree(pages);
 }
 
+#endif
+
 /****************************************************************************
 *
 *   vchiq_read
@@ -1643,6 +2151,7 @@ vchiq_read(struct file *file, char __user *buf,
 	size_t count, loff_t *ppos)
 {
 	DUMP_CONTEXT_T context;
+
 	context.buf = buf;
 	context.actual = 0;
 	context.space = count;
@@ -1673,6 +2182,9 @@ 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
@@ -1686,6 +2198,7 @@ int
 vchiq_videocore_wanted(VCHIQ_STATE_T *state)
 {
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	if (!arm_state)
 		/* autosuspend not supported - always return wanted */
 		return 1;
@@ -1753,6 +2266,7 @@ vchiq_keepalive_thread_func(void *v)
 
 	while (1) {
 		long rc = 0, uc = 0;
+
 		if (wait_for_completion_interruptible(&arm_state->ka_evt)
 				!= 0) {
 			vchiq_log_error(vchiq_susp_log_level,
@@ -1982,6 +2496,7 @@ static inline int
 need_resume(VCHIQ_STATE_T *state)
 {
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	return (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) &&
 			(arm_state->vc_resume_state < VC_RESUME_REQUESTED) &&
 			vchiq_videocore_wanted(state);
@@ -2155,6 +2670,7 @@ output_timeout_error(VCHIQ_STATE_T *state)
 	}
 	for (i = 0; i < active_services; i++) {
 		VCHIQ_SERVICE_T *service_ptr = state->services[i];
+
 		if (service_ptr && service_ptr->service_use_count &&
 			(service_ptr->srvstate != VCHIQ_SRVSTATE_FREE)) {
 			snprintf(err, sizeof(err), " %c%c%c%c(%d) service has "
@@ -2502,6 +3018,7 @@ vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 	if (ret == VCHIQ_SUCCESS) {
 		VCHIQ_STATUS_T 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);
@@ -2584,6 +3101,7 @@ void
 vchiq_on_remote_use(VCHIQ_STATE_T *state)
 {
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
 	atomic_inc(&arm_state->ka_use_count);
 	complete(&arm_state->ka_evt);
@@ -2593,6 +3111,7 @@ void
 vchiq_on_remote_release(VCHIQ_STATE_T *state)
 {
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
 	atomic_inc(&arm_state->ka_release_count);
 	complete(&arm_state->ka_evt);
@@ -2621,6 +3140,7 @@ vchiq_instance_get_use_count(VCHIQ_INSTANCE_T instance)
 {
 	VCHIQ_SERVICE_T *service;
 	int use_count = 0, i;
+
 	i = 0;
 	while ((service = next_service_by_instance(instance->state,
 		instance, &i)) != NULL) {
@@ -2647,6 +3167,7 @@ vchiq_instance_set_trace(VCHIQ_INSTANCE_T instance, int trace)
 {
 	VCHIQ_SERVICE_T *service;
 	int i;
+
 	i = 0;
 	while ((service = next_service_by_instance(instance->state,
 		instance, &i)) != NULL) {
@@ -2660,6 +3181,7 @@ static void suspend_timer_callback(unsigned long context)
 {
 	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *)context;
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	if (!arm_state)
 		goto out;
 	vchiq_log_info(vchiq_susp_log_level,
@@ -2674,6 +3196,7 @@ vchiq_use_service_no_resume(VCHIQ_SERVICE_HANDLE_T handle)
 {
 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+
 	if (service) {
 		ret = vchiq_use_internal(service->state, service,
 				USE_TYPE_SERVICE_NO_RESUME);
@@ -2687,6 +3210,7 @@ vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle)
 {
 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+
 	if (service) {
 		ret = vchiq_use_internal(service->state, service,
 				USE_TYPE_SERVICE);
@@ -2700,6 +3224,7 @@ vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle)
 {
 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
+
 	if (service) {
 		ret = vchiq_release_internal(service->state, service);
 		unlock_service(service);
@@ -2744,6 +3269,7 @@ vchiq_dump_service_use_state(VCHIQ_STATE_T *state)
 
 	for (i = 0; (i < active_services) && (j < local_max_services); i++) {
 		VCHIQ_SERVICE_T *service_ptr = state->services[i];
+
 		if (!service_ptr)
 			continue;
 
@@ -2832,12 +3358,14 @@ void vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state,
 	VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate)
 {
 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
+
 	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) {
 		write_lock_bh(&arm_state->susp_res_lock);
 		if (!arm_state->first_connect) {
 			char threadname[16];
+
 			arm_state->first_connect = 1;
 			write_unlock_bh(&arm_state->susp_res_lock);
 			snprintf(threadname, sizeof(threadname), "vchiq-keep/%d",
@@ -2962,6 +3490,6 @@ static struct platform_driver vchiq_driver = {
 };
 module_platform_driver(vchiq_driver);
 
-MODULE_LICENSE("GPL");
+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_core.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
index d587097b261c..4f9e738abddf 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -192,6 +192,7 @@ VCHIQ_SERVICE_T *
 find_service_by_port(VCHIQ_STATE_T *state, int localport)
 {
 	VCHIQ_SERVICE_T *service = NULL;
+
 	if ((unsigned int)localport <= VCHIQ_PORT_MAX) {
 		spin_lock(&service_spinlock);
 		service = state->services[localport];
@@ -268,6 +269,7 @@ next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance,
 	spin_lock(&service_spinlock);
 	while (idx < state->unused_service) {
 		VCHIQ_SERVICE_T *srv = state->services[idx++];
+
 		if (srv && (srv->srvstate != VCHIQ_SRVSTATE_FREE) &&
 			(srv->instance == instance)) {
 			service = srv;
@@ -381,6 +383,7 @@ make_service_callback(VCHIQ_SERVICE_T *service, VCHIQ_REASON_T reason,
 	VCHIQ_HEADER_T *header, void *bulk_userdata)
 {
 	VCHIQ_STATUS_T 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);
@@ -399,6 +402,7 @@ inline void
 vchiq_set_conn_state(VCHIQ_STATE_T *state, VCHIQ_CONNSTATE_T newstate)
 {
 	VCHIQ_CONNSTATE_T oldstate = state->conn_state;
+
 	vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id,
 		conn_state_names[oldstate],
 		conn_state_names[newstate]);
@@ -485,6 +489,7 @@ get_listening_service(VCHIQ_STATE_T *state, int fourcc)
 
 	for (i = 0; i < state->unused_service; i++) {
 		VCHIQ_SERVICE_T *service = state->services[i];
+
 		if (service &&
 			(service->public_fourcc == fourcc) &&
 			((service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
@@ -503,8 +508,10 @@ static VCHIQ_SERVICE_T *
 get_connected_service(VCHIQ_STATE_T *state, unsigned int port)
 {
 	int i;
+
 	for (i = 0; i < state->unused_service; i++) {
 		VCHIQ_SERVICE_T *service = state->services[i];
+
 		if (service && (service->srvstate == VCHIQ_SRVSTATE_OPEN)
 			&& (service->remoteport == port)) {
 			lock_service(service);
@@ -645,11 +652,13 @@ process_free_queue(VCHIQ_STATE_T *state)
 			VCHIQ_HEADER_T *header =
 				(VCHIQ_HEADER_T *)(data + pos);
 			int msgid = header->msgid;
+
 			if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
 				int port = VCHIQ_MSG_SRCPORT(msgid);
 				VCHIQ_SERVICE_QUOTA_T *service_quota =
 					&state->service_quotas[port];
 				int count;
+
 				spin_lock(&quota_spinlock);
 				count = service_quota->message_use_count;
 				if (count > 0)
@@ -719,6 +728,7 @@ process_free_queue(VCHIQ_STATE_T *state)
 
 		if (data_found) {
 			int count;
+
 			spin_lock(&quota_spinlock);
 			count = state->data_use_count;
 			if (count > 0)
@@ -745,9 +755,7 @@ memcpy_copy_callback(
 	void *context, void *dest,
 	size_t offset, size_t maxsize)
 {
-	void *src = context;
-
-	memcpy(dest + offset, src + offset, maxsize);
+	memcpy(dest + offset, context + offset, maxsize);
 	return maxsize;
 }
 
@@ -1059,6 +1067,7 @@ queue_message_sync(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
 
 	{
 		int oldmsgid = header->msgid;
+
 		if (oldmsgid != VCHIQ_MSGID_PADDING)
 			vchiq_log_error(vchiq_core_log_level,
 				"%d: qms - msgid %x, not PADDING",
@@ -1143,6 +1152,7 @@ release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info,
 
 	if (header) {
 		int msgid = header->msgid;
+
 		if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) ||
 			(service && service->closing)) {
 			mutex_unlock(&state->recycle_mutex);
@@ -1252,6 +1262,7 @@ notify_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue,
 				}
 				if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING) {
 					struct bulk_waiter *waiter;
+
 					spin_lock(&bulk_waiter_spinlock);
 					waiter = bulk->userdata;
 					if (waiter) {
@@ -1301,6 +1312,7 @@ poll_services(VCHIQ_STATE_T *state)
 
 	for (group = 0; group < BITSET_SIZE(state->unused_service); group++) {
 		u32 flags;
+
 		flags = atomic_xchg(&state->poll_services[group], 0);
 		for (i = 0; flags; i++) {
 			if (flags & (1 << i)) {
@@ -1308,6 +1320,7 @@ poll_services(VCHIQ_STATE_T *state)
 					find_service_by_port(state,
 						(group<<5) + i);
 				u32 service_flags;
+
 				flags &= ~(1 << i);
 				if (!service)
 					continue;
@@ -1421,6 +1434,7 @@ static void
 abort_outstanding_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *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',
@@ -1484,6 +1498,7 @@ static void
 resume_bulks(VCHIQ_STATE_T *state)
 {
 	int i;
+
 	if (unlikely(atomic_dec_return(&pause_bulks_count) != 0)) {
 		WARN_ON_ONCE(1);
 		atomic_set(&pause_bulks_count, 0);
@@ -1507,6 +1522,7 @@ resume_bulks(VCHIQ_STATE_T *state)
 		VCHIQ_SERVICE_T *service = state->services[i];
 		int resolved_rx = 0;
 		int resolved_tx = 0;
+
 		if (!service || (service->srvstate != VCHIQ_SRVSTATE_OPEN))
 			continue;
 
@@ -1550,6 +1566,7 @@ parse_open(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header)
 			/* A matching service exists */
 			short version = payload->version;
 			short version_min = payload->version_min;
+
 			if ((service->version < version_min) ||
 				(version < service->version_min)) {
 				/* Version mismatch */
@@ -1651,6 +1668,7 @@ parse_rx_slots(VCHIQ_STATE_T *state)
 	VCHIQ_SHARED_STATE_T *remote = state->remote;
 	VCHIQ_SERVICE_T *service = NULL;
 	int tx_pos;
+
 	DEBUG_INITIALISE(state->local)
 
 	tx_pos = remote->tx_pos;
@@ -1664,6 +1682,7 @@ parse_rx_slots(VCHIQ_STATE_T *state)
 		DEBUG_TRACE(PARSE_LINE);
 		if (!state->rx_data) {
 			int rx_index;
+
 			WARN_ON(!((state->rx_pos & VCHIQ_SLOT_MASK) == 0));
 			rx_index = remote->slot_queue[
 				SLOT_QUEUE_INDEX_FROM_POS(state->rx_pos) &
@@ -1841,6 +1860,7 @@ parse_rx_slots(VCHIQ_STATE_T *state)
 		case VCHIQ_MSG_BULK_RX:
 		case VCHIQ_MSG_BULK_TX: {
 			VCHIQ_BULK_QUEUE_T *queue;
+
 			WARN_ON(!state->is_master);
 			queue = (type == VCHIQ_MSG_BULK_RX) ?
 				&service->bulk_tx : &service->bulk_rx;
@@ -2054,6 +2074,7 @@ slot_handler_func(void *v)
 {
 	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
 	VCHIQ_SHARED_STATE_T *local = state->local;
+
 	DEBUG_INITIALISE(local)
 
 	while (1) {
@@ -2553,131 +2574,126 @@ vchiq_add_service_internal(VCHIQ_STATE_T *state,
 	VCHIQ_INSTANCE_T instance, VCHIQ_USERDATA_TERM_T userdata_term)
 {
 	VCHIQ_SERVICE_T *service;
+	VCHIQ_SERVICE_T **pservice = NULL;
+	VCHIQ_SERVICE_QUOTA_T *service_quota;
+	int i;
 
 	service = kmalloc(sizeof(VCHIQ_SERVICE_T), GFP_KERNEL);
-	if (service) {
-		service->base.fourcc   = params->fourcc;
-		service->base.callback = params->callback;
-		service->base.userdata = params->userdata;
-		service->handle        = VCHIQ_SERVICE_HANDLE_INVALID;
-		service->ref_count     = 1;
-		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->client_id     = 0;
-		service->auto_close    = 1;
-		service->sync          = 0;
-		service->closing       = 0;
-		service->trace         = 0;
-		atomic_set(&service->poll_flags, 0);
-		service->version       = params->version;
-		service->version_min   = params->version_min;
-		service->state         = state;
-		service->instance      = instance;
-		service->service_use_count = 0;
-		init_bulk_queue(&service->bulk_tx);
-		init_bulk_queue(&service->bulk_rx);
-		sema_init(&service->remove_event, 0);
-		sema_init(&service->bulk_remove_event, 0);
-		mutex_init(&service->bulk_mutex);
-		memset(&service->stats, 0, sizeof(service->stats));
-	} else {
-		vchiq_log_error(vchiq_core_log_level,
-			"Out of memory");
-	}
-
-	if (service) {
-		VCHIQ_SERVICE_T **pservice = NULL;
-		int i;
-
-		/* Although it is perfectly possible to use service_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++) {
-				VCHIQ_SERVICE_T *srv = state->services[i];
-				if (!srv) {
-					pservice = &state->services[i];
-					break;
-				}
-			}
-		} else {
-			for (i = (state->unused_service - 1); i >= 0; i--) {
-				VCHIQ_SERVICE_T *srv = 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;
-				}
+	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;
+	service->ref_count     = 1;
+	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->client_id     = 0;
+	service->auto_close    = 1;
+	service->sync          = 0;
+	service->closing       = 0;
+	service->trace         = 0;
+	atomic_set(&service->poll_flags, 0);
+	service->version       = params->version;
+	service->version_min   = params->version_min;
+	service->state         = state;
+	service->instance      = instance;
+	service->service_use_count = 0;
+	init_bulk_queue(&service->bulk_tx);
+	init_bulk_queue(&service->bulk_rx);
+	sema_init(&service->remove_event, 0);
+	sema_init(&service->bulk_remove_event, 0);
+	mutex_init(&service->bulk_mutex);
+	memset(&service->stats, 0, sizeof(service->stats));
+
+	/* Although it is perfectly possible to use service_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++) {
+			VCHIQ_SERVICE_T *srv = state->services[i];
+
+			if (!srv) {
+				pservice = &state->services[i];
+				break;
 			}
 		}
-
-		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;
-			*pservice = service;
-			if (pservice == &state->services[state->unused_service])
-				state->unused_service++;
-		}
-
-		mutex_unlock(&state->mutex);
-
-		if (!pservice) {
-			kfree(service);
-			service = NULL;
+	} else {
+		for (i = (state->unused_service - 1); i >= 0; i--) {
+			VCHIQ_SERVICE_T *srv = 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;
+			}
 		}
 	}
 
-	if (service) {
-		VCHIQ_SERVICE_QUOTA_T *service_quota =
-			&state->service_quotas[service->localport];
-		service_quota->slot_quota = state->default_slot_quota;
-		service_quota->message_quota = state->default_message_quota;
-		if (service_quota->slot_use_count == 0)
-			service_quota->previous_tx_index =
-				SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos)
-				- 1;
+	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;
+		*pservice = service;
+		if (pservice == &state->services[state->unused_service])
+			state->unused_service++;
+	}
 
-		/* Bring this service online */
-		vchiq_set_service_state(service, srvstate);
+	mutex_unlock(&state->mutex);
 
-		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);
+	if (!pservice) {
+		kfree(service);
+		return NULL;
 	}
 
+	service_quota = &state->service_quotas[service->localport];
+	service_quota->slot_quota = state->default_slot_quota;
+	service_quota->message_quota = state->default_message_quota;
+	if (service_quota->slot_use_count == 0)
+		service_quota->previous_tx_index =
+			SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos)
+			- 1;
+
+	/* Bring this service online */
+	vchiq_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;
@@ -2766,6 +2782,7 @@ release_service_messages(VCHIQ_SERVICE_T *service)
 					(VCHIQ_HEADER_T *)(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,
@@ -3498,6 +3515,7 @@ vchiq_release_message(VCHIQ_SERVICE_HANDLE_T handle, VCHIQ_HEADER_T *header)
 	if ((slot_index >= remote->slot_first) &&
 		(slot_index <= remote->slot_last)) {
 		int msgid = header->msgid;
+
 		if (msgid & VCHIQ_MSGID_CLAIMED) {
 			VCHIQ_SLOT_INFO_T *slot_info =
 				SLOT_INFO_FROM_INDEX(state, slot_index);
@@ -3656,9 +3674,9 @@ vchiq_dump_shared_state(void *dump_context, VCHIQ_STATE_T *state,
 		"COMPLETION_QUEUE_FULL_COUNT"
 	};
 	int i;
-
 	char buf[80];
 	int len;
+
 	len = snprintf(buf, sizeof(buf),
 		"  %s: slots %d-%d tx_pos=%x recycle=%x",
 		label, shared->slot_first, shared->slot_last,
@@ -3762,9 +3780,11 @@ vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
 			&service->state->service_quotas[service->localport];
 		int fourcc = service->base.fourcc;
 		int tx_pending, rx_pending;
+
 		if (service->remoteport != VCHIQ_PORT_FREE) {
 			int len2 = snprintf(remoteport, sizeof(remoteport),
 				"%u", service->remoteport);
+
 			if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
 				snprintf(remoteport + len2,
 					sizeof(remoteport) - len2,
@@ -3866,6 +3886,7 @@ vchiq_loud_error_footer(void)
 VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T *state)
 {
 	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+
 	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
 		status = queue_message(state, NULL,
 			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0),
@@ -3876,6 +3897,7 @@ VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T *state)
 VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T *state)
 {
 	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+
 	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
 		status = queue_message(state, NULL,
 			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_RELEASE, 0, 0),
@@ -3886,6 +3908,7 @@ VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T *state)
 VCHIQ_STATUS_T vchiq_send_remote_use_active(VCHIQ_STATE_T *state)
 {
 	VCHIQ_STATUS_T status = VCHIQ_RETRY;
+
 	if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
 		status = queue_message(state, NULL,
 			VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0),
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
index f07cd4448ddf..9367a9a5aa3c 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
@@ -81,9 +81,7 @@ static struct vchiq_debugfs_log_entry vchiq_debugfs_log_entries[] = {
 	{ "susp", &vchiq_susp_log_level },
 	{ "arm",  &vchiq_arm_log_level },
 };
-static int n_log_entries =
-	sizeof(vchiq_debugfs_log_entries)/sizeof(vchiq_debugfs_log_entries[0]);
-
+static int n_log_entries = ARRAY_SIZE(vchiq_debugfs_log_entries);
 
 static struct dentry *vchiq_clients_top(void);
 static struct dentry *vchiq_debugfs_top(void);
@@ -167,6 +165,7 @@ static int vchiq_debugfs_create_log_entries(struct dentry *top)
 	struct dentry *dir;
 	size_t i;
 	int ret = 0;
+
 	dir = debugfs_create_dir("log", vchiq_debugfs_top());
 	if (!dir)
 		return -ENOMEM;
@@ -174,6 +173,7 @@ static int vchiq_debugfs_create_log_entries(struct dentry *top)
 
 	for (i = 0; i < n_log_entries; i++) {
 		void *levp = (void *)vchiq_debugfs_log_entries[i].plevel;
+
 		dir = debugfs_create_file(vchiq_debugfs_log_entries[i].name,
 					  0644,
 					  debugfs_info.log_categories,
@@ -312,6 +312,7 @@ fail_top:
 void vchiq_debugfs_remove_instance(VCHIQ_INSTANCE_T instance)
 {
 	VCHIQ_DEBUGFS_NODE_T *node = vchiq_instance_get_debugfs_node(instance);
+
 	debugfs_remove_recursive(node->dentry);
 }
 
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h
index 377e8e48bb54..0e270852900d 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h
@@ -88,10 +88,10 @@ typedef struct vchiq_header_struct {
 	char data[0];           /* message */
 } VCHIQ_HEADER_T;
 
-typedef struct {
+struct vchiq_element {
 	const void *data;
 	unsigned int size;
-} VCHIQ_ELEMENT_T;
+};
 
 typedef unsigned int VCHIQ_SERVICE_HANDLE_T;
 
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
index 6137ae9de1c1..9f859953f45c 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
@@ -50,7 +50,7 @@ typedef struct {
 typedef struct {
 	unsigned int handle;
 	unsigned int count;
-	const VCHIQ_ELEMENT_T *elements;
+	const struct vchiq_element *elements;
 } VCHIQ_QUEUE_MESSAGE_T;
 
 typedef struct {
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
index 4317c06943a6..34f746db19cd 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_kern_lib.c
@@ -147,9 +147,11 @@ VCHIQ_STATUS_T vchiq_shutdown(VCHIQ_INSTANCE_T instance)
 
 	if (status == VCHIQ_SUCCESS) {
 		struct list_head *pos, *next;
+
 		list_for_each_safe(pos, next,
 				&instance->bulk_waiter_list) {
 			struct bulk_waiter_node *waiter;
+
 			waiter = list_entry(pos,
 					struct bulk_waiter_node,
 					list);
@@ -406,6 +408,7 @@ vchiq_blocking_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle, void *data,
 
 	if (waiter) {
 		VCHIQ_BULK_T *bulk = waiter->bulk_waiter.bulk;
+
 		if (bulk) {
 			/* This thread has an outstanding bulk transfer. */
 			if ((bulk->data != data) ||
@@ -435,6 +438,7 @@ vchiq_blocking_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle, void *data,
 	if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
 		!waiter->bulk_waiter.bulk) {
 		VCHIQ_BULK_T *bulk = waiter->bulk_waiter.bulk;
+
 		if (bulk) {
 			/* Cancel the signal when the transfer
 			 ** completes. */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_memdrv.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
index dd43458f306f..c233b866725b 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_memdrv.h
@@ -41,22 +41,10 @@
 
 /* ---- Constants and Types ---------------------------------------------- */
 
-typedef struct {
-	 void                   *arm_shared_mem_virt;
-	 dma_addr_t              arm_shared_mem_phys;
-	 size_t                  arm_shared_mem_size;
-
-	 void                   *vc_shared_mem_virt;
-	 dma_addr_t              vc_shared_mem_phys;
-	 size_t                  vc_shared_mem_size;
-} VCHIQ_SHARED_MEM_INFO_T;
-
 /* ---- Variable Externs ------------------------------------------------- */
 
 /* ---- Function Prototypes ---------------------------------------------- */
 
-void vchiq_get_shared_mem_info(VCHIQ_SHARED_MEM_INFO_T *info);
-
 VCHIQ_STATUS_T vchiq_memdrv_initialise(void);
 
 VCHIQ_STATUS_T vchiq_userdrv_create_instance(
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
index 12c304ceb952..926c247fd9d3 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_pagelist.h
@@ -34,9 +34,6 @@
 #ifndef VCHIQ_PAGELIST_H
 #define VCHIQ_PAGELIST_H
 
-#ifndef PAGE_SIZE
-#define PAGE_SIZE 4096
-#endif
 #define CACHE_LINE_SIZE 32
 #define PAGELIST_WRITE 0
 #define PAGELIST_READ 1
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c
index 48984abc3854..8af95fc361ed 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c
@@ -556,6 +556,7 @@ EXPORT_SYMBOL(vchi_connect);
 int32_t vchi_disconnect(VCHI_INSTANCE_T instance_handle)
 {
 	VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
+
 	return vchiq_status_to_vchi(vchiq_shutdown(instance));
 }
 EXPORT_SYMBOL(vchi_disconnect);
@@ -733,6 +734,7 @@ int32_t vchi_service_close(const VCHI_SERVICE_HANDLE_T handle)
 {
 	int32_t ret = -1;
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+
 	if (service) {
 		VCHIQ_STATUS_T status = vchiq_close_service(service->handle);
 		if (status == VCHIQ_SUCCESS) {
@@ -750,8 +752,10 @@ int32_t vchi_service_destroy(const VCHI_SERVICE_HANDLE_T handle)
 {
 	int32_t ret = -1;
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+
 	if (service) {
 		VCHIQ_STATUS_T status = vchiq_remove_service(service->handle);
+
 		if (status == VCHIQ_SUCCESS) {
 			service_free(service);
 			service = NULL;
@@ -770,6 +774,7 @@ int32_t vchi_service_set_option(const VCHI_SERVICE_HANDLE_T handle,
 	int32_t ret = -1;
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 	VCHIQ_SERVICE_OPTION_T vchiq_option;
+
 	switch (option) {
 	case VCHI_SERVICE_OPTION_TRACE:
 		vchiq_option = VCHIQ_SERVICE_OPTION_TRACE;
@@ -797,6 +802,7 @@ int32_t vchi_get_peer_version(const VCHI_SERVICE_HANDLE_T handle, short *peer_ve
 {
 	int32_t ret = -1;
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
+
 	if (service)
 	{
 		VCHIQ_STATUS_T status;
@@ -808,54 +814,6 @@ int32_t vchi_get_peer_version(const VCHI_SERVICE_HANDLE_T handle, short *peer_ve
 }
 EXPORT_SYMBOL(vchi_get_peer_version);
 
-/* ----------------------------------------------------------------------
- * read a uint32_t from buffer.
- * network format is defined to be little endian
- * -------------------------------------------------------------------- */
-uint32_t
-vchi_readbuf_uint32(const void *_ptr)
-{
-	const unsigned char *ptr = _ptr;
-	return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24);
-}
-
-/* ----------------------------------------------------------------------
- * write a uint32_t to buffer.
- * network format is defined to be little endian
- * -------------------------------------------------------------------- */
-void
-vchi_writebuf_uint32(void *_ptr, uint32_t value)
-{
-	unsigned char *ptr = _ptr;
-	ptr[0] = (unsigned char)((value >> 0)  & 0xFF);
-	ptr[1] = (unsigned char)((value >> 8)  & 0xFF);
-	ptr[2] = (unsigned char)((value >> 16) & 0xFF);
-	ptr[3] = (unsigned char)((value >> 24) & 0xFF);
-}
-
-/* ----------------------------------------------------------------------
- * read a uint16_t from buffer.
- * network format is defined to be little endian
- * -------------------------------------------------------------------- */
-uint16_t
-vchi_readbuf_uint16(const void *_ptr)
-{
-	const unsigned char *ptr = _ptr;
-	return ptr[0] | (ptr[1] << 8);
-}
-
-/* ----------------------------------------------------------------------
- * write a uint16_t into the buffer.
- * network format is defined to be little endian
- * -------------------------------------------------------------------- */
-void
-vchi_writebuf_uint16(void *_ptr, uint16_t value)
-{
-	unsigned char *ptr = _ptr;
-	ptr[0] = (value >> 0)  & 0xFF;
-	ptr[1] = (value >> 8)  & 0xFF;
-}
-
 /***********************************************************
  * Name: vchi_service_use
  *
@@ -869,6 +827,7 @@ vchi_writebuf_uint16(void *_ptr, uint16_t value)
 int32_t vchi_service_use(const VCHI_SERVICE_HANDLE_T handle)
 {
 	int32_t ret = -1;
+
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 	if (service)
 		ret = vchiq_status_to_vchi(vchiq_use_service(service->handle));
@@ -889,6 +848,7 @@ EXPORT_SYMBOL(vchi_service_use);
 int32_t vchi_service_release(const VCHI_SERVICE_HANDLE_T handle)
 {
 	int32_t ret = -1;
+
 	SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 	if (service)
 		ret = vchiq_status_to_vchi(
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c
index e0ba0ed704fd..7fa0310e7b9e 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.c
@@ -52,7 +52,7 @@ int vchiu_queue_init(VCHIU_QUEUE_T *queue, int size)
 	sema_init(&queue->push, 0);
 
 	queue->storage = kzalloc(size * sizeof(VCHIQ_HEADER_T *), GFP_KERNEL);
-	if (queue->storage == NULL) {
+	if (!queue->storage) {
 		vchiu_queue_delete(queue);
 		return 0;
 	}
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.h
index e63964f5a18a..5a1540d349d3 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_util.h
@@ -44,7 +44,6 @@
 #include <linux/jiffies.h>
 #include <linux/delay.h>
 #include <linux/string.h>
-#include <linux/types.h>
 #include <linux/interrupt.h>
 #include <linux/random.h>
 #include <linux/sched/signal.h>
@@ -52,7 +51,6 @@
 #include <linux/uaccess.h>
 #include <linux/time.h>  /* for time_t */
 #include <linux/slab.h>
-#include <linux/vmalloc.h>
 
 #include "vchiq_if.h"
 
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_version.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_version.c
index b6bfa21155e4..994b81798134 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_version.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_version.c
@@ -33,27 +33,27 @@
 #include "vchiq_build_info.h"
 #include <linux/broadcom/vc_debug_sym.h>
 
-VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_hostname, "dc4-arm-01" );
-VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_version, "9245b4c35b99b3870e1f7dc598c5692b3c66a6f0 (tainted)" );
-VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_time,    __TIME__ );
-VC_DEBUG_DECLARE_STRING_VAR( vchiq_build_date,    __DATE__ );
+VC_DEBUG_DECLARE_STRING_VAR(vchiq_build_hostname, "dc4-arm-01");
+VC_DEBUG_DECLARE_STRING_VAR(vchiq_build_version, "9245b4c35b99b3870e1f7dc598c5692b3c66a6f0 (tainted)");
+VC_DEBUG_DECLARE_STRING_VAR(vchiq_build_time,    __TIME__);
+VC_DEBUG_DECLARE_STRING_VAR(vchiq_build_date,    __DATE__);
 
-const char *vchiq_get_build_hostname( void )
+const char *vchiq_get_build_hostname(void)
 {
    return vchiq_build_hostname;
 }
 
-const char *vchiq_get_build_version( void )
+const char *vchiq_get_build_version(void)
 {
    return vchiq_build_version;
 }
 
-const char *vchiq_get_build_date( void )
+const char *vchiq_get_build_date(void)
 {
    return vchiq_build_date;
 }
 
-const char *vchiq_get_build_time( void )
+const char *vchiq_get_build_time(void)
 {
    return vchiq_build_time;
 }
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 69e9a7705afb..a3d4610fbdbe 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -17,7 +17,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/atomic.h>
+#include <linux/refcount.h>
 #include <linux/cdev.h>
 #include <linux/delay.h>
 #include <linux/device.h>
@@ -118,7 +118,7 @@ static const int type[VME_DEVS] = {	MASTER_MINOR,	MASTER_MINOR,
 
 struct vme_user_vma_priv {
 	unsigned int minor;
-	atomic_t refcnt;
+	refcount_t refcnt;
 };
 
 static ssize_t resource_to_user(int minor, char __user *buf, size_t count,
@@ -430,7 +430,7 @@ static void vme_user_vm_open(struct vm_area_struct *vma)
 {
 	struct vme_user_vma_priv *vma_priv = vma->vm_private_data;
 
-	atomic_inc(&vma_priv->refcnt);
+	refcount_inc(&vma_priv->refcnt);
 }
 
 static void vme_user_vm_close(struct vm_area_struct *vma)
@@ -438,7 +438,7 @@ 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 (!atomic_dec_and_test(&vma_priv->refcnt))
+	if (!refcount_dec_and_test(&vma_priv->refcnt))
 		return;
 
 	mutex_lock(&image[minor].mutex);
@@ -473,7 +473,7 @@ static int vme_user_master_mmap(unsigned int minor, struct vm_area_struct *vma)
 	}
 
 	vma_priv->minor = minor;
-	atomic_set(&vma_priv->refcnt, 1);
+	refcount_set(&vma_priv->refcnt, 1);
 	vma->vm_ops = &vme_user_vm_ops;
 	vma->vm_private_data = vma_priv;
 
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index c351e03f6ad2..feaf222574ee 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -63,16 +63,16 @@ BBuGetFrameTime(
 	unsigned short wRate
 );
 
-void vnt_get_phy_field(struct vnt_private *, u32 frame_length,
-		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *);
+void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy);
 
-bool BBbReadEmbedded(struct vnt_private *, unsigned char byBBAddr,
+bool BBbReadEmbedded(struct vnt_private *priv, unsigned char byBBAddr,
 		     unsigned char *pbyData);
-bool BBbWriteEmbedded(struct vnt_private *, unsigned char byBBAddr,
+bool BBbWriteEmbedded(struct vnt_private *priv, unsigned char byBBAddr,
 		      unsigned char byData);
 
-void BBvSetShortSlotTime(struct vnt_private *);
-void BBvSetVGAGainOffset(struct vnt_private *, unsigned char byData);
+void BBvSetShortSlotTime(struct vnt_private *priv);
+void BBvSetVGAGainOffset(struct vnt_private *priv, unsigned char byData);
 
 /* VT3253 Baseband */
 bool BBbVT3253Init(struct vnt_private *priv);
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index e0c92818ed70..5463cf869d1b 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -524,22 +524,22 @@ CARDvSafeResetTx(
 	struct vnt_tx_desc *pCurrTD;
 
 	/* initialize TD index */
-	priv->apTailTD[0] = &(priv->apTD0Rings[0]);
-	priv->apCurrTD[0] = &(priv->apTD0Rings[0]);
+	priv->apTailTD[0] = &priv->apTD0Rings[0];
+	priv->apCurrTD[0] = &priv->apTD0Rings[0];
 
-	priv->apTailTD[1] = &(priv->apTD1Rings[0]);
-	priv->apCurrTD[1] = &(priv->apTD1Rings[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 = &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 = &priv->apTD1Rings[uu];
 		pCurrTD->td0.owner = OWNED_BY_HOST;
 		/* init all Tx Packet pointer to NULL */
 	}
@@ -575,12 +575,12 @@ CARDvSafeResetRx(
 	struct vnt_rx_desc *pDesc;
 
 	/* initialize RD index */
-	priv->pCurrRD[0] = &(priv->aRD0Ring[0]);
-	priv->pCurrRD[1] = &(priv->aRD1Ring[0]);
+	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 = &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);
@@ -588,7 +588,7 @@ CARDvSafeResetRx(
 
 	/* init state, all RD is chip's */
 	for (uu = 0; uu < priv->opts.rx_descs1; uu++) {
-		pDesc = &(priv->aRD1Ring[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);
@@ -911,16 +911,13 @@ bool CARDbSoftwareReset(struct vnt_private *priv)
  */
 u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
 {
-	u64 qwTSFOffset = 0;
 	unsigned short wRxBcnTSFOffst;
 
 	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
 
 	qwTSF2 += (u64)wRxBcnTSFOffst;
 
-	qwTSFOffset = qwTSF1 - qwTSF2;
-
-	return qwTSFOffset;
+	return qwTSF1 - qwTSF2;
 }
 
 /*
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index b6e853784a26..37600093cab2 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -30,7 +30,7 @@
 /*---------------------  Export Definitions -------------------------*/
 /*
  * Baseband RF pair definition in eeprom (Bits 6..0)
-*/
+ */
 #define RF_RFMD2959             0x01
 #define RF_MAXIMAG              0x02
 #define RF_AIROHA               0x03
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
index 89de67115826..095258923ebd 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -187,10 +187,10 @@ struct vnt_tx_short_buf_head {
 	__le16 time_stamp_off;
 } __packed;
 
-int vnt_generate_fifo_header(struct vnt_private *, u32,
-			     struct vnt_tx_desc *head_td, struct sk_buff *);
-int vnt_beacon_make(struct vnt_private *, struct ieee80211_vif *);
-int vnt_beacon_enable(struct vnt_private *, struct ieee80211_vif *,
-		      struct ieee80211_bss_conf *);
+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/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 9e074e9daf4e..028f54b453d0 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -407,16 +407,6 @@ static void vnt_free_rx_bufs(struct vnt_private *priv)
 	}
 }
 
-static void usb_device_reset(struct vnt_private *priv)
-{
-	int status;
-
-	status = usb_reset_device(priv->usb);
-	if (status)
-		dev_warn(&priv->usb->dev,
-			 "usb_device_reset fail status=%d\n", status);
-}
-
 static void vnt_free_int_bufs(struct vnt_private *priv)
 {
 	kfree(priv->int_buf.data_buf);
@@ -995,7 +985,10 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 
 	SET_IEEE80211_DEV(priv->hw, &intf->dev);
 
-	usb_device_reset(priv);
+	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);
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index 068c1c89f653..23581afb4211 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -771,7 +771,7 @@ int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
 			ret &= vnt_rf_write_embedded(priv, 0x015C0800);
 		} else {
 			dev_dbg(&priv->usb->dev,
-				"@@@@ vnt_rf_set_txpower> 11G mode\n");
+				"@@@@ %s> 11G mode\n", __func__);
 
 			power_setting = ((0x3f - power) << 20) | (0x7 << 8);
 
@@ -876,7 +876,7 @@ void vnt_rf_table_download(struct vnt_private *priv)
 	memcpy(array, addr1, length1);
 
 	vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
-		        MESSAGE_REQUEST_RF_INIT, length1, array);
+			MESSAGE_REQUEST_RF_INIT, length1, array);
 
 	/* Channel Table 0 */
 	value = 0;
@@ -889,7 +889,7 @@ void vnt_rf_table_download(struct vnt_private *priv)
 		memcpy(array, addr2, length);
 
 		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
-			        value, MESSAGE_REQUEST_RF_CH0, length, array);
+				value, MESSAGE_REQUEST_RF_CH0, length, array);
 
 		length2 -= length;
 		value += length;
@@ -907,7 +907,7 @@ void vnt_rf_table_download(struct vnt_private *priv)
 		memcpy(array, addr3, length);
 
 		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
-			        value, MESSAGE_REQUEST_RF_CH1, length, array);
+				value, MESSAGE_REQUEST_RF_CH1, length, array);
 
 		length3 -= length;
 		value += length;
@@ -924,7 +924,7 @@ void vnt_rf_table_download(struct vnt_private *priv)
 
 		/* Init Table 2 */
 		vnt_control_out(priv, MESSAGE_TYPE_WRITE,
-			        0, MESSAGE_REQUEST_RF_INIT2, length1, array);
+				0, MESSAGE_REQUEST_RF_INIT2, length1, array);
 
 		/* Channel Table 0 */
 		value = 0;
@@ -937,7 +937,8 @@ void vnt_rf_table_download(struct vnt_private *priv)
 			memcpy(array, addr2, length);
 
 			vnt_control_out(priv, MESSAGE_TYPE_WRITE,
-				        value, MESSAGE_REQUEST_RF_CH2, length, array);
+					value, MESSAGE_REQUEST_RF_CH2,
+					length, array);
 
 			length2 -= length;
 			value += length;
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 1835cd13ef49..63413492e61d 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -114,7 +114,7 @@ static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
 }
 
 static u32 vnt_get_rsvtime(struct vnt_private *priv, u8 pkt_type,
-	                    u32 frame_length, u16 rate, int need_ack)
+			    u32 frame_length, u16 rate, int need_ack)
 {
 	u32 data_time, ack_time;
 
@@ -135,14 +135,14 @@ static u32 vnt_get_rsvtime(struct vnt_private *priv, u8 pkt_type,
 }
 
 static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
-	                             u32 frame_length, u16 rate, int need_ack)
+				     u32 frame_length, u16 rate, int need_ack)
 {
 	return cpu_to_le16((u16)vnt_get_rsvtime(priv, pkt_type,
 		frame_length, rate, need_ack));
 }
 
 static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv,
-	                                 u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate)
+					 u8 rsv_type, u8 pkt_type, u32 frame_length, u16 current_rate)
 {
 	u32 rrv_time, rts_time, cts_time, ack_time, data_time;
 
@@ -160,19 +160,19 @@ static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv,
 		rts_time = vnt_get_frame_time(priv->preamble_type,
 			pkt_type, 20, priv->top_cck_basic_rate);
 		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-			                      14, priv->top_cck_basic_rate);
+					      14, priv->top_cck_basic_rate);
 		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-			                      14, priv->top_ofdm_basic_rate);
+					      14, priv->top_ofdm_basic_rate);
 	} else if (rsv_type == 2) {
 		rts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-			                      20, priv->top_ofdm_basic_rate);
+					      20, priv->top_ofdm_basic_rate);
 		cts_time = ack_time = vnt_get_frame_time(priv->preamble_type,
 			pkt_type, 14, priv->top_ofdm_basic_rate);
 	} else if (rsv_type == 3) {
 		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-			                      14, priv->top_cck_basic_rate);
+					      14, priv->top_cck_basic_rate);
 		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-			                      14, priv->top_ofdm_basic_rate);
+					      14, priv->top_ofdm_basic_rate);
 
 		rrv_time = cts_time + ack_time + data_time + 2 * priv->sifs;
 
@@ -227,7 +227,7 @@ static __le16 vnt_get_rtscts_duration_le(struct vnt_usb_send_context *context,
 	case RTSDUR_AA_F0:
 	case RTSDUR_AA_F1:
 		cts_time = vnt_get_frame_time(priv->preamble_type,
-				              pkt_type, 14, priv->top_ofdm_basic_rate);
+					      pkt_type, 14, priv->top_ofdm_basic_rate);
 		dur_time = cts_time + 2 * priv->sifs +
 			vnt_get_rsvtime(priv, pkt_type,
 					frame_length, rate, need_ack);
@@ -410,7 +410,7 @@ static u16 vnt_rxtx_rts_g_head(struct vnt_usb_send_context *tx_context,
 	u16 current_rate = tx_context->tx_rate;
 
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_cck_basic_rate,
-		          PK_TYPE_11B, &buf->b);
+			  PK_TYPE_11B, &buf->b);
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
 			  tx_context->pkt_type, &buf->a);
 
@@ -437,7 +437,7 @@ static u16 vnt_rxtx_rts_g_fb_head(struct vnt_usb_send_context *tx_context,
 	u16 rts_frame_len = 20;
 
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_cck_basic_rate,
-		          PK_TYPE_11B, &buf->b);
+			  PK_TYPE_11B, &buf->b);
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
 			  tx_context->pkt_type, &buf->a);
 
@@ -683,9 +683,9 @@ static u16 vnt_rxtx_ab(struct vnt_usb_send_context *tx_context,
 }
 
 static u16 vnt_generate_tx_parameter(struct vnt_usb_send_context *tx_context,
-	                              struct vnt_tx_buffer *tx_buffer,
-	                              struct vnt_mic_hdr **mic_hdr, u32 need_mic,
-	                              bool need_rts)
+				      struct vnt_tx_buffer *tx_buffer,
+				      struct vnt_mic_hdr **mic_hdr, u32 need_mic,
+				      bool need_rts)
 {
 
 	if (tx_context->pkt_type == PK_TYPE_11GB ||
@@ -1024,7 +1024,7 @@ static int vnt_beacon_xmit(struct vnt_private *priv,
 
 		/* Get SignalField,ServiceField,Length */
 		vnt_get_phy_field(priv, frame_size, current_rate,
-			          PK_TYPE_11A, &short_head->ab);
+				  PK_TYPE_11A, &short_head->ab);
 
 		/* Get Duration and TimeStampOff */
 		short_head->duration = vnt_get_duration_le(priv,
@@ -1101,7 +1101,7 @@ 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)
+		       struct ieee80211_bss_conf *conf)
 {
 	vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
 
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index 1ae6a64c7fd4..dc11a05be8c4 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -47,15 +47,25 @@ int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
 		     u16 index, u16 length, u8 *buffer)
 {
 	int status = 0;
+	u8 *usb_buffer;
 
 	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags))
 		return STATUS_FAILURE;
 
 	mutex_lock(&priv->usb_lock);
 
+	usb_buffer = kmemdup(buffer, length, GFP_KERNEL);
+	if (!usb_buffer) {
+		mutex_unlock(&priv->usb_lock);
+		return -ENOMEM;
+	}
+
 	status = usb_control_msg(priv->usb,
-		usb_sndctrlpipe(priv->usb, 0), request, 0x40, value,
-			index, buffer, length, USB_CTL_WAIT);
+				 usb_sndctrlpipe(priv->usb, 0),
+				 request, 0x40, value,
+				 index, usb_buffer, length, USB_CTL_WAIT);
+
+	kfree(usb_buffer);
 
 	mutex_unlock(&priv->usb_lock);
 
@@ -75,15 +85,28 @@ int vnt_control_in(struct vnt_private *priv, u8 request, u16 value,
 		    u16 index, u16 length, u8 *buffer)
 {
 	int status;
+	u8 *usb_buffer;
 
 	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags))
 		return STATUS_FAILURE;
 
 	mutex_lock(&priv->usb_lock);
 
+	usb_buffer = kmalloc(length, GFP_KERNEL);
+	if (!usb_buffer) {
+		mutex_unlock(&priv->usb_lock);
+		return -ENOMEM;
+	}
+
 	status = usb_control_msg(priv->usb,
-		                usb_rcvctrlpipe(priv->usb, 0), request, 0xc0, value,
-			        index, buffer, length, USB_CTL_WAIT);
+				 usb_rcvctrlpipe(priv->usb, 0),
+				 request, 0xc0, value,
+				 index, usb_buffer, length, USB_CTL_WAIT);
+
+	if (status == length)
+		memcpy(buffer, usb_buffer, length);
+
+	kfree(usb_buffer);
 
 	mutex_unlock(&priv->usb_lock);
 
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 9f6cc2ef08dd..b2fc17f1381b 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -45,7 +45,6 @@ static void vnt_cmd_timer_wait(struct vnt_private *priv, unsigned long msecs)
 
 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 */
@@ -165,7 +164,6 @@ void vnt_run_command(struct work_struct *work)
 
 int vnt_schedule_command(struct vnt_private *priv, enum vnt_cmd command)
 {
-
 	if (priv->free_cmd_queue == 0)
 		return false;
 
@@ -178,7 +176,6 @@ int vnt_schedule_command(struct vnt_private *priv, enum vnt_cmd command)
 		vnt_cmd_complete(priv);
 
 	return true;
-
 }
 
 void vnt_reset_command_timer(struct vnt_private *priv)
diff --git a/drivers/staging/wilc1000/coreconfigurator.h b/drivers/staging/wilc1000/coreconfigurator.h
index cff16984167b..5256f40524bf 100644
--- a/drivers/staging/wilc1000/coreconfigurator.h
+++ b/drivers/staging/wilc1000/coreconfigurator.h
@@ -70,10 +70,10 @@ enum connect_status {
 	CONNECT_STS_FORCE_16_BIT = 0xFFFF
 };
 
-struct tstrRSSI {
-	u8 u8Full;
-	u8 u8Index;
-	s8 as8RSSI[NUM_RSSI];
+struct rssi_history_buffer {
+	bool full;
+	u8 index;
+	s8 samples[NUM_RSSI];
 };
 
 struct network_info {
@@ -93,7 +93,7 @@ struct network_info {
 	u8 *ies;
 	u16 ies_len;
 	void *join_params;
-	struct tstrRSSI str_rssi;
+	struct rssi_history_buffer rssi_history;
 	u64 tsf_hi;
 };
 
@@ -124,10 +124,7 @@ s32 wilc_parse_network_info(u8 *msg_buffer,
 			    struct network_info **ret_network_info);
 s32 wilc_parse_assoc_resp_info(u8 *buffer, u32 buffer_len,
 			       struct connect_resp_info **ret_connect_resp_info);
-void wilc_scan_complete_received(struct wilc *wilc, u8 *pu8Buffer,
-				 u32 u32Length);
-void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
-				u32 u32Length);
-void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
-				   u32 u32Length);
+void wilc_scan_complete_received(struct wilc *wilc, u8 *buffer, u32 length);
+void wilc_network_info_received(struct wilc *wilc, u8 *buffer, u32 length);
+void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *buffer, u32 length);
 #endif
diff --git a/drivers/staging/wilc1000/host_interface.c b/drivers/staging/wilc1000/host_interface.c
index c307ccef790e..c3a8af081880 100644
--- a/drivers/staging/wilc1000/host_interface.c
+++ b/drivers/staging/wilc1000/host_interface.c
@@ -287,7 +287,6 @@ static int wilc_enqueue_cmd(struct host_if_msg *msg)
 	return 0;
 }
 
-
 /* The u8IfIdx starts from 0 to NUM_CONCURRENT_IFC -1, but 0 index used as
  * special purpose in wilc device, so we add 1 to the index to starts from 1.
  * As a result, the returned index will be 1 to NUM_CONCURRENT_IFC.
@@ -385,7 +384,7 @@ static void handle_set_ip_address(struct wilc_vif *vif, u8 *ip_addr, u8 idx)
 
 	wid.id = (u16)WID_IP_ADDRESS;
 	wid.type = WID_STR;
-	wid.val = (u8 *)ip_addr;
+	wid.val = ip_addr;
 	wid.size = IP_ALEN;
 
 	ret = wilc_send_config_pkt(vif, SET_CFG, &wid, 1,
@@ -1350,19 +1349,17 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct wilc_vif *vif,
 
 				if (u32RcvdAssocRespInfoLen != 0) {
 					s32Err = wilc_parse_assoc_resp_info(rcv_assoc_resp, u32RcvdAssocRespInfoLen,
-								    &pstrConnectRespInfo);
+									    &pstrConnectRespInfo);
 					if (s32Err) {
 						netdev_err(vif->ndev, "wilc_parse_assoc_resp_info() returned error %d\n", s32Err);
 					} else {
 						strConnectInfo.status = pstrConnectRespInfo->status;
 
-						if (strConnectInfo.status == SUCCESSFUL_STATUSCODE) {
-							if (pstrConnectRespInfo->ies) {
-								strConnectInfo.resp_ies_len = pstrConnectRespInfo->ies_len;
-								strConnectInfo.resp_ies = kmalloc(pstrConnectRespInfo->ies_len, GFP_KERNEL);
-								memcpy(strConnectInfo.resp_ies, pstrConnectRespInfo->ies,
-								       pstrConnectRespInfo->ies_len);
-							}
+						if (strConnectInfo.status == SUCCESSFUL_STATUSCODE && pstrConnectRespInfo->ies) {
+							strConnectInfo.resp_ies_len = pstrConnectRespInfo->ies_len;
+							strConnectInfo.resp_ies = kmalloc(pstrConnectRespInfo->ies_len, GFP_KERNEL);
+							memcpy(strConnectInfo.resp_ies, pstrConnectRespInfo->ies,
+							       pstrConnectRespInfo->ies_len);
 						}
 
 						if (pstrConnectRespInfo) {
@@ -1928,6 +1925,8 @@ static s32 Handle_Get_InActiveTime(struct wilc_vif *vif,
 	wid.type = WID_STR;
 	wid.size = ETH_ALEN;
 	wid.val = kmalloc(wid.size, GFP_KERNEL);
+	if (!wid.val)
+		return -ENOMEM;
 
 	stamac = wid.val;
 	ether_addr_copy(stamac, strHostIfStaInactiveT->mac);
@@ -3348,10 +3347,6 @@ int wilc_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 	init_completion(&hif_drv->comp_inactive_time);
 
 	if (clients_count == 0)	{
-		if (result < 0) {
-			netdev_err(vif->ndev, "Failed to creat MQ\n");
-			goto _fail_;
-		}
 		hif_workqueue = create_singlethread_workqueue("WILC_wq");
 		if (!hif_workqueue) {
 			netdev_err(vif->ndev, "Failed to create workqueue\n");
@@ -3444,8 +3439,7 @@ int wilc_deinit(struct wilc_vif *vif)
 	return result;
 }
 
-void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
-				u32 u32Length)
+void wilc_network_info_received(struct wilc *wilc, u8 *buffer, u32 length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3453,7 +3447,7 @@ void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
 	struct host_if_drv *hif_drv = NULL;
 	struct wilc_vif *vif;
 
-	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	id = ((buffer[length - 4]) | (buffer[length - 3] << 8) | (buffer[length - 2] << 16) | (buffer[length - 1] << 24));
 	vif = wilc_get_vif_from_idx(wilc, id);
 	if (!vif)
 		return;
@@ -3469,17 +3463,16 @@ void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
 	msg.id = HOST_IF_MSG_RCVD_NTWRK_INFO;
 	msg.vif = vif;
 
-	msg.body.net_info.len = u32Length;
-	msg.body.net_info.buffer = kmalloc(u32Length, GFP_KERNEL);
-	memcpy(msg.body.net_info.buffer, pu8Buffer, u32Length);
+	msg.body.net_info.len = length;
+	msg.body.net_info.buffer = kmalloc(length, GFP_KERNEL);
+	memcpy(msg.body.net_info.buffer, buffer, length);
 
 	result = wilc_enqueue_cmd(&msg);
 	if (result)
 		netdev_err(vif->ndev, "message parameters (%d)\n", result);
 }
 
-void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
-				   u32 u32Length)
+void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *buffer, u32 length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3489,7 +3482,7 @@ void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
 
 	mutex_lock(&hif_deinit_lock);
 
-	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	id = ((buffer[length - 4]) | (buffer[length - 3] << 8) | (buffer[length - 2] << 16) | (buffer[length - 1] << 24));
 	vif = wilc_get_vif_from_idx(wilc, id);
 	if (!vif) {
 		mutex_unlock(&hif_deinit_lock);
@@ -3514,9 +3507,9 @@ void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
 	msg.id = HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO;
 	msg.vif = vif;
 
-	msg.body.async_info.len = u32Length;
-	msg.body.async_info.buffer = kmalloc(u32Length, GFP_KERNEL);
-	memcpy(msg.body.async_info.buffer, pu8Buffer, u32Length);
+	msg.body.async_info.len = length;
+	msg.body.async_info.buffer = kmalloc(length, GFP_KERNEL);
+	memcpy(msg.body.async_info.buffer, buffer, length);
 
 	result = wilc_enqueue_cmd(&msg);
 	if (result)
@@ -3525,8 +3518,7 @@ void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
 	mutex_unlock(&hif_deinit_lock);
 }
 
-void wilc_scan_complete_received(struct wilc *wilc, u8 *pu8Buffer,
-				 u32 u32Length)
+void wilc_scan_complete_received(struct wilc *wilc, u8 *buffer, u32 length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3534,7 +3526,7 @@ void wilc_scan_complete_received(struct wilc *wilc, u8 *pu8Buffer,
 	struct host_if_drv *hif_drv = NULL;
 	struct wilc_vif *vif;
 
-	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
+	id = ((buffer[length - 4]) | (buffer[length - 3] << 8) | (buffer[length - 2] << 16) | (buffer[length - 1] << 24));
 	vif = wilc_get_vif_from_idx(wilc, id);
 	if (!vif)
 		return;
@@ -3892,7 +3884,6 @@ static void *host_int_ParseJoinBssParam(struct network_info *ptstrNetworkInfo)
 					pNewJoinBssParam->supp_rates[i + 1] = pu8IEs[index + i];
 
 				index += suppRatesNo;
-				continue;
 			} else if (pu8IEs[index] == EXT_SUPP_RATES_IE) {
 				extSuppRatesNo = pu8IEs[index + 1];
 				if (extSuppRatesNo > (MAX_RATES_SUPPORTED - suppRatesNo))
@@ -3904,11 +3895,9 @@ static void *host_int_ParseJoinBssParam(struct network_info *ptstrNetworkInfo)
 					pNewJoinBssParam->supp_rates[suppRatesNo + i + 1] = pu8IEs[index + i];
 
 				index += extSuppRatesNo;
-				continue;
 			} else if (pu8IEs[index] == HT_CAPABILITY_IE) {
 				pNewJoinBssParam->ht_capable = true;
 				index += pu8IEs[index + 1] + 2;
-				continue;
 			} else if ((pu8IEs[index] == WMM_IE) &&
 				   (pu8IEs[index + 2] == 0x00) && (pu8IEs[index + 3] == 0x50) &&
 				   (pu8IEs[index + 4] == 0xF2) &&
@@ -3920,7 +3909,6 @@ static void *host_int_ParseJoinBssParam(struct network_info *ptstrNetworkInfo)
 				if (pu8IEs[index + 8] & BIT(7))
 					pNewJoinBssParam->uapsd_cap = true;
 				index += pu8IEs[index + 1] + 2;
-				continue;
 			} else if ((pu8IEs[index] == P2P_IE) &&
 				 (pu8IEs[index + 2] == 0x50) && (pu8IEs[index + 3] == 0x6f) &&
 				 (pu8IEs[index + 4] == 0x9a) &&
@@ -3950,8 +3938,6 @@ static void *host_int_ParseJoinBssParam(struct network_info *ptstrNetworkInfo)
 				memcpy(pNewJoinBssParam->start_time, pu8IEs + u16P2P_count, 4);
 
 				index += pu8IEs[index + 1] + 2;
-				continue;
-
 			} else if ((pu8IEs[index] == RSN_IE) ||
 				 ((pu8IEs[index] == WPA_IE) && (pu8IEs[index + 2] == 0x00) &&
 				  (pu8IEs[index + 3] == 0x50) && (pu8IEs[index + 4] == 0xF2) &&
@@ -3997,7 +3983,6 @@ static void *host_int_ParseJoinBssParam(struct network_info *ptstrNetworkInfo)
 				}
 				pNewJoinBssParam->rsn_found = true;
 				index += pu8IEs[index + 1] + 2;
-				continue;
 			} else {
 				index += pu8IEs[index + 1] + 2;
 			}
diff --git a/drivers/staging/wilc1000/linux_mon.c b/drivers/staging/wilc1000/linux_mon.c
index f328d75de0d1..c9782d452b07 100644
--- a/drivers/staging/wilc1000/linux_mon.c
+++ b/drivers/staging/wilc1000/linux_mon.c
@@ -197,6 +197,8 @@ static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
 
 	if (skb->data[0] == 0xc0 && (!(memcmp(broadcast, &skb->data[4], 6)))) {
 		skb2 = dev_alloc_skb(skb->len + sizeof(struct wilc_wfi_radiotap_cb_hdr));
+		if (!skb2)
+			return -ENOMEM;
 
 		memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
 
diff --git a/drivers/staging/wilc1000/linux_wlan.c b/drivers/staging/wilc1000/linux_wlan.c
index 2eebc6215cac..d6d803416be2 100644
--- a/drivers/staging/wilc1000/linux_wlan.c
+++ b/drivers/staging/wilc1000/linux_wlan.c
@@ -1074,7 +1074,7 @@ static int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
 {
 	u8 *buff = NULL;
 	s8 rssi;
-	u32 size = 0, length = 0;
+	u32 size = 0;
 	struct wilc_vif *vif;
 	s32 ret = 0;
 	struct wilc *wilc;
@@ -1098,7 +1098,7 @@ static int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
 			if (IS_ERR(buff))
 				return PTR_ERR(buff);
 
-			if (strncasecmp(buff, "RSSI", length) == 0) {
+			if (strncasecmp(buff, "RSSI", size) == 0) {
 				ret = wilc_get_rssi(vif, &rssi);
 				netdev_info(ndev, "RSSI :%d\n", rssi);
 
@@ -1251,11 +1251,12 @@ int wilc_netdev_init(struct wilc **wilc, struct device *dev, int io_type,
 		else
 			strcpy(ndev->name, "p2p%d");
 
-		vif->idx = wl->vif_num;
 		vif->wilc = *wilc;
 		vif->ndev = ndev;
 		wl->vif[i] = vif;
 		wl->vif_num = i;
+		vif->idx = wl->vif_num;
+
 		ndev->netdev_ops = &wilc_netdev_ops;
 
 		{
diff --git a/drivers/staging/wilc1000/wilc_sdio.c b/drivers/staging/wilc1000/wilc_sdio.c
index cd6b8badc5f3..0189e3edbbbe 100644
--- a/drivers/staging/wilc1000/wilc_sdio.c
+++ b/drivers/staging/wilc1000/wilc_sdio.c
@@ -1,11 +1,8 @@
-/* ////////////////////////////////////////////////////////////////////////// */
-/*  */
-/* Copyright (c) Atmel Corporation.  All rights reserved. */
-/*  */
-/* Module Name:  wilc_sdio.c */
-/*  */
-/*  */
-/* //////////////////////////////////////////////////////////////////////////// */
+/*
+ * Copyright (c) Atmel Corporation.  All rights reserved.
+ *
+ * Module Name:  wilc_sdio.c
+ */
 
 #include <linux/string.h>
 #include "wilc_wlan_if.h"
@@ -77,7 +74,7 @@ static int wilc_sdio_cmd52(struct wilc *wilc, struct sdio_cmd52 *cmd)
 	sdio_release_host(func);
 
 	if (ret)
-		dev_err(&func->dev, "wilc_sdio_cmd52..failed, err(%d)\n", ret);
+		dev_err(&func->dev, "%s..failed, err(%d)\n", __func__, ret);
 	return ret;
 }
 
@@ -106,7 +103,7 @@ static int wilc_sdio_cmd53(struct wilc *wilc, struct sdio_cmd53 *cmd)
 	sdio_release_host(func);
 
 	if (ret)
-		dev_err(&func->dev, "wilc_sdio_cmd53..failed, err(%d)\n", ret);
+		dev_err(&func->dev, "%s..failed, err(%d)\n", __func__,  ret);
 
 	return ret;
 }
@@ -246,15 +243,11 @@ static void wilc_sdio_disable_interrupt(struct wilc *dev)
 	struct sdio_func *func = container_of(dev->dev, struct sdio_func, dev);
 	int ret;
 
-	dev_dbg(&func->dev, "wilc_sdio_disable_interrupt IN\n");
-
 	sdio_claim_host(func);
 	ret = sdio_release_irq(func);
 	if (ret < 0)
 		dev_err(&func->dev, "can't release sdio_irq, err(%d)\n", ret);
 	sdio_release_host(func);
-
-	dev_info(&func->dev, "wilc_sdio_disable_interrupt OUT\n");
 }
 
 /********************************************
diff --git a/drivers/staging/wilc1000/wilc_spi.c b/drivers/staging/wilc1000/wilc_spi.c
index 55d53c3a95df..5ef84410e0f2 100644
--- a/drivers/staging/wilc1000/wilc_spi.c
+++ b/drivers/staging/wilc1000/wilc_spi.c
@@ -1,11 +1,9 @@
-/* ////////////////////////////////////////////////////////////////////////// */
-/*  */
-/* Copyright (c) Atmel Corporation.  All rights reserved. */
-/*  */
-/* Module Name:  wilc_spi.c */
-/*  */
-/*  */
-/* //////////////////////////////////////////////////////////////////////////// */
+/*
+ * Copyright (c) Atmel Corporation.  All rights reserved.
+ *
+ * Module Name:  wilc_spi.c
+ */
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -410,7 +408,7 @@ static int spi_cmd_complete(struct wilc *wilc, u8 cmd, u32 adr, u8 *b, u32 sz,
 
 	if (len2 > ARRAY_SIZE(wb)) {
 		dev_err(&spi->dev, "spi buffer size too small (%d) (%zu)\n",
-			 len2, ARRAY_SIZE(wb));
+			len2, ARRAY_SIZE(wb));
 		return N_FAIL;
 	}
 	/* zero spi write buffers. */
@@ -454,8 +452,8 @@ static int spi_cmd_complete(struct wilc *wilc, u8 cmd, u32 adr, u8 *b, u32 sz,
 		return N_FAIL;
 	}
 
-	if ((cmd == CMD_INTERNAL_READ) || (cmd == CMD_SINGLE_READ)
-	    || (cmd == CMD_DMA_READ) || (cmd == CMD_DMA_EXT_READ)) {
+	if ((cmd == CMD_INTERNAL_READ) || (cmd == CMD_SINGLE_READ) ||
+	    (cmd == CMD_DMA_READ) || (cmd == CMD_DMA_EXT_READ)) {
 		int retry;
 		/* u16 crc1, crc2; */
 		u8 crc[2];
@@ -929,14 +927,16 @@ static int wilc_spi_read_int(struct wilc *wilc, u32 *int_status)
 {
 	struct spi_device *spi = to_spi_device(wilc->dev);
 	int ret;
+	u32 tmp;
+	u32 byte_cnt;
+	int happened, j;
+	u32 unknown_mask;
+	u32 irq_flags;
 
 	if (g_spi.has_thrpt_enh) {
 		ret = spi_internal_read(wilc, 0xe840 - WILC_SPI_REG_BASE,
 					int_status);
 	} else {
-		u32 tmp;
-		u32 byte_cnt;
-
 		ret = wilc_spi_read_reg(wilc, WILC_VMM_TO_HOST_SIZE,
 					&byte_cnt);
 		if (!ret) {
@@ -946,37 +946,28 @@ static int wilc_spi_read_int(struct wilc *wilc, u32 *int_status)
 		}
 		tmp = (byte_cnt >> 2) & IRQ_DMA_WD_CNT_MASK;
 
-		{
-			int happended, j;
-
-			j = 0;
-			do {
-				u32 irq_flags;
-
-				happended = 0;
+		j = 0;
+		do {
+			happened = 0;
 
-				wilc_spi_read_reg(wilc, 0x1a90, &irq_flags);
-				tmp |= ((irq_flags >> 27) << IRG_FLAGS_OFFSET);
+			wilc_spi_read_reg(wilc, 0x1a90, &irq_flags);
+			tmp |= ((irq_flags >> 27) << IRG_FLAGS_OFFSET);
 
-				if (g_spi.nint > 5) {
-					wilc_spi_read_reg(wilc, 0x1a94,
-							  &irq_flags);
-					tmp |= (((irq_flags >> 0) & 0x7) << (IRG_FLAGS_OFFSET + 5));
-				}
+			if (g_spi.nint > 5) {
+				wilc_spi_read_reg(wilc, 0x1a94,
+						  &irq_flags);
+				tmp |= (((irq_flags >> 0) & 0x7) << (IRG_FLAGS_OFFSET + 5));
+			}
 
-				{
-					u32 unkmown_mask;
+			unknown_mask = ~((1ul << g_spi.nint) - 1);
 
-					unkmown_mask = ~((1ul << g_spi.nint) - 1);
+			if ((tmp >> IRG_FLAGS_OFFSET) & unknown_mask) {
+				dev_err(&spi->dev, "Unexpected interrupt (2): j=%d, tmp=%x, mask=%x\n", j, tmp, unknown_mask);
+					happened = 1;
+			}
 
-					if ((tmp >> IRG_FLAGS_OFFSET) & unkmown_mask) {
-						dev_err(&spi->dev, "Unexpected interrupt (2): j=%d, tmp=%x, mask=%x\n", j, tmp, unkmown_mask);
-						happended = 1;
-					}
-				}
-				j++;
-			} while (happended);
-		}
+			j++;
+		} while (happened);
 
 		*int_status = tmp;
 	}
@@ -1130,11 +1121,8 @@ static int wilc_spi_sync_ext(struct wilc *wilc, int nint)
 
 	return 1;
 }
-/********************************************
- *
- *      Global spi HIF function table
- *
- ********************************************/
+
+/* Global spi HIF function table */
 static const struct wilc_hif_func wilc_hif_spi = {
 	.hif_init = wilc_spi_init,
 	.hif_deinit = _wilc_spi_deinit,
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
index 2b4536318ca6..44a12bdd6a46 100644
--- a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
@@ -29,8 +29,8 @@
 #define P2P_INV_REQ			0x03
 #define P2P_INV_RSP			0x04
 #define PUBLIC_ACT_VENDORSPEC		0x09
-#define GAS_INTIAL_REQ			0x0a
-#define GAS_INTIAL_RSP			0x0b
+#define GAS_INITIAL_REQ			0x0a
+#define GAS_INITIAL_RSP			0x0b
 
 #define INVALID_CHANNEL			0
 
@@ -205,11 +205,11 @@ static u32 get_rssi_avg(struct network_info *network_info)
 {
 	u8 i;
 	int rssi_v = 0;
-	u8 num_rssi = (network_info->str_rssi.u8Full) ?
-		       NUM_RSSI : (network_info->str_rssi.u8Index);
+	u8 num_rssi = (network_info->rssi_history.full) ?
+		       NUM_RSSI : (network_info->rssi_history.index);
 
 	for (i = 0; i < num_rssi; i++)
-		rssi_v += network_info->str_rssi.as8RSSI[i];
+		rssi_v += network_info->rssi_history.samples[i];
 
 	rssi_v /= num_rssi;
 	return rssi_v;
@@ -346,13 +346,13 @@ static void add_network_to_shadow(struct network_info *pstrNetworkInfo,
 	} else {
 		ap_index = ap_found;
 	}
-	rssi_index = last_scanned_shadow[ap_index].str_rssi.u8Index;
-	last_scanned_shadow[ap_index].str_rssi.as8RSSI[rssi_index++] = pstrNetworkInfo->rssi;
+	rssi_index = last_scanned_shadow[ap_index].rssi_history.index;
+	last_scanned_shadow[ap_index].rssi_history.samples[rssi_index++] = pstrNetworkInfo->rssi;
 	if (rssi_index == NUM_RSSI) {
 		rssi_index = 0;
-		last_scanned_shadow[ap_index].str_rssi.u8Full = 1;
+		last_scanned_shadow[ap_index].rssi_history.full = true;
 	}
-	last_scanned_shadow[ap_index].str_rssi.u8Index = rssi_index;
+	last_scanned_shadow[ap_index].rssi_history.index = rssi_index;
 	last_scanned_shadow[ap_index].rssi = pstrNetworkInfo->rssi;
 	last_scanned_shadow[ap_index].cap_info = pstrNetworkInfo->cap_info;
 	last_scanned_shadow[ap_index].ssid_len = pstrNetworkInfo->ssid_len;
@@ -765,8 +765,8 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 		}
 	}
 
-	if ((sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)
-	    || (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
+	if ((sme->crypto.wpa_versions & NL80211_WPA_VERSION_1) ||
+	    (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
 		for (i = 0; i < sme->crypto.n_ciphers_pairwise; i++) {
 			if (sme->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_TKIP)
 				u8security = u8security | TKIP;
@@ -1301,16 +1301,16 @@ static int set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 
 	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
 		if (!memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
-				 ETH_ALEN)) {
+			    ETH_ALEN)) {
 			flag = PMKID_FOUND;
 			break;
 		}
 	}
 	if (i < WILC_MAX_NUM_PMKIDS) {
 		memcpy(priv->pmkid_list.pmkidlist[i].bssid, pmksa->bssid,
-			    ETH_ALEN);
+		       ETH_ALEN);
 		memcpy(priv->pmkid_list.pmkidlist[i].pmkid, pmksa->pmkid,
-			    PMKID_LEN);
+		       PMKID_LEN);
 		if (!(flag == PMKID_FOUND))
 			priv->pmkid_list.numpmkid++;
 	} else {
@@ -1334,7 +1334,7 @@ static int del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 
 	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
 		if (!memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
-				 ETH_ALEN)) {
+			    ETH_ALEN)) {
 			memset(&priv->pmkid_list.pmkidlist[i], 0, sizeof(struct host_if_pmkid));
 			break;
 		}
@@ -1343,11 +1343,11 @@ static int del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 	if (i < priv->pmkid_list.numpmkid && priv->pmkid_list.numpmkid > 0) {
 		for (; i < (priv->pmkid_list.numpmkid - 1); i++) {
 			memcpy(priv->pmkid_list.pmkidlist[i].bssid,
-				    priv->pmkid_list.pmkidlist[i + 1].bssid,
-				    ETH_ALEN);
+			       priv->pmkid_list.pmkidlist[i + 1].bssid,
+			       ETH_ALEN);
 			memcpy(priv->pmkid_list.pmkidlist[i].pmkid,
-				    priv->pmkid_list.pmkidlist[i].pmkid,
-				    PMKID_LEN);
+			       priv->pmkid_list.pmkidlist[i + 1].pmkid,
+			       PMKID_LEN);
 		}
 		priv->pmkid_list.numpmkid--;
 	} else {
@@ -1477,10 +1477,10 @@ void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size)
 			}
 			if (buff[ACTION_CAT_ID] == PUB_ACTION_ATTR_ID) {
 				switch (buff[ACTION_SUBTYPE_ID]) {
-				case GAS_INTIAL_REQ:
+				case GAS_INITIAL_REQ:
 					break;
 
-				case GAS_INTIAL_RSP:
+				case GAS_INITIAL_RSP:
 					break;
 
 				case PUBLIC_ACT_VENDORSPEC:
@@ -1497,8 +1497,8 @@ void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size)
 							}
 						}
 						if (p2p_local_random > p2p_recv_random)	{
-							if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
-							      || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
+							if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP ||
+							     buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
 								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < size; i++) {
 									if (buff[i] == P2PELEM_ATTR_ID && !(memcmp(p2p_oui, &buff[i + 2], 4))) {
 										WILC_WFI_CfgParseRxAction(&buff[i + 6], size - (i + 6));
@@ -1666,10 +1666,10 @@ static int mgmt_tx(struct wiphy *wiphy,
 					curr_channel = chan->hw_value;
 				}
 				switch (buf[ACTION_SUBTYPE_ID])	{
-				case GAS_INTIAL_REQ:
+				case GAS_INITIAL_REQ:
 					break;
 
-				case GAS_INTIAL_RSP:
+				case GAS_INITIAL_RSP:
 					break;
 
 				case PUBLIC_ACT_VENDORSPEC:
@@ -1682,8 +1682,8 @@ static int mgmt_tx(struct wiphy *wiphy,
 							}
 						}
 
-						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
-						      || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
+						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP ||
+						     buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
 							if (p2p_local_random > p2p_recv_random)	{
 								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < len; i++) {
 									if (buf[i] == P2PELEM_ATTR_ID && !(memcmp(p2p_oui, &buf[i + 2], 4))) {
diff --git a/drivers/staging/wilc1000/wilc_wlan.c b/drivers/staging/wilc1000/wilc_wlan.c
index bc5ad20af0a3..9addef1f1e12 100644
--- a/drivers/staging/wilc1000/wilc_wlan.c
+++ b/drivers/staging/wilc1000/wilc_wlan.c
@@ -287,7 +287,7 @@ static int wilc_wlan_txq_filter_dup_tcp_ack(struct net_device *dev)
 
 	while (dropped > 0) {
 		wait_for_completion_timeout(&wilc->txq_event,
-						msecs_to_jiffies(1));
+					    msecs_to_jiffies(1));
 		dropped--;
 	}
 
@@ -810,9 +810,9 @@ static void wilc_wlan_handle_rxq(struct wilc *wilc)
 				if (!is_cfg_packet) {
 					if (pkt_len > 0) {
 						wilc_frmw_to_linux(wilc,
-							      &buffer[offset],
-							      pkt_len,
-							      pkt_offset);
+								   &buffer[offset],
+								   pkt_len,
+								   pkt_offset);
 					}
 				} else {
 					struct wilc_cfg_rsp rsp;
@@ -1226,7 +1226,7 @@ int wilc_wlan_cfg_set(struct wilc_vif *vif, int start, u16 wid, u8 *buffer,
 			ret_size = 0;
 
 		if (!wait_for_completion_timeout(&wilc->cfg_event,
-					msecs_to_jiffies(CFG_PKTS_TIMEOUT))) {
+						 msecs_to_jiffies(CFG_PKTS_TIMEOUT))) {
 			netdev_dbg(vif->ndev, "Set Timed Out\n");
 			ret_size = 0;
 		}
diff --git a/drivers/staging/wilc1000/wilc_wlan.h b/drivers/staging/wilc1000/wilc_wlan.h
index 11365efcc5d0..7a5eba9b5f47 100644
--- a/drivers/staging/wilc1000/wilc_wlan.h
+++ b/drivers/staging/wilc1000/wilc_wlan.h
@@ -274,8 +274,8 @@ struct wilc_vif;
 
 int wilc_wlan_firmware_download(struct wilc *wilc, const u8 *buffer,
 				u32 buffer_size);
-int wilc_wlan_start(struct wilc *);
-int wilc_wlan_stop(struct wilc *);
+int wilc_wlan_start(struct wilc *wilc);
+int wilc_wlan_stop(struct wilc *wilc);
 int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 			      u32 buffer_size, wilc_tx_complete_func_t func);
 int wilc_wlan_handle_txq(struct net_device *dev, u32 *txq_count);
@@ -291,7 +291,7 @@ int wilc_wlan_txq_add_mgmt_pkt(struct net_device *dev, void *priv, u8 *buffer,
 void wilc_chip_sleep_manually(struct wilc *wilc);
 
 void wilc_enable_tcp_ack_filter(bool value);
-int wilc_wlan_get_num_conn_ifcs(struct wilc *);
+int wilc_wlan_get_num_conn_ifcs(struct wilc *wilc);
 int wilc_mac_xmit(struct sk_buff *skb, struct net_device *dev);
 
 void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size);
diff --git a/drivers/staging/wilc1000/wilc_wlan_cfg.c b/drivers/staging/wilc1000/wilc_wlan_cfg.c
index 926fc16319b6..d3e5b1b302f4 100644
--- a/drivers/staging/wilc1000/wilc_wlan_cfg.c
+++ b/drivers/staging/wilc1000/wilc_wlan_cfg.c
@@ -175,8 +175,9 @@ static int wilc_wlan_cfg_set_byte(u8 *frame, u32 offset, u16 id, u8 val8)
 	buf[0] = (u8)id;
 	buf[1] = (u8)(id >> 8);
 	buf[2] = 1;
-	buf[3] = val8;
-	return 4;
+	buf[3] = 0;
+	buf[4] = val8;
+	return 5;
 }
 
 static int wilc_wlan_cfg_set_hword(u8 *frame, u32 offset, u16 id, u16 val16)
@@ -191,10 +192,11 @@ static int wilc_wlan_cfg_set_hword(u8 *frame, u32 offset, u16 id, u16 val16)
 	buf[0] = (u8)id;
 	buf[1] = (u8)(id >> 8);
 	buf[2] = 2;
-	buf[3] = (u8)val16;
-	buf[4] = (u8)(val16 >> 8);
+	buf[3] = 0;
+	buf[4] = (u8)val16;
+	buf[5] = (u8)(val16 >> 8);
 
-	return 5;
+	return 6;
 }
 
 static int wilc_wlan_cfg_set_word(u8 *frame, u32 offset, u16 id, u32 val32)
@@ -209,19 +211,20 @@ static int wilc_wlan_cfg_set_word(u8 *frame, u32 offset, u16 id, u32 val32)
 	buf[0] = (u8)id;
 	buf[1] = (u8)(id >> 8);
 	buf[2] = 4;
-	buf[3] = (u8)val32;
-	buf[4] = (u8)(val32 >> 8);
-	buf[5] = (u8)(val32 >> 16);
-	buf[6] = (u8)(val32 >> 24);
+	buf[3] = 0;
+	buf[4] = (u8)val32;
+	buf[5] = (u8)(val32 >> 8);
+	buf[6] = (u8)(val32 >> 16);
+	buf[7] = (u8)(val32 >> 24);
 
-	return 7;
+	return 8;
 }
 
 static int wilc_wlan_cfg_set_str(u8 *frame, u32 offset, u16 id, u8 *str, u32 size)
 {
 	u8 *buf;
 
-	if ((offset + size + 3) >= MAX_CFG_FRAME_SIZE)
+	if ((offset + size + 4) >= MAX_CFG_FRAME_SIZE)
 		return 0;
 
 	buf = &frame[offset];
@@ -229,11 +232,12 @@ static int wilc_wlan_cfg_set_str(u8 *frame, u32 offset, u16 id, u8 *str, u32 siz
 	buf[0] = (u8)id;
 	buf[1] = (u8)(id >> 8);
 	buf[2] = (u8)size;
+	buf[3] = (u8)(size >> 8);
 
 	if ((str) && (size != 0))
-		memcpy(&buf[3], str, size);
+		memcpy(&buf[4], str, size);
 
-	return (size + 3);
+	return (size + 4);
 }
 
 static int wilc_wlan_cfg_set_bin(u8 *frame, u32 offset, u16 id, u8 *b, u32 size)
@@ -284,12 +288,12 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 					break;
 
 				if (g_cfg_byte[i].id == wid) {
-					g_cfg_byte[i].val = info[3];
+					g_cfg_byte[i].val = info[4];
 					break;
 				}
 				i++;
 			} while (1);
-			len = 2;
+			len = 3;
 			break;
 
 		case WID_SHORT:
@@ -298,12 +302,14 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 					break;
 
 				if (g_cfg_hword[i].id == wid) {
-					g_cfg_hword[i].val = cpu_to_le16(info[3] | (info[4] << 8));
+					g_cfg_hword[i].val =
+						cpu_to_le16(info[4] |
+							    (info[5] << 8));
 					break;
 				}
 				i++;
 			} while (1);
-			len = 3;
+			len = 4;
 			break;
 
 		case WID_INT:
@@ -312,12 +318,16 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 					break;
 
 				if (g_cfg_word[i].id == wid) {
-					g_cfg_word[i].val = cpu_to_le32(info[3] | (info[4] << 8) | (info[5] << 16) | (info[6] << 24));
+					g_cfg_word[i].val =
+						cpu_to_le32(info[4] |
+							    (info[5] << 8) |
+							    (info[6] << 16) |
+							    (info[7] << 24));
 					break;
 				}
 				i++;
 			} while (1);
-			len = 5;
+			len = 6;
 			break;
 
 		case WID_STR:
@@ -332,12 +342,13 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 						i += toggle;
 						toggle ^= 1;
 					}
-					memcpy(g_cfg_str[i].str, &info[2], (info[2] + 1));
+					memcpy(g_cfg_str[i].str, &info[2],
+					       (info[2] + 2));
 					break;
 				}
 				i++;
 			} while (1);
-			len = 1 + info[2];
+			len = 2 + info[2];
 			break;
 
 		default:
@@ -475,7 +486,8 @@ int wilc_wlan_cfg_get_wid_value(u16 wid, u8 *buffer, u32 buffer_size)
 				break;
 
 			if (g_cfg_str[i].id == wid) {
-				u32 size =  g_cfg_str[i].str[0];
+				u32 size = g_cfg_str[i].str[0] |
+						(g_cfg_str[i].str[1] << 8);
 
 				if (buffer_size >= size) {
 					if (g_cfg_str[i].id == WID_SITE_SURVEY_RESULTS)	{
@@ -485,7 +497,8 @@ int wilc_wlan_cfg_get_wid_value(u16 wid, u8 *buffer, u32 buffer_size)
 						toggle ^= 1;
 
 					}
-					memcpy(buffer,  &g_cfg_str[i].str[1], size);
+					memcpy(buffer,  &g_cfg_str[i].str[2],
+					       size);
 					ret = size;
 				}
 				break;
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 5f1851c85f12..310e2c454590 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -482,7 +482,7 @@ struct hfa384x_tx_frame {
 	u8 address3[6];
 	u16 sequence_control;
 	u8 address4[6];
-	u16 data_len;		/* little endian format */
+	__le16 data_len;		/* little endian format */
 
 	/*-- 802.3 Header Information --*/
 
@@ -801,41 +801,41 @@ struct hfa384x_usb_txfrm {
 } __packed;
 
 struct hfa384x_usb_cmdreq {
-	u16 type;
-	u16 cmd;
-	u16 parm0;
-	u16 parm1;
-	u16 parm2;
+	__le16 type;
+	__le16 cmd;
+	__le16 parm0;
+	__le16 parm1;
+	__le16 parm2;
 	u8 pad[54];
 } __packed;
 
 struct hfa384x_usb_wridreq {
-	u16 type;
-	u16 frmlen;
-	u16 rid;
+	__le16 type;
+	__le16 frmlen;
+	__le16 rid;
 	u8 data[HFA384x_RIDDATA_MAXLEN];
 } __packed;
 
 struct hfa384x_usb_rridreq {
-	u16 type;
-	u16 frmlen;
-	u16 rid;
+	__le16 type;
+	__le16 frmlen;
+	__le16 rid;
 	u8 pad[58];
 } __packed;
 
 struct hfa384x_usb_wmemreq {
-	u16 type;
-	u16 frmlen;
-	u16 offset;
-	u16 page;
+	__le16 type;
+	__le16 frmlen;
+	__le16 offset;
+	__le16 page;
 	u8 data[HFA384x_USB_RWMEM_MAXLEN];
 } __packed;
 
 struct hfa384x_usb_rmemreq {
-	u16 type;
-	u16 frmlen;
-	u16 offset;
-	u16 page;
+	__le16 type;
+	__le16 frmlen;
+	__le16 offset;
+	__le16 page;
 	u8 pad[56];
 } __packed;
 
@@ -854,16 +854,16 @@ struct hfa384x_usb_infofrm {
 
 struct hfa384x_usb_statusresp {
 	u16 type;
-	u16 status;
-	u16 resp0;
-	u16 resp1;
-	u16 resp2;
+	__le16 status;
+	__le16 resp0;
+	__le16 resp1;
+	__le16 resp2;
 } __packed;
 
 struct hfa384x_usb_rridresp {
 	u16 type;
-	u16 frmlen;
-	u16 rid;
+	__le16 frmlen;
+	__le16 rid;
 	u8 data[HFA384x_RIDDATA_MAXLEN];
 } __packed;
 
@@ -1078,8 +1078,8 @@ struct hfa384x_pdr_end_of_pda {
 } __packed;
 
 struct hfa384x_pdrec {
-	u16 len;		/* in words */
-	u16 code;
+	__le16 len;		/* in words */
+	__le16 code;
 	union pdr {
 		struct hfa384x_pdr_pcb_partnum pcb_partnum;
 		struct hfa384x_pdr_pcb_tracenum pcb_tracenum;
@@ -1408,7 +1408,7 @@ hfa384x_drvr_setconfig_async(struct hfa384x *hw,
 static inline int
 hfa384x_drvr_setconfig16_async(struct hfa384x *hw, u16 rid, u16 val)
 {
-	u16 value = cpu_to_le16(val);
+	__le16 value = cpu_to_le16(val);
 
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL, NULL);
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 6134eba5cad4..a812e55ba1b0 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -2316,7 +2316,7 @@ 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 result = 0;
-	u16 *pda = buf;
+	__le16 *pda = buf;
 	int pdaok = 0;
 	int morepdrs = 1;
 	int currpdr = 0;	/* word offset of the current pdr */
@@ -3513,7 +3513,7 @@ static void hfa384x_int_rxmonitor(struct wlandevice *wlandev,
 
 		caphdr->version = htonl(P80211CAPTURE_VERSION);
 		caphdr->length = htonl(sizeof(struct p80211_caphdr));
-		caphdr->mactime = __cpu_to_be64(rxdesc->time) * 1000;
+		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);
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 8b0905e7c9be..a062e80361ef 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -211,7 +211,7 @@ int skb_ether_to_p80211(struct wlandevice *wlandev, u32 ethconv,
 			return -ENOMEM;
 		foo = wep_encrypt(wlandev, skb->data, p80211_wep->data,
 				  skb->len,
-				  (wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK),
+				  wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK,
 				  p80211_wep->iv, p80211_wep->icv);
 		if (foo) {
 			netdev_warn(wlandev->netdev,
diff --git a/drivers/staging/wlan-ng/p80211conv.h b/drivers/staging/wlan-ng/p80211conv.h
index 04bac2ed0e8a..66332b1fb6d5 100644
--- a/drivers/staging/wlan-ng/p80211conv.h
+++ b/drivers/staging/wlan-ng/p80211conv.h
@@ -101,20 +101,20 @@ void p80211skb_rxmeta_detach(struct sk_buff *skb);
  * Frame capture header.  (See doc/capturefrm.txt)
  */
 struct p80211_caphdr {
-	u32 version;
-	u32 length;
-	u64 mactime;
-	u64 hosttime;
-	u32 phytype;
-	u32 channel;
-	u32 datarate;
-	u32 antenna;
-	u32 priority;
-	u32 ssi_type;
-	s32 ssi_signal;
-	s32 ssi_noise;
-	u32 preamble;
-	u32 encoding;
+	__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;
 };
 
 /* buffer free method pointer type */
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index 621df98183bf..afe847722cf7 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -198,7 +198,8 @@ 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 p80211itemd *mibitem =
+		(struct p80211itemd *)mib_msg->mibattribute.data;
 	struct p80211pstrd *pstr = (struct p80211pstrd *)mibitem->data;
 	u8 *key = mibitem->data + sizeof(struct p80211pstrd);
 
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 2e349f87e738..afd877fb4557 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -604,7 +604,7 @@ static int mkimage(struct imgchunk *clist, unsigned int *ccnt)
  */
 static int mkpdrlist(struct pda *pda)
 {
-	u16 *pda16 = (u16 *)pda->buf;
+	__le16 *pda16 = (__le16 *)pda->buf;
 	int curroff;		/* in 'words' */
 
 	pda->nrec = 0;
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index 0e671c3b308d..e23a0d0e7b09 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -1168,7 +1168,6 @@ int prism2mgmt_wlansniff(struct wlandevice *wlandev, void *msgp)
 			}
 		} else {
 			result = hfa384x_drvr_disable(hw, 0);
-
 		}
 
 		netdev_info(wlandev->netdev, "monitor mode disabled\n");
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 984804b92e05..9c2b4ef2de58 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -1311,7 +1311,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 		/* 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 the
+		 * 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
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index 6930f7eb741b..b450c740f626 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -178,9 +178,9 @@ static int XGIfb_mode_rate_to_ddata(struct vb_device_info *XGI_Pr,
 		*sync |= FB_SYNC_HOR_HIGH_ACT;
 
 	*vmode = FB_VMODE_NONINTERLACED;
-	if (XGI330_RefIndex[RefreshRateTableIndex].Ext_InfoFlag & 0x0080)
+	if (XGI330_RefIndex[RefreshRateTableIndex].Ext_InfoFlag & 0x0080) {
 		*vmode = FB_VMODE_INTERLACED;
-	else {
+	} else {
 		j = 0;
 		while (XGI330_EModeIDTable[j].Ext_ModeID != 0xff) {
 			if (XGI330_EModeIDTable[j].Ext_ModeID ==
@@ -878,30 +878,14 @@ static void XGIfb_post_setmode(struct xgifb_video_info *xgifb_info)
 			}
 
 			if ((filter >= 0) && (filter <= 7)) {
+				const u8 *f = XGI_TV_filter[filter_tb].filter[filter];
+
 				pr_debug("FilterTable[%d]-%d: %*ph\n",
-					 filter_tb, filter,
-					 4, XGI_TV_filter[filter_tb].
-						   filter[filter]);
-				xgifb_reg_set(
-					XGIPART2,
-					0x35,
-					(XGI_TV_filter[filter_tb].
-						filter[filter][0]));
-				xgifb_reg_set(
-					XGIPART2,
-					0x36,
-					(XGI_TV_filter[filter_tb].
-						filter[filter][1]));
-				xgifb_reg_set(
-					XGIPART2,
-					0x37,
-					(XGI_TV_filter[filter_tb].
-						filter[filter][2]));
-				xgifb_reg_set(
-					XGIPART2,
-					0x38,
-					(XGI_TV_filter[filter_tb].
-						filter[filter][3]));
+					 filter_tb, filter, 4, f);
+				xgifb_reg_set(XGIPART2, 0x35, f[0]);
+				xgifb_reg_set(XGIPART2, 0x36, f[1]);
+				xgifb_reg_set(XGIPART2, 0x37, f[2]);
+				xgifb_reg_set(XGIPART2, 0x38, f[3]);
 			}
 		}
 	}
@@ -1480,9 +1464,9 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 
 	cr32 = xgifb_reg_get(XGICR, IND_XGI_SCRATCH_REG_CR32);
 
-	if ((cr32 & SIS_CRT1) && !XGIfb_crt1off)
+	if ((cr32 & SIS_CRT1) && !XGIfb_crt1off) {
 		XGIfb_crt1off = 0;
-	else {
+	} else {
 		if (cr32 & 0x5F)
 			XGIfb_crt1off = 1;
 		else
@@ -1500,18 +1484,19 @@ static void XGIfb_detect_VB(struct xgifb_video_info *xgifb_info)
 			xgifb_info->display2 = XGIFB_DISP_NONE;
 	}
 
-	if (XGIfb_tvplug != -1)
+	if (XGIfb_tvplug != -1) {
 		/* Override with option */
 		xgifb_info->TV_plug = XGIfb_tvplug;
-	else if (cr32 & SIS_VB_HIVISION) {
+	} else if (cr32 & SIS_VB_HIVISION) {
 		xgifb_info->TV_type = TVMODE_HIVISION;
 		xgifb_info->TV_plug = TVPLUG_SVIDEO;
-	} else if (cr32 & SIS_VB_SVIDEO)
+	} else if (cr32 & SIS_VB_SVIDEO) {
 		xgifb_info->TV_plug = TVPLUG_SVIDEO;
-	else if (cr32 & SIS_VB_COMPOSITE)
+	} else if (cr32 & SIS_VB_COMPOSITE) {
 		xgifb_info->TV_plug = TVPLUG_COMPOSITE;
-	else if (cr32 & SIS_VB_SCART)
+	} else if (cr32 & SIS_VB_SCART) {
 		xgifb_info->TV_plug = TVPLUG_SCART;
+	}
 
 	if (xgifb_info->TV_type == 0) {
 		temp = xgifb_reg_get(XGICR, 0x38);
@@ -1659,7 +1644,7 @@ static int xgifb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	xgifb_info->mmio_base = pci_resource_start(pdev, 1);
 	xgifb_info->mmio_size = pci_resource_len(pdev, 1);
 	xgifb_info->vga_base = pci_resource_start(pdev, 2) + 0x30;
-	dev_info(&pdev->dev, "Relocate IO address: %Lx [%08lx]\n",
+	dev_info(&pdev->dev, "Relocate IO address: %llx [%08lx]\n",
 		 (u64)pci_resource_start(pdev, 2),
 		 xgifb_info->vga_base);
 
@@ -1754,13 +1739,13 @@ static int xgifb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 					    xgifb_info->mmio_size);
 
 	dev_info(&pdev->dev,
-		 "Framebuffer at 0x%Lx, mapped to 0x%p, size %dk\n",
+		 "Framebuffer at 0x%llx, mapped to 0x%p, size %dk\n",
 		 (u64)xgifb_info->video_base,
 		 xgifb_info->video_vbase,
 		 xgifb_info->video_size / 1024);
 
 	dev_info(&pdev->dev,
-		 "MMIO at 0x%Lx, mapped to 0x%p, size %ldk\n",
+		 "MMIO at 0x%llx, mapped to 0x%p, size %ldk\n",
 		 (u64)xgifb_info->mmio_base, xgifb_info->mmio_vbase,
 		 xgifb_info->mmio_size / 1024);
 
diff --git a/drivers/staging/xgifb/vb_init.h b/drivers/staging/xgifb/vb_init.h
index 500cabe41a3c..e835054b87bf 100644
--- a/drivers/staging/xgifb/vb_init.h
+++ b/drivers/staging/xgifb/vb_init.h
@@ -1,6 +1,5 @@
 #ifndef _VBINIT_
 #define _VBINIT_
 unsigned char XGIInitNew(struct pci_dev *pdev);
-void XGIRegInit(struct vb_device_info *, unsigned long);
+void XGIRegInit(struct vb_device_info *XGI_Pr, unsigned long BaseAddr);
 #endif
-
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 7c7c8c8f1df3..cea128bede52 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -221,8 +221,7 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeIdIndex,
 
 	for (; XGI330_RefIndex[RefreshRateTableIndex + (*i)].ModeID ==
 	       tempbx; (*i)--) {
-		infoflag = XGI330_RefIndex[RefreshRateTableIndex + (*i)].
-				Ext_InfoFlag;
+		infoflag = XGI330_RefIndex[RefreshRateTableIndex + (*i)].Ext_InfoFlag;
 		if (infoflag & tempax)
 			return 1;
 
@@ -231,8 +230,7 @@ static unsigned char XGI_AjustCRT2Rate(unsigned short ModeIdIndex,
 	}
 
 	for ((*i) = 0;; (*i)++) {
-		infoflag = XGI330_RefIndex[RefreshRateTableIndex + (*i)].
-				Ext_InfoFlag;
+		infoflag = XGI330_RefIndex[RefreshRateTableIndex + (*i)].Ext_InfoFlag;
 		if (XGI330_RefIndex[RefreshRateTableIndex + (*i)].ModeID
 				!= tempbx) {
 			return 0;
@@ -5092,8 +5090,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 
 	i = 0;
 	do {
-		if (XGI330_RefIndex[RefreshRateTableIndex + i].
-			ModeID != ModeNo)
+		if (XGI330_RefIndex[RefreshRateTableIndex + i].ModeID != ModeNo)
 			break;
 		temp = XGI330_RefIndex[RefreshRateTableIndex + i].Ext_InfoFlag;
 		temp &= ModeTypeMask;
@@ -5105,8 +5102,7 @@ unsigned short XGI_GetRatePtrCRT2(struct xgi_hw_device_info *pXGIHWDE,
 	} while (index != 0xFFFF);
 	if (!(pVBInfo->VBInfo & SetCRT2ToRAMDAC)) {
 		if (pVBInfo->VBInfo & SetInSlaveMode) {
-			temp = XGI330_RefIndex[RefreshRateTableIndex + i - 1].
-				Ext_InfoFlag;
+			temp = XGI330_RefIndex[RefreshRateTableIndex + i - 1].Ext_InfoFlag;
 			if (temp & InterlaceMode)
 				i++;
 		}